; http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
; Unit quaternion to orthogonal matrix
(define (q2m q) (apply (lambda (r i j k)(list
(list (- 1 (* 2 (+ (* j j)(* k k)))) (* 2 (- (* i j)(* r k))) (* 2 (+ (* i k)(* r j))))
(list (* 2 (+ (* i j)(* r k))) (- 1 (* 2 (+ (* i i)(* k k)))) (* 2 (- (* j k)(* r i))))
(list (* 2 (- (* i k)(* r j))) (* 2 (+ (* j k)(* r i))) (- 1 (* 2 (+ (* i i)(* j j)))))))
 q))

; quaternion to conformal matrix
(define (q2m q) (apply (lambda (r i j k)(let ((Q (+ (* r r)(* i i)(* j j)(* k k))))
(list
(list (- Q (* 2 (+ (* j j)(* k k)))) (* 2 (- (* i j)(* r k))) (* 2 (+ (* i k)(* r j))))
(list (* 2 (+ (* i j)(* r k))) (- Q (* 2 (+ (* i i)(* k k)))) (* 2 (- (* j k)(* r i))))
(list (* 2 (- (* i k)(* r j))) (* 2 (+ (* j k)(* r i))) (- Q (* 2 (+ (* i i)(* j j))))))))
 q))

; Inner product:
(define (ip x y) (let w ((s 0)(x x)(y y))
   (if (null? x) s (w (+ (* (car x)(car y)) s)(cdr x)(cdr y)))))