Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

How do I check if a simplex contains the origin?

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]
like image 722
sdasdadas Avatar asked Feb 17 '14 01:02

sdasdadas


2 Answers

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.

like image 60
ellisbben Avatar answered Nov 16 '22 17:11

ellisbben


(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:

simplex and point

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 this is d! times of oriented hypervolume of simplex constructed from points involved

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):

The Condition

but we can note, that

source for simplification

So we can calculate only one determinant from left hand side of comparison (?):

A^{j,j}, j = 1

and assume, that sign flips for next js.

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.

like image 6
Tomilov Anatoliy Avatar answered Nov 16 '22 15:11

Tomilov Anatoliy