45 m_localAnchorB{def.localAnchorB},
46 m_maxLength{def.maxLength}
64 auto& bodyConstraintA =
At(bodies,
GetBodyA());
65 auto& bodyConstraintB =
At(bodies,
GetBodyB());
67 const auto invMassA = bodyConstraintA->GetInvMass();
68 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
69 const auto posA = bodyConstraintA->GetPosition();
70 auto velA = bodyConstraintA->GetVelocity();
72 const auto invMassB = bodyConstraintB->GetInvMass();
73 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
74 const auto posB = bodyConstraintB->GetPosition();
75 auto velB = bodyConstraintB->GetVelocity();
80 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
81 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
82 const auto posDelta =
Length2{(posB.linear + m_rB) - (posA.linear + m_rA)};
84 const auto uvresult =
UnitVec::Get(posDelta[0], posDelta[1]);
85 const auto uv = std::get<UnitVec>(uvresult);
86 m_length = std::get<Length>(uvresult);
88 const auto C = m_length - m_maxLength;
104 const auto crA =
Cross(m_rA, m_u);
105 const auto crB =
Cross(m_rB, m_u);
108 const auto invMass = invMassA + invMassB + invRotMassA + invRotMassB;
110 m_mass = (invMass !=
InvMass{0}) ?
Real{1} / invMass : 0_kg;
117 const auto P = m_impulse * m_u;
123 velA -= Velocity{bodyConstraintA->GetInvMass() * P, invRotInertiaA * crossAP};
124 velB += Velocity{bodyConstraintB->GetInvMass() * P, invRotInertiaB * crossBP};
131 bodyConstraintA->SetVelocity(velA);
132 bodyConstraintB->SetVelocity(velB);
135 bool RopeJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
137 auto& bodyConstraintA =
At(bodies,
GetBodyA());
138 auto& bodyConstraintB =
At(bodies,
GetBodyB());
140 auto velA = bodyConstraintA->GetVelocity();
141 auto velB = bodyConstraintB->GetVelocity();
146 const auto C = m_length - m_maxLength;
153 auto impulse = -m_mass * Cdot;
154 const auto oldImpulse = m_impulse;
155 m_impulse = std::min(0_Ns, m_impulse + impulse);
156 impulse = m_impulse - oldImpulse;
158 const auto P = impulse * m_u;
164 velA -= Velocity{bodyConstraintA->GetInvMass() * P, bodyConstraintA->GetInvRotInertia() * crossAP};
165 velB += Velocity{bodyConstraintB->GetInvMass() * P, bodyConstraintB->GetInvRotInertia() * crossBP};
167 bodyConstraintA->SetVelocity(velA);
168 bodyConstraintB->SetVelocity(velB);
170 return impulse == 0_Ns;
173 bool RopeJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
175 auto& bodyConstraintA =
At(bodies,
GetBodyA());
176 auto& bodyConstraintB =
At(bodies,
GetBodyB());
178 auto posA = bodyConstraintA->GetPosition();
179 auto posB = bodyConstraintB->GetPosition();
184 const auto rA =
Length2{
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)};
185 const auto rB =
Length2{
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB)};
186 const auto posDelta = (posB.linear + rB) - (posA.linear + rA);
188 const auto uvresult =
UnitVec::Get(posDelta[0], posDelta[1]);
189 const auto u = std::get<UnitVec>(uvresult);
190 const auto length = std::get<Length>(uvresult);
192 const auto C = std::clamp(length - m_maxLength, 0_m, conf.maxLinearCorrection);
194 const auto impulse = -m_mass * C;
195 const auto linImpulse = impulse * u;
197 const auto angImpulseA =
Cross(rA, linImpulse) /
Radian;
198 const auto angImpulseB =
Cross(rB, linImpulse) /
Radian;
200 posA -= Position{bodyConstraintA->GetInvMass() * linImpulse, bodyConstraintA->GetInvRotInertia() * angImpulseA};
201 posB += Position{bodyConstraintB->GetInvMass() * linImpulse, bodyConstraintB->GetInvRotInertia() * angImpulseB};
203 bodyConstraintA->SetPosition(posA);
204 bodyConstraintB->SetPosition(posB);
206 return (length - m_maxLength) < conf.linearSlop;
221 return m_impulse * m_u;