;;;-*-LISP-*-
;;; A graphics utility package. - George Carrette.

;;; 3-D graphics.

(eval-when (eval compile)
  (or (get 'graphm 'version)
      (load (list (car (namelist infile)) 'graphm))))

(graphs-module graph3)

;;; in this stream the eye is looking down on the X-Y plane.
;;; here is a picture where it is looking right, down on the X-axis.
;;;
;;;                                                 ^
;;;                                   |       *     |
;;;                        |          |             |
;;;                        |          |             |
;;;                        |          |            X-AXIS
;;;                        |          |             |
;;;                        |          |             |
;;; Z                      |          |             |
;;;  eye <------Z-AXIS----------------|-------------#-
;;;                        |          |
;;;                      z-screen   z-clip
;;;
;;;
;;; lines are clipped at z-clip


;;;         V = T (V1 - V0) + V0
;;;
;;; [X, Y, Z] = [T (X1 - X0) + X0, Y0 + T (y1 - Y0), T (Z1 - Z0) + Z0]
;;;
;;;	     X0 (Z - Z1) + X1 (Z0 - Z)	      Y0 (Z - Z1) + Y1 (Z0 - Z)
;;;    X = - -------------------------, Y = - -------------------------
;;;		      Z1 - Z0			       Z1 - Z0
;;;


(defun z-clip-stream NARGS
  (COMMENT (ARG 1) = Z-CLIP-STREAM
	   (ARG 2) = COMMAND
	   (ARG 3) = X
	   (ARG 4) = Y
	   (ARG 5) = Z)
  (LET ((Z-CLIP-STREAM (ARG 1)))
    (LET ((OUT-STREAM (Z-CLIP-STREAM-OUT-STREAM))
	  (3D-CLIP (Z-CLIP-STREAM-3D-CLIP)))
      (caseq (ARG 2)
	((move-pen)
	 (NARG-CHECK NARGS 5 'Z-CLIP-STREAM)
	 (let ((z0 (3d-clip-z 3d-clip))
	       (x0 (3d-clip-x 3d-clip))
	       (y0 (3d-clip-y 3d-clip))
	       (clip (3d-clip-clip 3d-clip)))
	   (cond ((> z0 clip)
		  ;; left point must clipped.
		  (cond ((> (ARG 5) clip)
			 ;; whole line is clipped.
			 nil)
			(t
			 (CALL out-stream
			       'set-pen
			       (x-intercept3 z0 x0 (ARG 5) (ARG 3) clip)
			       (y-intercept3 z0 y0 (ARG 5) (ARG 4) clip)
			       clip)
			 (CALL out-stream
			       'move-pen (ARG 3)(ARG 4)(ARG 5)))))
		 ((> (ARG 5) clip)
		  ;; right point must be clipped.
		  (CALL out-stream 'set-pen x0 y0 z0)
		  (CALL out-stream
			'move-pen
			(x-intercept3 z0 x0 (ARG 5) (ARG 3) clip)
			(y-intercept3 z0 y0 (ARG 5) (ARG 4) clip)
			clip))
		 (t
		  (CALL out-stream 'set-pen x0 y0 z0)
		  (CALL out-stream 'move-pen (ARG 3) (ARG 4) (ARG 5)))))
	 (alter-3d-clip 3d-clip
			x (ARG 3)
			y (ARG 4)
			z (ARG 5))
	 t)
	((set-pen)
	 (NARG-CHECK NARGS 5 'Z-CLIP-STREAM)
	 (alter-3d-clip 3d-clip
			x (ARG 3)
			y (ARG 4)
			z (ARG 5))
	 t)
	((set-clip-z)
	 (NARG-CHECK NARGS 3 'Z-CLIP-STREAM)
	 (setf (3d-clip-clip 3d-clip) (ARG 3))
	 t)
	(T
	 (UNKNOWN-COMMAND (ARG 2) 'Z-CLIP-STREAM))))))


(defun make-z-clip-stream (out-stream)
  (make-z-clip-stream-1 out-stream out-stream))


;;;	Z    - Z	 X 		         Z    - Z       
;;;	 EYE    SCREEN	  SCREEN		  EYE    SCREEN
;;;     -------------- = -------     X       = (----------------) X
;;;	   Z    - Z	    X         SCREEN	     Z - Z
;;;	    EYE   			                  EYE
;;;

(declare (flonum (x-screen flonum flonum flonum flonum)))

(defun x-screen (z-eye z-screen z x)
       (//$ (*$ (-$ z-eye z-screen) x)
	    (-$ z z-eye)))

(defun z-perspective-stream NARGS
  (COMMENT (ARG 1) = Z-PERSPECTIVE-STREAM
	   (ARG 1) = COMMAND
	   (ARG 3) = X
	   (ARG 4) = Y
	   (ARG 5) = Z)
  (LET ((Z-PERSPECTIVE-STREAM (ARG 1)))
    (let ((out-stream (z-perspective-stream-out-stream))
	  (perspective (z-perspective-stream-perspective)))
      (caseq (ARG 2)
	((move-pen set-pen)
	 (NARG-CHECK NARGS 5 'Z-PERSPECTIVE-STREAM)
	 (let ((z-eye (3d-perspective-z-eye perspective))
	       (z-screen (3d-perspective-z-screen perspective)))
	   (CALL out-stream (ARG 2)
		 (x-screen z-eye z-screen (ARG 5) (ARG 3))
		 (y-screen z-eye z-screen (ARG 5) (ARG 4)))))
	((set-z-eye)
	 (NARG-CHECK NARGS 3 'Z-PERSPECTIVE-STREAM)
	 (setf (3d-perspective-z-eye perspective) (ARG 3))
	 t)
	((set-z-screen)
	 (NARG-CHECK NARGS 3 'Z-PERSPECTIVE-STREAM)
	 (setf (3d-perspective-z-screen perspective) (ARG 3))
	 t)
	(T
	 (UNKNOWN-COMMAND (ARG 2) 'Z-PERSPECTIVE-STREAM))))))

(defun make-z-perspective-stream (out-stream)
  (make-z-perspective-stream-1 Out-stream out-stream))


;;; orthogonal rotation by the eulerian angles. The convention
;;; here is from Goldstein, Classical Mechanics, a worthy source.

;;;		      [	 COS(PHI)   SIN(PHI)  0 ]
;;;		      [			        ]
;;;		  D = [ - SIN(PHI)  COS(PHI)  0 ]
;;;		      [			        ]
;;;		      [	    0	       0      1 ]


(defun phi-matrix (phi &optional
		       (m (make-3matrix))
		       &aux (cos-phi (cos phi)) (sin-phi (sin phi)))
  (alter-3matrix m
		 x-x cos-phi      x-y sin-phi   x-z 0.0
		 y-x (-$ sin-phi) y-y cos-phi   y-z 0.0
		 z-x 0.0          z-y 0.0       z-z 1.0)
  m)
;;;
;;;                 [ 1	      0		   0	  ]
;;;     	    [				  ]
;;;	        C = [ 0	  COS(THETA)   SIN(THETA) ]
;;;		    [				  ]
;;;		    [ 0	 - SIN(THETA)  COS(THETA) ]
;;;
 
(defun theta-matrix (theta &optional
			   (m (make-3matrix))
			   &aux
			   (cos-theta (cos theta))
			   (sin-theta (sin theta)))
       (alter-3matrix m
		      x-x 1.0   x-y 0.0            x-z 0.0
		      y-x 0.0   y-y cos-theta      y-z sin-theta
		      z-x 0.0   z-y (-$ sin-theta) z-z cos-theta)
       m)
;;;
;;;		      [	 COS(PSI)   SIN(PSI)  0 ]
;;;		      [			        ]
;;;         	  B = [ - SIN(PSI)  COS(PSI)  0 ]
;;;		      [			        ]
;;;		      [	    0	       0      1 ]

(defun psi-matrix (psi &optional
		       (m (make-3matrix))
		       &aux
		       (cos-psi (cos psi))
		       (sin-psi (sin psi)))
  (alter-3matrix m
		 x-x cos-psi      x-y sin-psi   x-z 0.0
		 y-x (-$ sin-psi) y-y cos-psi   y-z 0.0
		 z-x 0.0          z-y 0.0       z-z 1.0)
  m)


(defun 3matrix-mult (ma mb &optional (m (make-3matrix)))
  ;; this goes to 512 words of pdp10 code. gosh. well, it is fast.
  (alter-3matrix m
		 x-x (3row*col x z ma mb)
		 x-y (3row*col x y ma mb)
		 x-z (3row*col x x ma mb)

		 y-x (3row*col y z ma mb)
		 y-y (3row*col y y ma mb)
		 y-z (3row*col y x ma mb)

		 z-x (3row*col z z ma mb)
		 z-y (3row*col z y ma mb)
		 z-z (3row*col z x ma mb))
  ma)


(defun orthogonal-3d-stream NARGS
  (COMMENT (ARG 1) = orthogonal-3d-stream
	   (ARG 1) = COMMAND
	   (ARG 3) = X
	   (ARG 4) = Y
	   (ARG 5) = Z)
  (LET ((orthogonal-3d-stream (ARG 1)))
    (LET ((OUT-STREAM (orthogonal-3d-stream-OUT-STREAM))
	  (EULER (orthogonal-3d-stream-EULER)))
      (caseq (ARG 2)
	((move-pen set-pen)
	 (NARG-CHECK NARGS 5 'orthogonal-3d-stream)
	 (let ((m (euler-rot euler)))
	   (CALL out-stream
		 (ARG 2)
		 (+$ (*$ (ARG 3) (3matrix-x-x m))
		     (*$ (ARG 4) (3matrix-x-y m))
		     (*$ (ARG 5) (3matrix-x-z m)))

		 (+$ (*$ (ARG 3) (3matrix-y-x m))
		     (*$ (ARG 4) (3matrix-y-y m))
		     (*$ (ARG 5) (3matrix-y-z m)))
		 
		 (+$ (*$ (ARG 3) (3matrix-z-x m))
		     (*$ (ARG 4) (3matrix-z-y m))
		     (*$ (ARG 5) (3matrix-z-z m))))))
	
	((translate)
	 (NARG-CHECK NARGS 2 'orthogonal-3d-stream)
	 (3matrix-mult (euler-drot euler)
		       (euler-rot euler)
		       (euler-rot euler))
	 t)
	((translate-psi)
	 (NARG-CHECK NARGS 2 'orthogonal-3d-stream)
	 (3matrix-mult (euler-dpsi euler)
		       (euler-psi euler)
		       (euler-psi euler))
	 (setf (euler-psi-val euler)
	       (+$ (euler-psi-val euler) (euler-dpsi-val euler)))
	 (update-rot-matrix))
	((translate-theta)
	 (NARG-CHECK NARGS 2 'orthogonal-3d-stream)
	 (3matrix-mult (euler-dtheta euler)
		       (euler-theta euler)
		       (euler-theta euler))
	 (setf (euler-theta-val euler)
	       (+$ (euler-theta-val euler) (euler-dtheta-val euler)))
	 (update-rot-matrix))
	((translate-phi)
	 (NARG-CHECK NARGS 2 'orthogonal-3d-stream)
	 (3matrix-mult (euler-dphi euler)
		       (euler-phi euler)
		       (euler-phi euler))
	 (setf (euler-phi-val euler)
	       (+$ (euler-phi-val euler) (euler-dphi-val euler)))
	 (update-rot-matrix))
	((set-psi)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      psi-val (ARG 3)
		      psi (psi-matrix (ARG 3) (euler-psi euler)))
	 (update-rot-matrix))
	((set-theta)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      theta-val (ARG 3)
		      theta (theta-matrix (ARG 3) (euler-theta euler)))
	 (update-rot-matrix))
	((set-phi)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      phi-val (ARG 3)
		      phi (phi-matrix (ARG 3) (euler-phi euler)))
	 (update-rot-matrix))
	((set-dphi)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      dphi-val (ARG 3)
		      dphi (phi-matrix (ARG 3) (euler-dphi euler)))
	 (update-drot-matrix))
	((set-dpsi)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      dpsi-val (ARG 3)
		      dpsi (psi-matrix (ARG 3) (euler-dpsi euler)))
	 (update-drot-matrix))
	((set-dtheta)
	 (NARG-CHECK NARGS 3 'orthogonal-3d-stream)
	 (alter-euler euler
		      dtheta-val (ARG 3)
		      dtheta (theta-matrix (ARG 3) (euler-dtheta euler)))
	 (update-drot-matrix))
	((get-angles)
	 (NARG-CHECK NARGS 2 'orthogonal-3d-stream)
	 `((psi    . ,(euler-psi-val euler))
	   (theta  . ,(euler-theta-val euler))
	   (phi    . ,(euler-phi-val euler))
	   (dpsi   . ,(euler-dpsi-val euler))
	   (dtheta . ,(euler-dtheta-val euler))
	   (dphi   . ,(euler-dphi-val euler))))
	(t
	 (UNKNOWN-COMMAND (ARG 2) 'orthogonal-3d-stream))))))
	 

;;; rot = psi . theta . phi

(defun update-rot-matrix ()
  (let ((m (3matrix-copy (euler-phi euler) (euler-rot euler))))
    (3matrix-mult (euler-theta euler) m m)
    (3matrix-mult (euler-psi euler) m m)))

(defun update-drot-matrix ()
  (let ((m (3matrix-copy (euler-dphi euler) (euler-drot euler))))
    (3matrix-mult (euler-dtheta euler) m m)
    (3matrix-mult (euler-dpsi euler) m m)))

(defun make-orthogonal-3d-stream (out-stream)
  (make-orthogonal-3d-stream-1 out-stream out-stream))