I am implementing the Gilbert-Johnson-Keerthi algorithm which computes whether two objects are intersecting (ie. colliding).
The entry point to my code is the hasCollided
function which takes two lists of points and returns True
if they are intersecting. I believe I have implemented the paper correctly - however, I still have to implement the contains
function.
The contains
function should determine whether a simplex contains the origin. I am unsure as to how to implement this.
How do I efficiently determine if a simplex (collection of points) contains the origin?
The following is my implementation:
type Simplex = Set (Vector Double)
hasCollided :: [Vector Double] -> [Vector Double] -> Bool
hasCollided points1 points2 = gjk points1 points2 simplex (scale (-1) direction) p
where simplex = insert p empty
p = support points1 points2 direction
direction = fromList [1, 0, 0]
gjk :: [Vector Double] -> [Vector Double] -> Simplex -> Vector Double -> Vector Double -> Bool
gjk points1 points2 simplex direction lastAdded =
if p <.> direction < 0 then False
else
if contains simplex' (fromList [0, 0, 0]) direction p then True
else gjk points1 points2 simplex' direction' p
where p = support points1 points2 direction
simplex' = insert p simplex
direction' = cross ab $ cross ao ab
ab = sub p lastAdded
ao = sub origin3D lastAdded
The helper functions are:
contains :: Simplex -> Vector Double -> Vector Double -> Vector Double -> Bool
contains simplex point direction lastAdded = undefined
support :: [Vector Double] -> [Vector Double] -> Vector Double -> Vector Double
support points1 points2 direction = sub p1 p2
where p1 = getFarthestPoint points1 direction
p2 = getFarthestPoint points2 direction
getFarthestPoint :: [Vector Double] -> Vector Double -> Vector Double
getFarthestPoint points direction = points !! index
where index = fromJust $ elemIndex (maximum dotproducts) dotproducts
dotproducts = map (direction <.>) points
origin3D :: Vector Double
origin3D = fromList [0, 0, 0]
I'll take the non-clever, "let's do some linear algebra to solve it" approach.
Every point within a simplex is a convex combination of the points which define the simplex. A convex combination is just a linear combination where the coefficients are all >= 0 and add up to 1.
"Does a simplex contain the origin" is identical to asking if there is a convex combination of the simplex points that is equal to the zero vector. Can we write this as a matrix expression?
Let's say we're working with a simplex defined by four vectors, x1
through x4
.
We're going to form an arbitrary linear combination of those vectors, y = a1*x1 + a2*x2 + a3*x3 + a4*x4
.
We want to find a1
through a4
such that y
is the zero vector and a1 + a2 + a3 + a4 = 1
.
I'll show what the linear system would look like if the simplex is of points in a three-dimensional Euclidean space; let the vector xi
have components xi1
, xi2
, and xi3
.
[ x11 x21 x31 x41 ] [ a1 ] [ 0 ]
[ x12 x22 x32 x42 ] [ a2 ] = [ 0 ]
[ x13 x23 x33 x43 ] [ a3 ] [ 0 ]
[ 1 1 1 1 ] [ a4 ] [ 1 ]
The first three rows of this linear system correspond to the constraint that y
must be zero, i.e., that we can get to the origin through some linear combination of x1
through x4
. The last row corresponds to the constraint that the coefficients add up to 1 which is necessary but not sufficient for the linear combination to be a convex combination. The constraint not expressed by the matrix equation is that ai >= 0
.
Pick your favorite method for solving a linear system and apply it. If the vectors making up your simplex are linearly independent, you won't find any solutions. If the linear system has a solution or solutions and at least one solution has all the ai >= 0
, then the simplex contains the origin.
I don't have any ready description for an algorithm for that last step, determining if the solution set includes any points where all the coefficients are positive. I suggest working a few examples out on paper- I expect you can find one.
EDIT: determining if the solution set includes any points where all coefficients are positive is actually the same as determining whether the simplex defined by the intersection of the solution space with ai >= 0
includes any points other than the origin.
That means this method of solution reduces the problem of
"Does the input simplex contain the origin?"
to
"Does another simplex (derived from the input simplex) contain any points other than the origin?"
I think it's a cute example of reducing a problem to another (hopefully easier) problem.
(change theme to light one if formulae are not recognizible)
To strictly determine whether the point lies into the simplex or no you only need to know a signs of at maximum d + 2
determinants of d * d
size.
Let:
then we can construct a matrices (j,k
index means: exclude j
row and subtract vector from origin to point k
from each of d
remaining rows; all the left hand sides in rows defines a facet lying against j
vertex):
Determinant of the above matrix is d!
times d
-dimensional oriented hypervolume of a simplex, constructed from points involved in the formula (strictly saying is the oriented hypervolume of a parallelotope, whose edges are given by the matrix rows).
If point is inside the simplex, then all the below equations is true (matching the orientation (sign of oriented hypervolume) of j
and 0
pair of points relative to a facet):
but we can note, that
So we can calculate only one determinant from left hand side of comparison (?
):
and assume, that sign flips for next j
s.
Therefore, we should to compute at least 2
determinants of d*d
matrices, and maximum d + 2
(of A1,1 and of Aj,0 for all j
in {1, 2, ..., d + 1}). If sign is not matched on some step, then the point is outside of a current facet of the simplex, and, thereby, out of the simplex at all.
ADDITIONAL:
If some of the right hand side determinants are zero, then the point is coplanar to planes of corresponding facets.
If you love us? You can donate to us via Paypal or buy me a coffee so we can maintain and grow! Thank you!
Donate Us With