;;; -*- Mode: LISP; Package: (USER); Syntax:Common-Lisp; Lowercase: Yes -*-

#|
 3D Geometry classes  Version 0.5
 Daniel LaLiberte
 liberte@ncsa.uiuc.edu

 Defines classes and methods for Line, Plane, Implicit-Plane, Explicit-Plane,
 and Sphere.  Also defines functions for common vector operations.
 
 Most of the geometry calculations come from Graphic Gems, edited by 
 Andrew S. Glassner, but the errors are probably mine.  

 Please send your enhancements.

 To do:
 Many more basic functions, as needed.

 Other objects?
 Avoid creation of intermediate vectors to reduce GCing.

 History:  July '94, Kris R. Thorisson
                 line/plane intersection and more.
                 
|#

;(export '(make-line make-explicit-plane line-plane-intersection))

(in-package 'user)

;;==========================================================================

(defclass Line ()
  (
   (direction
    :documentation "In-line vector."
    :initform (vector 0.0 0.0 0.0)
    :initarg :direction
    :accessor direction)
   (offset
    :documentation "Offset from origin."
    :initform (vector 0.0 0.0 0.0)
    :initarg :offset
    :accessor offset)
   )
  (:documentation
  "Points P on line satisfy P == direction * t + offset, for some scalar t.")
  )

(defmethod make-line ((offset vector)(direction vector))
  (make-instance 'Line 
    :offset offset :direction direction))

(defmethod make-line ((off+dir vector) foo)
  ())

;;==========================================================================

(defclass Plane ()
  ()
  (:documentation
  "Abstract plane may be represented in implicit or 
explicit (parameterized) form.")
  )

(defclass Implicit-Plane (Plane)
  (
   (normal
    :documentation "Normal vector."
    :initform (vector 0.0 0.0 0.0)
    :initarg :normal
    :accessor normal)
   (distance
    :documentation "Distance from origin."
    :initform 0.0
    :initarg :distance
    :accessor distance)
   )
  (:documentation
  "Points P on plane satisfy normal . P + distance == 0.")
  )

(defun make-implicit-plane (normal distance)
  "Return an implicit plane with NORMAL and DISTANCE."
  (make-instance 'Implicit-Plane :normal normal :distance distance))


(defmethod as-implicit ((plane Implicit-Plane))
  plane)


(defclass Explicit-Plane (Plane)
  (
   (direction1
    :documentation "In-plane vector."
    :initform (vector 0.0 0.0 0.0)
    :initarg :direction1
    :accessor direction1)
   (direction2
    :documentation "In-plane vector."
    :initform (vector 0.0 0.0 0.0)
    :initarg :direction2
    :accessor direction2)
   (offset
    :documentation "Offset from origin."
    :initform (vector 0.0 0.0 0.0)
    :initarg :offset
    :accessor offset)
   )
  (:documentation
  "Points P on plane satisfy P == direction1 s + direction2 t + offset, for some scalar t."
  ))

(defun make-explicit-plane (offset dir1 dir2)
  "Return an explicit plane."
  (make-instance 'Explicit-Plane :direction1 dir1 :direction2 dir2 :offset offset))

;;==========================================================================

(defclass Sphere ()
  ((center :initform (vector 0.0 0.0 0.0) :initarg :center :accessor center)
   (radius :initform 0.0 :initarg :radius :accessor radius))
  )

(defun make-sphere (center radius)
  (make-instance 'Sphere :center center :radius radius))


;;==========================================================================
;; Vector operations
   
(defun dot-product (v1 v2)
  "Return the dot product of v1 and v2."
;;  (reduce '+ (map 'vector '* v1 v2))  ; same as:
  (+ (* (aref v1 0) (aref v2 0))
     (* (aref v1 1) (aref v2 1))
     (* (aref v1 2) (aref v2 2))))

(defun cross-product (v1 v2)
  "Return the cross product of v1 and v2"
  (vector (- (* (aref v1 1) (aref v2 2))
             (* (aref v1 2) (aref v2 1)))
          (- (* (aref v1 2) (aref v2 0))
             (* (aref v1 0) (aref v2 2)))
          (- (* (aref v1 0) (aref v2 1))
             (* (aref v1 1) (aref v2 0)))
          ))

(defun vector-add (v1 v2)
  (map 'vector '+ v1 v2))

(defun vector-sub (v1 v2)
  (map 'vector '- v1 v2))

(defun vector-reflect (v)
  (map 'vector '- v))

(defvar aa (vector 0 0 0))
(setf   aa (vector 0 0 0))

(defun vector-scale (v s)
  "Scale the vector V in place by S."
  (map-into aa #'(lambda (x) (* x s)) v))
; same as the following but which is faster?
;  (dotimes (i (array-dimension v 0))
;    (setf (aref v i) (* s (aref v i))))
;  v)

(defun vector-normalize (p)
  "p = p / length(p)"
  (let ((d (vector-length p)))
    (unless (= 0 d)
      (progn
        (setf (aref p 0) (/ (aref p 0) d))
        (setf (aref p 1) (/ (aref p 1) d))
        (setf (aref p 2) (/ (aref p 2) d))))
    p))

(defun vector-length (v)
  (sqrt (squared-length v)))

(defun squared-length (v)
  (+ (expt (aref v 0) 2)
     (expt (aref v 1) 2)
     (expt (aref v 2) 2)))

(defun rad-to-deg (rad)
  (* rad  (/ 180 pi)))

(defun deg-to-rad (deg)
  (* deg (/ pi 180)))


;;=============================================================


(defun make-line-between (p1 p2)
  "Return a new instance of Line that goes between p1 and p2."
  (make-line p1 (vector-normalize (vector-sub p2 p1))))

(defmethod intersect ((plane Plane) (line Line))
  "Return the intersection point of the plane and line."
  (let* ((plane (as-implicit plane))
         (s (- (/ (+ (distance plane) 
                     (dot-product (offset line) (normal plane)))
                  (dot-product (direction line) (normal plane))))))
    (vector-add (offset line) (vector-scale (copy-seq (direction line)) s))))


(defun make-plane-between (p1 p2 p3)
  "Return a plane through the points P1 P2 and P3."
  (let ((new-p (cross-product (vector-sub p2 p1) (vector-sub p3 p1))))
    (make-instance 'Implicit-Plane 
      :distance (- (dot-product p2 new-p))
      :normal new-p)))

#|
(defmethod make-plane-through-point-parallel-to-plane
       (p (plane Plane))
  )
|#

(defmethod distance-between ((v1 vector)(v2 vector))
  (vector-length (vector-sub v1 v2)))

(defmethod angle-between ((line Line)(plane Implicit-plane))
  "Returns the angle in rads."
  (let* ((plane (as-implicit plane))
         (normal (normal plane))
         (direction (direction line))
         (denom (* (squared-length normal)
                   (squared-length direction))))
    (if (< denom 0.0E-20)
        (error "Can't compute angle.")
      (- 1.570796 ; pi/2
         (acos (/ (dot-product normal direction) (sqrt denom))))
      )))

(defmethod angle-between ((line Line)(plane Explicit-plane))
  "Returns the angle in rads."
  (let* ((normal (cross-product (direction1 plane)(direction2 plane)))
         (direction (direction line))
         (denom (* (squared-length normal)
                   (squared-length direction))))
    (if (< denom 0.0E-20)
      (error "Can't compute angle.")
      (- 1.570796 ; pi/2
         (acos (/ (dot-product normal direction) (sqrt denom))))
      )))

(defmethod angle-between ((vect1 vector)(vect2 vector))
  "Returns the angle in rads."
  (let ((a (copy-seq vect1))
        (b (copy-seq vect2)))
    (acos (dot-product (vector-normalize a)
                       (vector-normalize b)))))

(defmethod angle-between ((line Line)(vect vector))
  "Returns the angle in rads."
  (let ((a (copy-seq (direction line)))
        (b (copy-seq vect)))
    (acos (dot-product (vector-normalize a)
                       (vector-normalize b)))))

(defmethod angle-between ((vect vector)(line Line))
  "Returns the angle in rads."
  (let ((a (copy-seq vect))
        (b (copy-seq (direction line))))
    (acos (dot-product (vector-normalize a)
                       (vector-normalize b)))))

(defmethod angle-between ((line1 Line)(line2 Line))
  "Returns the angle in rads."
  (let ((a (copy-seq (direction line1)))
        (b (copy-seq (direction line2))))
    (acos (dot-product (vector-normalize a)
                       (vector-normalize b)))))

(defmethod point-on-line ((line Line) s)
  "Return the point on the LINE for parameter S."
  (let ((p (copy-seq (direction line)))
        (offset (offset line)))
    (setf (aref p 0) (+ (aref offset 0) (* (aref p 0) s)))
    (setf (aref p 1) (+ (aref offset 1) (* (aref p 1) s)))
    (setf (aref p 2) (+ (aref offset 2) (* (aref p 2) s)))
    p))

(defmethod point-on-line ((vector vector) s)
  "Return the point on the LINE for parameter S."
  (let ((p (copy-seq vector)))
    (setf (aref p 0) (* (aref p 0) s))
    (setf (aref p 1) (* (aref p 1) s))
    (setf (aref p 2) (* (aref p 2) s))
    p))


(defmethod nearest-point (point (points list))
  "Return the nearest point to POINT among the list of POINTS, and its distance."
  (let ((min (squared-length (vector-sub (car points) point)))
        (min-point (car points))
        (len 0.0))
    (setq points (cdr points))
    (dolist (p points)
      (setq len (squared-length (vector-sub p point)))
      (when (< len min)
        (setq min len
              min-point p)))
    (values min-point min)))

(defmethod nearest-point (point (line Line))
  "Return the nearest point to POINT on the LINE."
  (intersect (make-implicit-plane (copy-seq (direction line))
                                  (- (dot-product point (direction line))))
             line))

(defmethod nearest-point (point (plane Plane))
  "Return the nearest point to POINT on the PLANE."
  (let* ((plane (as-implicit plane))
         (normal (normal plane)))
    (vector-sub point
                (vector-scale 
                 normal 
                 (/ (+ (distance plane) (dot-product normal point))
                    (squared-length normal))))))

(defmethod nearest-point (point (sphere Sphere))
  "Return the nearest point to POINT on the SPHERE.
   Works if point is inside the sphere too. 
   If point is the same as center point, it returns the center point."
  (vector-add (center sphere) 
              (vector-scale (vector-normalize (vector-sub point (center sphere)))
                            (radius sphere))))

(defmethod angle-between-points ((sphere Sphere) p1 p2)
  "Return the angle and normal on the SPHERE between P1 and P2."
  (let* ((v1 (vector-normalize (vector-sub p1 (center sphere))))
         (v2 (vector-normalize (vector-sub p2 (center sphere))))
         (a (cross-product v1 v2))
         (angle (asin (vector-length a))))
    (if (< (dot-product v1 v2) 0)
      (setq angle (+ angle 1.570796))) ; pi/2
    (values angle a)))



(defmethod intersect ((sphere Sphere) (line Line))
  "Return nil if there is no intersection, or a list of two points on
   the SPHERE where the LINE intersects."
  (let* ((G (vector-sub (offset line) (center sphere)))
         (a (squared-length (direction line)))
         (b (* 2 (dot-product (direction line) G)))
         (c (- (squared-length G) (expt (radius sphere) 2)))
         (d (- (* b b) (* 4 a c))))
    (if (< d 0)
      nil
      (let ((d (sqrt d)))
        (list
         (point-on-line line (/ (- d b) (* 2 a)))
         (point-on-line line (/ (- 0.0 d b) (* 2 a)))
         )))))

(defmethod intersect ((line Line)(plane Explicit-plane))
  "Returns 2 scalars, 1 for each distance along the plane's 2 dimensions."
  (let* ((dir1scalar 0.0)
         (dir2scalar 0.0)
         ;First scalar
         (v1 (cross-product (direction2 plane)(direction line)))
         (denominator (dot-product v1 (direction1 plane)))
         (num1 (dot-product v1 (offset line)))
         (num2 (dot-product v1 (offset plane)))
         ;Second scalar
         (v2 (cross-product (direction1 plane)(direction line)))
         (denom (dot-product v2 (direction2 plane)))
         (num3 (dot-product v2 (offset line)))
         (num4 (dot-product v2 (offset plane))))
    (unless (= 0 denominator)
      (setf dir1scalar (/ (- num1 num2) denominator)))
    (unless (= 0 denom)
      (setf dir2scalar (/ (- num3 num4) denom)))
    (values dir1scalar dir2scalar)))

(defmethod intersect-to-point ((line Line)(plane Explicit-plane))
  "Returns a vector from origin to the line-plane intersection."
  (multiple-value-bind (wb wc)
                       (intersect line plane)
    (let* ((extent1 (point-on-line (direction1 plane) wb))
           (extent2 (point-on-line (direction2 plane) wc))
           (temp (vector-add extent1 extent2))
           (point (vector-add (offset plane) temp)))
      point)))


(defmethod plane-center ((plane Explicit-plane))
  "Returns a vector from origin to center of plane."
  (let* ((half-dir1 (point-on-line (direction1 plane) 0.5))
         (half-dir2 (point-on-line (direction2 plane) 0.5))
         (ctr1 (vector-add half-dir1 half-dir2)))
    (vector-add (offset plane) ctr1))) ;returns center.



(provide :3DGeometry)