Change vector math interface

engine/math.scm

@@ -1,7 +1,9 @@
 (module (engine math) ()
 (import scheme
 	(chicken base)
-	(chicken module))
+	(chicken module)
+	(srfi 1)
+	(srfi 99))
 
 (export PI PI/2)
 (define PI
@@ -24,4 +26,157 @@
 ;; Approximately equal - for real number comparison
 (define (approx-= x y)
   (< (abs (- x y)) (*float-precision*)))
+
+;; Vector exports
+(export vec vec? vec2? v-x
+	set-v-x! v-y set-v-y!)
+
+;; 2D Vector type
+;; TODO: this could be done with a macro to save some definitions
+(define-record-type <vector2>
+  (int:make-vector2 x y)
+  vec2?
+  (x vector2-x int:set-vector2-x!)
+  (y vector2-y int:set-vector2-y!))
+
+;; Type safe 2D vector constructor
+(define (vec . args)
+  (assert (every number? args))
+  (apply (case (length args)
+	   ((2) int:make-vector2))
+	 args))
+
+(define vec?
+  (disjoin vec2?))
+
+;; Vector utility functions
+(define (v-x component)
+  (assert (record? component))
+  ((rtd-accessor (record-rtd component) 'x) component))
+
+(define (set-v-x! component x)
+  (assert (record? component))
+  (assert (number? x))
+  ((rtd-mutator (record-rtd component) 'x) component x))
+
+(define (v-y component)
+  (assert (record? component))
+  ((rtd-accessor (record-rtd component) 'y) component))
+
+(define (set-v-y! component y)
+  (assert (record? component))
+  (assert (number? y))
+  ((rtd-mutator (record-rtd component) 'y) component y))
+
+;; Vector operations
+(export v= v+ v- v* v/)
+
+;; Vector equality
+(define (v= . vecs)
+  (assert (every record? vecs))
+  (assert (every vec? vecs))
+  (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
+		 (map (compose rtd-name record-rtd) vecs)))
+  (and (apply = (map v-x vecs))
+       (apply = (map v-y vecs))))
+
+;; Vector addition
+;; Note that each operand can be either a vector OR a number
+;; If a number, that number is added to EVERY member of the vector
+(define (v+ . operands)
+  (assert (every (disjoin number? (conjoin record? vec?)) operands))
+  (let ((vecs (filter vec? operands)))
+    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
+		   (map (compose rtd-name record-rtd) vecs))))
+  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
+	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
+    (vec (apply + x-parts)
+	 (apply + y-parts))))
+
+;; Vector subtractions
+;; Note that each operand can be either a vector OR a number
+;; If a number, that number is subtracted from EVERY member of the vector
+(define (v- . operands)
+  (assert (every (disjoin number? (conjoin record? vec?)) operands))
+  (let ((vecs (filter vec? operands)))
+    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
+		   (map (compose rtd-name record-rtd) vecs))))
+  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
+	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
+    (vec (apply - x-parts)
+	 (apply - y-parts))))
+
+;; Vector multiplication
+;; Note that each operand can be either a vector OR a number
+;; If a number, that number is multiplied to EVERY member of the vector
+(define (v* . operands)
+  (assert (every (disjoin number? (conjoin record? vec?)) operands))
+  (let ((vecs (filter vec? operands)))
+    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
+		   (map (compose rtd-name record-rtd) vecs))))
+  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
+	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
+    (vec (apply * x-parts)
+	 (apply * y-parts))))
+
+;; Vector division
+;; Note that each operand can be either a vector OR a number
+;; If a number, EVERY member of the vector is divided by that number
+(define (v/ . operands)
+  (assert (every (disjoin number? (conjoin record? vec?)) operands))
+  (let ((vecs (filter vec? operands)))
+    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
+		   (map (compose rtd-name record-rtd) vecs))))
+  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
+	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
+    (vec (apply / x-parts)
+	 (apply / y-parts))))
+
+;; More complex vector functions
+(export vector-magnitude vector-normalize vector-dot
+	vector-angle-between)
+
+;; Magnitude
+(define (vector-magnitude vec)
+  (assert ((disjoin vec2?) vec))
+  (cond
+   ((vec2? vec)
+    (sqrt (+ (expt (v-x vec) 2)
+	     (expt (v-y vec) 2))))))
+
+;; Dot product of vectors
+(define (vector-dot vec1 vec2)
+  (assert (and (record? vec1)
+	       (record? vec2)))
+  (assert (eq? (rtd-name (record-rtd vec1))
+	       (rtd-name (record-rtd vec2))))
+  (assert ((disjoin vec2?) vec1))
+  (cond
+   ((vec2? vec1)
+    (+ (* (v-x vec1) (v-x vec2))
+       (* (v-y vec1) (v-y vec2))))))
+
+;; Angle between vectors
+(define (vector-angle-between vec1 vec2)
+  (assert (and (record? vec1)
+	       (record? vec2)))
+  (assert (eq? (rtd-name (record-rtd vec1))
+	       (rtd-name (record-rtd vec2))))
+  (assert ((disjoin vec2?) vec1))
+  (cond
+   ((vec2? vec1)
+    (acos (/ (vector-dot vec1 vec2)
+	     (* (vector-magnitude vec1)
+		(vector-magnitude vec2)))))))
+
+;; Normalization
+(define (vector-normalize v)
+  (assert ((disjoin vec2?) v)) ;; TODO: This assertion should be moved out of here
+  (let ((magnitude (vector-magnitude v)))
+    (cond
+     ((vec2? v)
+      (vec (/ (v-x v)
+	      magnitude)
+	   (/ (v-y v)
+	      magnitude))))))
 )