39 const auto totInvI = invRotInertiaA + invRotInertiaB;
93 m_localAnchorB{def.localAnchorB},
94 m_enableMotor{def.enableMotor},
95 m_maxMotorTorque{def.maxMotorTorque},
96 m_motorSpeed{def.motorSpeed},
97 m_enableLimit{def.enableLimit},
98 m_referenceAngle{def.referenceAngle},
99 m_lowerAngle{def.lowerAngle},
100 m_upperAngle{def.upperAngle}
107 visitor.
Visit(*
this);
112 visitor.
Visit(*
this);
119 auto& bodyConstraintA =
At(bodies,
GetBodyA());
120 auto& bodyConstraintB =
At(bodies,
GetBodyB());
122 const auto invMassA = bodyConstraintA->GetInvMass();
123 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
124 const auto aA = bodyConstraintA->GetPosition().angular;
125 auto velA = bodyConstraintA->GetVelocity();
127 const auto invMassB = bodyConstraintB->GetInvMass();
128 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
129 const auto aB = bodyConstraintB->GetPosition().angular;
130 auto velB = bodyConstraintB->GetVelocity();
135 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
136 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
147 const auto totInvI = invRotInertiaA + invRotInertiaB;
150 m_mass = GetMat33(invMassA, m_rA, invRotInertiaA, invMassB, m_rB, invRotInertiaB);
153 if (!m_enableMotor || fixedRotation)
158 if (m_enableLimit && !fixedRotation)
165 else if (jointAngle <= m_lowerAngle)
173 else if (jointAngle >= m_upperAngle)
196 m_motorImpulse *= step.
dtRatio;
207 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
208 velB += Velocity{invMassB * P, invRotInertiaB * LB};
216 bodyConstraintA->SetVelocity(velA);
217 bodyConstraintB->SetVelocity(velB);
220 bool RevoluteJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
222 auto& bodyConstraintA =
At(bodies,
GetBodyA());
223 auto& bodyConstraintB =
At(bodies,
GetBodyB());
225 const auto oldVelA = bodyConstraintA->GetVelocity();
227 const auto invMassA = bodyConstraintA->GetInvMass();
228 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
230 const auto oldVelB = bodyConstraintB->GetVelocity();
232 const auto invMassB = bodyConstraintB->GetInvMass();
233 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
235 const auto fixedRotation = (invRotInertiaA + invRotInertiaB ==
InvRotInertia{0});
238 if (m_enableMotor && (m_limitState !=
e_equalLimits) && !fixedRotation)
240 const auto impulse =
AngularMomentum{-m_motorMass * (velB.angular - velA.angular - m_motorSpeed)};
241 const auto oldImpulse = m_motorImpulse;
242 const auto maxImpulse = step.GetTime() * m_maxMotorTorque;
243 m_motorImpulse = std::clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
244 const auto incImpulse = m_motorImpulse - oldImpulse;
246 velA.angular -= invRotInertiaA * incImpulse;
247 velB.angular += invRotInertiaB * incImpulse;
252 const auto vDelta = vb - va;
255 if (m_enableLimit && (m_limitState !=
e_inactiveLimit) && !fixedRotation)
257 const auto Cdot =
Vec3{
262 auto impulse = -
Solve33(m_mass, Cdot);
264 auto UpdateImpulseProc = [&]() {
265 const auto rhs = -
Vec2{
269 const auto reduced =
Solve22(m_mass, rhs);
278 switch (m_limitState)
282 const auto newImpulse =
GetZ(m_impulse) +
GetZ(impulse);
289 m_impulse += impulse;
295 const auto newImpulse =
GetZ(m_impulse) +
GetZ(impulse);
302 m_impulse += impulse;
308 m_impulse += impulse;
317 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
318 velB += Velocity{invMassB * P, invRotInertiaB * LB};
334 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
335 velB += Velocity{invMassB * P, invRotInertiaB * LB};
338 if ((velA != oldVelA) || (velB != oldVelB))
340 bodyConstraintA->SetVelocity(velA);
341 bodyConstraintB->SetVelocity(velB);
347 bool RevoluteJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
349 auto& bodyConstraintA =
At(bodies,
GetBodyA());
350 auto& bodyConstraintB =
At(bodies,
GetBodyB());
352 auto posA = bodyConstraintA->GetPosition();
353 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
355 auto posB = bodyConstraintB->GetPosition();
356 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
358 const auto fixedRotation = ((invRotInertiaA + invRotInertiaB) ==
InvRotInertia{0});
361 auto angularError = 0_rad;
362 if (m_enableLimit && (m_limitState !=
e_inactiveLimit) && !fixedRotation)
369 switch (m_limitState)
373 auto C = angle - m_lowerAngle;
377 C = std::clamp(C + conf.angularSlop, -conf.maxAngularCorrection, 0_rad);
378 limitImpulse = -m_motorMass * C;
383 auto C = angle - m_upperAngle;
387 C = std::clamp(C - conf.angularSlop, 0_rad, conf.maxAngularCorrection);
388 limitImpulse = -m_motorMass * C;
395 const auto C = std::clamp(angle - m_lowerAngle,
396 -conf.maxAngularCorrection, conf.maxAngularCorrection);
397 limitImpulse = -m_motorMass * C;
398 angularError =
abs(C);
404 posA.angular -= invRotInertiaA * limitImpulse;
405 posB.angular += invRotInertiaB * limitImpulse;
409 auto positionError = 0_m2;
414 const auto rA =
Length2{
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)};
415 const auto rB =
Length2{
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB)};
417 const auto C = (posB.linear + rB) - (posA.linear + rA);
420 const auto invMassA = bodyConstraintA->GetInvMass();
421 const auto invMassB = bodyConstraintB->GetInvMass();
441 const auto P = -
Solve(K, C);
443 posA -= Position{invMassA * P, invRotInertiaA *
Cross(rA, P) /
Radian};
444 posB += Position{invMassB * P, invRotInertiaB *
Cross(rB, P) /
Radian};
447 bodyConstraintA->SetPosition(posA);
448 bodyConstraintB->SetPosition(posB);
450 return (positionError <=
Square(conf.linearSlop)) && (angularError <= conf.angularSlop);
476 if (m_enableMotor != flag)
478 m_enableMotor = flag;
488 if (m_motorSpeed != speed)
490 m_motorSpeed = speed;
500 if (m_maxMotorTorque != torque)
502 m_maxMotorTorque = torque;
512 if (flag != m_enableLimit)
514 m_enableLimit = flag;
524 assert(lower <= upper);
526 if ((lower != m_lowerAngle) || (upper != m_upperAngle))
529 m_lowerAngle = lower;
530 m_upperAngle = upper;