;This code written in ANSI Common Lisp (Allegro CL 5.0) by Prof. Robert
;McGhee at the Naval Postgraduate School, Monterey, CA93943. Contact: 
;mcghee@cs.nps.navy.mil. Date of last modification: May 9, 2000.

(load "D:\\cs4314\\math-routines\\robot-kinematics")

(defun quaternion-product (Q Q1)
  (let ((w (first Q)) (x (second Q)) (y (third Q)) (z (fourth Q)) 
        (w1 (first Q1)) (x1 (second Q1)) (y1 (third Q1)) (z1 (fourth Q1)))
       (list (- (* w w1) (* x x1) (* y y1) (* z z1))
             (+ (* x w1) (* w x1) (- (* z y1)) (* y z1))
             (+ (* y w1) (* z x1) (* w y1) (- (* x z1)))
             (+ (* z w1) (- (* y x1)) (* x y1) (* w z1)))))

(defun quaternion-conjugate (Q)
  (list (first Q) (- (second Q)) (- (third Q)) (- (fourth Q))))

(defun quaternion-inverse (Q)
  (scalar-multiply (/ 1 (dot-product Q Q)) (quaternion-conjugate Q)))

(defun rotate-vector (quaternion vector) ;Vector is quaternion with leading
  (let* ((q quaternion) (v vector)       ;element zero.
         (q-inv (quaternion-inverse q)))
        (quaternion-product q (quaternion-product v q-inv))))

(defun quaternion-i (angle)
  (list (cos (* .5 angle)) (sin (* .5 angle)) 0 0))

(defun quaternion-j (angle)
  (list (cos (* .5 angle)) 0 (sin (* .5 angle)) 0))

(defun quaternion-k (angle)
  (list (cos (* .5 angle)) 0 0 (sin (* .5 angle))))

(defun euler-to-quaternion (azimuth elevation roll)
  (quaternion-product (quaternion-k azimuth)
                      (quaternion-product (quaternion-j elevation)
                                          (quaternion-i roll))))

(defun unit-quaternion (angle axis)
  (cons (cos angle) (scalar-multiply (sin angle) axis)))

(defun quaternion-rotation (unit-quaternion)
  (transpose (list (rest (rotate-vector unit-quaternion '(0 1 0 0)))
                   (rest (rotate-vector unit-quaternion '(0 0 1 0)))
                   (rest (rotate-vector unit-quaternion '(0 0 0 1))))))

(defun quaternion-derivative (quaternion pqr)
  (scalar-multiply .5 (quaternion-product quaternion (cons 0 pqr))))

(defun quaternion-homogeneous-transform (quaternion position)
  (let* ((matrix (quaternion-rotation quaternion)))
        (append (concat-matrix matrix (transpose (list position)))
                (list (list 0 0 0 1)))))

(defun normalize (quaternion)
  (scalar-multiply (/ 1 (vector-magnitude quaternion)) quaternion))

(defvar q1 (list (sqrt .5) (sqrt .5) 0 0))

(defvar q2 (list (sqrt .5) 0 (sqrt .5) 0))

(defvar h '(1 0 0 0))

(defvar i '(0 1 0 0))

(defvar j '(0 0 1 0))

(defvar k '(0 0 0 1))