A divide-and-conquer implementation of the GJK algorithm

The Gilbert-Johnson-Keerthi (GJK) algorithm is a method for collision detection of convex polyhedra. The method is used in multiple rigid body simulations for computer games.


The algorithm returns:

An n-dimensional simplex is the convex hull of n+1 points as shown in the figure below.

n-dimensional simplices for different values of n

The algorithm makes use of the fact, that the distance between two sets A and B is the distance of the Minkowski difference to the origin:


The GJK algorithm iteratively updates a simplex until the closest point to the origin is found.

The algorithm iterates using support points. Given a set M and a vector d, the support point is defined as the furthest point of M in direction d:

The GJK algorithm detects the two closest points of A and B as follows:

  1. Choose any point m in M(A,B) in the Minkowski difference of A and B.
  2. Set the initial simplex w0 to w0={m}.
  3. Let d be the closest point of wk to the origin.
  4. If d s(wk, -d)>=d s(M,-d) then return d.
  5. Set w'k+1=wk∪{s(M,-d)}
  6. Set wk+1 to the smallest simplex in w'k+1 still containing s(M,-d).
  7. Continue with step 3.

Note that step 3 requires finding the closest point of the simplex wk to the origin. Most implementations of the GJK algorithm seem to use the following approach:

A much more compact implementation can be obtained using a divide-and-conquer approach:

Note that this approach will visit each edge up to two times, and each point up to three times. The performance is not optimal, but it makes for a much more concise implementation.

The least squares solution is:

Here follows an implementation in Scheme (GNU Guile). By using pairs of points from A and B instead of the Minkowski difference, one can keep track of the information required to determine the pair of closest points of A and B (instead of the closest point of M to the origin).

(use-modules (oop goops) (srfi srfi-1) (srfi srfi-26))

(define-method (- (a <list>))
  "Negate vector."
  (map - a))

(define-method (+ (a <list>) (b <list>))
  "Add two vectors."
  (map + a b))

(define-method (+ (a <list>) (b <real>))
  "Add vector and scalar."
  (map (cut + <> b) a))

(define-method (+ (a <real>) (b <list>))
  "Add vector and scalar."
  (map (cut + a <>) b))

(define-method (- (a <list>) (b <list>))
  "Subtract a vector from another."
  (map - a b))

(define-method (- (a <list>) (b <real>))
  "Subtract a vector from another."
  (map (cut - <> b) a))

(define-method (- (a <real>) (b <list>))
  "Subtract a vector from another."
  (map (cut - a <>) b))

(define-method (* (a <list>) (b <number>))
  "Multiply a vector with a scalar."
  (map (cut * <> b) a))

(define-method (* (a <number>) (b <list>))
  "Multiply a scalar with a vector."
  (map (cut * <> a) b))

(define (argop op fun lst)
  (let* [(vals  (map fun lst))
         (opval (apply op vals))]
    (list-ref (reverse lst) (1- (length (member opval vals))))))

(define (argmin fun lst) (argop min fun lst))

(define (argmax fun lst) (argop max fun lst))

(define (leave-one-out lst)
  (map (lambda (i) (append (take lst i) (drop lst (1+ i)))) (iota (length lst))))

(define (inner-product a b)
  "Compute inner product of two vectors."
  (reduce + 0 (map * a b)))

(define (norm v)
  "Return norm of a vector."
  (sqrt (inner-product v v)))

(define (transpose mat)
  "Transpose a matrix"
  (if (null? mat)
    (map (lambda (i) (map (cut list-ref <> i) mat)) (iota (length (car mat))))))

(define (dot mat vec)
  "Multiply a matrix with another matrix or a vector"
  (map (cut inner-product <> vec) mat))

(define (permutations lst)
  "Return all permutations of list LST. The permutations are ordered so that every alternate permutation is even."
  (if (zero? (length lst))
        (lambda (item index)
          (let [(remaining (delete item lst))
                (order     (if (even? index) identity reverse))]
            (map (cut cons item <>) (permutations (order remaining)))))
        (iota (length lst))))))

(define (determinant mat)
  "Compute determinant of a matrix"
  (let* [(n       (length mat))
         (indices (iota n))
         (perms   (permutations indices))]
    (reduce + 0
        (lambda (perm k)
          (* (reduce * 1 (map (lambda (j i) (list-ref (list-ref mat j) i))
                              indices perm))
             (if (even? k) 1 -1)))
         (iota (length perms))))))

(define (submatrix mat row column)
  "Return submatrix with specified ROW and COLUMN removed."
  (let [(rows    (delete row    (iota (length mat))))
        (columns (delete column (iota (length (car mat)))))]
    (map (lambda (j) (map (lambda (i) (list-ref (list-ref mat j) i)) columns)) rows)))

(define (inverse mat)
  "Compute inverse of matrix"
  (let [(det     (determinant mat))
        (indices (iota (length mat)))
        (sgn     (lambda (v j i) (if (eq? (even? j) (even? i)) v (- v))))]
    (map (lambda (j)
           (map (lambda (i) (sgn (/ (determinant (submatrix mat i j)) det) j i))

(define (least-squares design-matrix observation)
  "Least-squares solver"
  (if (null? design-matrix)
    (let [(design-matrix-transposed (transpose design-matrix))]
      (dot (inverse (dot design-matrix-transposed design-matrix))
           (dot design-matrix-transposed observation)))))

(define (support-point direction points)
  "Get outermost point of POINTS in given DIRECTION."
  (argmax (cut inner-product direction <>) points))

(define (center-of-gravity points)
  "Compute average of given points"
  (* (reduce + #f points) (/ 1 (length points))))

(define (closest-simplex-points simplex-a simplex-b)
  "Determine closest point pair of two simplices"
  (let* [(observation   (- (car simplex-a) (car simplex-b)))
         (design-matrix (- observation (transpose (- (cdr simplex-a)
                                                     (cdr simplex-b)))))
         (factors       (least-squares design-matrix observation))]
      (if (and (every positive? factors) (< (reduce + 0 factors) 1))
        (cons (cons (fold + (car simplex-a)
                          (map * factors
                               (map (cut - <> (car simplex-a)) (cdr simplex-a))))
                    (fold + (car simplex-b)
                          (map * factors
                               (map (cut - <> (car simplex-b)) (cdr simplex-b)))))
              (cons simplex-a simplex-b))
        (argmin (lambda (result) (norm (- (caar result) (cdar result))))
                (map closest-simplex-points
                     (leave-one-out simplex-a)
                     (leave-one-out simplex-b))))))

(define (gjk-algorithm convex-a convex-b)
  "Get pair of closest points of two convex hulls each defined by a set of points"
  (let [(simplex-a '())
        (simplex-b '())
        (closest (cons (center-of-gravity convex-a) (center-of-gravity convex-b)))]
    (while #t
      (let* [(direction  (- (car closest) (cdr closest)))
             (candidates (cons (support-point (- direction) convex-a)
                               (support-point direction convex-b)))]
        (if (>= (+ (inner-product direction (- direction)) 1e-12)
                (inner-product (- (car candidates) (cdr candidates)) (- direction)))
          (break closest))
        (let [(result (closest-simplex-points (cons (car candidates) simplex-a)
                                              (cons (cdr candidates) simplex-b)))]
          (set! closest (car result))
          (set! simplex-a (cadr result))
          (set! simplex-b (cddr result)))))))

Here an example of two colliding cuboids simulated using this implementation is shown:

Any feedback, comments, and suggestions are welcome.



I noticed that Baraff just uses a separating plane algorithm to achieve the same!