90 m_localAnchorA(def.localAnchorA),
91 m_localAnchorB(def.localAnchorB),
92 m_referenceAngle(def.referenceAngle),
93 m_frequency(def.frequency),
94 m_dampingRatio(def.dampingRatio)
101 visitor.
Visit(*
this);
106 visitor.
Visit(*
this);
112 auto& bodyConstraintA =
At(bodies,
GetBodyA());
113 auto& bodyConstraintB =
At(bodies,
GetBodyB());
115 auto velA = bodyConstraintA->GetVelocity();
116 const auto posA = bodyConstraintA->GetPosition();
117 const auto invMassA = bodyConstraintA->GetInvMass();
118 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
120 auto velB = bodyConstraintB->GetVelocity();
121 const auto posB = bodyConstraintB->GetPosition();
122 const auto invMassB = bodyConstraintB->GetInvMass();
123 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
128 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
129 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
140 const auto K = GetMat33(invMassA, m_rA, invRotInertiaA, invMassB, m_rB, invRotInertiaB);
141 if (m_frequency > 0_Hz)
147 auto invRotInertia =
InvRotInertia{invRotInertiaA + invRotInertiaB};
150 const auto C =
Angle{posB.angular - posA.angular - m_referenceAngle};
151 const auto omega =
Real(2) *
Pi * m_frequency;
152 const auto d =
Real(2) * rotInertia * m_dampingRatio * omega;
155 const auto k = rotInertia * omega * omega;
159 const auto invGamma =
RotInertia{h * (d + h * k)};
164 invRotInertia += m_gamma;
193 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
194 velB += Velocity{invMassB * P, invRotInertiaB * LB};
201 bodyConstraintA->SetVelocity(velA);
202 bodyConstraintB->SetVelocity(velB);
207 auto& bodyConstraintA =
At(bodies,
GetBodyA());
208 auto& bodyConstraintB =
At(bodies,
GetBodyB());
210 const auto oldVelA = bodyConstraintA->GetVelocity();
212 const auto invMassA = bodyConstraintA->GetInvMass();
213 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
215 const auto oldVelB = bodyConstraintB->GetVelocity();
217 const auto invMassB = bodyConstraintB->GetInvMass();
218 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
220 if (m_frequency > 0_Hz)
222 const auto Cdot2 = velB.angular - velA.angular;
229 GetZ(m_impulse) += impulse2;
237 const auto Cdot1 = vb - va;
247 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
248 velB += Velocity{invMassB * P, invRotInertiaB * LB};
255 const auto Cdot1 = vb - va;
259 const auto impulse = -
Transform(Cdot, m_mass);
260 m_impulse += impulse;
269 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
270 velB += Velocity{invMassB * P, invRotInertiaB * LB};
273 if ((velA != oldVelA) || (velB != oldVelB))
275 bodyConstraintA->SetVelocity(velA);
276 bodyConstraintB->SetVelocity(velB);
282 bool WeldJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
284 auto& bodyConstraintA =
At(bodies,
GetBodyA());
285 auto& bodyConstraintB =
At(bodies,
GetBodyB());
287 auto posA = bodyConstraintA->GetPosition();
288 auto posB = bodyConstraintB->GetPosition();
293 const auto invMassA = bodyConstraintA->GetInvMass();
294 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
295 const auto invMassB = bodyConstraintB->GetInvMass();
296 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
298 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
299 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
301 auto positionError = 0_m;
302 auto angularError = 0_deg;
304 const auto K = GetMat33(invMassA, rA, invRotInertiaA, invMassB, rB, invRotInertiaB);
305 if (m_frequency > 0_Hz)
307 const auto C1 =
Length2{(posB.linear + rB) - (posA.linear + rA)};
310 angularError = 0_deg;
316 posA -= Position{invMassA * P, invRotInertiaA * LA};
317 posB += Position{invMassB * P, invRotInertiaB * LB};
321 const auto C1 =
Length2{(posB.linear + rB) - (posA.linear + rA)};
322 const auto C2 =
Angle{posB.angular - posA.angular - m_referenceAngle};
325 angularError =
abs(C2);
345 posA -= Position{invMassA * P, invRotInertiaA * LA};
346 posB += Position{invMassB * P, invRotInertiaB * LB};
349 bodyConstraintA->SetPosition(posA);
350 bodyConstraintB->SetPosition(posB);
352 return (positionError <= conf.linearSlop) && (angularError <= conf.angularSlop);