public b2PrismaticJoint(b2PrismaticJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_localXAxisA = def.localAxisA;
m_localXAxisA.Normalize();
m_localYAxisA = m_localXAxisA.NegUnitCross(); // b2Math.b2Cross(1.0f, m_localXAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse.SetZero();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = b2LimitState.e_inactiveLimit;
m_axis.SetZero();
m_perp.SetZero();
}
public b2RopeJoint(b2RopeJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_maxLength = def.maxLength;
m_mass = 0.0f;
m_impulse = 0.0f;
m_state = b2LimitState.e_inactiveLimit;
m_length = 0.0f;
}
public b2RopeJoint(b2RopeJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_maxLength = def.maxLength;
m_mass = 0.0f;
m_impulse = 0.0f;
m_state = b2LimitState.e_inactiveLimit;
m_length = 0.0f;
}
public b2RevoluteJoint(b2RevoluteJointDef def)
: base(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 = b2LimitState.e_inactiveLimit;
}
public b2RevoluteJoint(b2RevoluteJointDef def)
: base(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 = b2LimitState.e_inactiveLimit;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.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]
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
m_mass = new b2Mat33(ex, ey, ez);
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)
{
float jointAngle = aB - aA - m_referenceAngle;
if (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atUpperLimit;
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.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 = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.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;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
InvertedMassA = m_bodyA.InvertedMass;
InvertedMassB = m_bodyB.InvertedMass;
InvertedIA = m_bodyA.InvertedI;
InvertedIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
// Compute the effective masses.
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Math.b2Mul(qA, m_localXAxisA);
m_a1 = b2Math.b2Cross(d + rA, m_axis);
m_a2 = b2Math.b2Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = b2Math.b2Mul(qA, m_localYAxisA);
m_s1 = b2Math.b2Cross(d + rA, m_perp);
m_s2 = b2Math.b2Cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = b2Math.b2Dot(m_axis, d);
if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_limitState = b2LimitState.e_atLowerLimit;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_limitState = b2LimitState.e_atUpperLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
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;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
m_length = m_u.Length;
float C = m_length - m_maxLength;
if (C > 0.0f)
{
m_state = b2LimitState.e_atUpperLimit;
}
else
{
m_state = b2LimitState.e_inactiveLimit;
}
if (m_length > b2Settings.b2_linearSlop)
{
m_u *= 1.0f / m_length;
}
else
{
m_u.SetZero();
m_mass = 0.0f;
m_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = b2Math.b2Cross(m_rA, m_u);
float crB = b2Math.b2Cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = m_impulse * m_u;
vA -= m_invMassA * P;
wA -= m_invIA * b2Math.b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * b2Math.b2Cross(m_rB, P);
}
else
{
m_impulse = 0.0f;
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
m_length = m_u.Length();
float C = m_length - m_maxLength;
if (C > 0.0f)
{
m_state = b2LimitState.e_atUpperLimit;
}
else
{
m_state = b2LimitState.e_inactiveLimit;
}
if (m_length > b2Settings.b2_linearSlop)
{
m_u *= 1.0f / m_length;
}
else
{
m_u.SetZero();
m_mass = 0.0f;
m_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = b2Math.b2Cross(m_rA, m_u);
float crB = b2Math.b2Cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = m_impulse * m_u;
vA -= m_invMassA * P;
wA -= m_invIA * b2Math.b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * b2Math.b2Cross(m_rB, P);
}
else
{
m_impulse = 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;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
InvertedMassA = m_bodyA.InvertedMass;
InvertedMassB = m_bodyB.InvertedMass;
InvertedIA = m_bodyA.InvertedI;
InvertedIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
// Compute the effective masses.
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Math.b2Mul(qA, m_localXAxisA);
m_a1 = b2Math.b2Cross(d + rA, m_axis);
m_a2 = b2Math.b2Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = b2Math.b2Mul(qA, m_localYAxisA);
m_s1 = b2Math.b2Cross(d + rA, m_perp);
m_s2 = b2Math.b2Cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = b2Math.b2Dot(m_axis, d);
if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_limitState = b2LimitState.e_atLowerLimit;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_limitState = b2LimitState.e_atUpperLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
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;
}
public b2PrismaticJoint(b2PrismaticJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_localXAxisA = def.localAxisA;
m_localXAxisA.Normalize();
m_localYAxisA = m_localXAxisA.NegUnitCross(); // b2Math.b2Cross(1.0f, m_localXAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse.SetZero();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = b2LimitState.e_inactiveLimit;
m_axis.SetZero();
m_perp.SetZero();
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.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]
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
m_mass = new b2Mat33(ex, ey, ez);
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)
{
float jointAngle = aB - aA - m_referenceAngle;
if (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atUpperLimit;
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.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 = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
