Skip to content

Instantly share code, notes, and snippets.

Last active August 23, 2021 21:05
Show Gist options
  • Save IGJoshua/2cd1cb648bd68cc9f91123175c77bf7c to your computer and use it in GitHub Desktop.
Save IGJoshua/2cd1cb648bd68cc9f91123175c77bf7c to your computer and use it in GitHub Desktop.
A dimension-independent implementation of the GJK algorithm in Clojure with core.matrix
(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?
(> (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)
(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))
;; 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
(def overlapping? (comp some? #'overlapping-simplex))
(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
(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))))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment