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

;;-*-Lisp-*-
(in-package goal)

;; name: vector-h.gc
;; name in dgo: vector-h
;; dgos: ENGINE, GAME

#|@file
Changes:
- vector+!, vector-!, vector-dot, vector4-dot replaced with actual implementations.
|#

;; DECOMP BEGINS

;; Array of booleans, stored as bits.
(deftype bit-array (basic)
  ((length            int32)
   (allocated-length  int32)
   (_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) (arg0 int))
  "Allocate a new bit-array with room arg0 bits."
  (let ((v0-0 (object-new allocation type-to-make (+ (/ (logand -8 (+ arg0 7)) 8) -1 (-> type-to-make size)))))
    (set! (-> v0-0 length) arg0)
    (set! (-> v0-0 allocated-length) arg0)
    v0-0
    )
  )

(defmethod length ((obj bit-array))
  "Get the number of bits."
  (-> obj length)
  )

(defmethod asize-of ((obj bit-array))
  "Get the size in memory."
  (the-as int (+ (-> obj type size) (/ (logand -8 (+ (-> obj allocated-length) 7)) 8)))
  )

(defmethod get-bit ((obj bit-array) (arg0 int))
  "Get the nth bit as a boolean."
  (let ((v1-2 (-> obj bytes (/ arg0 8))))
    (logtest? v1-2 (ash 1 (logand arg0 7)))
    )
  )

(defmethod clear-bit ((obj bit-array) (arg0 int))
  "Set the nth bit to 0."
  (logclear! (-> obj bytes (/ arg0 8)) (ash 1 (logand arg0 7)))
  0
  )

(defmethod set-bit ((obj bit-array) (arg0 int))
  "Set the nth bit to 1."
  (logior! (-> obj bytes (/ arg0 8)) (ash 1 (logand arg0 7)))
  0
  )

(defmethod clear-all! ((obj bit-array))
  "Set all bits to 0."
  (countdown (v1-2 (/ (logand -8 (+ (-> obj allocated-length) 7)) 8))
    (nop!)
    (nop!)
    (set! (-> obj bytes v1-2) (the-as uint 0))
    )
  obj
  )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; vector types
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmacro init-vf0-vector ()
  "Initializes the VF0 vector which is a constant vector in the VU set to <0,0,0,1>"
  `(.lvf vf0 (new 'static 'vector :x 0.0 :y 0.0 :z 0.0 :w 1.0))
  )

;; generally named after number of elements.
;; u is unsigned, b = byte, h = halfword (16-bit), w = word (32-bit), d = doubleword (64-bit)
;; s = float (single precision)

;; BYTE vectors

(deftype vector16ub (structure)
  ((data  uint8    16)
   (quad  uint128  :overlay-at (-> data 0))
   )
  )

(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
  )

(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))
   (clr   int32  :overlay-at (-> data 0))
   )
  :pack-me
  )

(deftype vector2ub (structure)
  ((data  uint8   2)
   (x     uint8   :overlay-at (-> data 0))
   (y     uint8   :overlay-at (-> data 1))
   (clr   uint16  :overlay-at (-> data 0))
   )
  :pack-me
  )

(deftype vector2b (structure)
  ((data  int8   2)
   (x     int8   :overlay-at (-> data 0))
   (y     int8   :overlay-at (-> data 1))
   (clr   int16  :overlay-at (-> data 0))
   )
  )

;; HALFWORD vectors

(deftype vector2h (structure)
  ((data  int16  2)
   (x     int16  :overlay-at (-> data 0))
   (y     int16  :overlay-at (-> data 1))
   )
  :pack-me
  )

(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
  )

(deftype vector3h (structure)
  ((data  int16  3)
   (x     int16  :overlay-at (-> data 0))
   (y     int16  :overlay-at (-> data 1))
   (z     int16  :overlay-at (-> data 2))
   )
  )

(deftype vector3uh (structure)
  ((data  uint16  3)
   (x     uint16  :overlay-at (-> data 0))
   (y     uint16  :overlay-at (-> data 1))
   (z     uint16  :overlay-at (-> data 2))
   )
  )

;; WORD vectors

(deftype vector2w (structure)
  ((data  int32  2)
   (x     int32  :overlay-at (-> data 0))
   (y     int32  :overlay-at (-> data 1))
   )
  )

(deftype vector3w (structure)
  ((data  int32  3)
   (x     int32  :overlay-at (-> data 0))
   (y     int32  :overlay-at (-> data 1))
   (z     int32  :overlay-at (-> data 2))
   )
  )

(deftype vector4w (structure)
  ((data   int32    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))
   )
  )

;; FLOAT vectors

(deftype vector2 (structure)
  ((data  float  2)
   (x     float  :overlay-at (-> data 0))
   (y     float  :overlay-at (-> data 1))
   )
  :allow-misaligned
  )

(deftype vector3 (structure)
  ((data  float  3)
   (x     float  :overlay-at (-> data 0))
   (y     float  :overlay-at (-> data 1))
   (z     float  :overlay-at (-> data 2))
   )
  )

;; Note: typically vector is used instead of vector4.
;; vector usually means "xyz only, with w = 1", and vector4
;; is sometimes used when w is more meaningful.
(deftype vector4 (structure)
  ((data   float    4)
   (x      float      :overlay-at (-> data 0))
   (y      float      :overlay-at (-> data 1))
   (z      float      :overlay-at (-> data 2))
   (w      float      :overlay-at (-> data 3))
   (dword  uint64   2 :overlay-at (-> data 0))
   (quad   uint128    :overlay-at (-> data 0))
   )
  )


;; SPECIAL vectors

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

;; group of 2 vector4w's
(deftype vector4w-2 (structure)
  ((data    int32     8)
   (quad    uint128   2         :overlay-at (-> data 0))
   (vector  vector4w  2 :inline :overlay-at (-> data 0))
   )
  )


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

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

(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
  )

(deftype vector8h (structure)
  ((data  int16    8)
   (quad  uint128  :overlay-at (-> data 0))
   )
  )


(deftype vector16b (structure)
  ((data  int8     16)
   (quad  uint128  :overlay-at (-> data 0))
   )
  )


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

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

;; Constant Vectors

(define *null-vector* (new 'static 'vector :w 1.0))
(define *identity-vector* (new 'static 'vector :x 1.0 :y 1.0 :z 1.0 :w 1.0))
(define *x-vector* (new 'static 'vector :x 1.0 :w 1.0))
(define *y-vector* (new 'static 'vector :y 1.0 :w 1.0))
(define *z-vector* (new 'static 'vector :z 1.0 :w 1.0))
(define *up-vector* (new 'static 'vector :y 1.0 :w 1.0))

;; SPECIAL vector (of floats)

(deftype vector4s-3 (structure)
  ((data    float    12)
   (quad    uint128  3         :overlay-at (-> data 0))
   (vector  vector   3 :inline :overlay-at (-> data 0))
   )
  )

;; definition of type vector-array
(deftype vector-array (inline-array-class)
  ((data  vector  :inline :dynamic)
   )
  )
(set! (-> vector-array heap-base) (the-as uint 16))

(deftype rgbaf (vector)
  ((r  float  :overlay-at (-> data 0))
   (g  float  :overlay-at (-> data 1))
   (b  float  :overlay-at (-> data 2))
   (a  float  :overlay-at (-> data 3))
   )
  )

;; ax + by + cz = d form
(deftype plane (vector)
  ((a  float  :overlay-at (-> data 0))
   (b  float  :overlay-at (-> data 1))
   (c  float  :overlay-at (-> data 2))
   (d  float  :overlay-at (-> data 3))
   )
  )

(deftype sphere (vector)
  ((r  float  :overlay-at (-> data 3))
   )
  )

(deftype isphere (vec4s)
  ()
  )

(defmacro static-vector (x y z w)
  "creates a static vector."
  `(new 'static 'vector :x ,x :y ,y :z ,z :w ,w)
  )
(defmacro static-vectorm (x y z)
  "creates a static vector using meters. w is 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)
  "creates a static vector using meters where the w component is used as sphere radius. for a 'real' sphere use static-bspherem."
  `(new 'static 'vector :x (meters ,x) :y (meters ,y) :z (meters ,z) :w (meters ,r))
  )
(defmacro static-bspherem (x y z r)
  "creates a static sphere using meters."
  `(new 'static 'sphere :x (meters ,x) :y (meters ,y) :z (meters ,z) :w (meters ,r))
  )

;; box, stored as 8 floats (min/max along each dim, unused w's)
(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)
   )
  )
(set! (-> box8s-array heap-base) (the-as uint 32))

;; actually a capsule
(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 normal cylinder
(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)
    )
  )

(deftype vertical-planes (structure)
  ((data  uint128  4)
   )
  )

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

(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))
   )
  )

(deftype vector3s (structure)
  ((data  float  3)
   (x     float  :overlay-at (-> data 0))
   (y     float  :overlay-at (-> data 1))
   (z     float  :overlay-at (-> data 2))
   )
  :pack-me
  )

(define-extern vector-cross! (function vector vector vector vector))
(define-extern vector-float*! (function vector vector float vector))
(define-extern vector-identity! (function vector vector))
(define-extern vector-normalize! (function vector float vector))
(define-extern vector-normalize-copy! (function vector vector float vector))
(define-extern vector-length (function vector float))
(define-extern vector-length-squared (function vector float))
(define-extern vector-xz-normalize! (function vector float vector))
(define-extern vector-xz-length (function vector float))
(define-extern vector+float*! (function vector vector vector float vector))
(define-extern vector-vector-distance-squared (function vector vector float))
(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))

;; Macros and inline functions.
;; These are inlined for performance reasons

(defmacro set-vector! (v xv yv zv wv)
  "Set all fields in a vector"
  (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 xyz fields in a vector"
  (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))
  "Take the dot product of two vectors.
   Only does the x, y, z compoments.
   Originally handwritten assembly to space out loads and use FPU accumulator"
  (declare (inline))
  (let ((result 0.))
    (+! result (* (-> a x) (-> b x)))
    (+! result (* (-> a y) (-> b y)))
    (+! result (* (-> a z) (-> b z)))
    result
    )
  )

(defun vector-dot-vu ((arg0 vector) (arg1 vector))
  "Take the dot product (xyz only). Using VU0."
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 quad))
    (.mul.vf vf1 vf1 vf2)
    (.add.y.vf.x vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0
    )
  )

(defun vector4-dot ((a vector) (b vector))
  "Take the dot product of two vectors.
   Does the x, y, z, and w compoments"
  (declare (inline))
  (let ((result 0.))
    (+! result (* (-> a x) (-> b x)))
    (+! result (* (-> a y) (-> b y)))
    (+! result (* (-> a z) (-> b z)))
    (+! result (* (-> a w) (-> b w)))
    result
    )
  )

(defun vector4-dot-vu ((arg0 vector) (arg1 vector))
  "Take the dot product (xyzw). Using VU0."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 quad))
    (.mul.vf vf1 vf1 vf2)
    (.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 v0-0 vf1)
    v0-0
    )
  )

(defmacro print-vf (vf &key (name #f))
  "Print out a vf register as a vector."
  `(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 out a vf register as 4x 32-bit hexadecimal integers"
  `(let ((temp (new 'stack 'vector4w)))
     (.svf temp ,vf)
     (format #t "~`vector4w`P~%" temp)
     )
  )

(defmacro print-vf-dec (vf)
  "Print out a vf register as 4x 32-bit base-10 integers"
  `(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 vector+! ((dst vector) (a vector) (b vector))
  "Set dst = a + b. The w component of dst is set to 0."
  (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 = a - b. The w componenent of dst is set to 0."
  (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! ((dest vector))
  "Set xyzw to 0."
  (declare (inline))
  (rlet ((vf1 :class vf :reset-here #t))
    ;; set vf1 = 0
    (.xor.vf vf1 vf1 vf1)
    ;; store the 0
    (.svf dest vf1)
    )
  dest
  )

(defun vector-reset! ((dst vector))
  "Set vector to 0,0,0,1."
  (declare (inline))
  (vector-zero! dst)
  (set! (-> dst w) 1.0)
  dst
  )

(defun vector-copy! ((arg0 vector) (arg1 vector))
  "Copy arg1 to arg0."
  (declare (inline))
  (set! (-> arg0 quad) (-> arg1 quad))
  arg0
  )

(defun vector-length< ((arg0 vector) (arg1 float))
  (let ((f0-0 (vector-length-squared arg0))
        (f1-0 arg1)
        )
    (< f0-0 (* f1-0 f1-0))
    )
  )

(defun vector-length> ((arg0 vector) (arg1 float))
  (< (* arg1 arg1) (vector-length-squared arg0))
  )

(define *zero-vector* (new 'static 'vector))

(defmacro new-stack-vector0 ()
  "Get a stack vector that's set to 0.
   This is more efficient than (new 'stack 'vector) because
   this doesn't call the constructor."
  `(let ((vec (new 'stack-no-clear 'vector)))
     (set! (-> vec quad) (the-as uint128 0))
     vec
     )
  )