jak-project/goal_src/jak1/engine/math/vector-h.gc

;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "kernel/gcommon.gc")
(require "engine/util/types-h.gc")

;; Type definitions/inline functions for bit array and vector types.

;; DECOMP BEGINS

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; bit array
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; A dynamically sized, bit-addressable array. The first storage byte overlays _pad,
;; so allocation adds only the remaining bytes after the fixed type size.

(deftype bit-array (basic)
  ((length           int32)
   (allocated-length int32)
   ;; The generated inspector omits the overlaid storage fields.
   (_pad             uint8)
   (bytes            uint8 :dynamic :overlay-at _pad))
  (:methods
   (new (symbol type int) _type_)
   (get-bit (_type_ int) symbol)
   (clear-bit (_type_ int) int)
   (set-bit (_type_ int) int)
   (clear-all! (_type_) _type_)))

(defmethod new bit-array ((allocation symbol) (type-to-make type) (length int))
  "Allocate a bit-array for length addressable bits."
  (let ((this (object-new allocation type-to-make (+ (/ (logand -8 (+ length 7)) 8) -1 (-> type-to-make size)))))
    (set! (-> this length) length)
    (set! (-> this allocated-length) length)
    this))

(defmethod length ((this bit-array))
  "Return the active length in bits."
  (-> this length))

(defmethod asize-of ((this bit-array))
  "Return the nominal type size plus the rounded byte capacity. Because the dynamic bytes overlay _pad, this is one byte larger than the allocation."
  (the-as int (+ (-> this type size) (/ (logand -8 (+ (-> this allocated-length) 7)) 8))))

(defmethod get-bit ((this bit-array) (i int))
  "Return true when the bit at unchecked index i is set."
  (let ((byte (-> this bytes (/ i 8)))) (logtest? byte (ash 1 (logand i 7)))))

(defmethod clear-bit ((this bit-array) (i int))
  "Clear the bit at unchecked index i and return zero."
  (logclear! (-> this bytes (/ i 8)) (ash 1 (logand i 7)))
  0)

(defmethod set-bit ((this bit-array) (i int))
  "Set the bit at unchecked index i and return zero."
  (logior! (-> this bytes (/ i 8)) (ash 1 (logand i 7)))
  0)

(defmethod clear-all! ((this bit-array))
  "Clear every storage byte, including unused bits in the final byte, and return this array."
  (countdown (i (/ (logand -8 (+ (-> this allocated-length) 7)) 8))
    ;; Space the counter update from the byte-address calculation.
    (nop!)
    (nop!)
    (set! (-> this bytes i) (the-as uint 0)))
  this)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; vector types (integer)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmacro init-vf0-vector ()
  "Make the VU0 constant vector (0, 0, 0, 1) available to portable VU operations."
  `(.lvf vf0 (new 'static 'vector :x 0.0 :y 0.0 :z 0.0 :w 1.0)))

;; the GOAL vector types are structures, storing values in memory.

;; Vector of 4 unsigned bytes.
(deftype vector4ub (structure)
  ((data uint8 4)
   (x    uint8 :overlay-at (-> data 0))
   (y    uint8 :overlay-at (-> data 1))
   (z    uint8 :overlay-at (-> data 2))
   (w    uint8 :overlay-at (-> data 3))
   (clr  uint32 :overlay-at (-> data 0)))
  :pack-me)

;; Vector of 4 signed bytes.
(deftype vector4b (structure)
  ((data int8 4)
   (x    int8 :overlay-at (-> data 0))
   (y    int8 :overlay-at (-> data 1))
   (z    int8 :overlay-at (-> data 2))
   (w    int8 :overlay-at (-> data 3))))

;; Vector of 2 signed halfwords.
(deftype vector2h (structure)
  ((data int16 2)
   (x    int16 :overlay-at (-> data 0))
   (y    int16 :overlay-at (-> data 1)))
  :pack-me)

;; Vector of 2 unsigned halfwords.
(deftype vector2uh (structure)
  ((data uint16 2)
   (x    uint16 :overlay-at (-> data 0))
   (y    uint16 :overlay-at (-> data 1))
   (val  uint32 :overlay-at (-> data 0)))
  :pack-me)

;; Vector of 3 signed halfwords. data names only the first two; z follows them.
(deftype vector3h (structure)
  ((data int16 2)
   (x    int16 :overlay-at (-> data 0))
   (y    int16 :overlay-at (-> data 1))
   (z    int16)))

;; Vector of 2 signed words.
(deftype vector2w (structure)
  ((data int32 2)
   (x    int32 :overlay-at (-> data 0))
   (y    int32 :overlay-at (-> data 1)))
  :pack-me)

;; Vector of 3 signed words.
(deftype vector3w (structure)
  ((data int32 3)
   (x    int32 :overlay-at (-> data 0))
   (y    int32 :overlay-at (-> data 1))
   (z    int32 :overlay-at (-> data 2)))
  :allow-misaligned)

;; Vector of 4 signed words.
(deftype vector4w (structure)
  ((data  uint32 4)
   (x     int32 :overlay-at (-> data 0))
   (y     int32 :overlay-at (-> data 1))
   (z     int32 :overlay-at (-> data 2))
   (w     int32 :overlay-at (-> data 3))
   (dword uint64 2 :overlay-at (-> data 0))
   (quad  uint128 :overlay-at (-> data 0))))

(defmethod print ((this vector4w))
  (format #t "#<vector4w ~D ~D ~D ~D @ #x~X>" (-> this x) (-> this y) (-> this z) (-> this w) this)
  this)

;; Two vector4w values.
(deftype vector4w-2 (structure)
  ((data   int32 8)
   (quad   uint128 2 :overlay-at (-> data 0))
   (vector vector4w 2 :inline :overlay-at (-> data 0))))

;; Three vector4w values.
(deftype vector4w-3 (structure)
  ((data   int32 12)
   (quad   uint128 3 :overlay-at (-> data 0))
   (vector vector4w 3 :inline :overlay-at (-> data 0))))

;; Four vector4w values.
(deftype vector4w-4 (structure)
  ((data   int32 16)
   (quad   uint128 4 :overlay-at (-> data 0))
   (vector vector4w 4 :inline :overlay-at (-> data 0))))

;; Vector of 4 signed halfwords.
(deftype vector4h (structure)
  ((data int16 4)
   (x    int16 :overlay-at (-> data 0))
   (y    int16 :overlay-at (-> data 1))
   (z    int16 :overlay-at (-> data 2))
   (w    int16 :overlay-at (-> data 3))
   (long uint64 :overlay-at (-> data 0)))
  :pack-me)

;; Vector of 8 signed halfwords.
(deftype vector8h (structure)
  ((data int16 8)
   (quad uint128 :overlay-at (-> data 0))))

;; Vector of 16 signed bytes
(deftype vector16b (structure)
  ((data int8 16)
   (quad uint128 :overlay-at (-> data 0))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; vector types (floating point)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Vector of 4 floats. Shortened to "vector" because it is the most commonly used.
(deftype vector (structure)
  ((x    float :offset 0)
   (y    float :offset 4)
   (z    float :offset 8)
   (w    float :offset 12)
   (data float 4 :overlay-at x)
   (quad uint128 :overlay-at x)))

(defmethod inspect ((this vector))
  (format #t "[~8x] vector~%" this)
  (format #t "~T[~F] [~F] [~F] [~F]~%" (-> this x) (-> this y) (-> this z) (-> this w))
  this)

(defmethod print ((this vector))
  (format #t "#<vector ~F ~F ~F ~F @ #x~X>" (-> this x) (-> this y) (-> this z) (-> this w) this)
  this)

(define *null-vector* (new 'static 'vector :x 0. :y 0. :z 0. :w 1.))

(define *identity-vector* (new 'static 'vector :x 1. :y 1. :z 1. :w 1.))

(define *x-vector* (new 'static 'vector :x 1. :y 0. :z 0. :w 1.))

(define *y-vector* (new 'static 'vector :x 0. :y 1. :z 0. :w 1.))

(define *z-vector* (new 'static 'vector :x 0. :y 0. :z 1. :w 1.))

;; note: y is up.
(define *up-vector* (new 'static 'vector :x 0. :y 1. :z 0. :w 1.))

;; Three vectors in one 48-byte block.
(deftype vector4s-3 (structure)
  ((data   float 12)
   (quad   uint128 3 :overlay-at (-> data 0))
   (vector vector 3 :inline :overlay-at (-> data 0))))

(deftype vector-array (inline-array-class)
  ((data vector :inline :dynamic :offset 16)))

(set! (-> vector-array heap-base) 16)

(deftype rgbaf (vector)
  ((r float :overlay-at x)
   (g float :overlay-at y)
   (b float :overlay-at z)
   (a float :overlay-at w)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; other geometric things
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Plane in ax + by + cz = d form.
(deftype plane (vector)
  ((a float :overlay-at x)
   (b float :overlay-at y)
   (c float :overlay-at z)
   (d float :overlay-at w)))

;; x, y, and z are the center; r overlays w.
(deftype sphere (vector)
  ((r float :overlay-at w)))

(deftype isphere (vec4s) ())

(defmacro static-vector (x y z w)
  "Construct a static vector from four components."
  `(new 'static 'vector :x ,x :y ,y :z ,z :w ,w))

(defmacro static-vectorm (x y z)
  "Construct a static position vector from coordinates in meters, with w set to 1.0."
  `(new 'static 'vector :x (meters ,x) :y (meters ,y) :z (meters ,z) :w 1.0))

(defmacro static-spherem (x y z r)
  "Construct a static vector whose xyz center and w radius are specified in meters."
  `(new 'static 'vector :x (meters ,x) :y (meters ,y) :z (meters ,z) :w (meters ,r)))

(defmacro static-bspherem (x y z r)
  "Construct a static sphere whose center and radius are specified in meters."
  `(new 'static 'sphere :x (meters ,x) :y (meters ,y) :z (meters ,z) :w (meters ,r)))

;; A compact bounding box stored as minimum and maximum points.
(deftype box8s (structure)
  ((data   float 8)
   (quad   uint128 2 :overlay-at (-> data 0))
   (vector vector 2 :overlay-at (-> data 0))
   (min    vector :inline :overlay-at (-> data 0))
   (max    vector :inline :overlay-at (-> data 4))))

(deftype box8s-array (inline-array-class)
  ((data box8s :inline :dynamic :offset 16)))

(set! (-> box8s-array heap-base) 32)

;; A capsule: a cylinder with hemispherical ends. origin is the center of the first end, axis is a
;; unit vector toward the second end, and length is the distance between the two end centers.
(deftype cylinder (structure)
  ((origin vector :inline)
   (axis   vector :inline)
   (radius float)
   (length float))
  (:methods
   (debug-draw (_type_ vector4w) none)
   (ray-capsule-intersect (_type_ vector vector) float)))

;; A flat-ended cylinder. origin is the center of the first cap, axis is a unit vector toward the
;; second cap, and length is the distance between the cap centers.
(deftype cylinder-flat (structure)
  ((origin vector :inline)
   (axis   vector :inline)
   (radius float)
   (length float))
  (:methods
   (debug-draw (_type_ vector4w) none)
   (ray-flat-cyl-intersect (_type_ vector vector) float)))

;; Four vertical planes, used by the light-volume types.
(deftype vertical-planes (structure)
  ((data uint128 4)))

(deftype vertical-planes-array (basic)
  ((length uint32)
   (data   vertical-planes :inline :dynamic)))

;; Common 16-byte quadword storage with scalar, packed-integer, and vector views.
(deftype qword (structure)
  ((data     uint32 4)
   (byte     uint8 16 :overlay-at (-> data 0))
   (hword    uint16 8 :overlay-at (-> data 0))
   (word     uint32 4 :overlay-at (-> data 0))
   (dword    uint64 2 :overlay-at (-> data 0))
   (quad     uint128 :overlay-at (-> data 0))
   (vector   vector :inline :overlay-at (-> data 0))
   (vector4w vector4w :inline :overlay-at (-> data 0))))

;; Compact 12-byte, three-component floating-point vector.
(deftype vector3s (structure)
  ((data float 3)
   (x    float :overlay-at (-> data 0))
   (y    float :overlay-at (-> data 1))
   (z    float :overlay-at (-> data 2))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Macros and inline functions
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmacro set-vector! (v xv yv zv wv)
  "Set all four components of v and return v."
  (with-gensyms (vec)
    `(let ((,vec ,v)) (set! (-> ,vec x) ,xv) (set! (-> ,vec y) ,yv) (set! (-> ,vec z) ,zv) (set! (-> ,vec w) ,wv) ,vec)))

(defmacro set-vector-xyz! (v xv yv zv)
  "Set the xyz components of v, leave w unchanged, and return v."
  (with-gensyms (vec)
    `(let ((,vec ,v)) (set! (-> ,vec x) ,xv) (set! (-> ,vec y) ,yv) (set! (-> ,vec z) ,zv) ,vec)))

(defun vector-dot ((a vector) (b vector))
  "Return the xyz dot product using the EE scalar floating-point accumulator."
  (declare (inline))
  (#unless PC_PORT
    (rlet ((a-reg :reg a0)
           (b-reg :reg a1)
           (sum :reg f0)
           (a-y :reg f1)
           (a-z :reg f2)
           (b-x :reg f3)
           (b-y :reg f4)
           (b-z :reg f5)
           (result :reg v0))
      ;; Interleave the loads before using the EE floating-point accumulator.
      (l.s sum a-reg)
      (l.s a-y a-reg 4)
      (l.s a-z a-reg 8)
      (l.s b-x b-reg)
      (l.s b-y b-reg 4)
      (l.s b-z b-reg 8)
      (mula.s sum b-x)
      (madda.s a-y b-y)
      (madd.s sum a-z b-z)
      (m result sum)
      (j ra :delay (nop!))))
  (#when PC_PORT
    (let ((result 0.0))
      (+! result (* (-> a x) (-> b x)))
      (+! result (* (-> a y) (-> b y)))
      (+! result (* (-> a z) (-> b z)))
      result)))

(defun vector-dot-vu ((a vector) (b vector))
  "Return the xyz dot product using VU0 packed arithmetic."
  (declare (inline))
  (local-vars (result float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf))
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mul.vf vf1 vf1 vf2)
    (.add.y.vf.x vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov result vf1)
    result))

(defun vector4-dot ((a vector) (b vector))
  "Return the xyzw dot product using the EE scalar floating-point accumulator."
  (declare (inline))
  (#unless PC_PORT
    (rlet ((a-reg :reg a0)
           (b-reg :reg a1)
           (sum :reg f0)
           (a-y :reg f1)
           (a-z :reg f2)
           (a-w :reg f3)
           (b-x :reg f4)
           (b-y :reg f5)
           (b-z :reg f6)
           (b-w :reg f7)
           (result :reg v0))
      (l.s sum a-reg)
      (l.s a-y a-reg 4)
      (l.s a-z a-reg 8)
      (l.s a-w a-reg 12)
      (l.s b-x b-reg)
      (l.s b-y b-reg 4)
      (l.s b-z b-reg 8)
      (l.s b-w b-reg 12)
      (mula.s sum b-x)
      (madda.s a-y b-y)
      (madda.s a-z b-z)
      (madd.s sum a-w b-w)
      (m result sum)
      (j ra :delay (nop!))))
  (#when PC_PORT
    (let ((result 0.0))
      (+! result (* (-> a x) (-> b x)))
      (+! result (* (-> a y) (-> b y)))
      (+! result (* (-> a z) (-> b z)))
      (+! result (* (-> a w) (-> b w)))
      result)))

(defmacro print-vf (vf &key (name #f))
  "Print a VU vector register as four floating-point components."
  (with-gensyms (temp)
    `(let ((,temp (new 'stack 'vector)))
      (.svf ,temp ,vf)
      ,(if name `(format #t "~A: ~`vector`P~%" (quote ,name) ,temp) `(format #t "~`vector`P~%" ,temp)))))

(defmacro print-vf-hex (vf)
  "Print a VU vector register through vector4w's decimal printer, despite the historical name."
  (with-gensyms (temp)
    `(let ((,temp (new 'stack 'vector4w))) (.svf ,temp ,vf) (format #t "~`vector4w`P~%" ,temp))))

(defmacro print-vf-dec (vf)
  "Print a VU vector register as four 32-bit decimal integers."
  (with-gensyms (temp)
    `(let ((,temp (new 'stack 'vector4w)))
      (.svf ,temp ,vf)
      (format #t " ~d ~d ~d ~d~%" (-> ,temp data 0) (-> ,temp data 1) (-> ,temp data 2) (-> ,temp data 3)))))

(defun vector4-dot-vu ((a vector) (b vector))
  "Return the xyzw dot product using VU0 packed arithmetic."
  (declare (inline))
  (local-vars (result float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mul.vf vf1 vf1 vf2)
    ;; og:preserve-this Clear every host SIMD lane before the masked write; stale NaNs are very slow on x86.
    (.xor.vf vf3 vf3 vf3)
    ;; Use vf3.x = 1.0 as the other multiplicand while the accumulator sums the packed products.
    (.add.w.vf.x vf3 vf0 vf0)
    (.mul.x.vf.x acc vf3 vf1)
    (.add.mul.y.vf.x acc vf3 vf1 acc)
    (.add.mul.z.vf.x acc vf3 vf1 acc)
    (.add.mul.w.vf.x vf1 vf3 vf1 acc)
    (.mov result vf1)
    result))

(defun vector+! ((dst vector) (a vector) (b vector))
  "Set dst.xyz to a.xyz + b.xyz, set dst.w to 1.0, and return dst."
  (declare (inline))
  (rlet ((vf0 :class vf :reset-here #t)
         (vf1 :class vf :reset-here #t)
         (vf2 :class vf :reset-here #t)
         (vf3 :class vf :reset-here #t))
    ;; load vectors
    (.lvf vf2 a)
    (.lvf vf3 b)
    (init-vf0-vector)
    ;; add
    (.add.vf vf1 vf2 vf3)
    ;; set w = 1
    (.blend.vf.w vf1 vf1 vf0)
    ;; store
    (.svf dst vf1))
  dst)

(defun vector-! ((dst vector) (a vector) (b vector))
  "Set dst.xyz to a.xyz - b.xyz, set dst.w to 1.0, and return dst."
  (declare (inline))
  (rlet ((vf0 :class vf :reset-here #t)
         (vf1 :class vf :reset-here #t)
         (vf2 :class vf :reset-here #t)
         (vf3 :class vf :reset-here #t))
    ;; load vectors
    (.lvf vf2 a)
    (.lvf vf3 b)
    (init-vf0-vector)
    ;; subtract
    (.sub.vf vf1 vf2 vf3)
    ;; set w = 1
    (.blend.vf.w vf1 vf1 vf0)
    ;; store
    (.svf dst vf1))
  dst)

(defun vector-zero! ((dst vector))
  "Set all four components of dst to zero and return dst."
  (declare (inline))
  (#unless PC_PORT
    (set! (-> dst quad) (the-as uint128 0))
    dst)
  (#when PC_PORT
    (rlet ((value :class vf :reset-here #t)) (.xor.vf value value value) (.svf dst value))
    dst))

(defun vector-reset! ((dst vector))
  "Set dst to (0, 0, 0, 1) and return dst."
  (declare (inline))
  (#unless PC_PORT
    (rlet ((vf0 :class vf)) (init-vf0-vector) (.svf (&-> dst quad) vf0) dst))
  (#when PC_PORT
    (vector-zero! dst)
    (set! (-> dst w) 1.0)
    dst))

(defun vector-copy! ((dst vector) (src vector))
  "Copy all four components from src to dst as one aligned quadword and return dst."
  (declare (inline))
  (#unless PC_PORT
    (set! (-> dst quad) (-> src quad))
    dst)
  (#when PC_PORT
    (rlet ((value :class vf :reset-here #t)) (.lvf value src) (.svf dst value))
    dst))

(defmacro new-stack-vector0 ()
  "Allocate a zeroed stack vector without calling the constructor."
  (with-gensyms (vec)
    `(let ((,vec (new 'stack-no-clear 'vector))) (set! (-> ,vec quad) (the-as uint128 0)) ,vec)))

(defmacro print-vector4m (vec &key (dst #t))
  "Print all four vector components as distances in meters."
  (with-gensyms (value)
    `(let ((,value ,vec)) (format ,dst "~m ~m ~m ~m~%" (-> ,value x) (-> ,value y) (-> ,value z) (-> ,value w)))))

(define *zero-vector* (new 'static 'vector :x 0. :y 0. :z 0. :w 0.))

(define-extern vector-identity! (function vector vector))

(define-extern vector-length (function vector float))

(define-extern vector-xz-normalize! (function vector float vector))

(define-extern vector-xz-length (function vector float))

(defun-extern vector+float*! vector vector vector float vector)

(defun-extern vector-normalize! vector float vector)

(defun-extern vector-float*! vector vector float vector)

(define-extern vector-normalize-copy! (function vector vector float vector))

(define-extern vector-cross! (function vector vector vector vector))

(define-extern vector-negate! (function vector vector vector))

(define-extern vector-normalize-ret-len! (function vector float float))

(define-extern vector-vector-distance (function vector vector float))

(define-extern vector-vector-distance-squared (function vector vector float))