48 m_linearOffset(def.linearOffset),
49 m_angularOffset(def.angularOffset),
50 m_maxForce(def.maxForce),
51 m_maxTorque(def.maxTorque),
52 m_correctionFactor(def.correctionFactor)
69 auto& bodyConstraintA =
At(bodies,
GetBodyA());
70 auto& bodyConstraintB =
At(bodies,
GetBodyB());
72 const auto posA = bodyConstraintA->GetPosition();
73 auto velA = bodyConstraintA->GetVelocity();
75 const auto posB = bodyConstraintB->GetPosition();
76 auto velB = bodyConstraintB->GetVelocity();
82 m_rA =
Rotate(m_linearOffset - bodyConstraintA->GetLocalCenter(), qA);
83 m_rB =
Rotate(-bodyConstraintB->GetLocalCenter(), qB);
93 const auto invMassA = bodyConstraintA->GetInvMass();
94 const auto invMassB = bodyConstraintB->GetInvMass();
95 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
96 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
100 invMassA + invMassB +
109 invMassA + invMassB +
114 const auto k22 =
InvMass22{Vector<InvMass, 2>{exx, exy}, Vector<InvMass, 2>{exy, eyy}};
115 m_linearMass =
Invert(k22);
118 const auto invRotInertia = invRotInertiaA + invRotInertiaB;
121 m_linearError = (posB.linear + m_rB) - (posA.linear + m_rA);
122 m_angularError = (posB.angular - posA.angular) - m_angularOffset;
127 m_linearImpulse *= step.
dtRatio;
128 m_angularImpulse *= step.
dtRatio;
130 const auto P = m_linearImpulse;
135 velA -= Velocity{invMassA * P, invRotInertiaA * (crossAP + m_angularImpulse)};
136 velB += Velocity{invMassB * P, invRotInertiaB * (crossBP + m_angularImpulse)};
144 bodyConstraintA->SetVelocity(velA);
145 bodyConstraintB->SetVelocity(velB);
148 bool MotorJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
150 auto& bodyConstraintA =
At(bodies,
GetBodyA());
151 auto& bodyConstraintB =
At(bodies,
GetBodyB());
153 auto velA = bodyConstraintA->GetVelocity();
154 auto velB = bodyConstraintB->GetVelocity();
156 const auto invMassA = bodyConstraintA->GetInvMass();
157 const auto invMassB = bodyConstraintB->GetInvMass();
158 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
159 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
161 const auto h = step.GetTime();
162 const auto inv_h = step.GetInvTime();
168 const auto Cdot =
AngularVelocity{(velB.angular - velA.angular) + inv_h * m_correctionFactor * m_angularError};
171 const auto oldAngularImpulse = m_angularImpulse;
172 const auto maxAngularImpulse = h * m_maxTorque;
173 const auto newAngularImpulse = std::clamp(oldAngularImpulse + angularImpulse,
174 -maxAngularImpulse, maxAngularImpulse);
175 m_angularImpulse = newAngularImpulse;
176 const auto incAngularImpulse = newAngularImpulse - oldAngularImpulse;
182 velA.angular -= invRotInertiaA * incAngularImpulse;
183 velB.angular += invRotInertiaB * incAngularImpulse;
190 const auto Cdot =
LinearVelocity2{(vb - va) + inv_h * m_correctionFactor * m_linearError};
192 const auto impulse = -
Transform(Cdot, m_linearMass);
193 const auto oldImpulse = m_linearImpulse;
194 m_linearImpulse += impulse;
196 const auto maxImpulse = h * m_maxForce;
203 const auto incImpulse = m_linearImpulse - oldImpulse;
212 velA -= Velocity{invMassA * incImpulse, invRotInertiaA * angImpulseA};
213 velB += Velocity{invMassB * incImpulse, invRotInertiaB * angImpulseB};
216 bodyConstraintA->SetVelocity(velA);
217 bodyConstraintB->SetVelocity(velB);
222 bool MotorJoint::SolvePositionConstraints(
BodyConstraintsMap&,
const ConstraintSolverConf&)
const
239 assert(
IsValid(factor) && (0 <= factor) && (factor <=
Real{1}));
240 m_correctionFactor = factor;
245 if (m_linearOffset != linearOffset)
247 m_linearOffset = linearOffset;
255 if (angularOffset != m_angularOffset)
257 m_angularOffset = angularOffset;