void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
if (m_state == e_atUpperLimit)
{
b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_pulleyMass * Cdot;
float32 oldForce = m_force;
m_force = b2Max(0.0f, m_force + force);
force = m_force - oldForce;
b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * m_u2;
b1->m_linearVelocity += b1->m_invMass * P1;
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
b2->m_linearVelocity += b2->m_invMass * P2;
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
if (m_limitState1 == e_atUpperLimit)
{
b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
float32 Cdot = -b2Dot(m_u1, v1);
float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass1 * Cdot;
float32 oldForce = m_limitForce1;
m_limitForce1 = b2Max(0.0f, m_limitForce1 + force);
force = m_limitForce1 - oldForce;
b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
b1->m_linearVelocity += b1->m_invMass * P1;
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
}
if (m_limitState2 == e_atUpperLimit)
{
b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
float32 Cdot = -b2Dot(m_u2, v2);
float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass2 * Cdot;
float32 oldForce = m_limitForce2;
m_limitForce2 = b2Max(0.0f, m_limitForce2 + force);
force = m_limitForce2 - oldForce;
b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
b2->m_linearVelocity += b2->m_invMass * P2;
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
}
void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b = m_body2;
b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
// Cdot = v + cross(w, r)
b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
b2Vec2 force = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C + B2FORCE_SCALE(step.dt) * (m_gamma * m_impulse));
b2Vec2 oldForce = m_impulse;
m_impulse += force;
float32 forceMagnitude = m_impulse.Length();
if (forceMagnitude > m_maxForce)
{
m_impulse *= m_maxForce / forceMagnitude;
}
force = m_impulse - oldForce;
b2Vec2 P = B2FORCE_SCALE(step.dt) * force;
b->m_linearVelocity += b->m_invMass * P;
b->m_angularVelocity += b->m_invI * b2Cross(r, P);
}
void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b = m_body2;
// Compute the effective mass matrix.
b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
// 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]
float32 invMass = b->m_invMass;
float32 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 * r.y * r.y; K2.col2.x = -invI * r.x * r.y;
K2.col1.y = -invI * r.x * r.y; K2.col2.y = invI * r.x * r.x;
b2Mat22 K = K1 + K2;
K.col1.x += m_gamma;
K.col2.y += m_gamma;
m_mass = K.Invert();
m_C = b->m_sweep.c + r - m_target;
// Cheat with some damping
b->m_angularVelocity *= 0.98f;
// Warm starting.
b2Vec2 P = B2FORCE_SCALE(step.dt) * m_impulse;
b->m_linearVelocity += invMass * P;
b->m_angularVelocity += invI * b2Cross(r, P);
}
void b2LineJoint::SetMaxMotorForce(float32 force)
{
m_body1->WakeUp();
m_body2->WakeUp();
m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
}
void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
// Get the pulley axes.
m_u1 = p1 - s1;
m_u2 = p2 - s2;
float32 length1 = m_u1.Length();
float32 length2 = m_u2.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_constant - length1 - m_ratio * length2;
if (C > 0.0f)
{
m_state = e_inactiveLimit;
m_force = 0.0f;
}
else
{
m_state = e_atUpperLimit;
m_positionImpulse = 0.0f;
}
if (length1 < m_maxLength1)
{
m_limitState1 = e_inactiveLimit;
m_limitForce1 = 0.0f;
}
else
{
m_limitState1 = e_atUpperLimit;
m_limitPositionImpulse1 = 0.0f;
}
if (length2 < m_maxLength2)
{
m_limitState2 = e_inactiveLimit;
m_limitForce2 = 0.0f;
}
else
{
m_limitState2 = e_atUpperLimit;
m_limitPositionImpulse2 = 0.0f;
}
// Compute effective mass.
float32 cr1u1 = b2Cross(r1, m_u1);
float32 cr2u2 = b2Cross(r2, m_u2);
m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
b2Assert(m_limitMass1 > B2_FLT_EPSILON);
b2Assert(m_limitMass2 > B2_FLT_EPSILON);
b2Assert(m_pulleyMass > B2_FLT_EPSILON);
m_limitMass1 = 1.0f / m_limitMass1;
m_limitMass2 = 1.0f / m_limitMass2;
m_pulleyMass = 1.0f / m_pulleyMass;
if (step.warmStarting)
{
// Warm starting.
b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
b1->m_linearVelocity += b1->m_invMass * P1;
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
b2->m_linearVelocity += b2->m_invMass * P2;
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
else
{
m_force = 0.0f;
m_limitForce1 = 0.0f;
m_limitForce2 = 0.0f;
}
}
b2Vec2 b2PulleyJoint::GetReactionForce() const
{
b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
return F;
}
b2Vec2 b2MouseJoint::GetReactionForce() const
{
return B2FORCE_SCALE(float32(1.0))*m_impulse;
}
