fix macOS build (following Projucer changes made in Windows, which removed /Applications/JUCE/modules from its headers). move JUCE headers under source control, so that Windows and macOS can both build against same version of JUCE. remove AUv3 target (I think it's an iOS thing, so it will never work with this macOS fluidsynth dylib).
This commit is contained in:
Alex Birch
502
modules/juce_box2d/box2d/Dynamics/Joints/b2RevoluteJoint.cpp
Normal file
502
modules/juce_box2d/box2d/Dynamics/Joints/b2RevoluteJoint.cpp
Normal file
@ -0,0 +1,502 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||
*
|
||||
* This software is provided 'as-is', without any express or implied
|
||||
* warranty. In no event will the authors be held liable for any damages
|
||||
* arising from the use of this software.
|
||||
* Permission is granted to anyone to use this software for any purpose,
|
||||
* including commercial applications, and to alter it and redistribute it
|
||||
* freely, subject to the following restrictions:
|
||||
* 1. The origin of this software must not be misrepresented; you must not
|
||||
* claim that you wrote the original software. If you use this software
|
||||
* in a product, an acknowledgment in the product documentation would be
|
||||
* appreciated but is not required.
|
||||
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||
* misrepresented as being the original software.
|
||||
* 3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b2RevoluteJoint.h"
|
||||
#include "../b2Body.h"
|
||||
#include "../b2TimeStep.h"
|
||||
|
||||
// Point-to-point constraint
|
||||
// C = p2 - p1
|
||||
// Cdot = v2 - v1
|
||||
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
||||
// J = [-I -r1_skew I r2_skew ]
|
||||
// Identity used:
|
||||
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||
|
||||
// Motor constraint
|
||||
// Cdot = w2 - w1
|
||||
// J = [0 0 -1 0 0 1]
|
||||
// K = invI1 + invI2
|
||||
|
||||
void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
|
||||
{
|
||||
bodyA = bA;
|
||||
bodyB = bB;
|
||||
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||
referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
|
||||
}
|
||||
|
||||
b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
|
||||
: b2Joint(def)
|
||||
{
|
||||
m_localAnchorA = def->localAnchorA;
|
||||
m_localAnchorB = def->localAnchorB;
|
||||
m_referenceAngle = def->referenceAngle;
|
||||
|
||||
m_impulse.SetZero();
|
||||
m_motorImpulse = 0.0f;
|
||||
|
||||
m_lowerAngle = def->lowerAngle;
|
||||
m_upperAngle = def->upperAngle;
|
||||
m_maxMotorTorque = def->maxMotorTorque;
|
||||
m_motorSpeed = def->motorSpeed;
|
||||
m_enableLimit = def->enableLimit;
|
||||
m_enableMotor = def->enableMotor;
|
||||
m_limitState = e_inactiveLimit;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||
{
|
||||
m_indexA = m_bodyA->m_islandIndex;
|
||||
m_indexB = m_bodyB->m_islandIndex;
|
||||
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||
m_invMassA = m_bodyA->m_invMass;
|
||||
m_invMassB = m_bodyB->m_invMass;
|
||||
m_invIA = m_bodyA->m_invI;
|
||||
m_invIB = m_bodyB->m_invI;
|
||||
|
||||
float32 aA = data.positions[m_indexA].a;
|
||||
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||
float32 wA = data.velocities[m_indexA].w;
|
||||
|
||||
float32 aB = data.positions[m_indexB].a;
|
||||
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||
float32 wB = data.velocities[m_indexB].w;
|
||||
|
||||
b2Rot qA(aA), qB(aB);
|
||||
|
||||
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||
|
||||
// J = [-I -r1_skew I r2_skew]
|
||||
// [ 0 -1 0 1]
|
||||
// r_skew = [-ry; rx]
|
||||
|
||||
// Matlab
|
||||
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
||||
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
||||
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
||||
|
||||
float32 mA = m_invMassA, mB = m_invMassB;
|
||||
float32 iA = m_invIA, iB = m_invIB;
|
||||
|
||||
bool fixedRotation = (iA + iB == 0.0f);
|
||||
|
||||
m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
|
||||
m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
|
||||
m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
|
||||
m_mass.ex.y = m_mass.ey.x;
|
||||
m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
|
||||
m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
|
||||
m_mass.ex.z = m_mass.ez.x;
|
||||
m_mass.ey.z = m_mass.ez.y;
|
||||
m_mass.ez.z = iA + iB;
|
||||
|
||||
m_motorMass = iA + iB;
|
||||
if (m_motorMass > 0.0f)
|
||||
{
|
||||
m_motorMass = 1.0f / m_motorMass;
|
||||
}
|
||||
|
||||
if (m_enableMotor == false || fixedRotation)
|
||||
{
|
||||
m_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
if (m_enableLimit && fixedRotation == false)
|
||||
{
|
||||
float32 jointAngle = aB - aA - m_referenceAngle;
|
||||
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
|
||||
{
|
||||
m_limitState = e_equalLimits;
|
||||
}
|
||||
else if (jointAngle <= m_lowerAngle)
|
||||
{
|
||||
if (m_limitState != e_atLowerLimit)
|
||||
{
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
m_limitState = e_atLowerLimit;
|
||||
}
|
||||
else if (jointAngle >= m_upperAngle)
|
||||
{
|
||||
if (m_limitState != e_atUpperLimit)
|
||||
{
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
m_limitState = e_atUpperLimit;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_limitState = e_inactiveLimit;
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_limitState = e_inactiveLimit;
|
||||
}
|
||||
|
||||
if (data.step.warmStarting)
|
||||
{
|
||||
// Scale impulses to support a variable time step.
|
||||
m_impulse *= data.step.dtRatio;
|
||||
m_motorImpulse *= data.step.dtRatio;
|
||||
|
||||
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_impulse.SetZero();
|
||||
m_motorImpulse = 0.0f;
|
||||
}
|
||||
|
||||
data.velocities[m_indexA].v = vA;
|
||||
data.velocities[m_indexA].w = wA;
|
||||
data.velocities[m_indexB].v = vB;
|
||||
data.velocities[m_indexB].w = wB;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||
{
|
||||
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||
float32 wA = data.velocities[m_indexA].w;
|
||||
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||
float32 wB = data.velocities[m_indexB].w;
|
||||
|
||||
float32 mA = m_invMassA, mB = m_invMassB;
|
||||
float32 iA = m_invIA, iB = m_invIB;
|
||||
|
||||
bool fixedRotation = (iA + iB == 0.0f);
|
||||
|
||||
// Solve motor constraint.
|
||||
if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
|
||||
{
|
||||
float32 Cdot = wB - wA - m_motorSpeed;
|
||||
float32 impulse = -m_motorMass * Cdot;
|
||||
float32 oldImpulse = m_motorImpulse;
|
||||
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
|
||||
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||
impulse = m_motorImpulse - oldImpulse;
|
||||
|
||||
wA -= iA * impulse;
|
||||
wB += iB * impulse;
|
||||
}
|
||||
|
||||
// Solve limit constraint.
|
||||
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
|
||||
{
|
||||
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||
float32 Cdot2 = wB - wA;
|
||||
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
|
||||
|
||||
b2Vec3 impulse = -m_mass.Solve33(Cdot);
|
||||
|
||||
if (m_limitState == e_equalLimits)
|
||||
{
|
||||
m_impulse += impulse;
|
||||
}
|
||||
else if (m_limitState == e_atLowerLimit)
|
||||
{
|
||||
float32 newImpulse = m_impulse.z + impulse.z;
|
||||
if (newImpulse < 0.0f)
|
||||
{
|
||||
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
|
||||
b2Vec2 reduced = m_mass.Solve22(rhs);
|
||||
impulse.x = reduced.x;
|
||||
impulse.y = reduced.y;
|
||||
impulse.z = -m_impulse.z;
|
||||
m_impulse.x += reduced.x;
|
||||
m_impulse.y += reduced.y;
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_impulse += impulse;
|
||||
}
|
||||
}
|
||||
else if (m_limitState == e_atUpperLimit)
|
||||
{
|
||||
float32 newImpulse = m_impulse.z + impulse.z;
|
||||
if (newImpulse > 0.0f)
|
||||
{
|
||||
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
|
||||
b2Vec2 reduced = m_mass.Solve22(rhs);
|
||||
impulse.x = reduced.x;
|
||||
impulse.y = reduced.y;
|
||||
impulse.z = -m_impulse.z;
|
||||
m_impulse.x += reduced.x;
|
||||
m_impulse.y += reduced.y;
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_impulse += impulse;
|
||||
}
|
||||
}
|
||||
|
||||
b2Vec2 P(impulse.x, impulse.y);
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * (b2Cross(m_rB, P) + impulse.z);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Solve point-to-point constraint
|
||||
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||
b2Vec2 impulse = m_mass.Solve22(-Cdot);
|
||||
|
||||
m_impulse.x += impulse.x;
|
||||
m_impulse.y += impulse.y;
|
||||
|
||||
vA -= mA * impulse;
|
||||
wA -= iA * b2Cross(m_rA, impulse);
|
||||
|
||||
vB += mB * impulse;
|
||||
wB += iB * b2Cross(m_rB, impulse);
|
||||
}
|
||||
|
||||
data.velocities[m_indexA].v = vA;
|
||||
data.velocities[m_indexA].w = wA;
|
||||
data.velocities[m_indexB].v = vB;
|
||||
data.velocities[m_indexB].w = wB;
|
||||
}
|
||||
|
||||
bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||
{
|
||||
b2Vec2 cA = data.positions[m_indexA].c;
|
||||
float32 aA = data.positions[m_indexA].a;
|
||||
b2Vec2 cB = data.positions[m_indexB].c;
|
||||
float32 aB = data.positions[m_indexB].a;
|
||||
|
||||
b2Rot qA(aA), qB(aB);
|
||||
|
||||
float32 angularError = 0.0f;
|
||||
float32 positionError = 0.0f;
|
||||
|
||||
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
|
||||
|
||||
// Solve angular limit constraint.
|
||||
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
|
||||
{
|
||||
float32 angle = aB - aA - m_referenceAngle;
|
||||
float32 limitImpulse = 0.0f;
|
||||
|
||||
if (m_limitState == e_equalLimits)
|
||||
{
|
||||
// Prevent large angular corrections
|
||||
float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
|
||||
limitImpulse = -m_motorMass * C;
|
||||
angularError = b2Abs(C);
|
||||
}
|
||||
else if (m_limitState == e_atLowerLimit)
|
||||
{
|
||||
float32 C = angle - m_lowerAngle;
|
||||
angularError = -C;
|
||||
|
||||
// Prevent large angular corrections and allow some slop.
|
||||
C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
|
||||
limitImpulse = -m_motorMass * C;
|
||||
}
|
||||
else if (m_limitState == e_atUpperLimit)
|
||||
{
|
||||
float32 C = angle - m_upperAngle;
|
||||
angularError = C;
|
||||
|
||||
// Prevent large angular corrections and allow some slop.
|
||||
C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
|
||||
limitImpulse = -m_motorMass * C;
|
||||
}
|
||||
|
||||
aA -= m_invIA * limitImpulse;
|
||||
aB += m_invIB * limitImpulse;
|
||||
}
|
||||
|
||||
// Solve point-to-point constraint.
|
||||
{
|
||||
qA.Set(aA);
|
||||
qB.Set(aB);
|
||||
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||
|
||||
b2Vec2 C = cB + rB - cA - rA;
|
||||
positionError = C.Length();
|
||||
|
||||
float32 mA = m_invMassA, mB = m_invMassB;
|
||||
float32 iA = m_invIA, iB = m_invIB;
|
||||
|
||||
b2Mat22 K;
|
||||
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
|
||||
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
|
||||
K.ey.x = K.ex.y;
|
||||
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
|
||||
|
||||
b2Vec2 impulse = -K.Solve(C);
|
||||
|
||||
cA -= mA * impulse;
|
||||
aA -= iA * b2Cross(rA, impulse);
|
||||
|
||||
cB += mB * impulse;
|
||||
aB += iB * b2Cross(rB, impulse);
|
||||
}
|
||||
|
||||
data.positions[m_indexA].c = cA;
|
||||
data.positions[m_indexA].a = aA;
|
||||
data.positions[m_indexB].c = cB;
|
||||
data.positions[m_indexB].a = aB;
|
||||
|
||||
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
|
||||
}
|
||||
|
||||
b2Vec2 b2RevoluteJoint::GetAnchorA() const
|
||||
{
|
||||
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||
}
|
||||
|
||||
b2Vec2 b2RevoluteJoint::GetAnchorB() const
|
||||
{
|
||||
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||
}
|
||||
|
||||
b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
|
||||
{
|
||||
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||
return inv_dt * P;
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
|
||||
{
|
||||
return inv_dt * m_impulse.z;
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetJointAngle() const
|
||||
{
|
||||
b2Body* bA = m_bodyA;
|
||||
b2Body* bB = m_bodyB;
|
||||
return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetJointSpeed() const
|
||||
{
|
||||
b2Body* bA = m_bodyA;
|
||||
b2Body* bB = m_bodyB;
|
||||
return bB->m_angularVelocity - bA->m_angularVelocity;
|
||||
}
|
||||
|
||||
bool b2RevoluteJoint::IsMotorEnabled() const
|
||||
{
|
||||
return m_enableMotor;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::EnableMotor(bool flag)
|
||||
{
|
||||
m_bodyA->SetAwake(true);
|
||||
m_bodyB->SetAwake(true);
|
||||
m_enableMotor = flag;
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
|
||||
{
|
||||
return inv_dt * m_motorImpulse;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::SetMotorSpeed(float32 speed)
|
||||
{
|
||||
m_bodyA->SetAwake(true);
|
||||
m_bodyB->SetAwake(true);
|
||||
m_motorSpeed = speed;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
|
||||
{
|
||||
m_bodyA->SetAwake(true);
|
||||
m_bodyB->SetAwake(true);
|
||||
m_maxMotorTorque = torque;
|
||||
}
|
||||
|
||||
bool b2RevoluteJoint::IsLimitEnabled() const
|
||||
{
|
||||
return m_enableLimit;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::EnableLimit(bool flag)
|
||||
{
|
||||
if (flag != m_enableLimit)
|
||||
{
|
||||
m_bodyA->SetAwake(true);
|
||||
m_bodyB->SetAwake(true);
|
||||
m_enableLimit = flag;
|
||||
m_impulse.z = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetLowerLimit() const
|
||||
{
|
||||
return m_lowerAngle;
|
||||
}
|
||||
|
||||
float32 b2RevoluteJoint::GetUpperLimit() const
|
||||
{
|
||||
return m_upperAngle;
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
|
||||
{
|
||||
b2Assert(lower <= upper);
|
||||
|
||||
if (lower != m_lowerAngle || upper != m_upperAngle)
|
||||
{
|
||||
m_bodyA->SetAwake(true);
|
||||
m_bodyB->SetAwake(true);
|
||||
m_impulse.z = 0.0f;
|
||||
m_lowerAngle = lower;
|
||||
m_upperAngle = upper;
|
||||
}
|
||||
}
|
||||
|
||||
void b2RevoluteJoint::Dump()
|
||||
{
|
||||
int32 indexA = m_bodyA->m_islandIndex;
|
||||
int32 indexB = m_bodyB->m_islandIndex;
|
||||
|
||||
b2Log(" b2RevoluteJointDef jd;\n");
|
||||
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||
b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
|
||||
b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);
|
||||
b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle);
|
||||
b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle);
|
||||
b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);
|
||||
b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);
|
||||
b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
|
||||
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||
}
|
||||
Reference in New Issue
Block a user