31 inline WorldManifold GetForCircles(
const Manifold& manifold,
32 const Transformation xfA,
const Length radiusA,
33 const Transformation xfB,
const Length radiusB)
35 assert(manifold.GetPointCount() == 1);
37 const auto pointA =
Transform(manifold.GetLocalPoint(), xfA);
38 const auto pointB =
Transform(manifold.GetPoint(0).localPoint, xfB);
40 const auto cA = pointA + (radiusA * normal);
41 const auto cB = pointB - (radiusB * normal);
42 const auto p0 = (cA + cB) /
Real{2};
43 const auto c0 = manifold.GetContactImpulses(0);
44 const auto s0 =
Dot(cB - cA, normal);
45 return WorldManifold{normal, WorldManifold::PointData{p0, c0, s0}};
48 inline WorldManifold GetForFaceA(
const Manifold& manifold,
49 const Transformation xfA,
const Length radiusA,
50 const Transformation xfB,
const Length radiusB)
52 const auto normal =
Rotate(manifold.GetLocalNormal(), xfA.q);
53 const auto planePoint =
Transform(manifold.GetLocalPoint(), xfA);
55 const auto impulses = manifold.GetContactImpulses(index);
56 const auto clipPoint =
Transform(manifold.GetPoint(index).localPoint, xfB);
57 const auto cA = clipPoint + (radiusA -
Dot(clipPoint - planePoint, normal)) * normal;
58 const auto cB = clipPoint - (radiusB * normal);
59 return WorldManifold::PointData{(cA + cB) /
Real{2}, impulses,
Dot(cB - cA, normal)};
62 assert(manifold.GetPointCount() <= 2);
64 switch (manifold.GetPointCount())
66 case 1:
return WorldManifold{normal, pointFn(0)};
67 case 2:
return WorldManifold{normal, pointFn(0), pointFn(1)};
72 return WorldManifold{normal};
75 inline WorldManifold GetForFaceB(
const Manifold& manifold,
76 const Transformation xfA,
const Length radiusA,
77 const Transformation xfB,
const Length radiusB)
79 const auto normal =
Rotate(manifold.GetLocalNormal(), xfB.q);
80 const auto planePoint =
Transform(manifold.GetLocalPoint(), xfB);
82 const auto impulses = manifold.GetContactImpulses(index);
83 const auto clipPoint =
Transform(manifold.GetPoint(index).localPoint, xfA);
84 const auto cB = clipPoint + (radiusB -
Dot(clipPoint - planePoint, normal)) * normal;
85 const auto cA = clipPoint - (radiusA * normal);
86 return WorldManifold::PointData{(cA + cB) /
Real{2}, impulses,
Dot(cA - cB, normal)};
89 assert(manifold.GetPointCount() <= 2);
92 switch (manifold.GetPointCount())
94 case 1:
return WorldManifold{-normal, pointFn(0)};
95 case 2:
return WorldManifold{-normal, pointFn(0), pointFn(1)};
100 return WorldManifold{-normal};
109 const auto type = manifold.
GetType();
117 case Manifold::e_faceA:
return GetForFaceA(manifold, xfA, radiusA, xfB, radiusB);
118 case Manifold::e_faceB:
return GetForFaceB(manifold, xfA, radiusA, xfB, radiusB);