A dimension-independent implementation of the GJK algorithm in Clojure with core.matrix

gjk.clj

(require '[clojure.core.matrix :as mat])

(defprotocol Support
  :extend-via-metadata true
  (support [this direction]
    "Calculates a support point for the object in the given direction.")
  (dimensions [this]
    "Returns the number of dimensions this support operates in."))

(defn minkowski-support
  [direction obj-1 obj-2]
  (mat/sub (support obj-1 direction)
           (support obj-2 (mat/negate direction))))

(defn complete-simplex?
  [simplex]
  (> (count simplex) (count (first simplex))))

(defn project
  [v1 v2]
  (let [v2-norm (mat/normalise v2)]
    (mat/scale v2-norm (mat/dot v1 v2-norm))))

(defn reject
  [v1 v2]
  (mat/sub v1 (project v1 v2)))

(defn direction-towards-point
  [simplex point]
  (let [[a & more] simplex]
    (transduce (map #(mat/sub % a))
               (completing reject)
               (mat/sub point a)
               more)))

(defn dissoc-nth
  [v i]
  (into (subvec v 0 i) (subvec v (inc i))))

(defn next-simplex
  [simplex support]
  (letfn [(f [simplex idx]
            (if (< idx (count simplex))
              (let [alt-simplex (dissoc-nth simplex idx)]
                (if (neg? (mat/dot (direction-towards-point
                                    (conj alt-simplex support)
                                    (nth simplex idx))
                                   support))
                  ;; Origin is inside this side, try the other sides
                  (recur simplex (inc idx))
                  ;; Origin is outside this side, try a smaller simplex
                  (recur alt-simplex 0)))
              ;; Origin is inside this simplex
              (conj simplex support)))]
    (f simplex 0)))

(defn overlapping-simplex
  [obj-1 obj-2]
  {:pre [(= (dimensions obj-1) (dimensions obj-2))]}
  ;; This function implements the GJK algorithm, returning the internal state so
  ;; that it can be used by a manifold generation algorithm, such as EPA
  (let [initial-support (minkowski-support (mat/array (vec (cons 1 (repeat (dec (dimensions obj-1)) 0))))
                                           obj-1 obj-2)
        zero (mat/zero-vector (dimensions obj-1))]
    (loop [simplex [initial-support]
           direction (mat/normalise (mat/negate initial-support))]
      (let [support (minkowski-support direction obj-1 obj-2)]
        (when (pos? (mat/dot support direction))
          (let [simplex (next-simplex simplex support)]
            (if-not (complete-simplex? simplex)
              (let [direction (direction-towards-point simplex zero)]
                (if-not (mat/zero-matrix? direction)
                  ;; Try to find the origin in the next simplex
                  (recur simplex (mat/normalise direction))
                  ;; Our incomplete simplex contains the origin
                  simplex))
              simplex)))))))

(def overlapping? (comp some? #'overlapping-simplex))

support.clj

(require '[gjk :refer [Support]])
(require '[clojure.core.matrix :as mat])

(defn spheroid
  [position radius]
  (reify Support
    (support [_ direction]
      (mat/add position (mat/scale direction radius)))
    (dimensions [_]
      (mat/dimension-count position 0))))

(defn convex-hull
  [position pointcloud]
  (reify Support
    (support [_ direction]
      (mat/add position
               (first
                (transduce
                 (map (fn [point]
                        [point (mat/dot point direction)]))
                 (completing #(max (second %1) (second %2)))
                 [(first pointcloud) (mat/dot (first pointcloud) direction)]
                 (rest pointcloud)))))
    (dimensions [_]
      (mat/dimension-count position 0))))