50 m_localAnchorA(def.localAnchorA),
51 m_localAnchorB(def.localAnchorB),
52 m_localXAxisA(def.localAxisA),
54 m_frequency(def.frequency),
55 m_dampingRatio(def.dampingRatio),
56 m_maxMotorTorque(def.maxMotorTorque),
57 m_motorSpeed(def.motorSpeed),
58 m_enableMotor(def.enableMotor)
75 auto& bodyConstraintA =
At(bodies,
GetBodyA());
76 auto& bodyConstraintB =
At(bodies,
GetBodyB());
78 const auto posA = bodyConstraintA->GetPosition();
79 auto velA = bodyConstraintA->GetVelocity();
80 const auto invMassA = bodyConstraintA->GetInvMass();
81 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
83 const auto posB = bodyConstraintB->GetPosition();
84 auto velB = bodyConstraintB->GetVelocity();
85 const auto invMassB = bodyConstraintB->GetInvMass();
86 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
92 const auto rA =
Length2{
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)};
93 const auto rB =
Length2{
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB)};
94 const auto dd =
Length2{(posB.linear + rB) - (posA.linear + rA)};
98 m_ay =
Rotate(m_localYAxisA, qA);
99 m_sAy =
Cross(dd + rA, m_ay);
100 m_sBy =
Cross(rB, m_ay);
104 const auto invMass = invMassA + invMassB + invRotMassA + invRotMassB;
106 m_mass = (invMass >
InvMass{0})?
Real{1} / invMass: 0;
113 if (m_frequency > 0_Hz)
115 m_ax =
Rotate(m_localXAxisA, qA);
116 m_sAx =
Cross(dd + rA, m_ax);
117 m_sBx =
Cross(rB, m_ax);
121 const auto invMass = invMassA + invMassB + invRotMassA + invRotMassB;
125 m_springMass =
Real{1} / invMass;
130 const auto omega =
Real{2} *
Pi * m_frequency;
133 const auto d =
Real{2} * m_springMass * m_dampingRatio * omega;
136 const auto k = m_springMass * omega * omega;
141 const auto invGamma =
Mass{h * (d + h * k)};
142 m_gamma = (invGamma > 0_kg)?
Real{1} / invGamma: 0;
145 const auto totalInvMass = invMass + m_gamma;
146 m_springMass = (totalInvMass >
InvMass{0})?
Real{1} / totalInvMass: 0_kg;
161 const auto invRotInertia = invRotInertiaA + invRotInertiaB;
174 m_springImpulse *= step.
dtRatio;
175 m_motorImpulse *= step.
dtRatio;
177 const auto P = m_impulse * m_ay + m_springImpulse * m_ax;
181 const auto LA =
AngularMomentum{(m_impulse * m_sAy + m_springImpulse * m_sAx) /
Radian + m_motorImpulse};
182 const auto LB =
AngularMomentum{(m_impulse * m_sBy + m_springImpulse * m_sBx) /
Radian + m_motorImpulse};
184 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
185 velB += Velocity{invMassB * P, invRotInertiaB * LB};
194 bodyConstraintA->SetVelocity(velA);
195 bodyConstraintB->SetVelocity(velB);
198 bool WheelJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
200 auto& bodyConstraintA =
At(bodies,
GetBodyA());
201 auto& bodyConstraintB =
At(bodies,
GetBodyB());
203 const auto oldVelA = bodyConstraintA->GetVelocity();
204 const auto invMassA = bodyConstraintA->GetInvMass();
205 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
207 const auto oldVelB = bodyConstraintB->GetVelocity();
208 const auto invMassB = bodyConstraintB->GetInvMass();
209 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
217 const auto Cdot = dot + m_sBx * velB.angular /
Radian - m_sAx * velA.angular /
Radian;
218 const auto impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
219 m_springImpulse += impulse;
221 const auto P = impulse * m_ax;
225 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
226 velB += Velocity{invMassB * P, invRotInertiaB * LB};
231 const auto Cdot = (velB.angular - velA.angular - m_motorSpeed);
234 const auto oldImpulse = m_motorImpulse;
235 const auto maxImpulse =
AngularMomentum{step.GetTime() * m_maxMotorTorque};
236 m_motorImpulse = std::clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
237 impulse = m_motorImpulse - oldImpulse;
246 const auto Cdot = dot + m_sBy * velB.angular /
Radian - m_sAy * velA.angular /
Radian;
247 const auto impulse = -m_mass * Cdot;
248 m_impulse += impulse;
250 const auto P = impulse * m_ay;
254 velA -= Velocity{invMassA * P, invRotInertiaA * LA};
255 velB += Velocity{invMassB * P, invRotInertiaB * LB};
258 if ((velA != oldVelA) || (velB != oldVelB))
260 bodyConstraintA->SetVelocity(velA);
261 bodyConstraintB->SetVelocity(velB);
267 bool WheelJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
269 auto& bodyConstraintA =
At(bodies,
GetBodyA());
270 auto& bodyConstraintB =
At(bodies,
GetBodyB());
272 auto posA = bodyConstraintA->GetPosition();
273 const auto invMassA = bodyConstraintA->GetInvMass();
274 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
276 auto posB = bodyConstraintB->GetPosition();
277 const auto invMassB = bodyConstraintB->GetInvMass();
278 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
283 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
284 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
285 const auto d =
Length2{(posB.linear - posA.linear) + (rB - rA)};
287 const auto ay =
Rotate(m_localYAxisA, qA);
289 const auto sAy =
Cross(d + rA, ay);
290 const auto sBy =
Cross(rB, ay);
297 const auto k =
InvMass{invMassA + invMassB + invRotMassA + invRotMassB};
301 const auto P = impulse * ay;
302 const auto LA = impulse * sAy /
Radian;
303 const auto LB = impulse * sBy /
Radian;
305 posA -= Position{invMassA * P, invRotInertiaA * LA};
306 posB += Position{invMassB * P, invRotInertiaB * LB};
308 bodyConstraintA->SetPosition(posA);
309 bodyConstraintB->SetPosition(posB);
311 return abs(C) <= conf.linearSlop;
326 return m_impulse * m_ay + m_springImpulse * m_ax;
331 if (m_enableMotor != flag)
333 m_enableMotor = flag;
343 if (m_motorSpeed != speed)
345 m_motorSpeed = speed;
355 if (m_maxMotorTorque != torque)
357 m_maxMotorTorque = torque;
367 const auto pA =
GetWorldPoint(*joint.GetBodyA(), joint.GetLocalAnchorA());
368 const auto pB =
GetWorldPoint(*joint.GetBodyB(), joint.GetLocalAnchorB());
369 const auto d = pB - pA;
370 const auto axis =
GetWorldVector(*joint.GetBodyA(), joint.GetLocalAxisA());
376 return joint.GetBodyB()->GetVelocity().angular - joint.GetBodyA()->GetVelocity().angular;