compute method
Compute the closest points between two shapes. Supports any combination of: CircleShape and PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to zero.
Implementation
void compute(
DistanceOutput output,
SimplexCache cache,
DistanceInput input,
) {
gjkCalls++;
final proxyA = input.proxyA;
final proxyB = input.proxyB;
final transformA = input.transformA;
final transformB = input.transformB;
// Initialize the simplex.
_simplex.readCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
final vertices = _simplex.vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
var saveCount = 0;
_simplex.getClosestPoint(_closestPoint);
var distanceSqr1 = _closestPoint.length2;
var distanceSqr2 = distanceSqr1;
// Main iteration loop
var iter = 0;
while (iter < maxIterations) {
// Copy simplex so we can identify duplicates.
saveCount = _simplex.count;
for (var i = 0; i < saveCount; i++) {
_saveA[i] = vertices[i].indexA;
_saveB[i] = vertices[i].indexB;
}
switch (_simplex.count) {
case 1:
break;
case 2:
_simplex.solve2();
break;
case 3:
_simplex.solve3();
break;
default:
assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (_simplex.count == 3) {
break;
}
// Compute closest point.
_simplex.getClosestPoint(_closestPoint);
distanceSqr2 = _closestPoint.length2;
// ensure progress
if (distanceSqr2 >= distanceSqr1) {
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
_simplex.getSearchDirection(_d);
// Ensure the search direction is numerically fit.
if (_d.length2 < settings.epsilon * settings.epsilon) {
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to
// it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
final vertex = vertices[_simplex.count];
_temp.setFrom(Rot.mulTransVec2(transformA.q, _d..negate()));
vertex.indexA = proxyA.getSupport(_temp);
vertex.wA.setFrom(
Transform.mulVec2(transformA, proxyA.getVertex(vertex.indexA)),
);
// Vec2 wBLocal;
_temp.setFrom(Rot.mulTransVec2(transformB.q, _d..negate()));
vertex.indexB = proxyB.getSupport(_temp);
vertex.wB.setFrom(
Transform.mulVec2(transformB, proxyB.getVertex(vertex.indexB)),
);
(vertex.w..setFrom(vertex.wB)).sub(vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++gjkIterations;
// Check for duplicate support points. This is the main termination
// criteria.
var duplicate = false;
for (var i = 0; i < saveCount; ++i) {
if (vertex.indexA == _saveA[i] && vertex.indexB == _saveB[i]) {
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate) {
break;
}
// New vertex is ok and needed.
++_simplex.count;
}
gjkMaxIterations = max(gjkMaxIterations, iter);
// Prepare output.
_simplex.getWitnessPoints(output.pointA, output.pointB);
output.distance = output.pointA.distanceTo(output.pointB);
output.iterations = iter;
// Cache the simplex.
_simplex.writeCache(cache);
// Apply radii if requested.
if (input.useRadii) {
final rA = proxyA.radius;
final rB = proxyB.radius;
if (output.distance > rA + rB && output.distance > settings.epsilon) {
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
_normal
..setFrom(output.pointB)
..sub(output.pointA);
_normal.normalize();
_temp
..setFrom(_normal)
..scale(rA);
output.pointA.add(_temp);
_temp
..setFrom(_normal)
..scale(rB);
output.pointB.sub(_temp);
} else {
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA
..add(output.pointB)
..scale(.5);
output.pointB.setFrom(output.pointA);
output.distance = 0.0;
}
}
}