46 m_groundAnchorA(def.groundAnchorA),
47 m_groundAnchorB(def.groundAnchorB),
48 m_localAnchorA(def.localAnchorA),
49 m_localAnchorB(def.localAnchorB),
50 m_lengthA(def.lengthA),
51 m_lengthB(def.lengthB),
53 m_constant(def.lengthA + def.ratio * def.lengthB)
72 auto& bodyConstraintA =
At(bodies,
GetBodyA());
73 auto& bodyConstraintB =
At(bodies,
GetBodyB());
75 const auto posA = bodyConstraintA->GetPosition();
76 const auto invMassA = bodyConstraintA->GetInvMass();
77 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
78 auto velA = bodyConstraintA->GetVelocity();
80 const auto posB = bodyConstraintB->GetPosition();
81 const auto invMassB = bodyConstraintB->GetInvMass();
82 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
83 auto velB = bodyConstraintB->GetVelocity();
88 m_rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA);
89 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
92 const auto pulleyAxisA =
Length2{posA.linear + m_rA - m_groundAnchorA};
93 const auto pulleyAxisB =
Length2{posB.linear + m_rB - m_groundAnchorB};
99 const auto ruA =
Cross(m_rA, m_uA);
100 const auto ruB =
Cross(m_rB, m_uB);
105 const auto totalInvMass = totInvMassA + m_ratio * m_ratio * totInvMassB;
107 m_mass = (totalInvMass >
InvMass{0})?
Real{1} / totalInvMass: 0_kg;
115 const auto PA = -(m_impulse) * m_uA;
116 const auto PB = (-m_ratio * m_impulse) * m_uB;
118 velA += Velocity{invMassA * PA, invRotInertiaA *
Cross(m_rA, PA) /
Radian};
119 velB += Velocity{invMassB * PB, invRotInertiaB *
Cross(m_rB, PB) /
Radian};
126 bodyConstraintA->SetVelocity(velA);
127 bodyConstraintB->SetVelocity(velB);
130 bool PulleyJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf&)
132 auto& bodyConstraintA =
At(bodies,
GetBodyA());
133 auto& bodyConstraintB =
At(bodies,
GetBodyB());
135 const auto invMassA = bodyConstraintA->GetInvMass();
136 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
137 auto velA = bodyConstraintA->GetVelocity();
139 const auto invMassB = bodyConstraintB->GetInvMass();
140 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
141 auto velB = bodyConstraintB->GetVelocity();
147 const auto impulse = -m_mass * Cdot;
148 m_impulse += impulse;
150 const auto PA = -impulse * m_uA;
151 const auto PB = -m_ratio * impulse * m_uB;
152 velA += Velocity{invMassA * PA, invRotInertiaA *
Cross(m_rA, PA) /
Radian};
153 velB += Velocity{invMassB * PB, invRotInertiaB *
Cross(m_rB, PB) /
Radian};
155 bodyConstraintA->SetVelocity(velA);
156 bodyConstraintB->SetVelocity(velB);
158 return impulse == 0_Ns;
162 const ConstraintSolverConf& conf)
const
164 auto& bodyConstraintA =
At(bodies,
GetBodyA());
165 auto& bodyConstraintB =
At(bodies,
GetBodyB());
167 const auto invMassA = bodyConstraintA->GetInvMass();
168 const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
169 auto posA = bodyConstraintA->GetPosition();
171 const auto invMassB = bodyConstraintB->GetInvMass();
172 const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
173 auto posB = bodyConstraintB->GetPosition();
175 const auto rA =
Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(),
177 const auto rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(),
181 const auto pA =
Length2{posA.linear + rA - m_groundAnchorA};
183 const auto uA = std::get<UnitVec>(uvresultA);
184 const auto lengthA = std::get<Length>(uvresultA);
186 const auto pB =
Length2{posB.linear + rB - m_groundAnchorB};
188 const auto uB = std::get<UnitVec>(uvresultB);
189 const auto lengthB = std::get<Length>(uvresultB);
195 const auto totalInvMassA = invMassA + invRotInertiaA * ruA * ruA /
SquareRadian;
196 const auto totalInvMassB = invMassB + invRotInertiaB * ruB * ruB /
SquareRadian;
198 const auto totalInvMass = totalInvMassA + m_ratio * m_ratio * totalInvMassB;
199 const auto mass = (totalInvMass >
InvMass{0})?
Real{1} / totalInvMass: 0_kg;
201 const auto C =
Length{m_constant - lengthA - (m_ratio * lengthB)};
202 const auto linearError =
abs(C);
204 const auto impulse = -mass * C;
206 const auto PA = -impulse * uA;
207 const auto PB = -m_ratio * impulse * uB;
209 posA += Position{invMassA * PA, invRotInertiaA *
Cross(rA, PA) /
Radian};
210 posB += Position{invMassB * PB, invRotInertiaB *
Cross(rB, PB) /
Radian};
212 bodyConstraintA->SetPosition(posA);
213 bodyConstraintB->SetPosition(posB);
215 return linearError < conf.linearSlop;
230 return m_impulse * m_uB;
240 m_groundAnchorA -= newOrigin;
241 m_groundAnchorB -= newOrigin;