; Copyright (C) 2010 Dave Griffiths : See LICENCE file

(define (mat)
	(vector 
		1 0 0 0
		0 1 0 0
		0 0 1 0
		0 0 0 1))
		
(define (mat-copy a)
    (vector 
		(mref a 0 0) (mref a 0 1) (mref a 0 2) (mref a 0 3)		
		(mref a 1 0) (mref a 1 1) (mref a 1 2) (mref a 1 3)		
		(mref a 2 0) (mref a 2 1) (mref a 2 2) (mref a 2 3)		
		(mref a 3 0) (mref a 3 1) (mref a 3 2) (mref a 3 3)))		
		
(define (mref m x y)
	(vector-ref m (+ y (* x 4))))

; unrolling stuff for speed - need to actually test this theory!
(define (meq? a b)
  (and (feq? (mref a 0 0) (mref b 0 0))
       (feq? (mref a 1 0) (mref b 1 0))
       (feq? (mref a 2 0) (mref b 2 0))
       (feq? (mref a 3 0) (mref b 3 0))
       
       (feq? (mref a 0 1) (mref b 0 1))
       (feq? (mref a 1 1) (mref b 1 1))
       (feq? (mref a 2 1) (mref b 2 1))
       (feq? (mref a 3 1) (mref b 3 1))
	   
	   (feq? (mref a 0 2) (mref b 0 2))
       (feq? (mref a 1 2) (mref b 1 2))
       (feq? (mref a 2 2) (mref b 2 2))
       (feq? (mref a 3 2) (mref b 3 2))
	   
	   (feq? (mref a 0 3) (mref b 0 3))
       (feq? (mref a 1 3) (mref b 1 3))
       (feq? (mref a 2 3) (mref b 2 3))
       (feq? (mref a 3 3) (mref b 3 3))))
       

(define (vtransform v m)
	(vector
		(+ (* (vx v) (mref m 0 0)) (* (vy v) (mref m 1 0)) (* (vz v) (mref m 2 0)) (mref m 3 0))
		(+ (* (vx v) (mref m 0 1)) (* (vy v) (mref m 1 1)) (* (vz v) (mref m 2 1)) (mref m 3 1))
		(+ (* (vx v) (mref m 0 2)) (* (vy v) (mref m 1 2)) (* (vz v) (mref m 2 2)) (mref m 3 2))
		(+ (* (vx v) (mref m 0 3)) (* (vy v) (mref m 1 3)) (* (vz v) (mref m 2 3)) (mref m 3 3))))

(define (vtransformr v m)
	(vector
		(+ (* (vx v) (mref m 0 0)) (* (vy v) (mref m 1 0)) (* (vz v) (mref m 2 0)))
		(+ (* (vx v) (mref m 0 1)) (* (vy v) (mref m 1 1)) (* (vz v) (mref m 2 1)))
		(+ (* (vx v) (mref m 0 2)) (* (vy v) (mref m 1 2)) (* (vz v) (mref m 2 2)))
		(+ (* (vx v) (mref m 0 3)) (* (vy v) (mref m 1 3)) (* (vz v) (mref m 2 3)))))

(define (vtransformh v m)
	(vector
		(+ (* (vx v) (mref m 0 0)) (* (vy v) (mref m 1 0)) (* (vz v) (mref m 2 0)) (* (vw v) (mref m 3 0)))
		(+ (* (vx v) (mref m 0 1)) (* (vy v) (mref m 1 1)) (* (vz v) (mref m 2 1)) (* (vw v) (mref m 3 1)))
		(+ (* (vx v) (mref m 0 2)) (* (vy v) (mref m 1 2)) (* (vz v) (mref m 2 2)) (* (vw v) (mref m 3 2)))
		(+ (* (vx v) (mref m 0 3)) (* (vy v) (mref m 1 3)) (* (vz v) (mref m 2 3)) (* (vw v) (mref m 3 3)))))

(define (mmul a b)
	(vector
	   (+ (* (mref a 0 0) (mref b 0 0))(* (mref a 1 0)(mref b 0 1))(* (mref a 2 0)(mref b 0 2))(* (mref a 3 0)(mref b 0 3)))
       (+ (* (mref a 0 1) (mref b 0 0))(* (mref a 1 1)(mref b 0 1))(* (mref a 2 1)(mref b 0 2))(* (mref a 3 1)(mref b 0 3)))
       (+ (* (mref a 0 2) (mref b 0 0))(* (mref a 1 2)(mref b 0 1))(* (mref a 2 2)(mref b 0 2))(* (mref a 3 2)(mref b 0 3)))
       (+ (* (mref a 0 3) (mref b 0 0))(* (mref a 1 3)(mref b 0 1))(* (mref a 2 3)(mref b 0 2))(* (mref a 3 3)(mref b 0 3)))

       (+ (* (mref a 0 0) (mref b 1 0))(* (mref a 1 0)(mref b 1 1))(* (mref a 2 0)(mref b 1 2))(* (mref a 3 0)(mref b 1 3)))
       (+ (* (mref a 0 1) (mref b 1 0))(* (mref a 1 1)(mref b 1 1))(* (mref a 2 1)(mref b 1 2))(* (mref a 3 1)(mref b 1 3)))
       (+ (* (mref a 0 2) (mref b 1 0))(* (mref a 1 2)(mref b 1 1))(* (mref a 2 2)(mref b 1 2))(* (mref a 3 2)(mref b 1 3)))
       (+ (* (mref a 0 3) (mref b 1 0))(* (mref a 1 3)(mref b 1 1))(* (mref a 2 3)(mref b 1 2))(* (mref a 3 3)(mref b 1 3)))

       (+ (* (mref a 0 0) (mref b 2 0))(* (mref a 1 0)(mref b 2 1))(* (mref a 2 0)(mref b 2 2))(* (mref a 3 0)(mref b 2 3)))
       (+ (* (mref a 0 1) (mref b 2 0))(* (mref a 1 1)(mref b 2 1))(* (mref a 2 1)(mref b 2 2))(* (mref a 3 1)(mref b 2 3)))
       (+ (* (mref a 0 2) (mref b 2 0))(* (mref a 1 2)(mref b 2 1))(* (mref a 2 2)(mref b 2 2))(* (mref a 3 2)(mref b 2 3)))
       (+ (* (mref a 0 3) (mref b 2 0))(* (mref a 1 3)(mref b 2 1))(* (mref a 2 3)(mref b 2 2))(* (mref a 3 3)(mref b 2 3)))

       (+ (* (mref a 0 0) (mref b 3 0))(* (mref a 1 0)(mref b 3 1))(* (mref a 2 0)(mref b 3 2))(* (mref a 3 0)(mref b 3 3)))
       (+ (* (mref a 0 1) (mref b 3 0))(* (mref a 1 1)(mref b 3 1))(* (mref a 2 1)(mref b 3 2))(* (mref a 3 1)(mref b 3 3)))
       (+ (* (mref a 0 2) (mref b 3 0))(* (mref a 1 2)(mref b 3 1))(* (mref a 2 2)(mref b 3 2))(* (mref a 3 2)(mref b 3 3)))
       (+ (* (mref a 0 3) (mref b 3 0))(* (mref a 1 3)(mref b 3 1))(* (mref a 2 3)(mref b 3 2))(* (mref a 3 3)(mref b 3 3)))))

(define (mrot-x a)
	(vector
		1 0 0 0
		0 (cos a) (- (sin a)) 0
		0 (sin a) (cos a) 0
		0 0 0 1))

(define (mrot-y a)
	(vector
		(cos a) 0 (- (sin a)) 0
		0 1 0 0
		(sin a) 0 (cos a) 0
		0 0 0 1))


(define (mrot-z a)
	(vector
		(cos a) (- (sin a)) 0 0
		(sin a) (cos a) 0 0
		0 0 1 0
		0 0 0 1))

(define (mrotate v)
	(mmul
		(mmul (mrot-x (vx v)) (mrot-y (vy v)))
		(mrot-z (vz v))))

(define (mscale v)
	(vector
		(vx v) 0 0 0
		0 (vy v) 0 0
		0 0 (vz v) 0
		0 0 0 1))

(define (mtranslate v)
	(vector
		1 0 0 0
		0 1 0 0
		0 0 1 0
		(vx v) (vy v) (vz v) 1))