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);
}
internal override void InitVelocityConstraints(b2SolverData data)
{
m_indexB = m_bodyB.m_islandIndex;
m_localCenterB = m_bodyB.m_sweep.localCenter;
m_invMassB = m_bodyB.m_invMass;
m_invIB = m_bodyB.m_invI;
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 qB = new b2Rot(aB);
float mass = m_bodyB.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * m_frequencyHz;
// Damping coefficient
float d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > float.Epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = Utils.b2Mul(qB, m_localAnchorB - m_localCenterB);
// 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]
b2Mat22 K = new b2Mat22();
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * Utils.b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public override void InitVelocityConstraints(b2TimeStep step)
{
b2Mat22 tMat;
float tX;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
// Compute the effective mass matrix.
//b2Vec2 rA = b2Mul(bA->m_xf.R, m_localAnchorA - bA->GetLocalCenter());
tMat = bA.m_xf.R;
float rAX = m_localAnchorA.x - bA.m_sweep.localCenter.x;
float rAY = m_localAnchorA.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * rAX + tMat.col2.x * rAY);
rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
rAX = tX;
//b2Vec2 rB = b2Mul(bB->m_xf.R, m_localAnchorB - bB->GetLocalCenter());
tMat = bB.m_xf.R;
float rBX = m_localAnchorB.x - bB.m_sweep.localCenter.x;
float rBY = m_localAnchorB.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * rBX + tMat.col2.x * rBY);
rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
rBX = tX;
// 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 = bA.m_invMass;
float mB = bB.m_invMass;
float iA = bA.m_invI;
float iB = bB.m_invI;
b2Mat22 K = new b2Mat22();
K.col1.x = mA + mB; K.col2.x = 0.0f;
K.col1.y = 0.0f; K.col2.y = mA + mB;
K.col1.x += iA * rAY * rAY; K.col2.x += -iA * rAX * rAY;
K.col1.y += -iA * rAX * rAY; K.col2.y += iA * rAX * rAX;
K.col1.x += iB * rBY * rBY; K.col2.x += -iB * rBX * rBY;
K.col1.y += -iB * rBX * rBY; K.col2.y += iB * rBX * rBX;
K.GetInverse(m_linearMass);
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse.x *= step.dtRatio;
m_linearImpulse.y *= step.dtRatio;
m_angularImpulse *= step.dtRatio;
b2Vec2 P = m_linearImpulse;
bA.m_linearVelocity.x -= mA * P.x;
bA.m_linearVelocity.y -= mA * P.y;
bA.m_angularVelocity -= iA * (rAX * P.y - rAY * P.x + m_angularImpulse);
bB.m_linearVelocity.x += mB * P.x;
bB.m_linearVelocity.y += mB * P.y;
bB.m_angularVelocity += iB * (rBX * P.y - rBY * P.x + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
}
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;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
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 mass matrix.
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;
b2Mat22 K = new b2Mat22();
K.exx = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.exy = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.eyx = K.ex.y;
K.eyy = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 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;
}
internal override void InitVelocityConstraints(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;
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 mass matrix.
m_rA = Utils.b2Mul(qA, m_linearOffset - m_localCenterA);
m_rB = Utils.b2Mul(qB, -m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// 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;
float mB = m_invMassB;
float iA = m_invIA;
float iB = m_invIB;
// Upper 2 by 2 of K for point to point
b2Mat22 K = new b2Mat22();
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
m_linearError = cB + m_rB - cA - m_rA;
m_angularError = aB - aA - m_angularOffset;
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (Utils.b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (Utils.b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 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;
}
