math: vector: Add 3-dimensional vector type.

1 files changed, 147 insertions, 1 deletions

diff --git a/chickadee/math/vector.scm b/chickadee/math/vector.scm
index 6920cf5..89b4f33 100644
--- a/chickadee/math/vector.scm
+++ b/chickadee/math/vector.scm
@@ -36,7 +36,23 @@
             vec2-normalize!
             vec2-mult!
             vec2-add!
-            vec2-sub!))
+            vec2-sub!
+            vec3
+            vec3?
+            vec3->pointer
+            vec3-copy
+            vec3-copy!
+            vec3-x
+            vec3-y
+            vec3-magnitude
+            vec3-dot-product
+            vec3-normalize
+            set-vec3-x!
+            set-vec3-y!
+            vec3-normalize!
+            vec3-mult!
+            vec3-add!
+            vec3-sub!))
 
 (define-record-type <vec2>
   (wrap-vec2 bv pointer)
@@ -44,6 +60,12 @@
   (bv unwrap-vec2)
   (pointer vec2-pointer set-vec2-pointer!))
 
+(define-record-type <vec3>
+  (wrap-vec3 bv pointer)
+  vec3?
+  (bv unwrap-vec3)
+  (pointer vec3-pointer set-vec3-pointer!))
+
 (define (vec2->pointer v)
   "Return a foreign pointer to V."
   ;; Create foreign pointer lazily.
@@ -52,24 +74,50 @@
         (set-vec2-pointer! v pointer)
         pointer)))
 
+(define (vec3->pointer v)
+  "Return a foreign pointer to V."
+  ;; Create foreign pointer lazily.
+  (or (vec3-pointer v)
+      (let ((pointer (bytevector->pointer (unwrap-vec3 v))))
+        (set-vec3-pointer! v pointer)
+        pointer)))
+
 (define (make-null-vec2)
   (wrap-vec2 (make-f32vector 2) #f))
 
+(define (make-null-vec3)
+  (wrap-vec3 (make-f32vector 3) #f))
+
 (define-syntax-rule (with-new-vec2 name body ...)
   (let ((name (make-null-vec2))) body ... name))
 
+(define-syntax-rule (with-new-vec3 name body ...)
+  (let ((name (make-null-vec3))) body ... name))
+
 (define-inlinable (vec2-ref v i)
   (f32vector-ref (unwrap-vec2 v) i))
 
+(define-inlinable (vec3-ref v i)
+  (f32vector-ref (unwrap-vec3 v) i))
+
 (define-inlinable (vec2-set! v i x)
   (f32vector-set! (unwrap-vec2 v) i x))
 
+(define-inlinable (vec3-set! v i x)
+  (f32vector-set! (unwrap-vec3 v) i x))
+
 (define-inlinable (vec2 x y)
   "Return a new vec2 with coordinates (X, Y)."
   (with-new-vec2 v
     (vec2-set! v 0 x)
     (vec2-set! v 1 y)))
 
+(define-inlinable (vec3 x y z)
+  (with-new-vec3 v
+    (vec3-set! v 0 x)
+    (vec3-set! v 1 y)
+    (vec3-set! v 2 z)))
+
 (define-inlinable (vec2/polar r theta)
   "Return a new vec2 containing the Cartesian representation of the
 polar coordinate (R, THETA)."
@@ -79,43 +127,95 @@ polar coordinate (R, THETA)."
   "Return the x coordinate of the vec2 V."
   (vec2-ref v 0))
 
+(define-inlinable (vec3-x v)
+  "Return the x coordinate of the vec3 V."
+  (vec3-ref v 0))
+
 (define-inlinable (vec2-y v)
   "Return the y coordinate of the vec2 V."
   (vec2-ref v 1))
 
+(define-inlinable (vec3-y v)
+  "Return the y coordinate of the vec3 V."
+  (vec3-ref v 1))
+
+(define-inlinable (vec3-z v)
+  "Return the z coordinate of the vec3 V."
+  (vec3-ref v 2))
+
 (define-inlinable (set-vec2-x! v x)
   "Set the x coordinate of the vec2 V to X."
   (vec2-set! v 0 x))
 
+(define-inlinable (set-vec3-x! v x)
+  "Set the x coordinate of the vec3 V to X."
+  (vec3-set! v 0 x))
+
 (define-inlinable (set-vec2-y! v y)
   "Set the y coordinate of the vec2 V to Y."
   (vec2-set! v 1 y))
 
+(define-inlinable (set-vec3-y! v y)
+  "Set the y coordinate of the vec3 V to Y."
+  (vec3-set! v 1 y))
+
+(define-inlinable (set-vec3-z! v z)
+  "Set the z coordinate of the vec3 V to Z."
+  (vec3-set! v 2 z))
+
 (define (vec2-copy! source-vec2 target-vec2)
   "Copy TARGET-VEC2 to SOURCE-VEC2."
   (set-vec2-x! target-vec2 (vec2-x source-vec2))
   (set-vec2-y! target-vec2 (vec2-y source-vec2)))
 
+(define (vec3-copy! source-vec3 target-vec3)
+  "Copy TARGET-VEC3 to SOURCE-VEC3."
+  (set-vec3-x! target-vec3 (vec3-x source-vec3))
+  (set-vec3-y! target-vec3 (vec3-y source-vec3)))
+
 (define (vec2-copy vec2)
   "Return a new vec2 that is a copy of VEC2."
   (with-new-vec2 new
     (vec2-copy! vec2 new)))
 
+(define (vec3-copy vec3)
+  "Return a new vec3 that is a copy of VEC3."
+  (with-new-vec3 new
+    (vec3-copy! vec3 new)))
+
 (define-inlinable (vec2-magnitude v)
   "Return the magnitude of the vec2 V."
   (sqrt (+ (square (vec2-x v)) (square (vec2-y v)))))
 
+(define-inlinable (vec3-magnitude v)
+  "Return the magnitude of the vec3 V."
+  (sqrt (+ (square (vec3-x v))
+           (square (vec3-y v))
+           (square (vec3-z v)))))
+
 (define-inlinable (vec2-dot-product v1 v2)
   "Return the dot product of the vec2s V1 and V2."
   (+ (* (vec2-x v1) (vec2-x v2))
      (* (vec2-y v1) (vec2-y v2))))
 
+(define-inlinable (vec3-dot-product v1 v2)
+  "Return the dot product of the vec3s V1 and V2."
+  (+ (* (vec3-x v1) (vec3-x v2))
+     (* (vec3-y v1) (vec3-y v2))
+     (* (vec3-z v1) (vec3-z v2))))
+
 (define (vec2-normalize v)
   "Return the normalized form of the vec2 V."
   (with-new-vec2 new
     (vec2-copy! v new)
     (vec2-normalize! new)))
 
+(define (vec3-normalize v)
+  "Return the normalized form of the vec3 V."
+  (with-new-vec3 new
+    (vec3-copy! v new)
+    (vec3-normalize! new)))
+
 (define-inlinable (vec2-normalize! v)
   "Normalize the vec2 V in-place."
   (unless (and (zero? (vec2-x v)) (zero? (vec2-y v)))
@@ -123,6 +223,16 @@ polar coordinate (R, THETA)."
       (set-vec2-x! v (/ (vec2-x v) m))
       (set-vec2-y! v (/ (vec2-y v) m)))))
 
+(define-inlinable (vec3-normalize! v)
+  "Normalize the vec3 V in-place."
+  (unless (and (zero? (vec3-x v))
+               (zero? (vec3-y v))
+               (zero? (vec3-z v)))
+    (let ((m (vec3-magnitude v)))
+      (set-vec3-x! v (/ (vec3-x v) m))
+      (set-vec3-y! v (/ (vec3-y v) m))
+      (set-vec3-z! v (/ (vec3-z v) m)))))
+
 (define-inlinable (vec2-mult! v x)
   "Multiply the vec2 V by X, a real number or vec2."
   (if (real? x)
@@ -133,6 +243,18 @@ polar coordinate (R, THETA)."
         (set-vec2-x! v (* (vec2-x v) (vec2-x x)))
         (set-vec2-y! v (* (vec2-y v) (vec2-y x))))))
 
+(define-inlinable (vec3-mult! v x)
+  "Multiply the vec3 V by X, a real number or vec3."
+  (if (real? x)
+      (begin
+        (set-vec3-x! v (* (vec3-x v) x))
+        (set-vec3-y! v (* (vec3-y v) x))
+        (set-vec3-z! v (* (vec3-z v) x)))
+      (begin
+        (set-vec3-x! v (* (vec3-x v) (vec3-x x)))
+        (set-vec3-y! v (* (vec3-y v) (vec3-y x)))
+        (set-vec3-z! v (* (vec3-z v) (vec3-z x))))))
+
 (define-inlinable (vec2-add! v x)
   "Add X, a real number or vec2, to the vec2 V."
   (if (real? x)
@@ -143,6 +265,18 @@ polar coordinate (R, THETA)."
         (set-vec2-x! v (+ (vec2-x v) (vec2-x x)))
         (set-vec2-y! v (+ (vec2-y v) (vec2-y x))))))
 
+(define-inlinable (vec3-add! v x)
+  "Add X, a real number or vec3, to the vec3 V."
+  (if (real? x)
+      (begin
+        (set-vec3-x! v (+ (vec3-x v) x))
+        (set-vec3-y! v (+ (vec3-y v) x))
+        (set-vec3-z! v (+ (vec3-z v) x)))
+      (begin
+        (set-vec3-x! v (+ (vec3-x v) (vec3-x x)))
+        (set-vec3-y! v (+ (vec3-y v) (vec3-y x)))
+        (set-vec3-z! v (+ (vec3-z v) (vec3-z x))))))
+
 (define-inlinable (vec2-sub! v x)
   "Subtract X, a real number or vec2, from the vec2 V."
   (if (real? x)
@@ -152,3 +286,15 @@ polar coordinate (R, THETA)."
       (begin
         (set-vec2-x! v (- (vec2-x v) (vec2-x x)))
         (set-vec2-y! v (- (vec2-y v) (vec2-y x))))))
+
+(define-inlinable (vec3-sub! v x)
+  "Subtract X, a real number or vec3, from the vec3 V."
+  (if (real? x)
+      (begin
+        (set-vec3-x! v (- (vec3-x v) x))
+        (set-vec3-y! v (- (vec3-y v) x))
+        (set-vec3-z! v (- (vec3-z v) x)))
+      (begin
+        (set-vec3-x! v (- (vec3-x v) (vec3-x x)))
+        (set-vec3-y! v (- (vec3-y v) (vec3-y x)))
+        (set-vec3-z! v (- (vec3-z v) (vec3-z x))))))