55 inline bool IsValidType(
JointType t) noexcept
64 const auto t1 =
GetType(*def.joint1);
65 const auto t2 =
GetType(*def.joint2);
66 if (!IsValidType(t1) || !IsValidType(t2))
85 assert(IsValidType(m_typeA));
86 assert(IsValidType(m_typeB));
90 m_bodyC = m_joint1->GetBodyA();
102 m_localAnchorC = revolute->GetLocalAnchorA();
103 m_localAnchorA = revolute->GetLocalAnchorB();
104 m_referenceAngleA = revolute->GetReferenceAngle();
106 coordinateA = (aA - aC - m_referenceAngleA) /
Radian;
111 m_localAnchorC = prismatic->GetLocalAnchorA();
112 m_localAnchorA = prismatic->GetLocalAnchorB();
113 m_referenceAngleA = prismatic->GetReferenceAngle();
114 m_localAxisC = prismatic->GetLocalAxisA();
116 const auto pC = m_localAnchorC;
118 coordinateA =
Dot(pA - pC, m_localAxisC) /
Meter;
121 m_bodyD = m_joint2->GetBodyA();
127 const auto aD = m_bodyD->
GetAngle();
133 m_localAnchorD = revolute->GetLocalAnchorA();
134 m_localAnchorB = revolute->GetLocalAnchorB();
135 m_referenceAngleB = revolute->GetReferenceAngle();
137 coordinateB = (aB - aD - m_referenceAngleB) /
Radian;
142 m_localAnchorD = prismatic->GetLocalAnchorA();
143 m_localAnchorB = prismatic->GetLocalAnchorB();
144 m_referenceAngleB = prismatic->GetReferenceAngle();
145 m_localAxisD = prismatic->GetLocalAxisA();
147 const auto pD = m_localAnchorD;
149 coordinateB =
Dot(pB - pD, m_localAxisD) /
Meter;
152 m_constant = coordinateA + m_ratio * coordinateB;
157 visitor.
Visit(*
this);
162 visitor.
Visit(*
this);
168 auto& bodyConstraintA =
At(bodies,
GetBodyA());
169 auto& bodyConstraintB =
At(bodies,
GetBodyB());
170 auto& bodyConstraintC =
At(bodies, m_bodyC);
171 auto& bodyConstraintD =
At(bodies, m_bodyD);
173 auto velA = bodyConstraintA->GetVelocity();
174 const auto aA = bodyConstraintA->GetPosition().angular;
176 auto velB = bodyConstraintB->GetVelocity();
177 const auto aB = bodyConstraintB->GetPosition().angular;
179 auto velC = bodyConstraintC->GetVelocity();
180 const auto aC = bodyConstraintC->GetPosition().angular;
182 auto velD = bodyConstraintD->GetVelocity();
183 const auto aD = bodyConstraintD->GetPosition().angular;
190 auto invMass =
Real{0};
197 const auto invAngMass = bodyConstraintA->GetInvRotInertia() + bodyConstraintC->GetInvRotInertia();
202 const auto u =
Rotate(m_localAxisC, qC);
203 const auto rC =
Length2{
Rotate(m_localAnchorC - bodyConstraintC->GetLocalCenter(), qC)};
204 const auto rA =
Length2{
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)};
205 m_JvAC =
Real{1} * u;
206 m_JwC =
Cross(rC, u);
207 m_JwA =
Cross(rA, u);
210 const auto invLinMass =
InvMass{bodyConstraintC->GetInvMass() + bodyConstraintA->GetInvMass() + invRotMassC + invRotMassA};
217 m_JwB = m_ratio *
Meter;
218 m_JwD = m_ratio *
Meter;
219 const auto invAngMass =
InvRotInertia{
Square(m_ratio) * (bodyConstraintB->GetInvRotInertia() + bodyConstraintD->GetInvRotInertia())};
224 const auto u =
Rotate(m_localAxisD, qD);
225 const auto rD =
Rotate(m_localAnchorD - bodyConstraintD->GetLocalCenter(), qD);
226 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
227 m_JvBD = m_ratio * u;
228 m_JwD = m_ratio *
Cross(rD, u);
229 m_JwB = m_ratio *
Cross(rB, u);
232 const auto invLinMass =
InvMass{
233 Square(m_ratio) * (bodyConstraintD->GetInvMass() + bodyConstraintB->GetInvMass()) +
234 invRotMassD + invRotMassB
240 m_mass = (invMass >
Real{0})?
Real{1} / invMass:
Real{0};
245 (bodyConstraintA->GetInvMass() * m_impulse) * m_JvAC,
246 bodyConstraintA->GetInvRotInertia() * m_impulse * m_JwA /
Radian
249 (bodyConstraintB->GetInvMass() * m_impulse) * m_JvBD,
250 bodyConstraintB->GetInvRotInertia() * m_impulse * m_JwB /
Radian
253 (bodyConstraintC->GetInvMass() * m_impulse) * m_JvAC,
254 bodyConstraintC->GetInvRotInertia() * m_impulse * m_JwC /
Radian
257 (bodyConstraintD->GetInvMass() * m_impulse) * m_JvBD,
258 bodyConstraintD->GetInvRotInertia() * m_impulse * m_JwD /
Radian
266 bodyConstraintA->SetVelocity(velA);
267 bodyConstraintB->SetVelocity(velB);
268 bodyConstraintC->SetVelocity(velC);
269 bodyConstraintD->SetVelocity(velD);
274 auto& bodyConstraintA =
At(bodies,
GetBodyA());
275 auto& bodyConstraintB =
At(bodies,
GetBodyB());
276 auto& bodyConstraintC =
At(bodies, m_bodyC);
277 auto& bodyConstraintD =
At(bodies, m_bodyD);
279 auto velA = bodyConstraintA->GetVelocity();
280 auto velB = bodyConstraintB->GetVelocity();
281 auto velC = bodyConstraintC->GetVelocity();
282 auto velD = bodyConstraintD->GetVelocity();
286 const auto Cdot = acDot + bdDot
287 + (m_JwA * velA.angular - m_JwC * velC.angular) /
Radian
288 + (m_JwB * velB.angular - m_JwD * velD.angular) /
Radian;
291 m_impulse += impulse;
294 (bodyConstraintA->GetInvMass() * impulse) * m_JvAC,
295 bodyConstraintA->GetInvRotInertia() * impulse * m_JwA /
Radian
298 (bodyConstraintB->GetInvMass() * impulse) * m_JvBD,
299 bodyConstraintB->GetInvRotInertia() * impulse * m_JwB /
Radian
302 (bodyConstraintC->GetInvMass() * impulse) * m_JvAC,
303 bodyConstraintC->GetInvRotInertia() * impulse * m_JwC /
Radian
306 (bodyConstraintD->GetInvMass() * impulse) * m_JvBD,
307 bodyConstraintD->GetInvRotInertia() * impulse * m_JwD /
Radian
310 bodyConstraintA->SetVelocity(velA);
311 bodyConstraintB->SetVelocity(velB);
312 bodyConstraintC->SetVelocity(velC);
313 bodyConstraintD->SetVelocity(velD);
315 return impulse == 0_Ns;
318 bool GearJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
320 auto& bodyConstraintA =
At(bodies,
GetBodyA());
321 auto& bodyConstraintB =
At(bodies,
GetBodyB());
322 auto& bodyConstraintC =
At(bodies, m_bodyC);
323 auto& bodyConstraintD =
At(bodies, m_bodyD);
325 auto posA = bodyConstraintA->GetPosition();
326 auto posB = bodyConstraintB->GetPosition();
327 auto posC = bodyConstraintC->GetPosition();
328 auto posD = bodyConstraintD->GetPosition();
335 const auto linearError = 0_m;
338 Real JwA, JwB, JwC, JwD;
340 auto coordinateA =
Real{0};
341 auto coordinateB =
Real{0};
342 auto invMass =
Real{0};
349 const auto invAngMass = bodyConstraintA->GetInvRotInertia() + bodyConstraintC->GetInvRotInertia();
351 coordinateA = (posA.angular - posC.angular - m_referenceAngleA) /
Radian;
355 const auto u =
Rotate(m_localAxisC, qC);
356 const auto rC =
Rotate(m_localAnchorC - bodyConstraintC->GetLocalCenter(), qC);
357 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
361 const auto invLinMass =
InvMass{bodyConstraintC->GetInvMass() + bodyConstraintA->GetInvMass()};
364 invMass +=
StripUnit(invLinMass + invRotMassC + invRotMassA);
365 const auto pC = m_localAnchorC - bodyConstraintC->GetLocalCenter();
366 const auto pA =
InverseRotate(rA + (posA.linear - posC.linear), qC);
367 coordinateA =
Dot(pA - pC, m_localAxisC) /
Meter;
376 Square(m_ratio) * (bodyConstraintB->GetInvRotInertia() + bodyConstraintD->GetInvRotInertia())
379 coordinateB = (posB.angular - posD.angular - m_referenceAngleB) /
Radian;
383 const auto u =
Rotate(m_localAxisD, qD);
384 const auto rD =
Rotate(m_localAnchorD - bodyConstraintD->GetLocalCenter(), qD);
385 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
389 const auto invLinMass =
InvMass{
Square(m_ratio) * (bodyConstraintD->GetInvMass() + bodyConstraintB->GetInvMass())};
392 invMass +=
StripUnit(invLinMass + invRotMassD + invRotMassB);
393 const auto pD = m_localAnchorD - bodyConstraintD->GetLocalCenter();
394 const auto pB =
InverseRotate(rB + (posB.linear - posD.linear), qD);
395 coordinateB =
Dot(pB - pD, m_localAxisD) /
Meter;
398 const auto C = ((coordinateA + m_ratio * coordinateB) - m_constant);
400 const auto impulse = ((invMass > 0)? -C / invMass: 0) *
Kilogram *
Meter;
403 bodyConstraintA->GetInvMass() * impulse * JvAC,
404 bodyConstraintA->GetInvRotInertia() * impulse * JwA *
Meter /
Radian
407 bodyConstraintB->GetInvMass() * impulse * JvBD,
408 bodyConstraintB->GetInvRotInertia() * impulse * JwB *
Meter /
Radian
411 bodyConstraintC->GetInvMass() * impulse * JvAC,
412 bodyConstraintC->GetInvRotInertia() * impulse * JwC *
Meter /
Radian
415 bodyConstraintD->GetInvMass() * impulse * JvBD,
416 bodyConstraintD->GetInvRotInertia() * impulse * JwD *
Meter /
Radian
419 bodyConstraintA->SetPosition(posA);
420 bodyConstraintB->SetPosition(posB);
421 bodyConstraintC->SetPosition(posC);
422 bodyConstraintD->SetPosition(posD);
425 return linearError < conf.linearSlop;
440 return m_impulse * m_JvAC;
445 return m_impulse * m_JwA /
Radian;