61 lines
2.8 KiB
C++
61 lines
2.8 KiB
C++
/// @file GJK.hpp
|
|
/// Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test
|
|
|
|
#pragma once
|
|
|
|
#include <J3ML/LinearAlgebra.hpp>
|
|
#include <J3ML/Geometry.hpp>
|
|
|
|
namespace J3ML::Algorithms
|
|
{
|
|
|
|
Vector3 UpdateSimplex(Vector3 *s, int &n);
|
|
|
|
#define SUPPORT(dir, minS, maxS) (a.ExtremePoint(dir, maxS) - b.ExtremePoint(-dir, minS));
|
|
|
|
template <typename A, typename B>
|
|
bool GJKIntersect(const A &a, const B &b)
|
|
{
|
|
Vector3 support[4];
|
|
// Start with an arbitrary point in the Minkowski set shape.
|
|
support[0] = a.AnyPointFast() - b.AnyPointFast();
|
|
if (support[0].LengthSquared() < 1e-7f) // Robustness check: Test if the first arbitrary point we guessed produced the zero vector we are looking for!
|
|
return true;
|
|
Vector3 d = -support[0]; // First search direction is straight toward the origin from the found point.
|
|
int n = 1; // Stores the current number of points in the search simplex.
|
|
int nIterations = 50; // Robustness check: Limit the maximum number of iterations to perform to avoid infinite loop if types A or B are buggy!
|
|
while (nIterations-- > 0)
|
|
{
|
|
// Compute the extreme point to the direction d in the Minkowski set shape.
|
|
float maxS, minS;
|
|
Vector3 newSupport = SUPPORT(d, minS, maxS);
|
|
// If the most extreme point in that search direction did not walk past the origin, then the origin cannot be contained in the Minkowski
|
|
// convex shape, and the two convex objects a and b do not share a common point - no intersection!
|
|
if (minS + maxS < 0.f)
|
|
return false;
|
|
|
|
// Add the newly evaluated point to the search simplex
|
|
assert(n < 4);
|
|
support[n++] = newSupport;
|
|
// Examine the current simplex, prune a redundant part of it, and produce the next search direction.
|
|
d = UpdateSimplex(support, n);
|
|
|
|
if (n == 0) // Was the origin contained in the current simplex? If so, then the convex shapes a and b do share a common point - intersection!
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// This computes GJL intersection, but by first translating both objects to a coordinate frame that is as closely
|
|
// centered around world origin as possible, to gain floating point precision.
|
|
template <typename A, typename B>
|
|
bool FloatingPointOffsetedGJKIntersect(const A &a, const B &b)
|
|
{
|
|
AABB ab = a.MinimalEnclosingAABB();
|
|
AABB bb = b.MinimalEnclosingAABB();
|
|
|
|
Vector3 offset = (Vector3::Min(ab.minPoint, bb.minPoint) + Vector3::Max(ab.maxPoint, bb.maxPoint)) * 0.5f;
|
|
const Vector3 floatingPtPrecisionOffset = -offset;
|
|
return GJKIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
|
|
}
|
|
} |