103 m_localAnchorB{def.localAnchorB},
104 m_localXAxisA{def.localAxisA},
106 m_referenceAngle{def.referenceAngle},
107 m_lowerTranslation{def.lowerTranslation},
108 m_upperTranslation{def.upperTranslation},
109 m_maxMotorForce{def.maxMotorForce},
110 m_motorSpeed{def.motorSpeed},
111 m_enableLimit{def.enableLimit},
112 m_enableMotor{def.enableMotor}
119 visitor.
Visit(*
this);
124 visitor.
Visit(*
this);
131 auto& bodyConstraintA =
At(bodies,
GetBodyA());
132 auto& bodyConstraintB =
At(bodies,
GetBodyB());
134 const auto posA = bodyConstraintA->GetPosition();
135 const auto invMassA = bodyConstraintA->GetInvMass();
136 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
137 auto velA = bodyConstraintA->GetVelocity();
139 const auto posB = bodyConstraintB->GetPosition();
140 const auto invMassB = bodyConstraintB->GetInvMass();
141 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
142 auto velB = bodyConstraintB->GetVelocity();
148 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
149 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
150 const auto d = (posB.linear - posA.linear) + rB - rA;
153 m_axis =
Rotate(m_localXAxisA, qA);
154 m_a1 =
Cross(d + rA, m_axis);
155 m_a2 =
Cross(rB, m_axis);
159 const auto totalInvMass = invMassA + invMassB + invRotMassA + invRotMassB;
160 m_motorMass = (totalInvMass >
InvMass{0})?
Real{1} / totalInvMass: 0_kg;
164 m_perp =
Rotate(m_localYAxisA, qA);
166 m_s1 =
Cross(d + rA, m_perp);
167 m_s2 =
Cross(rB, m_perp);
171 const auto k11 =
StripUnit(invMassA + invMassB + invRotMassA2 + invRotMassB2);
176 const auto totalInvRotInertia = invRotInertiaA + invRotInertiaB;
180 const auto k33 =
StripUnit(totalInvMass);
190 const auto jointTranslation =
Length{
Dot(m_axis, d)};
195 else if (jointTranslation <= m_lowerTranslation)
203 else if (jointTranslation >= m_upperTranslation)
232 m_motorImpulse *= step.
dtRatio;
234 const auto ulImpulseX =
GetX(m_impulse) * m_perp;
239 const auto Pz =
Momentum2{PzLength * m_axis};
240 const auto P = Px + Pz;
244 const auto LA = L + (Pxs1 *
Meter + PzLength * m_a1) /
Radian;
245 const auto LB = L + (Pxs2 *
Meter + PzLength * m_a2) /
Radian;
248 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
249 velB += Velocity{invMassB * P, invRotInertiaB * LB};
257 bodyConstraintA->SetVelocity(velA);
258 bodyConstraintB->SetVelocity(velB);
261 bool PrismaticJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
263 auto& bodyConstraintA =
At(bodies,
GetBodyA());
264 auto& bodyConstraintB =
At(bodies,
GetBodyB());
266 const auto oldVelA = bodyConstraintA->GetVelocity();
268 const auto invMassA = bodyConstraintA->GetInvMass();
269 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
271 const auto oldVelB = bodyConstraintB->GetVelocity();
273 const auto invMassB = bodyConstraintB->GetInvMass();
274 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
280 const auto Cdot = vDot + (m_a2 * velB.angular - m_a1 * velA.angular) /
Radian;
282 const auto oldImpulse = m_motorImpulse;
283 const auto maxImpulse = step.GetTime() * m_maxMotorForce;
284 m_motorImpulse = std::clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
285 impulse = m_motorImpulse - oldImpulse;
287 const auto P =
Momentum2{impulse * m_axis};
290 const auto LA = impulse * m_a1 /
Radian;
291 const auto LB = impulse * m_a2 /
Radian;
293 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
294 velB += Velocity{invMassB * P, invRotInertiaB * LB};
297 const auto velDelta = velB.linear - velA.linear;
299 const auto Cdot1 =
Vec2{
309 const auto Cdot2 =
StripUnit(deltaDot + aRotSpeed);
310 const auto Cdot =
Vec3{
GetX(Cdot1),
GetY(Cdot1), Cdot2};
312 const auto f1 = m_impulse;
313 m_impulse +=
Solve33(m_K, -Cdot);
330 const auto df = m_impulse - f1;
332 const auto ulP =
GetX(df) * m_perp +
GetZ(df) * m_axis;
341 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
342 velB += Velocity{invMassB * P, invRotInertiaB * LB};
347 const auto df =
Solve22(m_K, -Cdot1);
353 const auto ulP =
GetX(df) * m_perp;
362 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
363 velB += Velocity{invMassB * P, invRotInertiaB * LB};
366 if ((velA != oldVelA) || (velB != oldVelB))
368 bodyConstraintA->SetVelocity(velA);
369 bodyConstraintB->SetVelocity(velB);
385 bool PrismaticJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
387 auto& bodyConstraintA =
At(bodies,
GetBodyA());
388 auto& bodyConstraintB =
At(bodies,
GetBodyB());
390 auto posA = bodyConstraintA->GetPosition();
391 const auto invMassA = bodyConstraintA->GetInvMass();
392 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
394 auto posB = bodyConstraintB->GetPosition();
395 const auto invMassB = bodyConstraintB->GetInvMass();
396 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
402 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
403 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
404 const auto d =
Length2{(posB.linear + rB) - (posA.linear + rA)};
406 const auto axis =
Rotate(m_localXAxisA, qA);
409 const auto perp =
Rotate(m_localYAxisA, qA);
414 const auto C1 =
Vec2{
416 (posB.angular - posA.angular - m_referenceAngle) /
Radian
426 const auto translation =
Length{
Dot(axis, d)};
427 if (
abs(m_upperTranslation - m_lowerTranslation) < (
Real{2} * conf.linearSlop))
430 C2 =
StripUnit(std::clamp(translation, -conf.maxLinearCorrection, conf.maxLinearCorrection));
431 linearError = std::max(linearError,
abs(translation));
434 else if (translation <= m_lowerTranslation)
437 C2 =
StripUnit(std::clamp(translation - m_lowerTranslation + conf.linearSlop, -conf.maxLinearCorrection, 0_m));
438 linearError = std::max(linearError, m_lowerTranslation - translation);
441 else if (translation >= m_upperTranslation)
444 C2 =
StripUnit(std::clamp(translation - m_upperTranslation - conf.linearSlop, 0_m, conf.maxLinearCorrection));
445 linearError = std::max(linearError, translation - m_upperTranslation);
467 auto k22 =
StripUnit(invRotInertiaA + invRotInertiaB);
482 const auto K =
Mat33{
Vec3{k11, k12, k13},
Vec3{k12, k22, k23},
Vec3{k13, k23, k33}};
497 auto k22 =
StripUnit(invRotInertiaA + invRotInertiaB);
505 const auto impulse1 =
Solve(K, -C1);
515 posA -= Position{
Length2{invMassA * P}, invRotInertiaA * LA};
516 posB += Position{invMassB * P, invRotInertiaB * LB};
518 bodyConstraintA->SetPosition(posA);
519 bodyConstraintB->SetPosition(posB);
521 return (linearError <= conf.linearSlop) && (angularError <= conf.angularSlop);
536 const auto ulImpulse =
GetX(m_impulse) * m_perp;
548 if (m_enableLimit != flag)
550 m_enableLimit = flag;
560 assert(lower <= upper);
561 if ((lower != m_lowerTranslation) || (upper != m_upperTranslation))
563 m_lowerTranslation = lower;
564 m_upperTranslation = upper;
574 if (m_enableMotor != flag)
576 m_enableMotor = flag;
586 if (m_motorSpeed != speed)
588 m_motorSpeed = speed;
598 if (m_maxMotorForce != force)
600 m_maxMotorForce = force;
610 const auto pA =
GetWorldPoint(*joint.GetBodyA(), joint.GetLocalAnchorA());
611 const auto pB =
GetWorldPoint(*joint.GetBodyB(), joint.GetLocalAnchorB());
617 const auto bA = joint.GetBodyA();
618 const auto bB = joint.GetBodyB();
620 const auto rA =
Rotate(joint.GetLocalAnchorA() - bA->GetLocalCenter(), bA->GetTransformation().q);
621 const auto rB =
Rotate(joint.GetLocalAnchorB() - bB->GetLocalCenter(), bB->GetTransformation().q);
622 const auto p1 = bA->GetWorldCenter() + rA;
623 const auto p2 = bB->GetWorldCenter() + rB;
624 const auto d = p2 - p1;
625 const auto axis =
Rotate(joint.GetLocalAxisA(), bA->GetTransformation().q);
627 const auto vA = bA->GetVelocity().linear;
628 const auto vB = bB->GetVelocity().linear;
629 const auto wA = bA->GetVelocity().angular;
630 const auto wB = bB->GetVelocity().angular;