saturday-night.clj

(ns user
  (:use arcadia.core)
  (:import [UnityEngine
            Vector3
            Time
            Gizmos
            Color
            Debug
            Plane
            Mathf]))

(defmacro v [x y z]
  `(Vector3. ~x ~y ~z))

(defmacro make-face [& vs ]
  )

(def mesh {:vertices [] 
           :faces [{:normal Vector3/up
                    :vertices [(v 0 1 0)
                               (v 1 1 0)
                               (v 1 1 1)
                               (v 0 1 1)]}
                   
                   {:normal Vector3/down
                    :vertices [(v 0 0 0)
                               (v 1 0 0)
                               (v 1 0 1)
                               (v 0 0 1)]}
                               
                   {:normal Vector3/right
                    :vertices [(v 1 0 0)
                               (v 1 1 0)
                               (v 1 1 1)
                               (v 1 0 1)]}
                               
                   {:normal Vector3/left
                    :vertices [(v 0 0 0)
                               (v 0 1 0)
                               (v 0 1 1)
                               (v 0 0 1)]}
                               
                   {:normal Vector3/forward
                    :vertices [(v 0 0 1)
                               (v 0 1 1)
                               (v 1 1 1)
                               (v 1 0 1)]}
                               
                   {:normal Vector3/back
                    :vertices [(v 0 0 0)
                               (v 0 1 0)
                               (v 1 1 0)
                               (v 1 0 0)]}
                   ]})

(defn v+ [^Vector3 a ^Vector3 b] (Vector3/op_Addition a b))
(defn v- [^Vector3 a ^Vector3 b] (Vector3/op_Subtraction a b))
(defn v* [a b] (Vector3/op_Multiply a b))
(defn v÷ [a b] (Vector3/op_Division a b))

(defn centroid [vs]
  (v÷ (reduce v+ vs)
      (count vs)))

(defn ortho [^Vector3 v]
  (Vector3. 2 2 (- (.x v) (.y v))))

(defn parallel? [^Vector3 a ^Vector3 b]
  (let [angle (Vector3/Angle a b)]
    (or (Mathf/Approximately 0.0 angle)
        (Mathf/Approximately 180.0 angle))))

(defn gizmo-color [^Color c]
  (set! Gizmos/color c))

(defn gizmo-line [^Vector3 from ^Vector3 to]
  (Gizmos/DrawLine from to))

(defn gizmo-ray [^Vector3 from ^Vector3 dir]
  (Gizmos/DrawRay from dir))

(defn gizmo-point [^Vector3 v]
  (Gizmos/DrawSphere v 0.075))

(defn double-cross [^Vector3 n ^Vector3 v]
  (.normalized (Vector3/Cross n (Vector3/Cross n v))))

(defn face->plane [f]
  {:normal (f :normal)
   :distance (Vector3/Dot (f :normal)
                          (centroid (f :vertices)))})

(defn gizmo-face-plane [f]
  (let [c (centroid (f :vertices))
        n (f :normal)
        pv1 (.normalized (apply v- (take 2 (f :vertices))))
        pv2 (.normalized (Vector3/Cross n pv1))]
        
    (gizmo-ray c n)
    (doseq [x (range -10 10)
            y (range -10 10)]
      (let [c (reduce v+ [c
                          (v* pv1 x)
                          (v* pv2 y)])]
        (gizmo-ray c pv1)
        (gizmo-ray c pv2)
        (gizmo-ray c (v* -1 pv1))
        (gizmo-ray c (v* -1 pv2))))))

(defn gizmo-plane [p]
  (let [n (.normalized (p :normal))
        c (v* (p :distance) n)
        crossh (.normalized 
                (Vector3/Cross n (Vector3/Cross Vector3/right n)))
        crossv (.normalized 
                (Vector3/Cross n (Vector3/Cross Vector3/up n)))]
    (gizmo-ray c n)
    (doseq [x (range -10 10)
            y (range -10 10)]
      (let [c (reduce v+ [c
                          (v* crossh x)
                          (v* crossv y)])]
        (gizmo-ray c crossh)
        (gizmo-ray c crossv)
        (gizmo-ray c (v* -1 crossh))
        (gizmo-ray c (v* -1 crossv))  ))))

(defn gizmo-face [f]
  (let [verts (f :vertices)
        closed-verts (conj verts (first verts))
        edges (partition 2 1 closed-verts)]
        
    ;; draw sides
    (gizmo-color Color/white)
    (doseq [e edges]
      (apply gizmo-line e))
    
    ;; draw normal
    (gizmo-color Color/yellow)
    (let [c (centroid (f :vertices))]
      (gizmo-line c
                  (v+ c
                      (v* 0.2 (f :normal)))))))

(defn gizmo-mesh [m]
  (doseq [f (m :faces)]
    (gizmo-face f)))

(defn plane* [a b c]
  (Plane. a b c))

(defn intersect-2-planes [{d1 :distance
                           n1 :normal
                           :keys [distance normal]}
                          {d2 :distance
                           n2 :normal
                           :keys [distance normal]}]
  (let [pos1 (v* d1 n1)
        pos2 (v* d2 n2)
        line-vec (Vector3/Cross n1 n2)
        ldir (Vector3/Cross n2 line-vec)
        den (Vector3/Dot n1 ldir)
        p1-to-p2 (v- pos1 pos2)
        t (/ (Vector3/Dot n1 n2) den)
        line-point (v+ pos2 (v* t ldir))]
    {:point line-point
     :direction line-vec}))

(defn intersect-line-plane [{:keys [point direction]}
                            {:keys [distance normal]}]
  (let [plane-point (v* distance normal)
        dot-num (Vector3/Dot (v- plane-point point) normal)
        dot-den (Vector3/Dot direction normal)
        len (/ dot-num dot-den)
        v (v* (.normalized direction) len)]
    (v+ point v)))

(defn intersect-3-planes [p1 p2 p3]
  (let [l (intersect-2-planes p1 p2)]
    (intersect-line-plane l p3)))

(defn intersect [p1 p2 p3]
  (let [d1 (:distance p1)
        n1 (:normal p1)
        d2 (:distance p2)
        n2 (:normal p2)
        d3 (:distance p3)
        n3 (:normal p3)]
    (v÷ (reduce v+ [(v* (- d1) (Vector3/Cross n2 n3))
                    (v* (- d2) (Vector3/Cross n3 n1))
                    (v* (- d3) (Vector3/Cross n1 n2))])
        (Vector3/Dot n1
                     (Vector3/Cross n2 n3)))))

(do
  (defn gizmos []
    (gizmo-mesh mesh)
    
    (let [p {:normal (Vector3. 1 1 1)
             :distance -2}
          faces (take 2 (drop 1 (mesh :faces)))]
          
      (gizmo-color Color/yellow)
      (gizmo-plane p)
      
      (gizmo-color Color/red)
      (doseq [f faces]
        (gizmo-face-plane f))
                  
      (intersect-2-planes
        (face->plane (first faces))
        (face->plane (second faces)))
      
      (gizmo-color Color/cyan)
      (gizmo-point (apply intersect-3-planes p (map face->plane faces)))
      )
    )
  (UnityEditor.SceneView/RepaintAll))