31 return pairs[0] == key || pairs[1] == key || pairs[2] == key;
34 inline SimplexEdge GetSimplexEdge(
const DistanceProxy& proxyA,
35 const Transformation& xfA,
37 const DistanceProxy& proxyB,
38 const Transformation& xfB,
41 const auto wA =
Transform(proxyA.GetVertex(idxA), xfA);
42 const auto wB =
Transform(proxyB.GetVertex(idxB), xfB);
43 return SimplexEdge{wA, idxA, wB, idxB};
47 const DistanceProxy& proxyA,
const Transformation& xfA,
48 const DistanceProxy& proxyB,
const Transformation& xfB)
58 simplexEdges[2] = GetSimplexEdge(proxyA, xfA,
std::get<0>(indexPairs[2]),
62 simplexEdges[1] = GetSimplexEdge(proxyA, xfA,
std::get<0>(indexPairs[1]),
66 simplexEdges[0] = GetSimplexEdge(proxyA, xfA,
std::get<0>(indexPairs[0]),
69 simplexEdges.size(
static_cast<size_type
>(count));
87 const auto numEdges =
size(simplex);
88 for (
auto i = decltype(numEdges){0}; i < numEdges; ++i)
90 const auto e = simplex.GetSimplexEdge(i);
91 const auto c = simplex.GetCoefficient(i);
93 pointA += e.GetPointA() * c;
94 pointB += e.GetPointB() * c;
99 if (
size == 3 && pointA != pointB)
101 std::cout <<
"odd: " << pointA <<
" != " << pointB;
102 std::cout << std::endl;
122 auto simplexEdges = GetSimplexEdges(savedIndices, proxyA, transformA, proxyB, transformB);
126 if (
size(simplexEdges) > 1)
130 if ((metric2 < (metric1 / 2)) || (metric2 > (metric1 * 2)) || (metric2 < 0) ||
AlmostZero(metric2))
132 simplexEdges.clear();
136 if (
empty(simplexEdges))
138 simplexEdges.push_back(GetSimplexEdge(proxyA, transformA, 0, proxyB, transformB, 0));
145 #if defined(DO_COMPUTE_CLOSEST_POINT)
156 simplexEdges = simplex.GetEdges();
165 #if defined(DO_COMPUTE_CLOSEST_POINT)
171 if (distanceSqr2 >= distanceSqr1)
175 distanceSqr1 = distanceSqr2;
201 if (HasKey(savedIndices,
IndexPair{indexA, indexB}))
208 simplexEdges.push_back(GetSimplexEdge(proxyA, transformA, indexA, proxyB, transformB, indexB));
221 const auto distanceInfo =
Distance(proxyA, xfA, proxyB, xfB, conf);
227 return totalRadiusSquared - distanceSquared;