34 inline PositionSolverManifold GetForCircles(
const Transformation& xfA,
Length2 lp,
35 const Transformation& xfB,
Length2 plp)
39 const auto delta = pointB - pointA;
41 const auto midpoint = (pointA + pointB) /
Real{2};
42 const auto separation =
Dot(delta, normal);
43 return PositionSolverManifold{normal, midpoint, separation};
55 inline PositionSolverManifold GetForFaceA(
const Transformation& xfA,
Length2 lp, UnitVec ln,
56 const Transformation& xfB,
Length2 plp)
58 const auto planePoint =
Transform(lp, xfA);
59 const auto normal =
Rotate(ln, xfA.q);
60 const auto clipPoint =
Transform(plp, xfB);
61 const auto separation =
Dot(clipPoint - planePoint, normal);
62 return PositionSolverManifold{normal, clipPoint, separation};
74 inline PositionSolverManifold GetForFaceB(
const Transformation& xfB,
Length2 lp, UnitVec ln,
75 const Transformation& xfA,
Length2 plp)
77 const auto planePoint =
Transform(lp, xfB);
78 const auto normal =
Rotate(ln, xfB.q);
79 const auto clipPoint =
Transform(plp, xfA);
80 const auto separation =
Dot(clipPoint - planePoint, normal);
82 return PositionSolverManifold{-normal, clipPoint, separation};