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);
}
}
b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
: b2Joint(def)
{
m_target = def->target;
m_localAnchor = b2MulT(m_body2->GetXForm(), m_target);
m_maxForce = B2FORCE_INV_SCALE(def->maxForce);
m_impulse.SetZero();
float32 mass = m_body2->m_mass;
// Frequency
float32 omega = 2.0f * b2_pi * def->frequencyHz;
// Damping coefficient
float32 d = 2.0f * mass * def->dampingRatio * omega;
// Spring stiffness
float32 k = (def->timeStep * mass) * (omega * omega);
// magic formulas
b2Assert(d + k > B2_FLT_EPSILON);
m_gamma = 1.0f / (d + k);
m_beta = k / (d + k);
}
b2LineJoint::b2LineJoint(const b2LineJointDef* def)
: b2Joint(def)
{
m_localAnchor1 = def->localAnchor1;
m_localAnchor2 = def->localAnchor2;
m_localXAxis1 = def->localAxis1;
m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
m_impulse.SetZero();
m_motorMass = 0.0;
m_motorImpulse = 0.0f;
m_lowerTranslation = def->lowerTranslation;
m_upperTranslation = def->upperTranslation;
m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
m_motorSpeed = def->motorSpeed;
m_enableLimit = def->enableLimit;
m_enableMotor = def->enableMotor;
m_limitState = e_inactiveLimit;
m_axis.SetZero();
m_perp.SetZero();
}
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);
}
