public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
// Cdot = v + cross(w, r)
b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB);
b2Vec2 impulse = b2Math.b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
b2Vec2 oldImpulse = m_impulse;
m_impulse += impulse;
float maxImpulse = data.step.dt * m_maxForce;
if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_impulse *= maxImpulse / m_impulse.Length();
}
impulse = m_impulse - oldImpulse;
vB += m_invMassB * impulse;
wB += m_invIB * b2Math.b2Cross(m_rB, impulse);
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public static float b2Distance(b2Vec2 a, b2Vec2 b)
{
b2Vec2 c = a - b;
return(c.Length());
}
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;
// Handle singularity.
float length = m_u.Length();
if (length > b2Settings.b2_linearSlop)
{
m_u *= 1.0f / length;
}
else
{
m_u.Set(0.0f, 0.0f);
}
float crAu = b2Math.b2Cross(m_rA, m_u);
float crBu = b2Math.b2Cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
// Compute the effective mass matrix.
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (m_frequencyHz > 0.0f)
{
float C = length - m_length;
// Frequency
float omega = 2.0f * (float)Math.PI * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
float k = m_mass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invMass += m_gamma;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
m_gamma = 0.0f;
m_bias = 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;
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);
// Get the pulley axes.
m_uA = cA + m_rA - m_groundAnchorA;
m_uB = cB + m_rB - m_groundAnchorB;
float lengthA = m_uA.Length();
float lengthB = m_uB.Length();
if (lengthA > 10.0f * b2Settings.b2_linearSlop)
{
m_uA *= 1.0f / lengthA;
}
else
{
m_uA.SetZero();
}
if (lengthB > 10.0f * b2Settings.b2_linearSlop)
{
m_uB *= 1.0f / lengthB;
}
else
{
m_uB.SetZero();
}
// Compute effective mass.
float ruA = b2Math.b2Cross(m_rA, m_uA);
float ruB = b2Math.b2Cross(m_rB, m_uB);
float mA = m_invMassA + m_invIA * ruA * ruA;
float mB = m_invMassB + m_invIB * ruB * ruB;
m_mass = mA + m_ratio * m_ratio * mB;
if (m_mass > 0.0f)
{
m_mass = 1.0f / m_mass;
}
if (data.step.warmStarting)
{
// Scale impulses to support variable time steps.
m_impulse *= data.step.dtRatio;
// Warm starting.
b2Vec2 PA = -(m_impulse) * m_uA;
b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
vA += m_invMassA * PA;
wA += m_invIA * b2Math.b2Cross(m_rA, PA);
vB += m_invMassB * PB;
wB += m_invIB * b2Math.b2Cross(m_rB, PB);
}
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;
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;
}
