public override void InitVelocityConstraints(b2TimeStep step)
{
b2Body b = m_bodyB;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Mathf.PI * m_frequencyHz;
// Damping co-efficient
float d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float k = mass * omega * omega;
// magic formulas
// gamma has units of inverse mass
// beta hs units of inverse time
//b2Settings.b2Assert(d + step.dt * k > Number.MIN_VALUE)
m_gamma = step.dt * (d + step.dt * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma:0.0f;
m_beta = step.dt * k * m_gamma;
b2Mat22 tMat;
// Compute the effective mass matrix.
//b2Vec2 r = b2Mul(b->m_xf.R, m_localAnchor - b->GetLocalCenter());
tMat = b.m_xf.R;
float rX = m_localAnchor.x - b.m_sweep.localCenter.x;
float rY = m_localAnchor.y - b.m_sweep.localCenter.y;
float tX = (tMat.col1.x * rX + tMat.col2.x * rY);
rY = (tMat.col1.y * rX + tMat.col2.y * rY);
rX = tX;
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
float invMass = b.m_invMass;
float invI = b.m_invI;
//b2Mat22 K1;
K1.col1.x = invMass; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = invMass;
//b2Mat22 K2;
K2.col1.x = invI * rY * rY; K2.col2.x = -invI * rX * rY;
K2.col1.y = -invI * rX * rY; K2.col2.y = invI * rX * rX;
//b2Mat22 K = K1 + K2;
K.SetM(K1);
K.AddM(K2);
K.col1.x += m_gamma;
K.col2.y += m_gamma;
//m_ptpMass = K.GetInverse();
K.GetInverse(m_mass);
//m_C = b.m_position + r - m_target;
m_C.x = b.m_sweep.c.x + rX - m_target.x;
m_C.y = b.m_sweep.c.y + rY - m_target.y;
// Cheat with some damping
b.m_angularVelocity *= 0.98f;
// Warm starting.
m_impulse.x *= step.dtRatio;
m_impulse.y *= step.dtRatio;
//b.m_linearVelocity += invMass * m_impulse;
b.m_linearVelocity.x += invMass * m_impulse.x;
b.m_linearVelocity.y += invMass * m_impulse.y;
//b.m_angularVelocity += invI * b2Cross(r, m_impulse);
b.m_angularVelocity += invI * (rX * m_impulse.y - rY * m_impulse.x);
}
public override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit
float oldLimitImpulse;
float C;
b2Mat22 tMat;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
float tX;
float impulseX;
float impulseY;
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
float angle = bB.m_sweep.a - bA.m_sweep.a - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
C = b2Math.Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Math.Abs(C);
}
else if (m_limitState == e_atLowerLimit)
{
C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Math.Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == e_atUpperLimit)
{
C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Math.Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
bA.m_sweep.a -= bA.m_invI * limitImpulse;
bB.m_sweep.a += bB.m_invI * limitImpulse;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
}
// Solve point-to-point constraint
{
//b2Vec2 r1 = b2Mul(bA->m_xf.R, m_localAnchor1 - bA->GetLocalCenter());
tMat = bA.m_xf.R;
float r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x;
float r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
r1X = tX;
//b2Vec2 r2 = b2Mul(bB->m_xf.R, m_localAnchor2 - bB->GetLocalCenter());
tMat = bB.m_xf.R;
float r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x;
float r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
r2X = tX;
//b2Vec2 C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1;
float CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X;
float CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y;
float CLengthSquared = CX * CX + CY * CY;
float CLength = Mathf.Sqrt(CLengthSquared);
positionError = CLength;
float invMass1 = bA.m_invMass;
float invMass2 = bB.m_invMass;
float invI1 = bA.m_invI;
float invI2 = bB.m_invI;
//Handle large detachment.
const float k_allowedStretch = 10.0f * b2Settings.b2_linearSlop;
if (CLengthSquared > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation)
//b2Vec2 u = C; u.Normalize();
float uX = CX / CLength;
float uY = CY / CLength;
float k = invMass1 + invMass2;
//b2Settings.b2Assert(k>Number.MIN_VALUE)
float m = 1.0f / k;
impulseX = m * (-CX);
impulseY = m * (-CY);
const float k_beta = 0.5f;
bA.m_sweep.c.x -= k_beta * invMass1 * impulseX;
bA.m_sweep.c.y -= k_beta * invMass1 * impulseY;
bB.m_sweep.c.x += k_beta * invMass2 * impulseX;
bB.m_sweep.c.y += k_beta * invMass2 * impulseY;
//C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1;
CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X;
CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y;
}
//b2Mat22 K1;
K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
//b2Mat22 K2;
K2.col1.x = invI1 * r1Y * r1Y; K2.col2.x = -invI1 * r1X * r1Y;
K2.col1.y = -invI1 * r1X * r1Y; K2.col2.y = invI1 * r1X * r1X;
//b2Mat22 K3;
K3.col1.x = invI2 * r2Y * r2Y; K3.col2.x = -invI2 * r2X * r2Y;
K3.col1.y = -invI2 * r2X * r2Y; K3.col2.y = invI2 * r2X * r2X;
//b2Mat22 K = K1 + K2 + K3;
K.SetM(K1);
K.AddM(K2);
K.AddM(K3);
//b2Vec2 impulse = K.Solve(-C);
K.Solve(tImpulse, -CX, -CY);
impulseX = tImpulse.x;
impulseY = tImpulse.y;
//bA.m_sweep.c -= bA.m_invMass * impulse;
bA.m_sweep.c.x -= bA.m_invMass * impulseX;
bA.m_sweep.c.y -= bA.m_invMass * impulseY;
//bA.m_sweep.a -= bA.m_invI * b2Cross(r1, impulse);
bA.m_sweep.a -= bA.m_invI * (r1X * impulseY - r1Y * impulseX);
//bB.m_sweep.c += bB.m_invMass * impulse;
bB.m_sweep.c.x += bB.m_invMass * impulseX;
bB.m_sweep.c.y += bB.m_invMass * impulseY;
//bB.m_sweep.a += bB.m_invI * b2Cross(r2, impulse);
bB.m_sweep.a += bB.m_invI * (r2X * impulseY - r2Y * impulseX);
bA.SynchronizeTransform();
bB.SynchronizeTransform();
}
return(positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}