30 FindMinSeparationForPoints(
const SeparationScenario& scenario,
31 const Transformation& xfA,
const Transformation& xfB)
37 const auto pointA =
Transform(scenario.proxyA.GetVertex(indexA), xfA);
38 const auto pointB =
Transform(scenario.proxyB.GetVertex(indexB), xfB);
39 const auto delta = pointB - pointA;
40 return LengthIndexPair{
Dot(delta, scenario.axis),
IndexPair{indexA, indexB}};
44 FindMinSeparationForFaceA(
const SeparationScenario& scenario,
45 const Transformation& xfA,
const Transformation& xfB)
47 const auto normal =
Rotate(scenario.axis, xfA.q);
49 const auto pointA =
Transform(scenario.localPoint, xfA);
52 const auto pointB =
Transform(scenario.proxyB.GetVertex(indexB), xfB);
53 const auto delta = pointB - pointA;
54 return LengthIndexPair{
Dot(delta, normal),
IndexPair{indexA, indexB}};
58 FindMinSeparationForFaceB(
const SeparationScenario& scenario,
59 const Transformation& xfA,
const Transformation& xfB)
61 const auto normal =
Rotate(scenario.axis, xfB.q);
64 const auto pointA =
Transform(scenario.proxyA.GetVertex(indexA), xfA);
66 const auto pointB =
Transform(scenario.localPoint, xfB);
67 const auto delta = pointA - pointB;
68 return LengthIndexPair{
Dot(delta, normal),
IndexPair{indexA, indexB}};
71 Length EvaluateForPoints(
const SeparationScenario& scenario,
72 const Transformation& xfA,
const Transformation& xfB,
77 const auto delta = pointB - pointA;
78 return Dot(delta, scenario.axis);
81 Length EvaluateForFaceA(
const SeparationScenario& scenario,
82 const Transformation& xfA,
const Transformation& xfB,
85 const auto normal =
Rotate(scenario.axis, xfA.q);
86 const auto pointA =
Transform(scenario.localPoint, xfA);
88 const auto delta = pointB - pointA;
89 return Dot(delta, normal);
92 Length EvaluateForFaceB(
const SeparationScenario& scenario,
93 const Transformation& xfA,
const Transformation& xfB,
96 const auto normal =
Rotate(scenario.axis, xfB.q);
97 const auto pointB =
Transform(scenario.localPoint, xfB);
99 const auto delta = pointA - pointB;
100 return Dot(delta, normal);
110 assert(!
empty(indices));
115 const auto type = (numIndices == 1)?
123 const auto ip0 = indices[0];
124 const auto ip1 = indices[1];
131 const auto normal =
Rotate(axis, xfB.
q);
132 const auto localPoint = (localPointB1 + localPointB2) / 2;
133 const auto pointB =
Transform(localPoint, xfB);
135 const auto pointA =
Transform(localPointA, xfA);
136 const auto deltaPoint = pointA - pointB;
137 const auto axisIt = (
Dot(deltaPoint, normal) < 0_m)? -axis: axis;
142 const auto ip0 = indices[0];
143 const auto ip1 = indices[1];
150 const auto normal =
Rotate(axis, xfA.
q);
151 const auto localPoint = (localPointA1 + localPointA2) / 2;
152 const auto pointA =
Transform(localPoint, xfA);
154 const auto pointB =
Transform(localPointB, xfB);
155 const auto deltaPoint = pointB - pointA;
156 const auto axisIt = (
Dot(deltaPoint, normal) < 0_m)? -axis: axis;
164 const auto ip0 = indices[0];
167 const auto pointA =
Transform(localPointA, xfA);
168 const auto pointB =
Transform(localPointB, xfB);
176 switch (scenario.
type)
183 return FindMinSeparationForPoints(scenario, xfA, xfB);
190 switch (scenario.
type)
197 return EvaluateForPoints(scenario, xfA, xfB, indexPair);