60 m_localAnchorA(def.localAnchorA),
61 m_localAnchorB(def.localAnchorB),
63 m_frequency(def.frequency),
64 m_dampingRatio(def.dampingRatio)
83 auto& bodyConstraintA =
At(bodies,
GetBodyA());
84 auto& bodyConstraintB =
At(bodies,
GetBodyB());
86 const auto invMassA = bodyConstraintA->GetInvMass();
87 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
88 const auto invMassB = bodyConstraintB->GetInvMass();
89 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
91 const auto posA = bodyConstraintA->GetPosition();
92 auto velA = bodyConstraintA->GetVelocity();
94 const auto posB = bodyConstraintB->GetPosition();
95 auto velB = bodyConstraintB->GetVelocity();
100 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
101 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
102 const auto deltaLocation =
Length2{(posB.linear + m_rB) - (posA.linear + m_rA)};
105 const auto uvresult =
UnitVec::Get(deltaLocation[0], deltaLocation[1]);
106 m_u = std::get<UnitVec>(uvresult);
107 const auto length = std::get<Length>(uvresult);
111 const auto invRotMassA = invRotInertiaA *
Square(crAu);
112 const auto invRotMassB = invRotInertiaB *
Square(crBu);
113 auto invMass =
InvMass{invMassA + invRotMassA + invMassB + invRotMassB};
116 m_mass = (invMass !=
InvMass{0}) ?
Real{1} / invMass: 0_kg;
118 if (m_frequency > 0_Hz)
120 const auto C = length - m_length;
123 const auto omega =
Real{2} *
Pi * m_frequency;
126 const auto d =
Real{2} * m_mass * m_dampingRatio * omega;
129 const auto k = m_mass *
Square(omega);
133 const auto gamma =
Mass{h * (d + h * k)};
134 m_invGamma = (gamma != 0_kg)?
Real{1} / gamma: 0;
135 m_bias = C * h * k * m_invGamma;
137 invMass += m_invGamma;
138 m_mass = (invMass !=
InvMass{0}) ?
Real{1} / invMass: 0;
151 const auto P = m_impulse * m_u;
159 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
160 velB += Velocity{invMassB * P, invRotInertiaB * LB};
167 bodyConstraintA->SetVelocity(velA);
168 bodyConstraintB->SetVelocity(velB);
171 bool DistanceJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf&)
173 auto& bodyConstraintA =
At(bodies,
GetBodyA());
174 auto& bodyConstraintB =
At(bodies,
GetBodyB());
176 const auto invMassA = bodyConstraintA->GetInvMass();
177 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
178 const auto invMassB = bodyConstraintB->GetInvMass();
179 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
181 auto velA = bodyConstraintA->GetVelocity();
182 auto velB = bodyConstraintB->GetVelocity();
189 const auto impulse =
Momentum{-m_mass * (Cdot + m_bias + m_invGamma * m_impulse)};
190 m_impulse += impulse;
192 const auto P = impulse * m_u;
195 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
196 velB += Velocity{invMassB * P, invRotInertiaB * LB};
198 bodyConstraintA->SetVelocity(velA);
199 bodyConstraintB->SetVelocity(velB);
201 return impulse == 0_Ns;
205 const ConstraintSolverConf& conf)
const
207 if (m_frequency > 0_Hz)
213 auto& bodyConstraintA =
At(bodies,
GetBodyA());
214 auto& bodyConstraintB =
At(bodies,
GetBodyB());
216 const auto invMassA = bodyConstraintA->GetInvMass();
217 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
218 const auto invMassB = bodyConstraintB->GetInvMass();
219 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
221 auto posA = bodyConstraintA->GetPosition();
222 auto posB = bodyConstraintB->GetPosition();
227 const auto rA =
Length2{
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)};
228 const auto rB =
Length2{
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB)};
229 const auto relLoc =
Length2{(posB.linear + rB) - (posA.linear + rA)};
231 const auto uvresult =
UnitVec::Get(relLoc[0], relLoc[1]);
232 const auto u = std::get<UnitVec>(uvresult);
233 const auto length = std::get<Length>(uvresult);
234 const auto deltaLength = length - m_length;
235 const auto C = std::clamp(deltaLength, -conf.maxLinearCorrection, conf.maxLinearCorrection);
237 const auto impulse = -m_mass * C;
238 const auto P = impulse * u;
240 posA -= Position{invMassA * P, invRotInertiaA *
Cross(rA, P) /
Radian};
241 posB += Position{invMassB * P, invRotInertiaB *
Cross(rB, P) /
Radian};
243 bodyConstraintA->SetPosition(posA);
244 bodyConstraintB->SetPosition(posB);
246 return abs(C) < conf.linearSlop;
261 return m_impulse * m_u;