47 m_localAnchorA(def.localAnchorA),
48 m_localAnchorB(def.localAnchorB),
49 m_maxForce(def.maxForce),
50 m_maxTorque(def.maxTorque)
68 auto& bodyConstraintA =
At(bodies,
GetBodyA());
69 auto& bodyConstraintB =
At(bodies,
GetBodyB());
70 const auto posA = bodyConstraintA->GetPosition();
71 auto velA = bodyConstraintA->GetVelocity();
72 const auto posB = bodyConstraintB->GetPosition();
73 auto velB = bodyConstraintB->GetVelocity();
76 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(),
UnitVec::Get(posA.angular));
77 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(),
UnitVec::Get(posB.angular));
88 const auto invMassA = bodyConstraintA->GetInvMass();
89 const auto invMassB = bodyConstraintB->GetInvMass();
90 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
91 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
114 const auto invRotInertia = invRotInertiaA + invRotInertiaB;
121 m_linearImpulse *= step.
dtRatio;
122 m_angularImpulse *= step.
dtRatio;
124 const auto P = m_linearImpulse;
130 velA -= Velocity{invMassA * P, invRotInertiaA * (crossAP + m_angularImpulse)};
131 velB += Velocity{invMassB * P, invRotInertiaB * (crossBP + m_angularImpulse)};
139 bodyConstraintA->SetVelocity(velA);
140 bodyConstraintB->SetVelocity(velB);
143 bool FrictionJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
145 auto& bodyConstraintA =
At(bodies,
GetBodyA());
146 auto& bodyConstraintB =
At(bodies,
GetBodyB());
148 auto velA = bodyConstraintA->GetVelocity();
149 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
151 auto velB = bodyConstraintB->GetVelocity();
152 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
154 const auto h = step.GetTime();
162 const auto angularImpulse =
AngularMomentum{-m_angularMass * (velB.angular - velA.angular)};
164 const auto oldAngularImpulse = m_angularImpulse;
165 const auto maxAngularImpulse = h * m_maxTorque;
166 m_angularImpulse = std::clamp(m_angularImpulse + angularImpulse,
167 -maxAngularImpulse, maxAngularImpulse);
168 const auto incAngularImpulse = m_angularImpulse - oldAngularImpulse;
175 velA.angular -= invRotInertiaA * incAngularImpulse;
176 velB.angular += invRotInertiaB * incAngularImpulse;
184 const auto impulse = -
Transform(vb - va, m_linearMass);
185 const auto oldImpulse = m_linearImpulse;
186 m_linearImpulse += impulse;
188 const auto maxImpulse = h * m_maxForce;
195 const auto incImpulse =
Momentum2{m_linearImpulse - oldImpulse};
204 velA -= Velocity{bodyConstraintA->GetInvMass() * incImpulse, invRotInertiaA * angImpulseA};
205 velB += Velocity{bodyConstraintB->GetInvMass() * incImpulse, invRotInertiaB * angImpulseB};
208 bodyConstraintA->SetVelocity(velA);
209 bodyConstraintB->SetVelocity(velB);
214 bool FrictionJoint::SolvePositionConstraints(
BodyConstraintsMap&,
const ConstraintSolverConf&)
const
231 return m_linearImpulse;
236 return m_angularImpulse;