25 #define PLAYRHO_MAGIC(x) (x)
32 inline InvMass3 ComputeK(
const VelocityConstraint& vc) noexcept
34 assert(vc.GetPointCount() == 2);
36 const auto normal = vc.GetNormal();
37 const auto bodyA = vc.GetBodyA();
38 const auto bodyB = vc.GetBodyB();
40 const auto relA0 = vc.GetPointRelPosA(0);
41 const auto relB0 = vc.GetPointRelPosB(0);
42 const auto relA1 = vc.GetPointRelPosA(1);
43 const auto relB1 = vc.GetPointRelPosB(1);
50 const auto invRotInertiaA = bodyA->GetInvRotInertia();
51 const auto invMassA = bodyA->GetInvMass();
53 const auto invRotInertiaB = bodyB->GetInvRotInertia();
54 const auto invMassB = bodyB->GetInvMass();
56 const auto invMass = invMassA + invMassB;
66 const auto k00 = invMass + invRotMassA0 + invRotMassB0;
67 const auto k11 = invMass + invRotMassA1 + invRotMassB1;
68 const auto k01 = invMass + invRotMassA + invRotMassB;
81 m_bodyA{&bA}, m_bodyB{&bB},
82 m_normal{worldManifold.GetNormal()},
83 m_friction{friction}, m_restitution{restitution}, m_tangentSpeed{tangentSpeed}
90 const auto pointCount = worldManifold.GetPointCount();
91 assert(pointCount > 0);
92 for (
auto j = decltype(pointCount){0}; j < pointCount; ++j)
94 const auto ci = worldManifold.GetImpulses(j);
95 const auto worldPoint = worldManifold.GetPoint(j);
96 const auto relA = worldPoint - bA.GetPosition().linear;
97 const auto relB = worldPoint - bB.GetPosition().linear;
101 if (conf.blockSolve && (pointCount == 2))
103 const auto k = ComputeK(*
this);
108 const auto k01_squared =
Square(get<2>(k));
109 const auto k_diff = k00_times_k11 - k01_squared;
111 if (k00_squared < maxCondNum * k_diff)
128 VelocityConstraint::Point
129 VelocityConstraint::GetPoint(
Momentum normalImpulse,
Momentum tangentImpulse,
132 assert(
IsValid(normalImpulse));
133 assert(
IsValid(tangentImpulse));
140 const auto invRotInertiaA = bodyA->GetInvRotInertia();
141 const auto invMassA = bodyA->GetInvMass();
142 const auto invRotInertiaB = bodyB->GetInvRotInertia();
143 const auto invMassB = bodyB->GetInvMass();
144 const auto invMass = invMassA + invMassB;
147 point.normalImpulse = normalImpulse;
148 point.tangentImpulse = tangentImpulse;
149 point.velocityBias = [&]() {
156 bodyB->GetVelocity(), relB);
158 return (vn < -conf.velocityThreshold)? -
GetRestitution() * vn: 0_mps;
162 point.normalMass = [&](){
165 const auto value = invMass + invRotMassA + invRotMassB;
166 return (value !=
InvMass{0})?
Real{1} / value : 0_kg;
168 point.tangentMass = [&]() {
171 const auto value = invMass + invRotMassA + invRotMassB;
172 return (value !=
InvMass{0})?
Real{1} / value : 0_kg;
178 void VelocityConstraint::AddPoint(
Momentum normalImpulse,
Momentum tangentImpulse,
182 m_points[m_pointCount] = GetPoint(normalImpulse * conf.dtRatio, tangentImpulse * conf.dtRatio,
190 conf.doWarmStart? conf.
dtRatio: 0,
191 conf.velocityThreshold,