41 #if defined(B2_DEBUG_SOLVER)
68 const auto bodyA = vc.GetBodyA();
69 const auto bodyB = vc.GetBodyB();
70 const auto normal = vc.GetNormal();
72 const auto invRotInertiaA = bodyA->GetInvRotInertia();
73 const auto invMassA = bodyA->GetInvMass();
75 const auto invRotInertiaB = bodyB->GetInvRotInertia();
76 const auto invMassB = bodyB->GetInvMass();
79 const auto P0 = impulses[0] * normal;
80 const auto P1 = impulses[1] * normal;
81 const auto P = P0 + P1;
89 -Velocity{invMassA * P, invRotInertiaA * LA},
90 +Velocity{invMassB * P, invRotInertiaB * LB}
97 vc.GetBodyA()->SetVelocity(vc.GetBodyA()->GetVelocity() +
std::get<0>(delta_v));
98 vc.GetBodyB()->SetVelocity(vc.GetBodyB()->GetVelocity() +
std::get<1>(delta_v));
100 return std::max(
abs(newImpulses[0]),
abs(newImpulses[1]));
103 Optional<Momentum> BlockSolveNormalCase1(VelocityConstraint& vc,
115 const auto normalMass = vc.GetNormalMass();
116 const auto newImpulses = -
Transform(b_prime, normalMass);
117 if ((newImpulses[0] >= 0_Ns) && (newImpulses[1] >= 0_Ns))
119 const auto max = BlockSolveUpdate(vc, newImpulses);
121 #if defined(B2_DEBUG_SOLVER)
122 auto& vcp0 = vc.GetPointAt(0);
123 auto& vcp1 = vc.GetPointAt(1);
124 const auto velA = vc.GetBodyA()->GetVelocity();
125 const auto velB = vc.GetBodyB()->GetVelocity();
132 const auto vn0 =
Dot(dv0, vc.GetNormal());
133 const auto vn1 =
Dot(dv1, vc.GetNormal());
135 assert(
abs(vn0 - vcp0.velocityBias) < k_errorTol);
136 assert(
abs(vn1 - vcp1.velocityBias) < k_errorTol);
138 return Optional<Momentum>{max};
140 return Optional<Momentum>{};
143 Optional<Momentum> BlockSolveNormalCase2(VelocityConstraint& vc,
const LinearVelocity2 b_prime)
154 const auto K = vc.GetK();
156 if ((
get<0>(newImpulses) >= 0_Ns) && (vn2 >= 0_mps))
158 const auto max = BlockSolveUpdate(vc, newImpulses);
160 #if defined(B2_DEBUG_SOLVER)
161 auto& vcp1 = vc.GetPointAt(0);
162 const auto velA = vc.GetBodyA()->GetVelocity();
163 const auto velB = vc.GetBodyB()->GetVelocity();
169 const auto vn1 =
Dot(dv1, vc.GetNormal());
171 assert(
abs(vn1 - vcp1.velocityBias) < k_errorTol);
173 return Optional<Momentum>{max};
175 return Optional<Momentum>{};
178 Optional<Momentum> BlockSolveNormalCase3(VelocityConstraint& vc,
const LinearVelocity2 b_prime)
189 const auto K = vc.GetK();
191 if ((
get<1>(newImpulses) >= 0_Ns) && (vn1 >= 0_mps))
193 const auto max = BlockSolveUpdate(vc, newImpulses);
195 #if defined(B2_DEBUG_SOLVER)
196 auto& vcp2 = vc.GetPointAt(1);
197 const auto velA = vc.GetBodyA()->GetVelocity();
198 const auto velB = vc.GetBodyB()->GetVelocity();
204 const auto vn2 =
Dot(dv2, vc.GetNormal());
206 assert(
abs(vn2 - vcp2.velocityBias) < k_errorTol);
208 return Optional<Momentum>{max};
210 return Optional<Momentum>{};
213 Optional<Momentum> BlockSolveNormalCase4(VelocityConstraint& vc,
const LinearVelocity2 b_prime)
220 const auto vn1 =
get<0>(b_prime);
221 const auto vn2 =
get<1>(b_prime);
222 if ((vn1 >= 0_mps) && (vn2 >= 0_mps))
224 return Optional<Momentum>{BlockSolveUpdate(vc,
Momentum2{0_Ns, 0_Ns})};
226 return Optional<Momentum>{};
229 inline Momentum BlockSolveNormalConstraint(VelocityConstraint& vc)
231 assert(vc.GetPointCount() == 2);
266 const auto b_prime = [=]() {
267 const auto normal = vc.GetNormal();
269 const auto velA = vc.GetBodyA()->GetVelocity();
270 const auto velB = vc.GetBodyB()->GetVelocity();
271 const auto ra0 = vc.GetPointRelPosA(0);
272 const auto rb0 = vc.GetPointRelPosB(0);
273 const auto ra1 = vc.GetPointRelPosA(1);
274 const auto rb1 = vc.GetPointRelPosB(1);
280 const auto vn0 =
Dot(dv0, normal);
281 const auto vn1 =
Dot(dv1, normal);
285 vn0 - vc.GetVelocityBiasAtPoint(0),
286 vn1 - vc.GetVelocityBiasAtPoint(1)
290 const auto K = vc.GetK();
294 auto maxIncImpulse = Optional<Momentum>{};
295 maxIncImpulse = BlockSolveNormalCase1(vc, b_prime);
296 if (maxIncImpulse.has_value())
298 return *maxIncImpulse;
300 maxIncImpulse = BlockSolveNormalCase2(vc, b_prime);
301 if (maxIncImpulse.has_value())
303 return *maxIncImpulse;
305 maxIncImpulse = BlockSolveNormalCase3(vc, b_prime);
306 if (maxIncImpulse.has_value())
308 return *maxIncImpulse;
310 maxIncImpulse = BlockSolveNormalCase4(vc, b_prime);
311 if (maxIncImpulse.has_value())
313 return *maxIncImpulse;
320 inline Momentum SeqSolveNormalConstraint(VelocityConstraint& vc)
322 auto maxIncImpulse = 0_Ns;
325 const auto count = vc.GetPointCount();
326 const auto bodyA = vc.GetBodyA();
327 const auto bodyB = vc.GetBodyB();
329 const auto invRotInertiaA = bodyA->GetInvRotInertia();
330 const auto invMassA = bodyA->GetInvMass();
331 const auto oldVelA = bodyA->GetVelocity();
333 const auto invRotInertiaB = bodyB->GetInvRotInertia();
334 const auto invMassB = bodyB->GetInvMass();
335 const auto oldVelB = bodyB->GetVelocity();
337 auto newVelA = oldVelA;
338 auto newVelB = oldVelB;
341 const auto vcp = vc.GetPointAt(index);
344 const auto lambda =
Momentum{vcp.normalMass * (vcp.velocityBias - directionalVel)};
345 const auto oldImpulse = vcp.normalImpulse;
346 const auto newImpulse = std::max(oldImpulse + lambda, 0_Ns);
349 const auto incImpulse =
AlmostEqual(newImpulse, oldImpulse)? 0_Ns: newImpulse - oldImpulse;
351 const auto incImpulse = newImpulse - oldImpulse;
356 newVelA -= Velocity{invMassA * P, invRotInertiaA * LA};
357 newVelB += Velocity{invMassB * P, invRotInertiaB * LB};
358 maxIncImpulse = std::max(maxIncImpulse,
abs(incImpulse));
362 vc.SetNormalImpulseAtPoint(index, oldImpulse + incImpulse);
371 bodyA->SetVelocity(newVelA);
372 bodyB->SetVelocity(newVelB);
374 return maxIncImpulse;
377 inline Momentum SolveTangentConstraint(VelocityConstraint& vc)
379 auto maxIncImpulse = 0_Ns;
382 const auto friction = vc.GetFriction();
383 const auto tangentSpeed = vc.GetTangentSpeed();
384 const auto count = vc.GetPointCount();
385 const auto bodyA = vc.GetBodyA();
386 const auto bodyB = vc.GetBodyB();
388 const auto invRotInertiaA = bodyA->GetInvRotInertia();
389 const auto invMassA = bodyA->GetInvMass();
390 const auto oldVelA = bodyA->GetVelocity();
392 const auto invRotInertiaB = bodyB->GetInvRotInertia();
393 const auto invMassB = bodyB->GetInvMass();
394 const auto oldVelB = bodyB->GetVelocity();
396 auto newVelA = oldVelA;
397 auto newVelB = oldVelB;
400 const auto vcp = vc.GetPointAt(index);
403 const auto lambda = vcp.tangentMass * directionalVel;
404 const auto maxImpulse = friction * vcp.normalImpulse;
405 const auto oldImpulse = vcp.tangentImpulse;
406 const auto newImpulse = std::clamp(oldImpulse + lambda, -maxImpulse, maxImpulse);
409 const auto incImpulse =
AlmostEqual(newImpulse, oldImpulse)? 0_Ns: newImpulse - oldImpulse;
411 const auto incImpulse = newImpulse - oldImpulse;
416 newVelA -= Velocity{invMassA * P, invRotInertiaA * LA};
417 newVelB += Velocity{invMassB * P, invRotInertiaB * LB};
418 maxIncImpulse = std::max(maxIncImpulse,
abs(incImpulse));
422 vc.SetTangentImpulseAtPoint(index, oldImpulse + incImpulse);
424 assert((count == 1) || (count == 2));
431 bodyA->SetVelocity(newVelA);
432 bodyB->SetVelocity(newVelB);
434 return maxIncImpulse;
437 inline Momentum SolveNormalConstraint(VelocityConstraint& vc)
443 const auto count = vc.GetPointCount();
444 assert((count == 1) || (count == 2));
445 if ((count == 1) || (vc.GetK() ==
InvMass22{}))
447 return SeqSolveNormalConstraint(vc);
449 return BlockSolveNormalConstraint(vc);
451 return SeqSolveNormalConstraint(vc);
479 namespace GaussSeidel {
483 auto maxIncImpulse = 0_Ns;
486 maxIncImpulse = std::max(maxIncImpulse, d2::SolveTangentConstraint(vc));
489 maxIncImpulse = std::max(maxIncImpulse, d2::SolveNormalConstraint(vc));
491 return maxIncImpulse;
495 const bool moveA,
const bool moveB,
506 const auto invRotInertiaA = moveA? bodyA->GetInvRotInertia():
InvRotInertia{0};
507 const auto localCenterA = bodyA->GetLocalCenter();
509 const auto invMassB = moveB? bodyB->GetInvMass():
InvMass{0};
510 const auto invRotInertiaB = moveB? bodyB->GetInvRotInertia():
InvRotInertia{0};
511 const auto localCenterB = bodyB->GetLocalCenter();
515 const auto invMassTotal = invMassA + invMassB;
516 assert(invMassTotal >=
InvMass{0});
536 const auto invRotMassA =
InvMass{invRotInertiaA *
Square(rnA)};
537 const auto invRotMassB =
InvMass{invRotInertiaB *
Square(rnB)};
538 return invMassTotal + invRotMassA + invRotMassB;
582 const auto s0 = solver_fn(psm0, posA.linear, posB.linear);
589 const auto s1 = solver_fn(psm1, posA.linear, posB.linear);
593 return PositionSolution{posA, posB, std::min(s0.min_separation, s1.min_separation)};
606 const auto s0 = solver_fn(psm0, posA.linear, posB.linear);
607 const auto s1 = solver_fn(psm1, posA.linear, posB.linear);
611 posA + s0.
pos_a + s1.pos_a,
612 posB + s0.pos_b + s1.pos_b,
616 if (psm0.m_separation < psm1.m_separation)
618 const auto s0 = solver_fn(psm0, posA.linear, posB.linear);
624 const auto s1 = solver_fn(psm1_prime, posA.linear, posB.linear);
629 if (psm1.m_separation < psm0.m_separation)
631 const auto s1 = solver_fn(psm1, posA.linear, posB.linear);
637 const auto s0 = solver_fn(psm0_prime, posA.linear, posB.linear);