From b6185e6b57099f09afd1e1a815de8a1c3ced40bb Mon Sep 17 00:00:00 2001 From: Karsten Schmidt Date: Tue, 26 May 2015 19:19:35 +0100 Subject: [PATCH] [core] add triangle-aabb & plane-aabb intersection tests --- geom-core/src/intersect.org | 128 ++++++++++++++++++++++++++++++++++-- 1 file changed, 123 insertions(+), 5 deletions(-) diff --git a/geom-core/src/intersect.org b/geom-core/src/intersect.org index 2f91f034..9a373dc4 100644 --- a/geom-core/src/intersect.org +++ b/geom-core/src/intersect.org @@ -3,21 +3,38 @@ * Contents :toc_3_gh: - [[#namespace-thinggeomcoreintersect][Namespace: thi.ng.geom.core.intersect]] - [[#circle][Circle]] + - [[#circle---circle][Circle - Circle]] - [[#rect][Rect]] - - [[#rect-circle][Rect-Circle]] + - [[#rect---rect][Rect - Rect]] + - [[#rect---circle][Rect - Circle]] - [[#aabb][AABB]] - - [[#aabb-sphere][AABB-Sphere]] + - [[#aabb---aabb][AABB - AABB]] + - [[#aabb---sphere][AABB - Sphere]] - [[#sphere][Sphere]] + - [[#sphere---sphere][Sphere - Sphere]] + - [[#sphere---ray][Sphere - Ray]] - [[#triangle3][Triangle3]] + - [[#triangle3---ray][Triangle3 - Ray]] + - [[#triangle3---aabb][Triangle3 - AABB]] - [[#line2][Line2]] + - [[#line2---line2][Line2 - Line2]] + - [[#line2---linestrip2][Line2 - Linestrip2]] - [[#plane][Plane]] + - [[#plane---plane][Plane - Plane]] + - [[#plane---ray][Plane - Ray]] + - [[#plane---aabb][Plane - AABB]] + - [[#plane---sphere][Plane - Sphere]] - [[#tetrahedron][Tetrahedron]] + - [[#helper-fns][Helper fns]] + - [[#tetrahedron---tetrahedron][Tetrahedron - Tetrahedron]] - [[#complete-namespace-definition][Complete namespace definition]] * Namespace: thi.ng.geom.core.intersect ** Circle +*** Circle - Circle + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-circle-circle? ([{p :p r1 :r} {q :p r2 :r}] @@ -36,6 +53,8 @@ ** Rect +*** Rect - Rect + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-rect-rect? ([{[px py] :p [w h] :size} {[qx qy] :p [qw qh] :size}] @@ -44,7 +63,7 @@ (not (or (> px1 qx2) (> px2 qx1) (> py1 qy2) (> py2 qy1))))) #+END_SRC -** Rect-Circle +*** Rect - Circle #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-rect-circle? @@ -58,6 +77,8 @@ ** AABB +*** AABB - AABB + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-aabb-aabb? ([{pa :p sa :size} {pb :p sb :size}] @@ -68,7 +89,7 @@ (and (<= (pa 2) (qb 2)) (<= (pb 2) (qa 2))))))) #+END_SRC -** AABB-Sphere +*** AABB - Sphere Source: Graphics Gems 2 / SO: @@ -89,6 +110,8 @@ Also works for rect/circle... ** Sphere +*** Sphere - Sphere + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-sphere-sphere? ([{p1 :p r1 :r} {p2 :p r2 :r}] @@ -97,6 +120,8 @@ Also works for rect/circle... (<= (g/dist-squared p1 p2) (mm/addm r1 r2 r1 r2)))) #+END_SRC +*** Sphere - Ray + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-ray-sphere? ([{rp :p dir :dir} {p :p r :r}] @@ -119,6 +144,8 @@ Also works for rect/circle... ** Triangle3 +*** Triangle3 - Ray + Source: http://geomalgorithms.com/a06-_intersect-2.html #+BEGIN_SRC clojure :noweb-ref isec @@ -146,8 +173,67 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html {:type :no-intersect :p i}))))))))) #+END_SRC +*** Triangle3 - AABB + +http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/tribox3.txt + +#+BEGIN_SRC clojure :noweb-ref isec + (defn- triaabb-axis-test + [pa1 pb1 pa2 pb2 a b fa fb sa sb] + (let [q (mm/madd a pa1 b pb1) + r (mm/madd a pa2 b pb2) + [min max] (if (< q r) [q r] [r q]) + rad (mm/madd fa sa fb sb)] + (if (<= min rad) (>= max (- rad))))) + + (defn- triaabb-edge-test-e0 + [[ax ay az :as a] [bx by bz :as b] [cx cy cz] [ex ey ez :as e] [sx sy sz]] + (let [[fx fy fz] (g/abs e)] + (if (triaabb-axis-test ay az cy cz ez (- ey) fz fy sy sz) + (if (triaabb-axis-test ax az cx cz (- ez) ex fz fx sx sz) + (triaabb-axis-test bx by cx cy ey (- ex) fy fx sx sy))))) + + (defn- triaabb-edge-test-e1 + [[ax ay az] [bx by bz :as b] [cx cy cz :as c] [ex ey ez :as e] [sx sy sz]] + (let [[fx fy fz] (g/abs e)] + (if (triaabb-axis-test ay az cy cz ez ey fz fy sy sz) + (if (triaabb-axis-test ax az cx cz (- ez) ex fz fx sx sz) + (triaabb-axis-test ax ay bx by ey (- ex) fy fx sx sy))))) + + (defn- triaabb-edge-test-e2 + [[ax ay az :as a] [bx by bz] [cx cy cz :as c] [ex ey ez :as e] [sx sy sz]] + (let [[fx fy fz] (g/abs e)] + (if (triaabb-axis-test ay az by bz ez (- ey) fz fy sy sz) + (if (triaabb-axis-test ax az bx bz (- ez) ex fz fx sx sz) + (triaabb-axis-test bx by cx cy ey (- ex) fy fx sx sy))))) + + (defn- triaabb-edge-minmax + [a b c s] + (if (< (min (min a b) c) s) + (>= (max (max a b) c) (- s)))) + + (defn intersect-triangle3-aabb? + [a b c p s] + (let [[sx sy sz :as s] (g/* s 0.5) + p (g/+ p s) + [ax ay az :as a] (g/- a p) + [bx by bz :as b] (g/- b p) + [cx cy cz :as c] (g/- c p) + e0 (g/- b a) + e1 (g/- c b)] + (if (triaabb-edge-test-e0 a b c e0 s) + (if (triaabb-edge-test-e1 a b c e1 s) + (if (triaabb-edge-test-e2 a b c (g/- a c) s) + (if (triaabb-edge-minmax ax bx cx sx) + (if (triaabb-edge-minmax ay by cy sy) + (if (triaabb-edge-minmax az bz cz sz) + (intersect-plane-aabb? a (g/cross e0 e1) s))))))))) +#+END_SRC + ** Line2 +*** Line2 - Line2 + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-line2-line2? [[px1 py1 :as p] [qx1 qy1 :as q] @@ -174,6 +260,8 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html {:type :intersect-outside :p i :ua ua :ub ub}))))) #+END_SRC +*** Line2 - Linestrip2 + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-line2-edges? [rp rq edges] @@ -193,11 +281,12 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html (defn intersect-ray2-edges? [rp rd edges] (intersect-line2-edges? rp (g/madd rd 1e29 rp) edges)) - #+END_SRC ** Plane +*** Plane - Plane + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-plane-plane? [an aw bn bw] @@ -209,6 +298,8 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html {:p (g/madd an u (g/* bn v)) :dir (gu/ortho-normal an bn)}))) #+END_SRC +*** Plane - Ray + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-ray-plane? [rp dir n w] @@ -223,6 +314,25 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html {:type :intersect-outside :p i}))))) #+END_SRC +*** Plane - AABB + +http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/tribox3.txt + +#+BEGIN_SRC clojure :noweb-ref isec + (defn intersect-plane-aabb? + ([p n q s] + (let [s2 (g/* s 0.5)] + (intersect-plane-aabb? (g/- p (g/+ q s2)) n s2))) + ([[px py pz] [nx ny nz :as n] [sx sy sz]] + (let [[vx1 vx2] (if (pos? nx) [(- (- sx) px) (- sx px)] [(- sx px) (- (- sx) px)]) + [vy1 vy2] (if (pos? ny) [(- (- sy) py) (- sy py)] [(- sy py) (- (- sy) py)]) + [vz1 vz2] (if (pos? nz) [(- (- sz) pz) (- sz pz)] [(- sz pz) (- (- sz) pz)])] + (if (<= (g/dot n (vec3 vx1 vy1 vz1)) 0.0) + (>= (g/dot n (vec3 vx2 vy2 vz2)) 0.0))))) +#+END_SRC + +*** Plane - Sphere + #+BEGIN_SRC clojure :noweb-ref isec (defn intersect-plane-sphere? [n w p r] (let [r (+ r *eps*)] (m/in-range? (- r) r (+ (g/dot n p) w)))) @@ -230,6 +340,8 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html ** Tetrahedron +*** Helper fns + #+BEGIN_SRC clojure :noweb-ref isec (defn- subdot "Computes sum((a-b)*c), where a, b, c are 3D vectors." @@ -318,7 +430,11 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html (face-b2? verts p (g/cross ea eb))) (recur edges (next s)))) true))) +#+END_SRC +*** Tetrahedron - Tetrahedron + +#+BEGIN_SRC clojure :noweb-ref isec (defn intersect-tetrahedra? "Takes 2 seqs of 4 3D points, each defining a tetrahedron. Returns true if they intersect. Orientation of points is irrelevant (unlike @@ -352,6 +468,8 @@ Source: http://geomalgorithms.com/a06-_intersect-2.html [thi.ng.math.core :as m :refer [*eps*]] #?(:clj [thi.ng.math.macros :as mm]))) + (declare intersect-plane-aabb?) + (defn- sq [x] (* x x)) <>