mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 11:26:18 -04:00
5eeaffcde0
Removes trailing whitespace from goal_src files, eventually the formatter will do this as well but it's not ready yet so this is a decent interim solution. A competent text editor will also do this / flag it for you.
1630 lines
54 KiB
Common Lisp
1630 lines
54 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
|
|
;; name: geometry.gc
|
|
;; name in dgo: geometry
|
|
;; dgos: GAME
|
|
|
|
;; DECOMP BEGINS
|
|
|
|
(defun vector-flatten! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Get the projection of src onto a plane with the given normal
|
|
The normal should have magnitude 1.0."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.mov.vf vf3 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf1 vf2)
|
|
(.outer.product.b.vf vf3 vf2 vf1 acc)
|
|
(.outer.product.a.vf acc vf2 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf2 acc)
|
|
(.svf (&-> arg0 quad) vf3)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun vector-reflect! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Reflect a vector off of a plane."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.mov.vf vf3 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf1 vf2)
|
|
(.outer.product.b.vf vf3 vf2 vf1 acc)
|
|
(.outer.product.a.vf acc vf2 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf2 acc)
|
|
(.add.vf acc vf3 vf3 :mask #b111)
|
|
(.sub.mul.w.vf vf3 vf1 vf0 acc :mask #b111)
|
|
(.svf (&-> arg0 quad) vf3)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun vector-reflect-flat! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"This is a weird one. It doesn't care about the value of src dot normal
|
|
and it effectively replaces the component of src normal to the plane with
|
|
the plane's normal. I think this requires src/normal to both be unit vectors
|
|
in order to make sense.
|
|
NOTE: src should point from positive halfspace to negative otherwise it
|
|
doesn't work."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.mov.vf vf3 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf1 vf2)
|
|
(.outer.product.b.vf vf3 vf2 vf1 acc)
|
|
(.outer.product.a.vf acc vf2 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf2 acc)
|
|
(.add.vf vf3 vf3 vf2 :mask #b111)
|
|
(.svf (&-> arg0 quad) vf3)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun vector-reflect-flat-above! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Not really a reflect. Same as flatten."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.mov.vf vf3 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf1 vf2)
|
|
(.outer.product.b.vf vf3 vf2 vf1 acc)
|
|
(.outer.product.a.vf acc vf2 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf2 acc)
|
|
(.svf (&-> arg0 quad) vf3)
|
|
(let* ((f0-0 (vector-length arg0))
|
|
(f1-1 (vector-dot arg0 arg2))
|
|
(f0-2 (- (* 0.02 f0-0) f1-1))
|
|
)
|
|
(vector+float*! arg0 arg0 arg2 (fmin 16384.0 (* 16.0 f0-2)))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun vector-reflect-flat-gravity! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
(let ((s4-0 (new 'stack-no-clear 'vector)))
|
|
(set! (-> s4-0 quad) (-> arg1 quad))
|
|
(vector-reflect-flat! arg0 s4-0 arg2)
|
|
;; og:preserve-this
|
|
(let* ((s2-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) s4-0 1.0)) ;; src normalized
|
|
(f28-0 (vector-length s4-0)) ;; src original len
|
|
(f30-0 (vector-length arg0)) ;; dst original len
|
|
(f0-1 (vector-dot s2-0 arg2)) ;; original (normalized) dot normal
|
|
(f1-1 (vector-dot arg3 arg2))
|
|
)
|
|
(when (and (< 0.0001 f1-1) (< 8192.0 f28-0))
|
|
(let ((f0-3 (- (/ f0-1 f1-1))))
|
|
(vector+float*! arg0 s4-0 arg3 (* f28-0 f0-3))
|
|
)
|
|
(vector+! arg0 arg0 arg2)
|
|
(vector-normalize! arg0 f30-0)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun vector-segment-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Compute the distance from a point to the closest point on the line segment.
|
|
arg0 is the point. arg1/arg2 are the endpoints of the line segment.
|
|
arg3 is an optional output closest point."
|
|
(local-vars (v0-0 float) (v1-0 float) (v1-1 float))
|
|
(rlet ((acc :class vf)
|
|
(Q :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
(vf8 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(nop!)
|
|
(.lvf vf3 (&-> arg1 quad))
|
|
(.lvf vf4 (&-> arg2 quad))
|
|
(.lvf vf5 (&-> arg0 quad))
|
|
(.sub.vf vf1 vf4 vf3)
|
|
(.sub.vf vf6 vf5 vf3)
|
|
(.mul.vf vf2 vf1 vf1)
|
|
(.mul.x.vf acc vf0 vf2 :mask #b1000)
|
|
(.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
|
|
(.add.mul.z.vf vf2 vf0 vf2 acc :mask #b1000)
|
|
(.sqrt.vf Q vf2 :ftf #b11)
|
|
(.wait.vf)
|
|
(.add.vf vf2 vf0 Q :mask #b1)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.div.vf Q vf0 vf2 :fsf #b11 :ftf #b0)
|
|
(.mov v1-0 vf2)
|
|
(let ((f2-0 v1-0))
|
|
(.wait.vf)
|
|
(.mul.vf vf1 vf1 Q)
|
|
(.mul.vf vf7 vf1 vf6)
|
|
(let ((f1-0 (the-as float 0.0)))
|
|
(.add.y.vf vf7 vf7 vf7 :mask #b1)
|
|
(.add.z.vf vf7 vf7 vf7 :mask #b1)
|
|
(.mov v1-1 vf7)
|
|
(let ((f0-0 v1-1))
|
|
(b! (< f0-0 f1-0) cfg-4 :likely-delay (set! f0-0 f1-0))
|
|
(b! (< f2-0 f0-0) cfg-4 :likely-delay (set! f0-0 f2-0))
|
|
(label cfg-4)
|
|
(let ((v1-2 f0-0))
|
|
(.mov vf7 v1-2)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(.mul.x.vf vf1 vf1 vf7)
|
|
(b! (= arg3 #f) cfg-6 :delay (.mov.vf vf8 vf0 :mask #b1000))
|
|
(.add.vf vf8 vf3 vf1 :mask #b111)
|
|
(.svf (&-> arg3 quad) vf8)
|
|
(label cfg-6)
|
|
(.sub.vf vf2 vf6 vf1)
|
|
(.mul.vf vf2 vf2 vf2)
|
|
(.mul.x.vf acc vf0 vf2 :mask #b1000)
|
|
(.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
|
|
(.add.mul.z.vf vf2 vf0 vf2 acc :mask #b1000)
|
|
(.sqrt.vf Q vf2 :ftf #b11)
|
|
(.wait.vf)
|
|
(.add.vf vf2 vf0 Q :mask #b1)
|
|
(.nop.vf)
|
|
(.mov v0-0 vf2)
|
|
v0-0
|
|
)
|
|
)
|
|
|
|
(defun vector-segment-xz-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
(let ((v1-0 (new 'stack-no-clear 'matrix)))
|
|
(set-vector! (-> v1-0 rvec) (-> arg0 x) 0.0 (-> arg0 z) 0.0)
|
|
(set-vector! (-> v1-0 uvec) (-> arg1 x) 0.0 (-> arg1 z) 1.0)
|
|
(set-vector! (-> v1-0 fvec) (-> arg2 x) 0.0 (-> arg2 z) 1.0)
|
|
(vector-segment-distance-point! (-> v1-0 rvec) (-> v1-0 uvec) (-> v1-0 fvec) arg3)
|
|
)
|
|
)
|
|
|
|
(defun vector-line-distance ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Weird function: given a point arg1, and an infinite line connecting arg2 and arg1, compute the distance
|
|
from arg0 to that line."
|
|
(let* ((a1-3 (vector-normalize! (vector-! (new-stack-vector0) arg2 arg1) 1.0))
|
|
(gp-1 (vector-! (new-stack-vector0) arg0 arg1))
|
|
(f0-1 (vector-dot a1-3 gp-1))
|
|
(v1-3 (vector-float*! (new-stack-vector0) a1-3 f0-1))
|
|
)
|
|
(vector-length (vector-! (new-stack-vector0) gp-1 v1-3))
|
|
)
|
|
)
|
|
|
|
(defun vector-line-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Same as above function, but returns the point on arg2/arg1 in arg3 (ignored if #f)"
|
|
(let* ((a1-3 (vector-normalize! (vector-! (new-stack-vector0) arg2 arg1) 1.0))
|
|
(s4-1 (vector-! (new-stack-vector0) arg0 arg1))
|
|
(f0-1 (vector-dot a1-3 s4-1))
|
|
(v1-4 (vector-float*! (new-stack-vector0) a1-3 f0-1))
|
|
)
|
|
(if arg3
|
|
(vector+! arg3 arg1 v1-4)
|
|
)
|
|
(vector-length (vector-! (new-stack-vector0) s4-1 v1-4))
|
|
)
|
|
)
|
|
|
|
(defun vector-line-xz-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
(let ((v1-0 (new 'stack-no-clear 'matrix)))
|
|
(set-vector! (-> v1-0 rvec) (-> arg0 x) 0.0 (-> arg0 z) 0.0)
|
|
(set-vector! (-> v1-0 uvec) (-> arg1 x) 0.0 (-> arg1 z) 1.0)
|
|
(set-vector! (-> v1-0 fvec) (-> arg2 x) 0.0 (-> arg2 z) 1.0)
|
|
(vector-line-distance-point! (-> v1-0 rvec) (-> v1-0 uvec) (-> v1-0 fvec) arg3)
|
|
)
|
|
)
|
|
|
|
(defun vector-segment-overlap ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Seems to compute (v1 - v0).dot(v2 - v1), but in a weird way."
|
|
(let* ((gp-1 (vector-! (new 'stack-no-clear 'vector) arg1 arg2))
|
|
(s5-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) gp-1 1.0))
|
|
)
|
|
;; og:preserve-this
|
|
;; this vector-line-distance-point! is totally unused.
|
|
(let ((a3-0 (new 'stack-no-clear 'vector)))
|
|
(vector-line-distance-point! arg0 arg1 arg2 a3-0)
|
|
)
|
|
(/ (vector-dot s5-0 (vector-! (new 'stack-no-clear 'vector) arg1 arg0)) (vector-length gp-1))
|
|
)
|
|
)
|
|
|
|
(defun line-sphere-intersection? ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Does [arg1, arg2] intersect sphere arg0?"
|
|
(let ((s5-0 (new 'stack-no-clear 'vector)))
|
|
(let ((s3-0 (new 'stack-no-clear 'vector))
|
|
(f30-0 0.0)
|
|
)
|
|
(vector-! s3-0 arg2 arg1)
|
|
(let ((f0-0 (vector-length-squared s3-0)))
|
|
(if (< 0.0 f0-0)
|
|
(set! f30-0 (/ 1.0 f0-0))
|
|
)
|
|
)
|
|
(let ((v1-6 (new 'stack-no-clear 'vector)))
|
|
(vector-! v1-6 arg0 arg1)
|
|
(let* ((f1-2 (* (vector-dot s3-0 v1-6) f30-0))
|
|
(f0-5 (fmax 0.0 (fmin 1.0 f1-2)))
|
|
)
|
|
(vector+float*! s5-0 arg1 s3-0 f0-5)
|
|
)
|
|
)
|
|
)
|
|
(let ((f0-6 (vector-vector-distance-squared s5-0 arg0))
|
|
(f1-4 (-> arg0 w))
|
|
)
|
|
(< f0-6 (* f1-4 f1-4))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun nearest-dist2-between-moving-points ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 float))
|
|
(let ((v1-0 (new 'stack-no-clear 'inline-array 'vector 2)))
|
|
(vector-! (-> v1-0 0) arg2 arg0)
|
|
(vector-! (-> v1-0 1) arg3 arg1)
|
|
(let ((f0-1 (vector-dot (-> v1-0 0) (-> v1-0 0)))
|
|
(f1-1 (vector-dot (-> v1-0 1) (-> v1-0 1)))
|
|
(f2-1 (vector-dot (-> v1-0 0) (-> v1-0 1)))
|
|
(f3-0 0.0)
|
|
)
|
|
(if (< 0.0 f1-1)
|
|
(set! f3-0 (fmax 0.0 (fmin (/ (- f2-1) f1-1) arg4)))
|
|
)
|
|
(+ f0-1 (* 2.0 f2-1 f3-0) (* f3-0 f3-0 f1-1))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun vector-orient-by-quat! ((arg0 vector) (arg1 vector) (arg2 quaternion))
|
|
"Rotate a vector by a quaternion."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg2 quad))
|
|
(.lvf vf6 (&-> arg1 quad))
|
|
(.add.vf vf5 vf1 vf1)
|
|
(.add.w.vf vf2 vf0 vf1 :mask #b1)
|
|
(.add.z.vf vf2 vf0 vf1 :mask #b10)
|
|
(.sub.y.vf vf2 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf2 vf0 vf0 :mask #b1000)
|
|
(.sub.z.vf vf3 vf0 vf1 :mask #b1)
|
|
(.add.w.vf vf3 vf0 vf1 :mask #b10)
|
|
(.add.x.vf vf3 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf3 vf0 vf0 :mask #b1000)
|
|
(.add.y.vf vf4 vf0 vf1 :mask #b1)
|
|
(.sub.x.vf vf4 vf0 vf1 :mask #b10)
|
|
(.add.w.vf vf4 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf4 vf0 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf5 vf2)
|
|
(.outer.product.b.vf vf2 vf2 vf5 acc)
|
|
(.outer.product.a.vf acc vf5 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf5 acc)
|
|
(.outer.product.a.vf acc vf5 vf4)
|
|
(.outer.product.b.vf vf4 vf4 vf5 acc)
|
|
(.add.w.vf vf2 vf2 vf0 :mask #b1)
|
|
(.add.w.vf vf3 vf3 vf0 :mask #b10)
|
|
(.add.w.vf vf4 vf4 vf0 :mask #b100)
|
|
(.mul.w.vf acc vf0 vf6)
|
|
(.add.mul.x.vf acc vf2 vf6 acc)
|
|
(.add.mul.y.vf acc vf3 vf6 acc)
|
|
(.add.mul.z.vf vf6 vf4 vf6 acc)
|
|
(.svf (&-> arg0 quad) vf6)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
;; ERROR: Bad vector register dependency: vf7
|
|
(defun vector-inv-orient-by-quat! ((arg0 vector) (arg1 vector) (arg2 quaternion))
|
|
"Rotate a vector by the inverse rotation."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
;; og:preserve-this
|
|
;; (.sub.vf vf7 vf7 vf7)
|
|
(.xor.vf vf7 vf7 vf7)
|
|
(.lvf vf1 (&-> arg2 quad))
|
|
(.lvf vf6 (&-> arg1 quad))
|
|
(.sub.vf vf1 vf7 vf1 :mask #b111)
|
|
(.add.vf vf5 vf1 vf1)
|
|
(.add.w.vf vf2 vf0 vf1 :mask #b1)
|
|
(.add.z.vf vf2 vf0 vf1 :mask #b10)
|
|
(.sub.y.vf vf2 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf2 vf0 vf0 :mask #b1000)
|
|
(.sub.z.vf vf3 vf0 vf1 :mask #b1)
|
|
(.add.w.vf vf3 vf0 vf1 :mask #b10)
|
|
(.add.x.vf vf3 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf3 vf0 vf0 :mask #b1000)
|
|
(.add.y.vf vf4 vf0 vf1 :mask #b1)
|
|
(.sub.x.vf vf4 vf0 vf1 :mask #b10)
|
|
(.add.w.vf vf4 vf0 vf1 :mask #b100)
|
|
(.sub.w.vf vf4 vf0 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf5 vf2)
|
|
(.outer.product.b.vf vf2 vf2 vf5 acc)
|
|
(.outer.product.a.vf acc vf5 vf3)
|
|
(.outer.product.b.vf vf3 vf3 vf5 acc)
|
|
(.outer.product.a.vf acc vf5 vf4)
|
|
(.outer.product.b.vf vf4 vf4 vf5 acc)
|
|
(.add.w.vf vf2 vf2 vf0 :mask #b1)
|
|
(.add.w.vf vf3 vf3 vf0 :mask #b10)
|
|
(.add.w.vf vf4 vf4 vf0 :mask #b100)
|
|
(.mul.w.vf acc vf0 vf6)
|
|
(.add.mul.x.vf acc vf2 vf6 acc)
|
|
(.add.mul.y.vf acc vf3 vf6 acc)
|
|
(.add.mul.z.vf vf6 vf4 vf6 acc)
|
|
(.svf (&-> arg0 quad) vf6)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
;; The "forward down" function take a direction for forward (+z) and down (-y)
|
|
;; and convert to a transform.
|
|
;; Note that the normal functions take their pitch from the forward vector, but
|
|
;; the "nopitch" ones use the pitch from the up/down. Of course, if you are
|
|
;; consistent and provide orthogonal forward/down, they do the same thing.
|
|
|
|
(defun forward-down->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
|
|
"Create a matrix representing an inverse transform where arg1 is forward (+z)
|
|
and arg2 is down (-y). Will have the pitch of forward."
|
|
(vector-normalize-copy! (-> arg0 fvec) arg1 1.0)
|
|
(vector-cross! (-> arg0 rvec) (-> arg0 fvec) arg2)
|
|
(vector-normalize! (-> arg0 rvec) 1.0)
|
|
(vector-cross! (-> arg0 uvec) arg1 (-> arg0 rvec))
|
|
(vector-normalize! (-> arg0 uvec) 1.0)
|
|
(set! (-> arg0 trans quad) (the-as uint128 0))
|
|
(set! (-> arg0 rvec w) 0.0)
|
|
(set! (-> arg0 uvec w) 0.0)
|
|
(set! (-> arg0 fvec w) 0.0)
|
|
(set! (-> arg0 trans w) 1.0)
|
|
arg0
|
|
)
|
|
|
|
(defun forward-down-nopitch->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
|
|
"Create a matrix representing an inverse transform where arg1 is forward (+z)
|
|
and arg2 is down (-y). Will not use the pitch of forward."
|
|
(vector-normalize-copy! (-> arg0 uvec) arg2 1.0)
|
|
(vector-negate! (-> arg0 uvec) (-> arg0 uvec))
|
|
(vector-cross! (-> arg0 rvec) (-> arg0 uvec) arg1)
|
|
(vector-normalize! (-> arg0 rvec) 1.0)
|
|
(vector-cross! (-> arg0 fvec) (-> arg0 rvec) (-> arg0 uvec))
|
|
(vector-normalize! (-> arg0 fvec) 1.0)
|
|
(set! (-> arg0 trans quad) (the-as uint128 0))
|
|
(set! (-> arg0 rvec w) 0.0)
|
|
(set! (-> arg0 uvec w) 0.0)
|
|
(set! (-> arg0 fvec w) 0.0)
|
|
(set! (-> arg0 trans w) 1.0)
|
|
arg0
|
|
)
|
|
|
|
(defun forward-up->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
|
|
"Create a matrix representing an inverse transform where arg1 is forward (+z)
|
|
and arg2 is up (+y). Will use the pitch of forward."
|
|
(forward-down->inv-matrix arg0 arg1 (vector-negate! (new 'stack-no-clear 'vector) arg2))
|
|
)
|
|
|
|
(defun forward-up-nopitch->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
|
|
"Create a matrix representing an inverse transform where arg1 is forward (+z)
|
|
and arg2 is up (+y). Will not use the pitch of forward."
|
|
(forward-down-nopitch->inv-matrix arg0 arg1 (vector-negate! (new-stack-vector0) arg2))
|
|
)
|
|
|
|
(defun forward-up-nopitch->quaternion ((arg0 quaternion) (arg1 vector) (arg2 vector))
|
|
"Create a quaternion representing a transform where arg1 is forward (+z)
|
|
and arg2 is up (+y). Will not use the pitch of forward."
|
|
;; og:preserve-this
|
|
(matrix->quaternion arg0 (forward-up-nopitch->inv-matrix (new-stack-matrix0) arg1 arg2))
|
|
)
|
|
|
|
(defun forward-up->quaternion ((arg0 quaternion) (arg1 vector) (arg2 vector))
|
|
"Create a quaternion representing a transform where arg1 is forward (+z)
|
|
and arg2 is up (+y). Will use the pitch of forward."
|
|
;; og:preserve-this
|
|
(matrix->quaternion
|
|
arg0
|
|
(forward-down->inv-matrix (new-stack-matrix0) arg1 (vector-negate! (new 'stack-no-clear 'vector) arg2))
|
|
)
|
|
)
|
|
|
|
(defun quaternion-from-two-vectors! ((arg0 quaternion) (arg1 vector) (arg2 vector))
|
|
"Create a quaternion representing the rotation between two vectors."
|
|
(let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
|
|
(f1-0 (vector-length s5-0))
|
|
(f0-1 (vector-dot arg1 arg2))
|
|
)
|
|
(let ((f1-1 (/ (sqrtf (* 0.5 (- 1.0 f0-1))) f1-0)))
|
|
(set! (-> arg0 x) (* (-> s5-0 x) f1-1))
|
|
(set! (-> arg0 y) (* (-> s5-0 y) f1-1))
|
|
(set! (-> arg0 z) (* (-> s5-0 z) f1-1))
|
|
)
|
|
(set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f0-1))))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun quaternion-from-two-vectors-partial! ((arg0 quaternion) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Create a quaternion representing the rotation between two vectors,
|
|
doing arg3 fraction of the total rotation."
|
|
(let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
|
|
(f0-0 (vector-length s5-0))
|
|
(f1-1 (+ 1.0 (* arg3 (+ -1.0 (vector-dot arg1 arg2)))))
|
|
)
|
|
(let ((f0-1 (/ (sqrtf (* 0.5 (- 1.0 f1-1))) f0-0)))
|
|
(set! (-> arg0 x) (* (-> s5-0 x) f0-1))
|
|
(set! (-> arg0 y) (* (-> s5-0 y) f0-1))
|
|
(set! (-> arg0 z) (* (-> s5-0 z) f0-1))
|
|
)
|
|
(set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f1-1))))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun quaternion-from-two-vectors-max-angle! ((arg0 quaternion) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Create a quaternion representing the rotation between two vectors,
|
|
allowing at most a rotation of arg3 degrees."
|
|
(let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
|
|
(f30-0 (vector-length s5-0))
|
|
(f0-1 (fmax (cos arg3) (vector-dot arg1 arg2)))
|
|
)
|
|
(let ((f1-5 (/ (sqrtf (* 0.5 (- 1.0 f0-1))) f30-0)))
|
|
(set! (-> arg0 x) (* (-> s5-0 x) f1-5))
|
|
(set! (-> arg0 y) (* (-> s5-0 y) f1-5))
|
|
(set! (-> arg0 z) (* (-> s5-0 z) f1-5))
|
|
)
|
|
(set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f0-1))))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun quaternion-from-two-vectors-max-angle-partial! ((arg0 quaternion) (arg1 vector) (arg2 vector) (arg3 float) (arg4 float))
|
|
"Create a quaternion representing the arg4 fraction of the rotation between two vectors,
|
|
allowing at most a rotation of arg3 degrees."
|
|
(let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
|
|
(f30-0 (vector-length s5-0))
|
|
(f0-1 (fmax (cos arg3) (+ 1.0 (* arg4 (+ -1.0 (vector-dot arg1 arg2))))))
|
|
)
|
|
(let ((f1-5 (/ (sqrtf (* 0.5 (- 1.0 f0-1))) f30-0)))
|
|
(set! (-> arg0 x) (* (-> s5-0 x) f1-5))
|
|
(set! (-> arg0 y) (* (-> s5-0 y) f1-5))
|
|
(set! (-> arg0 z) (* (-> s5-0 z) f1-5))
|
|
)
|
|
(set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f0-1))))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun matrix-from-two-vectors! ((arg0 matrix) (arg1 vector) (arg2 vector))
|
|
"Create a rotation matrix representing the rotation between two vectors."
|
|
(let* ((a1-3 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
|
|
(f0-1 (vector-dot arg1 arg2))
|
|
(f1-0 1.0)
|
|
(f2-0 f0-1)
|
|
(f1-2 (sqrtf (- f1-0 (* f2-0 f2-0))))
|
|
)
|
|
(matrix-axis-sin-cos! arg0 a1-3 f1-2 f0-1)
|
|
)
|
|
)
|
|
|
|
(defun matrix-from-two-vectors-max-angle! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Create a rotation matrix representing the rotation between two vectors,
|
|
allowing at most a rotation of arg3 degrees."
|
|
(let ((s4-1 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
|
|
(f30-0 (vector-dot arg1 arg2))
|
|
(f28-0 (cos arg3))
|
|
)
|
|
(cond
|
|
((< f30-0 f28-0)
|
|
(matrix-axis-sin-cos! arg0 s4-1 (sin arg3) f28-0)
|
|
)
|
|
(else
|
|
(let ((t9-5 matrix-axis-sin-cos!)
|
|
(a0-6 arg0)
|
|
(a1-4 s4-1)
|
|
(f0-1 1.0)
|
|
(f1-0 f30-0)
|
|
)
|
|
(t9-5 a0-6 a1-4 (sqrtf (- f0-1 (* f1-0 f1-0))) f30-0)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
;; og:preserve-this
|
|
;; hack for the smoothed matrix from vectors to handle cases where they accidentally set turnvf to 0.
|
|
;; this causes the smoothing to go away on ps2, due to some behavior of inf/nan that differ.
|
|
;; if we cheat this to a small but nonzero value, it behaves like on ps2.
|
|
(defmacro int-to-float-nonzero-hack (f)
|
|
`(if (= ,f 0)
|
|
0.00000000001
|
|
(the float ,f)
|
|
)
|
|
)
|
|
|
|
(defun matrix-from-two-vectors-smooth! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float) (arg4 int))
|
|
"This function can help smoothly rotate from a current heading vector to a target one.
|
|
It returns a rotation to move arg1 closer to arg2, subject to two different speed limits.
|
|
arg3 is a rotations-per-frame rate. This limit takes frame rate into account (when lagging, the rotation is larger)
|
|
arg4 is a 'slow down when getting close to the end' limit.
|
|
This is used in rotate-toward-orientation, which is much improved from jak 1."
|
|
(let* ((s5-1 (vector-normalize! (vector-cross! (new 'stack-no-clear 'vector) arg2 arg1) 1.0))
|
|
(f0-1 (vector-dot arg1 arg2))
|
|
(f0-2 (acos f0-1))
|
|
;; og:preserve-this
|
|
(f1-2 (fmin (* arg3 (seconds-per-frame)) (/ (* 5.0 (fabs f0-2)) (int-to-float-nonzero-hack arg4))))
|
|
(f30-0 (fmax (fmin f0-2 f1-2) (- f1-2)))
|
|
)
|
|
(matrix-axis-sin-cos! arg0 s5-1 (sin f30-0) (cos f30-0))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun matrix-from-two-vectors-the-long-way-smooth! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float) (arg4 int))
|
|
"Same as above, but rotates you away from the target.
|
|
Note that the 'near the end' smoothing will apply when you're near the target."
|
|
(let* ((s5-1 (vector-normalize! (vector-cross! (new 'stack-no-clear 'vector) arg2 arg1) 1.0))
|
|
(f0-1 (vector-dot arg1 arg2))
|
|
(f0-3 (- (acos f0-1)))
|
|
;; og:preserve-this
|
|
(f1-2 (fmin (* arg3 (seconds-per-frame)) (/ (* 5.0 (fabs f0-3)) (int-to-float-nonzero-hack arg4))))
|
|
(f30-0 (fmax (fmin f0-3 f1-2) (- f1-2)))
|
|
)
|
|
(matrix-axis-sin-cos! arg0 s5-1 (sin f30-0) (cos f30-0))
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun quaternion-from-two-vectors-smooth! ((arg0 quaternion) (arg1 vector) (arg2 vector) (arg3 float) (arg4 int))
|
|
"Same as above, but returns a quaternion."
|
|
(let ((a1-1 (matrix-from-two-vectors-smooth! (new 'stack-no-clear 'matrix) arg1 arg2 arg3 arg4)))
|
|
(matrix->quaternion arg0 a1-1)
|
|
)
|
|
)
|
|
|
|
(defun matrix-from-two-vectors-max-angle-partial! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float) (arg4 float))
|
|
"Create a rotation matrix representing the given fraction of the rotation between two heading vectors,
|
|
rotating by at most the given angle."
|
|
(let* ((s4-1 (vector-normalize! (vector-cross! (new 'stack-no-clear 'vector) arg2 arg1) 1.0))
|
|
(f28-0 (vector-dot arg1 arg2))
|
|
(f30-0 (cos arg3))
|
|
(f0-2 (+ 1.0 (* (+ -1.0 f28-0) arg4)))
|
|
)
|
|
(cond
|
|
((< f0-2 f30-0)
|
|
(matrix-axis-sin-cos! arg0 s4-1 (sin arg3) f30-0)
|
|
)
|
|
(else
|
|
(let ((t9-5 matrix-axis-sin-cos!)
|
|
(a0-6 arg0)
|
|
(a1-4 s4-1)
|
|
(f1-3 1.0)
|
|
(f2-1 f0-2)
|
|
)
|
|
(t9-5 a0-6 a1-4 (sqrtf (- f1-3 (* f2-1 f2-1))) f0-2)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun matrix-from-two-vectors-partial-linear! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Create a rotation matrix representing doing arg3 fraction of the rotation between two vectors."
|
|
(let ((gp-1 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
|
|
(f0-1 (vector-dot arg1 arg2))
|
|
)
|
|
(cond
|
|
((< 0.9999 (fabs f0-1))
|
|
(matrix-identity! arg0)
|
|
)
|
|
(else
|
|
(let* ((f0-4 (cos (* arg3 (acos f0-1))))
|
|
(t9-5 matrix-axis-sin-cos!)
|
|
(a0-6 arg0)
|
|
(f1-1 1.0)
|
|
(f2-1 f0-4)
|
|
)
|
|
(t9-5 a0-6 gp-1 (sqrtf (- f1-1 (* f2-1 f2-1))) f0-4)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun matrix-remove-z-rot ((arg0 matrix) (arg1 vector))
|
|
"Remove the z rotation component of a rotation."
|
|
(let ((s4-0 (new-stack-vector0)))
|
|
0.0
|
|
0.0
|
|
(let ((s5-0 (new-stack-matrix0)))
|
|
(vector-negate! s4-0 arg1)
|
|
(vector-flatten! s4-0 s4-0 (-> arg0 vector 2))
|
|
(vector-normalize! s4-0 1.0)
|
|
(let ((f30-0 (vector-dot (-> arg0 vector 1) s4-0)))
|
|
(when (< f30-0 0.99999)
|
|
(vector-cross! s4-0 (-> arg0 vector 1) s4-0)
|
|
(let ((f0-4 (vector-length s4-0)))
|
|
(if (< 0.0 (vector-dot s4-0 (-> arg0 vector 2)))
|
|
(set! f0-4 (- f0-4))
|
|
)
|
|
(matrix-axis-sin-cos! s5-0 (-> arg0 vector 2) f0-4 f30-0)
|
|
)
|
|
(matrix*! arg0 arg0 s5-0)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun matrix-rot-diff! ((arg0 vector) (arg1 matrix) (arg2 matrix))
|
|
"Get the difference of rotation between two matrices, expressed as a quaternion."
|
|
(let ((s3-0 (new-stack-quaternion0))
|
|
(s2-0 (new-stack-quaternion0))
|
|
(s5-0 (new-stack-quaternion0))
|
|
)
|
|
0.0
|
|
(matrix->quaternion s3-0 arg1)
|
|
(matrix->quaternion s2-0 arg2)
|
|
(quaternion-conjugate! s5-0 s3-0)
|
|
(quaternion*! s5-0 s2-0 s5-0)
|
|
(quaternion-normalize! s5-0)
|
|
(if (< (-> s5-0 w) 0.0)
|
|
(quaternion-negate! s5-0 s5-0)
|
|
)
|
|
(let ((f30-1 (* 2.0 (acos (-> s5-0 w)))))
|
|
(set! (-> arg0 quad) (-> s5-0 quad))
|
|
(vector-negate! arg0 arg0)
|
|
(if (= (vector-normalize-ret-len! arg0 1.0) 0.0)
|
|
(set! (-> arg0 y) 1.0)
|
|
)
|
|
f30-1
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun quaternion-seek ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float) (arg4 float))
|
|
"Strange quaternion rotate toward function. Arg3 is ignored. Arg4 is the max seek amount."
|
|
(let ((s5-0 (new-stack-matrix0))
|
|
(s4-0 (new-stack-matrix0))
|
|
)
|
|
(quaternion->matrix s5-0 arg1)
|
|
(quaternion->matrix s4-0 arg2)
|
|
(let ((s2-1 (new-stack-quaternion0)))
|
|
(quaternion-from-two-vectors-max-angle! s2-1 (-> s5-0 vector 2) (-> s4-0 vector 2) arg4)
|
|
(quaternion-normalize! (quaternion*! arg0 arg0 s2-1))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun vector-deg-seek ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Make one vector closer to another, doing at most a rotation by arg3 degrees."
|
|
(let ((s4-0 (new-stack-matrix0)))
|
|
(matrix-from-two-vectors-max-angle! s4-0 arg1 arg2 arg3)
|
|
(vector-matrix*! arg0 arg1 s4-0)
|
|
)
|
|
)
|
|
|
|
(defun vector-deg-slerp ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
|
|
"Slerp for vectors. (imagine that they are the z axis of two frames)"
|
|
(cond
|
|
((>= 0.0 arg3)
|
|
(set! (-> arg0 quad) (-> arg1 quad))
|
|
arg0
|
|
)
|
|
((>= arg3 1.0)
|
|
(set! (-> arg0 quad) (-> arg2 quad))
|
|
arg0
|
|
)
|
|
(else
|
|
(let ((s1-0 (new-stack-matrix0)))
|
|
(let ((s2-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg1 1.0))
|
|
(a2-3 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg2 1.0))
|
|
)
|
|
(matrix-from-two-vectors-partial-linear! s1-0 s2-0 a2-3 arg3)
|
|
)
|
|
(vector-matrix*! arg0 arg1 s1-0)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun vector-vector-deg-slerp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float) (arg4 vector))
|
|
"Unused. No clue what this does."
|
|
(local-vars (sv-112 (function float float float float)))
|
|
(cond
|
|
((>= 0.0 arg3)
|
|
(set! (-> arg0 quad) (-> arg1 quad))
|
|
)
|
|
((>= arg3 1.0)
|
|
(set! (-> arg0 quad) (-> arg2 quad))
|
|
)
|
|
(else
|
|
(let* ((s0-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg1 1.0))
|
|
(s1-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg2 1.0))
|
|
(s0-1 (forward-up->quaternion (new 'stack-no-clear 'quaternion) s0-0 arg4))
|
|
(a2-5 (forward-up->quaternion (new 'stack-no-clear 'quaternion) s1-0 arg4))
|
|
(a1-6 (quaternion-slerp! (new 'stack-no-clear 'quaternion) s0-1 a2-5 arg3))
|
|
(s2-1 vector-normalize-copy!)
|
|
(s1-1 arg0)
|
|
(s0-2 (vector-z-quaternion! (new 'stack-no-clear 'vector) a1-6))
|
|
)
|
|
(set! sv-112 lerp)
|
|
(let ((s3-1 (vector-length arg1))
|
|
(a1-7 (vector-length arg2))
|
|
)
|
|
(s2-1 s1-1 s0-2 (sv-112 s3-1 a1-7 arg3))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun normal-of-plane ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Given three points on a plane, compute the plane's normal."
|
|
(rlet ((acc :class vf)
|
|
(Q :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf3 (&-> arg2 quad))
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg3 quad))
|
|
(.sub.vf vf1 vf3 vf1)
|
|
(.sub.vf vf2 vf3 vf2)
|
|
(.outer.product.a.vf acc vf2 vf1)
|
|
(.outer.product.b.vf vf4 vf1 vf2 acc)
|
|
(.mul.vf vf5 vf4 vf4)
|
|
(.add.y.vf vf5 vf5 vf5 :mask #b1)
|
|
(.add.z.vf vf5 vf5 vf5 :mask #b1)
|
|
(.isqrt.vf Q vf0 vf5 :fsf #b11 :ftf #b0)
|
|
(.mov.vf vf4 vf0 :mask #b1000)
|
|
(.wait.vf)
|
|
(.mul.vf vf4 vf4 Q :mask #b111)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.svf (&-> arg0 quad) vf4)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun vector-3pt-cross! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Cross product of 2 - 1 and 3 - 1. (will give a normal to the plane, but not of magnitude 1)"
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.lvf vf3 (&-> arg3 quad))
|
|
(.add.x.vf vf4 vf0 vf0 :mask #b1000)
|
|
(.sub.vf vf2 vf2 vf1)
|
|
(.sub.vf vf3 vf3 vf1)
|
|
(.outer.product.a.vf acc vf2 vf3)
|
|
(.outer.product.b.vf vf4 vf3 vf2 acc)
|
|
(.svf (&-> arg0 quad) vf4)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun closest-pt-in-triangle ((arg0 vector) (arg1 vector) (arg2 matrix) (arg3 vector))
|
|
"arg2 is the vertices of the triangle, arg3 is the normal, arg1 is the input point, arg0 is the output."
|
|
;; (declare (print-asm))
|
|
(local-vars
|
|
;; og:preserve-this float -> uint
|
|
(v1-0 uint)
|
|
(v1-4 uint)
|
|
(v1-5 uint)
|
|
(v1-6 uint)
|
|
(v1-7 uint)
|
|
(v1-10 uint)
|
|
;; og:preserve-this float -> uint
|
|
(a0-1 uint)
|
|
(a1-1 uint)
|
|
)
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf10 :class vf)
|
|
(vf11 :class vf)
|
|
(vf12 :class vf)
|
|
(vf13 :class vf)
|
|
(vf14 :class vf)
|
|
(vf15 :class vf)
|
|
(vf16 :class vf)
|
|
(vf17 :class vf)
|
|
(vf18 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
(vf8 :class vf)
|
|
(vf9 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(nop!)
|
|
(nop!)
|
|
(.lvf vf3 (&-> arg2 quad 1))
|
|
(nop!)
|
|
(.lvf vf4 (&-> arg2 quad 2))
|
|
(nop!)
|
|
(.lvf vf5 (&-> arg1 quad))
|
|
(.sub.vf vf6 vf3 vf4)
|
|
(.lvf vf2 (&-> arg2 quad 0))
|
|
(.sub.vf vf7 vf3 vf5)
|
|
(.lvf vf1 (&-> arg3 quad))
|
|
(.sub.vf vf8 vf3 vf2)
|
|
(.sub.vf vf9 vf5 vf4)
|
|
(.sub.vf vf10 vf5 vf2)
|
|
(.outer.product.a.vf acc vf7 vf8)
|
|
(.outer.product.b.vf vf14 vf8 vf7 acc)
|
|
(.outer.product.a.vf acc vf6 vf7)
|
|
(.outer.product.b.vf vf15 vf7 vf6 acc)
|
|
(.mul.vf vf11 vf14 vf1)
|
|
(.outer.product.a.vf acc vf9 vf10)
|
|
(.outer.product.b.vf vf16 vf10 vf9 acc)
|
|
(.mul.vf vf12 vf15 vf1)
|
|
(.add.x.vf vf11 vf11 vf11 :mask #b10)
|
|
(.mul.vf vf13 vf16 vf1)
|
|
(.add.x.vf vf12 vf12 vf12 :mask #b10)
|
|
(.add.x.vf vf13 vf13 vf13 :mask #b10)
|
|
(.add.z.vf vf11 vf11 vf11 :mask #b10)
|
|
(.add.z.vf vf12 vf12 vf12 :mask #b10)
|
|
(.add.z.vf vf13 vf13 vf13 :mask #b10)
|
|
;; og:preserve-this these types were changed to uint to make this copy 64 bits.
|
|
(.mov v1-0 vf11)
|
|
(.mov a1-1 vf12)
|
|
(.mov a0-1 vf13)
|
|
(let* ((v1-1 (shr (the-as int v1-0) 63))
|
|
(a1-2 (shr (the-as int a1-1) 63))
|
|
(a0-2 (shr (the-as int a0-1) 63))
|
|
(a1-3 (* a1-2 2))
|
|
(a0-3 (* a0-2 4))
|
|
(v1-3 (logior (logior v1-1 a1-3) a0-3))
|
|
)
|
|
(b! (nonzero? v1-3) cfg-3 :delay (set! v1-4 (the-as uint (+ v1-3 -1))))
|
|
)
|
|
(.sub.vf vf17 vf5 vf2)
|
|
(.mov.vf vf18 vf0 :mask #b1000)
|
|
(.outer.product.a.vf acc vf17 vf1)
|
|
(.outer.product.b.vf vf18 vf1 vf17 acc)
|
|
(.outer.product.a.vf acc vf1 vf18)
|
|
(.outer.product.b.vf vf18 vf18 vf1 acc)
|
|
(.add.vf vf18 vf18 vf2 :mask #b111)
|
|
(b! #t cfg-24 :delay (.svf (&-> arg0 quad) vf18))
|
|
(nop!)
|
|
(label cfg-3)
|
|
(b! (nonzero? v1-4) cfg-6 :delay (set! v1-5 (+ v1-4 -1)))
|
|
(vector-segment-distance-point! arg1 (the-as vector (-> arg2 vector)) (-> arg2 vector 1) arg0)
|
|
(goto cfg-24)
|
|
(label cfg-6)
|
|
(b! (nonzero? v1-5) cfg-9 :delay (set! v1-6 (+ v1-5 -1)))
|
|
(vector-segment-distance-point! arg1 (-> arg2 vector 1) (-> arg2 vector 2) arg0)
|
|
(goto cfg-24)
|
|
(label cfg-9)
|
|
(b! (nonzero? v1-6) cfg-14 :delay (set! v1-7 (+ v1-6 -1)))
|
|
(let ((f30-0 (vector-segment-distance-point! arg1 (-> arg2 vector 1) (the-as vector (-> arg2 vector)) arg0))
|
|
(s3-0 (new 'stack-no-clear 'vector))
|
|
)
|
|
(if (< (vector-segment-distance-point! arg1 (-> arg2 vector 1) (-> arg2 vector 2) s3-0) f30-0)
|
|
(set! (-> arg0 quad) (-> s3-0 quad))
|
|
)
|
|
)
|
|
(goto cfg-24)
|
|
(label cfg-14)
|
|
(b! (nonzero? v1-7) cfg-17 :delay (set! v1-10 (+ v1-7 -1)))
|
|
(vector-segment-distance-point! arg1 (-> arg2 vector 2) (the-as vector (-> arg2 vector)) arg0)
|
|
(goto cfg-24)
|
|
(label cfg-17)
|
|
(b! (nonzero? v1-10) cfg-22 :delay (nop!))
|
|
(let ((f30-1 (vector-segment-distance-point! arg1 (the-as vector (-> arg2 vector)) (-> arg2 vector 1) arg0))
|
|
(s3-1 (new 'stack-no-clear 'vector))
|
|
)
|
|
(if (< (vector-segment-distance-point! arg1 (the-as vector (-> arg2 vector)) (-> arg2 vector 2) s3-1) f30-1)
|
|
(set! (-> arg0 quad) (-> s3-1 quad))
|
|
)
|
|
)
|
|
(goto cfg-24)
|
|
(label cfg-22)
|
|
(let ((f30-2 (vector-segment-distance-point! arg1 (-> arg2 vector 2) (the-as vector (-> arg2 vector)) arg0))
|
|
(s3-2 (new 'stack-no-clear 'vector))
|
|
)
|
|
(if (< (vector-segment-distance-point! arg1 (-> arg2 vector 2) (-> arg2 vector 1) s3-2) f30-2)
|
|
(set! (-> arg0 quad) (-> s3-2 quad))
|
|
)
|
|
)
|
|
(label cfg-24)
|
|
0
|
|
(none)
|
|
)
|
|
)
|
|
|
|
(defun point-in-triangle-cross ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 vector))
|
|
"Check if point is in the triangle using cross product check (so you have to get the order of points right)"
|
|
;; og:preserve-this float -> int
|
|
(local-vars (v1-0 int) (a0-1 int) (a1-1 int))
|
|
(rlet ((acc :class vf)
|
|
(vf1 :class vf)
|
|
(vf10 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
(vf8 :class vf)
|
|
(vf9 :class vf)
|
|
)
|
|
(.lvf vf3 (&-> arg3 quad))
|
|
(.lvf vf4 (&-> arg4 quad))
|
|
(.lvf vf5 (&-> arg0 quad))
|
|
(.lvf vf2 (&-> arg2 quad))
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.sub.vf vf6 vf3 vf4)
|
|
(.sub.vf vf7 vf3 vf5)
|
|
(.sub.vf vf8 vf3 vf2)
|
|
(.sub.vf vf9 vf5 vf4)
|
|
(.sub.vf vf10 vf5 vf2)
|
|
(.outer.product.a.vf acc vf6 vf7)
|
|
(.outer.product.b.vf vf2 vf7 vf6 acc)
|
|
(.outer.product.a.vf acc vf7 vf8)
|
|
(.outer.product.b.vf vf3 vf8 vf7 acc)
|
|
(.outer.product.a.vf acc vf9 vf10)
|
|
(.outer.product.b.vf vf4 vf10 vf9 acc)
|
|
(.mul.vf vf2 vf2 vf1)
|
|
(.mul.vf vf3 vf3 vf1)
|
|
(.nop.vf)
|
|
(.mul.vf vf4 vf4 vf1)
|
|
(.add.x.vf vf2 vf2 vf2 :mask #b10)
|
|
(.add.x.vf vf3 vf3 vf3 :mask #b10)
|
|
(.add.x.vf vf4 vf4 vf4 :mask #b10)
|
|
(.nop.vf)
|
|
(.add.z.vf vf2 vf2 vf2 :mask #b10)
|
|
(.add.z.vf vf3 vf3 vf3 :mask #b10)
|
|
(.add.z.vf vf4 vf4 vf4 :mask #b10)
|
|
(.nop.vf)
|
|
(.mov a0-1 vf2)
|
|
(.mov a1-1 vf3)
|
|
(.mov v1-0 vf4)
|
|
;; og:preserve-this
|
|
(>= (the-as int (logior (logior a0-1 a1-1) v1-0)) 0)
|
|
)
|
|
)
|
|
|
|
(defun point-in-plane-<-point+normal! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Very strange function. Takes a plane, in point-normal form, then returns some other point on that plane.
|
|
It will move 1m in two of {x, y, z} directions. The direction not moved in is the one which is closest to point-in-triangle-cross
|
|
in the same direction of the normal (this prevent moving huge distances for nearly vertical planes for example)."
|
|
(let ((f0-3 (+ (* (-> arg2 x) (-> arg1 x)) (* (-> arg2 y) (-> arg1 y)) (* (-> arg2 z) (-> arg1 z)))))
|
|
(set! (-> arg0 w) 1.0)
|
|
(let ((f1-7 (fabs (-> arg2 x)))
|
|
(f2-3 (fabs (-> arg2 y)))
|
|
(f3-1 (fabs (-> arg2 z)))
|
|
)
|
|
(cond
|
|
((and (< f2-3 f1-7) (< f3-1 f1-7))
|
|
(set! (-> arg0 y) (+ 4096.0 (-> arg1 y)))
|
|
(set! (-> arg0 z) (+ 4096.0 (-> arg1 z)))
|
|
(set! (-> arg0 x) (/ (+ (- (- (* (-> arg2 y) (-> arg0 y))) (* (-> arg2 z) (-> arg0 z))) f0-3) (-> arg2 x)))
|
|
)
|
|
((and (< f1-7 f2-3) (< f3-1 f2-3))
|
|
(set! (-> arg0 x) (+ 4096.0 (-> arg1 x)))
|
|
(set! (-> arg0 z) (+ 4096.0 (-> arg1 z)))
|
|
(set! (-> arg0 y) (/ (- (- f0-3 (* (-> arg2 x) (-> arg0 x))) (* (-> arg2 z) (-> arg0 z))) (-> arg2 y)))
|
|
)
|
|
(else
|
|
(set! (-> arg0 x) (+ 4096.0 (-> arg1 x)))
|
|
(set! (-> arg0 y) (+ 4096.0 (-> arg1 y)))
|
|
(set! (-> arg0 z) (/ (+ (- (- (* (-> arg2 x) (-> arg0 x))) (* (-> arg2 y) (-> arg0 y))) f0-3) (-> arg2 z)))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun circle-circle-xz-intersect ((arg0 sphere) (arg1 sphere) (arg2 vector) (arg3 vector))
|
|
;; this function is unused and really complicated, so not implementing it for now.
|
|
(format 0 "circle-circle-xz-intersect~%")
|
|
(crash!)
|
|
0
|
|
)
|
|
|
|
;; WARN: Return type mismatch object vs none.
|
|
(defun circle-test ()
|
|
"Test the circle-circle-xz-intersect function."
|
|
(let ((s4-0 (new 'stack 'sphere))
|
|
(a1-2 (new 'stack 'sphere))
|
|
(s5-0 (new-stack-vector0))
|
|
(gp-0 (new-stack-vector0))
|
|
)
|
|
(let ((v1-3 s4-0))
|
|
(set! (-> v1-3 x) 0.0)
|
|
(set! (-> v1-3 y) 0.0)
|
|
(set! (-> v1-3 z) 0.0)
|
|
(set! (-> v1-3 r) 1.0)
|
|
)
|
|
(let ((v1-4 a1-2))
|
|
(set! (-> v1-4 x) 100.0)
|
|
(set! (-> v1-4 y) 0.0)
|
|
(set! (-> v1-4 z) 0.0)
|
|
(set! (-> v1-4 r) 10000.0)
|
|
)
|
|
(let ((a2-1 (circle-circle-xz-intersect s4-0 a1-2 s5-0 gp-0)))
|
|
(format #t "res = ~d~%" a2-1)
|
|
)
|
|
(format #t "(~f, ~f)~%" (-> s5-0 x) (-> s5-0 z))
|
|
(format #t "(~f, ~f)~%" (-> gp-0 x) (-> gp-0 z))
|
|
)
|
|
(none)
|
|
)
|
|
|
|
(defun vector-circle-tangent-new ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Unused."
|
|
(rlet ((Q :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(let ((a1-2 (new 'stack 'sphere)))
|
|
(set! (-> (new 'stack-no-clear 'vector) quad) (the-as uint128 0))
|
|
(set! (-> (new 'stack-no-clear 'vector) quad) (the-as uint128 0))
|
|
(let ((v1-3 #x3f000000))
|
|
(.lvf vf3 (&-> arg1 quad))
|
|
(.mov vf2 v1-3)
|
|
)
|
|
(.lvf vf4 (&-> arg0 quad))
|
|
(.add.vf vf1 vf3 vf4)
|
|
(.sub.vf vf5 vf4 vf3)
|
|
(.mul.x.vf vf1 vf1 vf2)
|
|
(.mul.x.vf vf5 vf5 vf2)
|
|
(.mul.vf vf5 vf5 vf5 :mask #b101)
|
|
(.add.z.vf vf5 vf5 vf5 :mask #b1)
|
|
(.sqrt.vf Q vf5 :ftf #b0)
|
|
(.wait.vf)
|
|
(.mul.vf vf1 vf0 Q :mask #b1000)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.svf (&-> a1-2 quad) vf1)
|
|
(circle-circle-xz-intersect (the-as sphere arg1) a1-2 arg2 arg3)
|
|
)
|
|
0
|
|
(none)
|
|
)
|
|
)
|
|
|
|
(defun vector-circle-tangent ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"Also unused."
|
|
(let* ((s3-1 (vector-! (new 'stack-no-clear 'vector) arg1 arg0))
|
|
(f0-0 (vector-xz-length s3-1))
|
|
(f28-0 (acos (/ (-> arg1 w) f0-0)))
|
|
)
|
|
(cond
|
|
((>= 546.13336 f28-0)
|
|
(set! (-> arg2 x) (- (-> arg0 x) (-> s3-1 z)))
|
|
(set! (-> arg2 y) 0.0)
|
|
(set! (-> arg2 z) (+ (-> arg0 z) (-> s3-1 x)))
|
|
(set! (-> arg3 x) (+ (-> arg0 x) (-> s3-1 z)))
|
|
(set! (-> arg3 y) 0.0)
|
|
(set! (-> arg3 z) (- (-> arg0 z) (-> s3-1 x)))
|
|
)
|
|
(else
|
|
(let ((f0-15 (atan (-> s3-1 z) (-> s3-1 x)))
|
|
(f30-0 (- (-> arg1 w)))
|
|
(s3-2 (new 'stack-no-clear 'vector))
|
|
)
|
|
(let ((s2-1 (new 'stack-no-clear 'vector)))
|
|
(let ((a2-1 (new 'stack-no-clear 'vector)))
|
|
(set! (-> a2-1 x) (- f0-15 f28-0))
|
|
(set! (-> a2-1 y) (+ f0-15 f28-0))
|
|
(vector-sincos! s3-2 s2-1 a2-1)
|
|
)
|
|
(set! (-> arg2 x) (+ (-> arg1 x) (* f30-0 (-> s2-1 x))))
|
|
(set! (-> arg2 z) (+ (-> arg1 z) (* f30-0 (-> s3-2 x))))
|
|
(set! (-> arg3 x) (+ (-> arg1 x) (* f30-0 (-> s2-1 y))))
|
|
)
|
|
(set! (-> arg3 z) (+ (-> arg1 z) (* f30-0 (-> s3-2 y))))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
0
|
|
(none)
|
|
)
|
|
|
|
(defun find-knot-span ((arg0 int) (arg1 int) (arg2 float) (arg3 (inline-array vector)))
|
|
"Binary serach over knots to find which contains the value float in (arg0 arg1). Unused."
|
|
(local-vars (v0-0 int))
|
|
(b! (= arg2 (-> (&-> arg3 0 data (+ arg0 1)) 0)) cfg-11 :delay (set! v0-0 arg0))
|
|
(let ((v1-3 (the int arg2)))
|
|
(let* ((a2-1 (+ v1-3 3))
|
|
(t0-1 (&-> arg3 0 data a2-1))
|
|
(f1-2 (-> t0-1 0))
|
|
(f2-0 (-> t0-1 1))
|
|
)
|
|
(b! (> f1-2 arg2) cfg-4)
|
|
(b! (>= arg2 f2-0) cfg-4 :delay (set! v0-0 a2-1))
|
|
)
|
|
(b! #t cfg-11 :delay (nop!))
|
|
(label cfg-4)
|
|
(let ((a1-1 arg1)
|
|
(a0-1 (+ arg0 1))
|
|
)
|
|
(label cfg-5)
|
|
(let ((a2-3 (/ (+ a1-1 a0-1) 2)))
|
|
(let ((t0-3 (&-> arg3 0 data a2-3)))
|
|
(b! (>= arg2 (-> t0-3 0)) cfg-7)
|
|
(b! #t cfg-5 :delay (set! a0-1 a2-3))
|
|
(label cfg-7)
|
|
(b! (< arg2 (-> t0-3 1)) cfg-9)
|
|
)
|
|
(b! #t cfg-5 :delay (set! a1-1 a2-3))
|
|
(label cfg-9)
|
|
(set! v0-0 a2-3)
|
|
)
|
|
)
|
|
(b! (= v0-0 v1-3) cfg-11 :delay (nop!))
|
|
)
|
|
(nop!)
|
|
(nop!)
|
|
(label cfg-11)
|
|
v0-0
|
|
)
|
|
|
|
(defun calculate-basis-functions-vector! ((arg0 vector) (arg1 int) (arg2 float) (arg3 (pointer float)))
|
|
"Calculate polynomial basis for a given control point."
|
|
(local-vars (v1-0 int) (v1-1 object))
|
|
;; og:preserve-this
|
|
;;(.sll v1-0 arg1 2)
|
|
(set! v1-0 (* 4 arg1)) ;; originally used 32-bit asm
|
|
(let ((a1-1 #x3f800000)
|
|
(f3-0 arg2)
|
|
)
|
|
;; og:preserve-this
|
|
;;(.addu v1-1 arg3 v1-0)
|
|
(set! v1-1 (&+ arg3 v1-0))
|
|
(let* ((f1-0 (the-as float a1-1)) ;; trick to load float constant.
|
|
(f5-0 f1-0)
|
|
)
|
|
0.0
|
|
0.0
|
|
(let* ((f0-2 (-> (the-as (pointer float) v1-1) 0))
|
|
(f2-0 (-> (the-as (pointer float) v1-1) 1))
|
|
(f0-3 (- f3-0 f0-2))
|
|
(f4-0 (- f2-0 f3-0))
|
|
(f10-0 (/ f1-0 (+ f4-0 f0-3)))
|
|
(f2-2 (-> (the-as (pointer float) v1-1) -1))
|
|
(f8-0 (-> (the-as (pointer float) v1-1) 2))
|
|
(f2-3 (- f3-0 f2-2))
|
|
(f9-0 (+ f4-0 f2-3))
|
|
(f6-0 (-> (the-as (pointer float) v1-1) -2))
|
|
(f7-0 (-> (the-as (pointer float) v1-1) 3))
|
|
(f9-1 (/ f1-0 f9-0))
|
|
(f5-1 (* f5-0 f10-0))
|
|
(f11-0 (* f4-0 f5-1))
|
|
(f10-1 (* f0-3 f5-1))
|
|
(f5-2 (- f8-0 f3-0))
|
|
(f8-1 (* f11-0 f9-1))
|
|
(f11-1 (/ f1-0 (+ f5-2 f0-3)))
|
|
(f9-3 (* f4-0 f8-1))
|
|
(f8-2 (* f2-3 f8-1))
|
|
(f11-2 (* f10-1 f11-1))
|
|
(f10-3 (+ (* f5-2 f11-2) f8-2))
|
|
(f8-3 (* f0-3 f11-2))
|
|
(f6-1 (- f3-0 f6-0))
|
|
(f3-1 (- f7-0 f3-0))
|
|
(f7-3 (* f9-3 (/ f1-0 (+ f4-0 f6-1))))
|
|
(f4-1 (* f4-0 f7-3))
|
|
(f6-2 (* f6-1 f7-3))
|
|
(f7-6 (* f10-3 (/ f1-0 (+ f5-2 f2-3))))
|
|
(f5-4 (+ (* f5-2 f7-6) f6-2))
|
|
(f2-4 (* f2-3 f7-6))
|
|
(f1-2 (* f8-3 (the-as float (/ f1-0 (+ f3-1 f0-3)))))
|
|
(f2-5 (+ (* f3-1 f1-2) f2-4))
|
|
(f0-4 (* f0-3 f1-2))
|
|
)
|
|
(set! (-> arg0 x) f4-1)
|
|
(set! (-> arg0 y) f5-4)
|
|
(set! (-> arg0 z) f2-5)
|
|
(set! (-> arg0 w) f0-4)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
(defun curve-evaluate! ((arg0 vector) (arg1 float) (arg2 (inline-array vector)) (arg3 int) (arg4 (pointer float)) (arg5 int))
|
|
"Evaluate a curve.
|
|
arg0 is the output
|
|
arg1 is the input.
|
|
arg2 is control vertices
|
|
arg3 is the number of control vertices
|
|
arg4 is the knot points
|
|
arg5 is the number of knots
|
|
"
|
|
(local-vars (v1-7 int) (v1-8 int) (v1-10 float) (s3-0 int))
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(let ((s4-0 (new 'static 'vector)))
|
|
0
|
|
;; lookup knot
|
|
(let* ((f0-0 (-> arg4 0))
|
|
(f1-0 (-> (&-> arg4 (+ arg5 -1)) 0))
|
|
(a2-1 (fmax (fmin (* arg1 f1-0) f1-0) f0-0))
|
|
)
|
|
(let* ((a1-1 (+ arg5 -5))
|
|
(a3-1 3)
|
|
(f0-2 a2-1)
|
|
(v1-5 arg4)
|
|
(f0-3 f0-2)
|
|
)
|
|
(b! (= f0-3 (-> (&-> v1-5 (+ a1-1 1)) 0))
|
|
cfg-11
|
|
:delay (set! s3-0 a1-1)
|
|
)
|
|
(let ((a0-4 (the int f0-3)))
|
|
(let* ((t1-1 (+ a0-4 3))
|
|
(t2-1 (&-> v1-5 t1-1))
|
|
(f1-4 (-> t2-1 0))
|
|
(f2-3 (-> t2-1 1))
|
|
)
|
|
(b! (> f1-4 f0-3) cfg-4)
|
|
(b! (>= f0-3 f2-3) cfg-4 :delay (set! s3-0 t1-1))
|
|
)
|
|
(b! #t cfg-11)
|
|
(label cfg-4)
|
|
(let ((a3-2 a3-1)
|
|
(a1-2 (+ a1-1 1))
|
|
)
|
|
(label cfg-5)
|
|
(let ((t1-3 (/ (+ a3-2 a1-2) 2)))
|
|
(let ((t2-3 (&-> v1-5 t1-3)))
|
|
(b! (>= f0-3 (-> t2-3 0)) cfg-7)
|
|
(b! #t cfg-5 :delay (set! a1-2 t1-3))
|
|
(label cfg-7)
|
|
(b! (< f0-3 (-> t2-3 1)) cfg-9)
|
|
)
|
|
(b! #t cfg-5 :delay (set! a3-2 t1-3))
|
|
(label cfg-9)
|
|
(set! s3-0 t1-3)
|
|
)
|
|
)
|
|
(b! (= s3-0 a0-4) cfg-11)
|
|
)
|
|
)
|
|
(nop!)
|
|
(nop!)
|
|
(label cfg-11)
|
|
;; og:preserve-this
|
|
;; calculate coefficients for this knot's polynomial, store in s4-0
|
|
(calculate-basis-functions-vector!
|
|
s4-0
|
|
s3-0
|
|
a2-1
|
|
(the-as (pointer float) arg4)
|
|
)
|
|
)
|
|
;; og:preserve-this
|
|
;;(.addiu v1-7 s3-0 -3)
|
|
(set! v1-7 (- s3-0 3))
|
|
(.lvf vf6 s4-0)
|
|
)
|
|
;; og:preserve-this
|
|
;; evaluate polynomial!
|
|
;;(.sll v1-8 v1-7 4)
|
|
(set! v1-8 (* v1-7 16))
|
|
(.add.x.vf vf1 vf0 vf0 :mask #b1000)
|
|
(let ((v1-9 (+ v1-8 (the-as int arg2))))
|
|
(nop!)
|
|
(nop!)
|
|
(.lvf vf2 (&-> (the-as (pointer int128) v1-9)))
|
|
(nop!)
|
|
(.lvf vf3 (+ v1-9 16))
|
|
(nop!)
|
|
(.lvf vf4 (+ v1-9 32))
|
|
(nop!)
|
|
(.lvf vf5 (+ v1-9 48))
|
|
)
|
|
(.mul.x.vf acc vf2 vf6)
|
|
(nop!)
|
|
(.add.mul.y.vf acc vf3 vf6 acc :mask #b111)
|
|
(nop!)
|
|
(.add.mul.z.vf acc vf4 vf6 acc :mask #b111)
|
|
(nop!)
|
|
(.add.mul.w.vf vf1 vf5 vf6 acc :mask #b111)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(.svf (&-> arg0 quad) vf1)
|
|
(.mov v1-10 vf1)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun curve-get-pos! ((arg0 vector) (arg1 float) (arg2 curve))
|
|
"Get the position on the curve at the given input."
|
|
(curve-evaluate! arg0 arg1 (-> arg2 cverts) (-> arg2 num-cverts) (-> arg2 knots) (-> arg2 num-knots))
|
|
)
|
|
|
|
(defun curve-length ((arg0 curve))
|
|
"Compute the approximate curve length as the sum of distances between knots."
|
|
(let ((s5-0 (new 'stack-no-clear 'vector))
|
|
(s4-0 (new 'stack-no-clear 'vector))
|
|
(s3-0 (* 3 (-> arg0 num-cverts)))
|
|
(f30-0 0.0)
|
|
)
|
|
(when (nonzero? s3-0)
|
|
(curve-evaluate!
|
|
s4-0
|
|
(-> arg0 knots 0)
|
|
(-> arg0 cverts)
|
|
(-> arg0 num-cverts)
|
|
(-> arg0 knots)
|
|
(-> arg0 num-knots)
|
|
)
|
|
(dotimes (s2-0 s3-0)
|
|
(set! (-> s5-0 quad) (-> s4-0 quad))
|
|
(curve-evaluate!
|
|
s4-0
|
|
(/ (the float (+ s2-0 1)) (the float s3-0))
|
|
(-> arg0 cverts)
|
|
(-> arg0 num-cverts)
|
|
(-> arg0 knots)
|
|
(-> arg0 num-knots)
|
|
)
|
|
(+! f30-0 (vector-vector-distance s5-0 s4-0))
|
|
)
|
|
)
|
|
f30-0
|
|
)
|
|
)
|
|
|
|
(defun curve-copy! ((arg0 curve) (arg1 curve))
|
|
"Shallow copy a curve."
|
|
(set! (-> arg0 cverts) (-> arg1 cverts))
|
|
(set! (-> arg0 num-cverts) (-> arg1 num-cverts))
|
|
(set! (-> arg0 knots) (-> arg1 knots))
|
|
(set! (-> arg0 num-knots) (-> arg1 num-knots))
|
|
(set! (-> arg0 length) (-> arg1 length))
|
|
arg0
|
|
)
|
|
|
|
(defun curve-closest-point ((arg0 curve) (arg1 vector) (arg2 float) (arg3 float) (arg4 int) (arg5 float))
|
|
"Get the input value for the point on the curve. Approximate! And is O(n_knots)."
|
|
(local-vars (sv-48 float))
|
|
(set! sv-48 arg3)
|
|
(let ((s3-0 arg4)
|
|
(gp-0 arg5)
|
|
(f30-0 (curve-length arg0))
|
|
(s2-0 (new 'stack-no-clear 'vector))
|
|
(s1-0 (new 'stack-no-clear 'vector))
|
|
)
|
|
0.0
|
|
0.0
|
|
0.0
|
|
0.0
|
|
(let ((f28-0 0.5))
|
|
0.0
|
|
(if (< 0.0 sv-48)
|
|
(set! f28-0 (/ sv-48 f30-0))
|
|
)
|
|
(let* ((s0-1 (- arg2 (/ gp-0 f30-0)))
|
|
(f26-0 (- s0-1 f28-0))
|
|
(f24-0 (+ s0-1 f28-0))
|
|
)
|
|
(curve-get-pos! s2-0 f26-0 arg0)
|
|
(curve-get-pos! s1-0 f24-0 arg0)
|
|
(let ((f22-0 (vector-vector-distance-squared s2-0 arg1))
|
|
(f20-0 (vector-vector-distance-squared s1-0 arg1))
|
|
)
|
|
(while (> s3-0 0)
|
|
(+! s3-0 -1)
|
|
(set! f28-0 (* 0.5 f28-0))
|
|
(let ((v1-6 (cond
|
|
((< f22-0 f20-0)
|
|
(curve-get-pos! s1-0 s0-1 arg0)
|
|
(set! f20-0 (vector-vector-distance-squared s1-0 arg1))
|
|
(set! f24-0 s0-1)
|
|
(- s0-1 f28-0)
|
|
)
|
|
(else
|
|
(curve-get-pos! s2-0 s0-1 arg0)
|
|
(set! f22-0 (vector-vector-distance-squared s2-0 arg1))
|
|
(set! f26-0 s0-1)
|
|
(+ s0-1 f28-0)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set! s0-1 (fmin 1.0 (fmax 0.0 v1-6)))
|
|
)
|
|
)
|
|
(+ (if (< f22-0 f20-0)
|
|
f26-0
|
|
f24-0
|
|
)
|
|
(/ gp-0 f30-0)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun vector-plane-distance ((arg0 vector) (arg1 plane) (arg2 vector))
|
|
"Unused."
|
|
(vector-dot (vector-! (new 'stack-no-clear 'vector) arg0 (the-as vector (&-> arg1 x))) arg2)
|
|
)
|
|
|
|
(defun intersect-ray-plane ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
"arg1 is ray direction, arg3 is plane normal, others don't really make sense to me."
|
|
(let ((f0-1 (vector-dot arg3 arg1)))
|
|
(if (= f0-1 0.0)
|
|
-1.0
|
|
(/ (- (vector-dot arg3 arg0) (vector-dot arg3 arg2)) (- f0-1))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun line-line-find-intersection-xz ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 vector))
|
|
(let* ((f0-0 (-> arg1 x))
|
|
(f3-0 (-> arg1 z))
|
|
(f4-0 (-> arg3 x))
|
|
(f5-0 (-> arg3 z))
|
|
(f1-3 (+ (* -1.0 f5-0 f0-0) (* f3-0 f4-0)))
|
|
)
|
|
(cond
|
|
((< 0.0001 (fabs f1-3))
|
|
(let* ((f2-3 (* -1.0 f1-3))
|
|
(f5-2 (* -1.0 f5-0 (- (-> arg2 x) (-> arg0 x))))
|
|
(f4-1 (* (- (-> arg2 z) (-> arg0 z)) f4-0))
|
|
(f3-2 (* -1.0 f3-0 (- (-> arg0 x) (-> arg2 x))))
|
|
(f0-1 (* (- (-> arg0 z) (-> arg2 z)) f0-0))
|
|
(f1-4 (/ (+ f5-2 f4-1) f1-3))
|
|
)
|
|
(let ((f0-3 (/ (+ f3-2 f0-1) f2-3)))
|
|
(when arg4
|
|
(set! (-> arg4 y) f0-3)
|
|
(set! (-> arg4 x) f1-4)
|
|
)
|
|
)
|
|
(return f1-4)
|
|
)
|
|
)
|
|
(else
|
|
(when arg4
|
|
(set! (-> arg4 y) -100000000.0)
|
|
(set! (-> arg4 x) -100000000.0)
|
|
)
|
|
(return -100000000.0)
|
|
)
|
|
)
|
|
)
|
|
0.0
|
|
)
|
|
|
|
(defun segment-segment-find-intersection-xz ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
|
|
(let ((gp-0 (new 'stack-no-clear 'vector)))
|
|
(line-line-find-intersection-xz arg0 arg1 arg2 arg3 gp-0)
|
|
(if (and (>= (-> gp-0 x) 0.0) (>= (-> gp-0 y) 0.0) (>= 1.0 (-> gp-0 x)) (>= 1.0 (-> gp-0 y)))
|
|
(-> gp-0 x)
|
|
-100000000.0
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun generate-rand-vector-on-sphere ((arg0 vector))
|
|
(let* ((f30-0 65536.0)
|
|
(v1-2 (/ (the-as int (rand-uint31-gen *random-generator*)) 256))
|
|
(v1-3 (the-as number (logior #x3f800000 v1-2)))
|
|
(f30-1 (* f30-0 (+ -1.0 (the-as float v1-3))))
|
|
(f28-0 65536.0)
|
|
(v1-7 (/ (the-as int (rand-uint31-gen *random-generator*)) 256))
|
|
(v1-8 (the-as number (logior #x3f800000 v1-7)))
|
|
(f28-1 (* f28-0 (+ -1.0 (the-as float v1-8))))
|
|
)
|
|
0.0
|
|
(let ((f26-0 (cos f30-1)))
|
|
(set-vector! arg0 (* f26-0 (cos f28-1)) (* f26-0 (sin f28-1)) (sin f30-1) 1.0)
|
|
)
|
|
)
|
|
(vector-normalize! arg0 1.0)
|
|
)
|
|
|
|
(defmethod lissajous-interp-method-9 ((this lissajous-interp) (arg0 vector))
|
|
(lissajous-method-9 (-> this current) arg0)
|
|
)
|
|
|
|
(defmethod lissajous-interp-method-10 ((this lissajous-interp))
|
|
(seek! (-> this current x-mag) (-> this dest x-mag) (-> this rate x-mag))
|
|
(seek! (-> this current y-mag) (-> this dest y-mag) (-> this rate y-mag))
|
|
(seek! (-> this current theta-rate) (-> this dest theta-rate) (-> this rate theta-rate))
|
|
(seek! (-> this current theta) (-> this dest theta) (* (-> this current theta-rate) (-> this rate theta)))
|
|
(seek! (-> this current wx) (-> this dest wx) (-> this rate wx))
|
|
(seek! (-> this current wy) (-> this dest wy) (-> this rate wy))
|
|
(set! (-> this current period-shift)
|
|
(seek (-> this current period-shift) (-> this dest period-shift) (-> this rate period-shift))
|
|
)
|
|
)
|
|
|
|
(defmethod lissajous-method-9 ((this lissajous) (arg0 vector))
|
|
0.0
|
|
0.0
|
|
(let ((f30-0 (* (cos (* (-> this theta) (-> this wx))) (-> this x-mag)))
|
|
(f0-8 (* (cos (+ (-> this period-shift) (* (-> this theta) (-> this wy)))) (-> this y-mag)))
|
|
)
|
|
(set-vector! arg0 f30-0 f0-8 0.0 1.0)
|
|
)
|
|
arg0
|
|
)
|