From 090a84eba766a0ca32761a587efa94169c609063 Mon Sep 17 00:00:00 2001 From: water111 Date: Sun, 28 Jun 2026 18:57:30 -0400 Subject: [PATCH] up to quat --- decompiler/config/jak1/all-types.gc | 2 +- .../config/jak1/ntsc_v1/var_names.jsonc | 546 ++++++++++++++- docs/jak1_cleanup_inputs/method_names.jsonc | 204 ++++++ .../jak1/engine/common-obs/nav-enemy-h.gc | 2 +- goal_src/jak1/engine/math/quaternion.gc | 643 +++++++++--------- goal_src/jak1/engine/math/transformq-h.gc | 12 +- 6 files changed, 1070 insertions(+), 339 deletions(-) diff --git a/decompiler/config/jak1/all-types.gc b/decompiler/config/jak1/all-types.gc index c9f5e3467e..8c1d2e199e 100644 --- a/decompiler/config/jak1/all-types.gc +++ b/decompiler/config/jak1/all-types.gc @@ -3080,7 +3080,7 @@ (define-extern quaternion-rotate-x! (function quaternion quaternion float quaternion)) (define-extern quaternion-rotate-z! (function quaternion quaternion float quaternion)) (define-extern quaternion-delta-y (function quaternion quaternion float)) -(define-extern quaternion-rotate-y-to-vector! (function quaternion quaternion quaternion float quaternion)) +(define-extern quaternion-rotate-y-to-vector! (function quaternion quaternion vector float quaternion)) (define-extern vector-rotate-y! (function vector vector float vector)) (define-extern vector-x-angle (function vector float)) (define-extern quaterion<-rotate-y-vector (function quaternion vector quaternion)) diff --git a/decompiler/config/jak1/ntsc_v1/var_names.jsonc b/decompiler/config/jak1/ntsc_v1/var_names.jsonc index f569574ee1..551e37f9a6 100644 --- a/decompiler/config/jak1/ntsc_v1/var_names.jsonc +++ b/decompiler/config/jak1/ntsc_v1/var_names.jsonc @@ -923,6 +923,7 @@ "automatic-death-delay" ] }, + "(method 10 bounding-box)": {}, "(method 10 bsp-header)": { "args": [ "this", @@ -1286,6 +1287,7 @@ "act" ] }, + "(method 11 bounding-box)": {}, "(method 11 cam-float-seeker)": { "args": [ "this", @@ -1546,6 +1548,7 @@ "v1-2": "idx" } }, + "(method 12 bounding-box)": {}, "(method 12 cavecrystal-light-control)": { "args": [ "this" @@ -1796,6 +1799,7 @@ "v1-9": "janim-group" } }, + "(method 13 bounding-box)": {}, "(method 13 engine)": { "vars": { "s4-0": [ @@ -1997,6 +2001,7 @@ "v1-9": "janim-group" } }, + "(method 14 bounding-box)": {}, "(method 14 collide-edge-work)": { "args": [ "this", @@ -2220,6 +2225,7 @@ } }, "(method 15 actor-link-info)": {}, + "(method 15 bounding-box)": {}, "(method 15 dead-pool)": {}, "(method 15 dead-pool-heap)": {}, "(method 15 engine)": { @@ -2828,8 +2834,12 @@ "(method 2 bfloat)": {}, "(method 2 handle)": {}, "(method 2 process)": {}, + "(method 2 state)": {}, "(method 2 thread)": {}, + "(method 2 transform)": {}, "(method 2 vec4s)": {}, + "(method 2 vector)": {}, + "(method 2 vector4w)": {}, "(method 20 actor-link-info)": { "args": [ "this", @@ -4214,6 +4224,12 @@ "s5-1": "delta" } }, + "(method 23 trsqv)": { + "args": [ + "this", + "ptr" + ] + }, "(method 24 actor-link-info)": { "args": [ "this", @@ -4536,6 +4552,12 @@ "s5-0": "cshape" } }, + "(method 24 trsqv)": { + "args": [ + "this", + "pt" + ] + }, "(method 25 actor-link-info)": { "vars": { "a0-2": "act", @@ -5403,15 +5425,28 @@ "(method 3 background-work)": {}, "(method 3 baseplat)": {}, "(method 3 bfloat)": {}, + "(method 3 bit-array)": {}, + "(method 3 border-plane)": {}, + "(method 3 bounding-box)": {}, + "(method 3 bounding-box-both)": {}, + "(method 3 bounding-box4w)": {}, + "(method 3 box8s)": {}, + "(method 3 box8s-array)": {}, "(method 3 catch-frame)": {}, "(method 3 connectable)": {}, "(method 3 cpu-thread)": {}, + "(method 3 curve)": {}, + "(method 3 cylinder)": {}, + "(method 3 cylinder-flat)": {}, "(method 3 dead-pool)": {}, "(method 3 dead-pool-heap)": {}, "(method 3 dead-pool-heap-rec)": {}, + "(method 3 dgo-entry)": {}, + "(method 3 dgo-file)": {}, "(method 3 display-env)": {}, "(method 3 double-lurker-top)": {}, "(method 3 draw-control)": {}, + "(method 3 euler-angles)": {}, "(method 3 handle)": {}, "(method 3 inline-array-class)": {}, "(method 3 joint-anim)": {}, @@ -5426,18 +5461,58 @@ "(method 3 list-field)": {}, "(method 3 lod-group)": {}, "(method 3 lod-set)": {}, + "(method 3 lowmemmap)": {}, + "(method 3 matrix)": {}, + "(method 3 matrix3)": {}, + "(method 3 matrix4h)": {}, + "(method 3 plane)": {}, "(method 3 process)": {}, "(method 3 process-tree)": {}, "(method 3 protect-frame)": {}, + "(method 3 quaternion)": { + "vars": { + "f0-5": "inv-magnitude", + "f0-9": "angle" + } + }, + "(method 3 qword)": {}, + "(method 3 random-generator)": {}, "(method 3 res-lump)": { "vars": { "s5-0": "i" } }, + "(method 3 rgbaf)": {}, "(method 3 skeleton-group)": {}, + "(method 3 sphere)": {}, "(method 3 thread)": {}, "(method 3 tippy)": {}, + "(method 3 transform)": {}, + "(method 3 transformq)": {}, + "(method 3 trs)": {}, + "(method 3 trsq)": {}, + "(method 3 trsqv)": {}, "(method 3 vec4s)": {}, + "(method 3 vector)": {}, + "(method 3 vector-array)": {}, + "(method 3 vector16b)": {}, + "(method 3 vector2h)": {}, + "(method 3 vector2uh)": {}, + "(method 3 vector2w)": {}, + "(method 3 vector3h)": {}, + "(method 3 vector3s)": {}, + "(method 3 vector3w)": {}, + "(method 3 vector4b)": {}, + "(method 3 vector4h)": {}, + "(method 3 vector4s-3)": {}, + "(method 3 vector4ub)": {}, + "(method 3 vector4w)": {}, + "(method 3 vector4w-2)": {}, + "(method 3 vector4w-3)": {}, + "(method 3 vector4w-4)": {}, + "(method 3 vector8h)": {}, + "(method 3 vertical-planes)": {}, + "(method 3 vertical-planes-array)": {}, "(method 3 vif-disasm-element)": {}, "(method 3 wobbler)": {}, "(method 3 yeti-slave)": {}, @@ -5922,6 +5997,7 @@ "(method 39 babak-with-cannon)": {}, "(method 39 baby-spider)": {}, "(method 4 array)": {}, + "(method 4 bit-array)": {}, "(method 4 inline-array-class)": {}, "(method 4 pair)": { "vars": { @@ -6275,6 +6351,7 @@ } }, "(method 5 array)": {}, + "(method 5 bit-array)": {}, "(method 5 cpu-thread)": {}, "(method 5 dead-pool-heap)": {}, "(method 5 inline-array-class)": {}, @@ -6903,6 +6980,7 @@ "event-death" ] }, + "(method 9 bit-array)": {}, "(method 9 bouncing-float)": { "args": [ "this", @@ -6915,6 +6993,7 @@ "damping" ] }, + "(method 9 bounding-box)": {}, "(method 9 cam-index)": { "args": [ "this", @@ -7057,6 +7136,7 @@ "version" ] }, + "(method 9 matrix)": {}, "(method 9 merc-ctrl)": { "vars": { "a0-4": "idx-with-bit1", @@ -8525,6 +8605,8 @@ "f0-1": "step" } }, + "deinstall-debug-handler": {}, + "deinstall-debug-handlers": {}, "delete!": { "args": [ "item", @@ -9021,6 +9103,7 @@ ] }, "effect-param->sound-spec": {}, + "enter-state": {}, "entity-actor-count": { "args": [ "res", @@ -9121,6 +9204,11 @@ "clut-addr" ] }, + "fractional-part": { + "args": [ + "x" + ] + }, "generic-add-constants": { "vars": { "a0-1": [ @@ -9253,6 +9341,7 @@ "this" ] }, + "inherit-state": {}, "init-background": { "vars": { "v1-0": "i" @@ -9301,6 +9390,12 @@ } }, "inspect-process-tree": {}, + "install-default-debug-handler": {}, + "integral?": { + "args": [ + "x" + ] + }, "internal-draw-debug-line": { "vars": { "a0-31": [ @@ -9369,7 +9464,17 @@ "mat" ] }, + "kernel-check-hardwired-addresses": {}, + "kernel-copy-function": {}, + "kernel-copy-to-kernel-ram": {}, "kernel-dispatcher": {}, + "kernel-read": {}, + "kernel-read-function": {}, + "kernel-set-exception-vector": {}, + "kernel-set-interrupt-vector": {}, + "kernel-set-level2-vector": {}, + "kernel-write": {}, + "kernel-write-function": {}, "kill-by-name": {}, "kill-by-type": {}, "kill-current-level-hint": { @@ -9567,6 +9672,11 @@ "allocation" ] }, + "log2": { + "args": [ + "s" + ] + }, "logand": { "args": [ "x", @@ -9609,6 +9719,7 @@ "v1-0": "rest" } }, + "looping-code": {}, "main-cheats": { "vars": { "gp-10": "timeout", @@ -9726,6 +9837,15 @@ "src" ] }, + "matrix->quaternion": { + "args": [ + "dst", + "mat" + ], + "vars": { + "f0-2": "trace" + } + }, "matrix-axis-angle!": { "args": [ "dst", @@ -9733,6 +9853,9 @@ "angle-deg" ] }, + "matrix-axis-sin-cos!": {}, + "matrix-axis-sin-cos-vu!": {}, + "matrix-copy!": {}, "matrix-from-control!": { "vars": { "v1-10": [ @@ -9749,6 +9872,11 @@ "f0-0": "one" } }, + "matrix-identity!": { + "args": [ + "dst" + ] + }, "matrix-inv-scale!": { "args": [ "dst", @@ -9898,6 +10026,20 @@ "src" ] }, + "matrix-with-scale->quaternion": { + "args": [ + "dst", + "mat" + ], + "vars": { + "f0-1": "xx", + "f0-3": "x-inv", + "f1-1": "yy", + "f1-3": "y-inv", + "f2-1": "zz", + "f2-3": "z-inv" + } + }, "matrix-y-angle": { "args": [ "mat" @@ -9906,6 +10048,7 @@ "v1-0": "z-row" } }, + "matrix3-determinant": {}, "matrix3-inverse-transpose!": { "args": [ "dst", @@ -10287,6 +10430,39 @@ } }, "quad-copy!": {}, + "quaterion<-rotate-y-vector": { + "args": [ + "quat", + "forward" + ] + }, + "quaternion*!": { + "args": [ + "dst", + "a", + "b" + ] + }, + "quaternion+!": { + "args": [ + "result", + "a", + "b" + ] + }, + "quaternion-!": { + "args": [ + "dest", + "a", + "b" + ] + }, + "quaternion->matrix": { + "args": [ + "dst", + "q" + ] + }, "quaternion-axis-angle!": { "args": [ "quat", @@ -10294,13 +10470,287 @@ "y", "z", "angle" + ], + "vars": { + "f0-1": "c", + "f28-0": "half-angle", + "f30-0": "s" + } + }, + "quaternion-conjugate!": { + "args": [ + "dst", + "src" ] }, + "quaternion-copy!": { + "args": [ + "dst", + "src" + ] + }, + "quaternion-delta-y": { + "args": [ + "a", + "b" + ] + }, + "quaternion-dot": { + "args": [ + "a", + "b" + ] + }, + "quaternion-exp!": { + "args": [ + "dst", + "src" + ], + "vars": { + "f0-6": "scaled-sincos", + "f30-0": "len", + "s5-0": "sincos" + } + }, + "quaternion-float*!": { + "args": [ + "dst", + "src", + "x" + ] + }, + "quaternion-float/!": { + "args": [ + "dst", + "src", + "x" + ], + "vars": { + "f0-1": "inv" + } + }, + "quaternion-i!": { + "args": [ + "quat" + ] + }, + "quaternion-identity!": { + "args": [ + "quat" + ] + }, + "quaternion-inverse!": { + "args": [ + "dst", + "src" + ] + }, + "quaternion-j!": { + "args": [ + "quat" + ] + }, + "quaternion-k!": { + "args": [ + "quat" + ] + }, + "quaternion-left-mult-matrix!": { + "args": [ + "dst", + "q" + ], + "vars": { + "f0-0": "k", + "f1-0": "j", + "f2-0": "i", + "f3-0": "w" + } + }, + "quaternion-log!": { + "args": [ + "dst", + "src" + ] + }, + "quaternion-negate!": { + "args": [ + "dst", + "src" + ] + }, + "quaternion-norm": { + "args": [ + "q" + ] + }, + "quaternion-norm2": { + "args": [ + "q" + ] + }, + "quaternion-normalize!": { + "args": [ + "q" + ] + }, + "quaternion-pseudo-slerp!": {}, + "quaternion-right-mult-matrix!": { + "args": [ + "dst", + "q" + ], + "vars": { + "f0-0": "w", + "f1-0": "k", + "f2-0": "j", + "f3-0": "i" + } + }, + "quaternion-rotate-local-x!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a2-1": "new-rot" + } + }, + "quaternion-rotate-local-y!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a2-1": "new-rot" + } + }, + "quaternion-rotate-local-z!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a2-1": "new-rot" + } + }, + "quaternion-rotate-x!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a1-3": "new-rot" + } + }, + "quaternion-rotate-y!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a1-2": "new-rot" + } + }, + "quaternion-rotate-y-to-vector!": { + "args": [ + "dest", + "src", + "vec", + "max-angle" + ], + "vars": { + "a0-1": "xz-dir", + "s0-0": "xz-unit-dir", + "s5-0": "adjust-quat" + } + }, + "quaternion-rotate-z!": { + "args": [ + "dst", + "quat", + "angle" + ], + "vars": { + "a1-3": "new-rot" + } + }, + "quaternion-set!": { + "args": [ + "quat", + "i", + "j", + "k", + "w" + ] + }, + "quaternion-slerp!": {}, + "quaternion-validate": { + "args": [ + "quat" + ], + "vars": { + "f0-0": "norm" + } + }, "quaternion-vector-angle!": { "args": [ "quat", "axis", "angle" + ], + "vars": { + "f0-1": "c", + "f28-0": "half-angle", + "f30-0": "s" + } + }, + "quaternion-vector-len": { + "args": [ + "quat" + ] + }, + "quaternion-vector-y-angle": { + "args": [ + "quat", + "dir" + ], + "vars": { + "f0-2": "dir-yaw", + "f30-0": "quat-yaw" + } + }, + "quaternion-xz-angle": { + "args": [ + "quat" + ], + "vars": { + "gp-0": "mat", + "s5-0": "forward" + } + }, + "quaternion-y-angle": { + "args": [ + "quat" + ], + "vars": { + "v1-1": "vec" + } + }, + "quaternion-zero!": { + "args": [ + "quat" + ] + }, + "quaternion-zxy!": { + "args": [ + "quat", + "rots" ] }, "ramdisk-load": { @@ -10314,6 +10764,11 @@ "v1-1": "cmd" } }, + "rand-uint31-gen": {}, + "rand-vu": {}, + "rand-vu-float-range": {}, + "rand-vu-init": {}, + "rand-vu-int-count": {}, "rand-vu-int-range": { "args": [ "first", @@ -10323,6 +10778,8 @@ "f0-4": "float-in-range" } }, + "rand-vu-nostep": {}, + "rand-vu-percent?": {}, "raw-ray-sphere-intersect": { "vars": { "v0-0": [ @@ -10402,6 +10859,7 @@ ] } }, + "resend-exception": {}, "reset-and-call": {}, "reset-display-gs-state": { "args": [ @@ -10427,6 +10885,7 @@ "v1-0": "hoff" } }, + "return-from-exception": {}, "return-from-thread": {}, "return-from-thread-dead": {}, "run-function-in-process": {}, @@ -10462,6 +10921,8 @@ "f1-4": "min-step" } }, + "seekl": {}, + "send-event-function": {}, "service-cpads": { "vars": { "gp-0": "pad-list", @@ -11532,14 +11993,22 @@ "args": [ "tf", "dst-mat" - ] + ], + "vars": { + "s3-0": "temp1", + "s4-0": "temp0" + } }, "transform-matrix-parent-calc!": { "args": [ "tf", "dst-mat", "inv-scale" - ] + ], + "vars": { + "s3-0": "temp1", + "s4-0": "temp0" + } }, "trs-matrix-calc!": { "args": [ @@ -11548,6 +12017,11 @@ ] }, "true-func": {}, + "truncate": { + "args": [ + "x" + ] + }, "type-type?": { "args": [ "child-type", @@ -11874,6 +12348,20 @@ "v1-1": "in-goal-mem" } }, + "vector+!": {}, + "vector-!": {}, + "vector-angle<-quaternion!": { + "args": [ + "axis-angle", + "quat" + ], + "vars": { + "f0-3": "ang", + "f30-0": "inv-mag" + } + }, + "vector-copy!": {}, + "vector-cross!": {}, "vector-deg-lerp-clamp!": { "args": [ "out", @@ -11882,6 +12370,8 @@ "in" ] }, + "vector-dot": {}, + "vector-dot-vu": {}, "vector-flatten!": { "args": [ "dst", @@ -11940,6 +12430,7 @@ "plane-normal" ] }, + "vector-reset!": {}, "vector-rotate*!": { "args": [ "dst", @@ -11947,6 +12438,17 @@ "mat" ] }, + "vector-rotate-y!": { + "args": [ + "out", + "in", + "angle" + ], + "vars": { + "a1-2": "rot-quat", + "s4-0": "rot-mat" + } + }, "vector-seek-2d-xz-smooth!": { "args": [ "vec", @@ -12031,6 +12533,44 @@ "velocity" ] }, + "vector-x-angle": { + "args": [ + "vec" + ] + }, + "vector-x-quaternion!": { + "args": [ + "vec", + "quat" + ], + "vars": { + "s5-0": "mat" + } + }, + "vector-y-angle": { + "args": [ + "vec" + ] + }, + "vector-y-quaternion!": { + "args": [ + "vec", + "quat" + ], + "vars": { + "s5-0": "mat" + } + }, + "vector-z-quaternion!": { + "args": [ + "vec", + "quat" + ], + "vars": { + "s5-0": "mat" + } + }, + "vector-zero!": {}, "vector3s-matrix*!": { "args": [ "dst", @@ -12051,6 +12591,8 @@ "s5-0": "temp-vec3" } }, + "vector4-dot": {}, + "vector4-dot-vu": {}, "vector4-lerp!": { "args": [ "out", diff --git a/docs/jak1_cleanup_inputs/method_names.jsonc b/docs/jak1_cleanup_inputs/method_names.jsonc index 9ab7cf3587..28dfa1b8bc 100644 --- a/docs/jak1_cleanup_inputs/method_names.jsonc +++ b/docs/jak1_cleanup_inputs/method_names.jsonc @@ -1622,6 +1622,14 @@ "level-hint-task-process", "If this entity's hint can be played, run its command array. This can mutate game state and check conditions.\nIf all conditions pass, return the text-id. Otherwise, return -1." ], + "matrix->quaternion": [ + "matrix->quaternion", + "Convert rotation matrix to quaternion." + ], + "matrix-with-scale->quaternion": [ + "matrix-with-scale->quaternion", + "Convert a matrix which contains scaling in addition to rotation to a quaternion." + ], "name=": [ "name=", "Are `a` and `b` the same name? This works for strings and symbols." @@ -1642,6 +1650,174 @@ "points-in-air?", "Are both `p1` and `p2` inside any of the boxes?" ], + "quaterion<-rotate-y-vector": [ + "quaterion<-rotate-y-vector", + "Create a quaternion representing only the yaw of the given vector." + ], + "quaternion*!": [ + "quaternion*!", + "Real quaternion multiplication." + ], + "quaternion+!": [ + "quaternion+!", + "Add quaternions as vectors." + ], + "quaternion-!": [ + "quaternion-!", + "Subtract quaternions as vectors." + ], + "quaternion->matrix": [ + "quaternion->matrix", + "Convert quaternion to rotation matrix." + ], + "quaternion-axis-angle!": [ + "quaternion-axis-angle!", + "Construct a quaternion from a normalized axis and angle." + ], + "quaternion-conjugate!": [ + "quaternion-conjugate!", + "Set `dst` to the quaternion conjugate (invert i,j, k). This represents the inverse rotation." + ], + "quaternion-copy!": [ + "quaternion-copy!", + "Copy a quaternion." + ], + "quaternion-delta-y": [ + "quaternion-delta-y", + "Difference in yaw between two quaternions." + ], + "quaternion-dot": [ + "quaternion-dot", + "Treat quaternions as vectors and take the dot product." + ], + "quaternion-exp!": [ + "quaternion-exp!", + "Inverse of quaternion-log!" + ], + "quaternion-float*!": [ + "quaternion-float*!", + "Multiply all components by `x`" + ], + "quaternion-float/!": [ + "quaternion-float/!", + "Divide each element in the quaternion by `x`." + ], + "quaternion-i!": [ + "quaternion-i!", + "Create unit `i` quaternion." + ], + "quaternion-identity!": [ + "quaternion-identity!", + "Set quaternion to represent an identity rotation (no rotation)" + ], + "quaternion-inverse!": [ + "quaternion-inverse!", + "Return `q^-1` such that `q * q^-1 = identity`, even if `q` isn't normalized.\nIf `q` is normalized, it's faster to use `quaternion-conjugate`" + ], + "quaternion-j!": [ + "quaternion-j!", + "Create unit `j` quaternion." + ], + "quaternion-k!": [ + "quaternion-k!", + "Create unit `k` quaternion." + ], + "quaternion-left-mult-matrix!": [ + "quaternion-left-mult-matrix!", + "Place quaternion coefficients into a matrix such that the product of `right-mult` and `left-mult` are\nan equivalent rotation matrix. This method is not used, `quaternion->matrix` is faster." + ], + "quaternion-log!": [ + "quaternion-log!", + "Get the logarithm of a quaternion. This is unused, but is effectively the rotation vector." + ], + "quaternion-negate!": [ + "quaternion-negate!", + "Flip the sign of all quaternion components." + ], + "quaternion-norm": [ + "quaternion-norm", + "Get the norm of a quaternion." + ], + "quaternion-norm2": [ + "quaternion-norm2", + "Get the squared norm of a quaternion." + ], + "quaternion-normalize!": [ + "quaternion-normalize!", + "normalize quaternion in place." + ], + "quaternion-pseudo-slerp!": [ + "quaternion-pseudo-slerp!", + "Simpler quaternion interpolation by interpolating coefficients and normalizing.\nThis handles the \"opposite sign\" issue correctly, but is still poorly behaved for far-away rotations." + ], + "quaternion-right-mult-matrix!": [ + "quaternion-right-mult-matrix!", + "Place quaternion coefficients into a matrix such that the product of `right-mult` and `left-mult` are\nan equivalent rotation matrix. This method is not used, `quaternion->matrix` is faster." + ], + "quaternion-rotate-local-x!": [ + "quaternion-rotate-local-x!", + "Rotate existing quaternion along local x-axis." + ], + "quaternion-rotate-local-y!": [ + "quaternion-rotate-local-y!", + "Rotate existing quaternion along local y-axis." + ], + "quaternion-rotate-local-z!": [ + "quaternion-rotate-local-z!", + "Rotate existing quaternion along local z-axis." + ], + "quaternion-rotate-x!": [ + "quaternion-rotate-x!", + "Rotate existing quaternion along world x-axis." + ], + "quaternion-rotate-y!": [ + "quaternion-rotate-y!", + "Rotate existing quaternion along world y-axis." + ], + "quaternion-rotate-y-to-vector!": [ + "quaternion-rotate-y-to-vector!", + "Rotate along world y axis so local z axis points to match another vector. Max rotation of `max-rot`." + ], + "quaternion-rotate-z!": [ + "quaternion-rotate-z!", + "Rotate existing quaternion along world z-axis." + ], + "quaternion-set!": [ + "quaternion-set!", + "Set quaternion components." + ], + "quaternion-validate": [ + "quaternion-validate", + "Verify quaternion has valid norm, print an error if not." + ], + "quaternion-vector-angle!": [ + "quaternion-vector-angle!", + "Construct a quaternion from a normalized axis and angle." + ], + "quaternion-vector-len": [ + "quaternion-vector-len", + "Assuming quaternion is normalized, get the length of the xyz part." + ], + "quaternion-vector-y-angle": [ + "quaternion-vector-y-angle", + "Angle between quaternion and axis in the xz plane." + ], + "quaternion-xz-angle": [ + "quaternion-xz-angle", + "Calculate the \"yaw\" of the quaternion. This is particularly inefficient." + ], + "quaternion-y-angle": [ + "quaternion-y-angle", + "Get the y rotation angle of a quaternion - not very efficient." + ], + "quaternion-zero!": [ + "quaternion-zero!", + "Set all components to zero." + ], + "quaternion-zxy!": [ + "quaternion-zxy!", + "Make a quaternion from a sequence of z, x, y axis rotations." + ], "string->int": [ "string->int", "String to int. Supports binary, hex, and decimal, and negatives." @@ -1713,5 +1889,33 @@ "string>?": [ "string>?", "In dictionary order, is a > b?" + ], + "vector-angle<-quaternion!": [ + "vector-angle<-quaternion!", + "Convert quaternion into axis-angle with normalized axis and angle in radians in `w`." + ], + "vector-rotate-y!": [ + "vector-rotate-y!", + "Rotate a vector along the y-axis, not very efficient." + ], + "vector-x-angle": [ + "vector-x-angle", + "Get the pitch angle of a vector." + ], + "vector-x-quaternion!": [ + "vector-x-quaternion!", + "Get the first row of the rotation matrix for this quaternion - the direction of the x axis." + ], + "vector-y-angle": [ + "vector-y-angle", + "Get the yaw angle of a vector." + ], + "vector-y-quaternion!": [ + "vector-y-quaternion!", + "Get the second row of the rotation matrix for this quaternion - the direction of the y axis." + ], + "vector-z-quaternion!": [ + "vector-z-quaternion!", + "Get the third row of the rotation matrix for this quaternion - the direction of the z axis." ] } diff --git a/goal_src/jak1/engine/common-obs/nav-enemy-h.gc b/goal_src/jak1/engine/common-obs/nav-enemy-h.gc index 62c6aab948..8ef02678ed 100644 --- a/goal_src/jak1/engine/common-obs/nav-enemy-h.gc +++ b/goal_src/jak1/engine/common-obs/nav-enemy-h.gc @@ -20,7 +20,7 @@ (navenmf2 2) (enable-rotate 3) (enable-travel 4) - (navenmf5 5) + (vulnurable 5) (navenmf6 6) (navenmf7 7) (navenmf8 8) diff --git a/goal_src/jak1/engine/math/quaternion.gc b/goal_src/jak1/engine/math/quaternion.gc index 7e6d75a6e7..fc335475c2 100644 --- a/goal_src/jak1/engine/math/quaternion.gc +++ b/goal_src/jak1/engine/math/quaternion.gc @@ -7,159 +7,155 @@ ;; DECOMP BEGINS (defmethod inspect ((this quaternion)) - "Print a quaternion. Prints the values and axis-angle" (format #t "[~8x] quaternion~%" this) (format #t "~T[~F] [~F] [~F] [~F]~%" (-> this x) (-> this y) (-> this z) (-> this w)) - (let ((f0-5 (/ 1.0 (sqrtf (+ (* (-> this x) (-> this x)) (* (-> this y) (-> this y)) (* (-> this z) (-> this z))))))) - (format #t "~Taxis: ~F ~F ~F" (* f0-5 (-> this x)) (* f0-5 (-> this y)) (* f0-5 (-> this z)))) - (let ((f0-9 (* 2.0 (acos (-> this w))))) (format #t "~T~Tangle: (deg ~R)~%" f0-9)) + (let ((inv-magnitude (/ 1.0 (sqrtf (+ (* (-> this x) (-> this x)) (* (-> this y) (-> this y)) (* (-> this z) (-> this z))))))) + (format #t "~Taxis: ~F ~F ~F" (* inv-magnitude (-> this x)) (* inv-magnitude (-> this y)) (* inv-magnitude (-> this z)))) + (let ((angle (* 2.0 (acos (-> this w))))) (format #t "~T~Tangle: (deg ~R)~%" angle)) this) (defun quaternion-axis-angle! ((quat quaternion) (x float) (y float) (z float) (angle float)) - "Construct a quaternion from an axis and angle. The axis should be normalized." - (let* ((f28-0 (* 0.5 angle)) - (f30-0 (sin f28-0)) - (f0-1 (cos f28-0))) - (set! (-> quat x) (* x f30-0)) - (set! (-> quat y) (* y f30-0)) - (set! (-> quat z) (* z f30-0)) - (set! (-> quat w) f0-1)) + "Construct a quaternion from a normalized axis and angle." + (let* ((half-angle (/ angle 2)) + (s (sin half-angle)) + (c (cos half-angle))) + (set! (-> quat x) (* x s)) + (set! (-> quat y) (* y s)) + (set! (-> quat z) (* z s)) + (set! (-> quat w) c)) quat) (defun quaternion-vector-angle! ((quat quaternion) (axis vector) (angle float)) - "Construct a quaternion from an axis and angle. The axis should be normalized." - (let* ((f28-0 (* 0.5 angle)) - (f30-0 (sin f28-0)) - (f0-1 (cos f28-0))) - (set! (-> quat x) (* (-> axis x) f30-0)) - (set! (-> quat y) (* (-> axis y) f30-0)) - (set! (-> quat z) (* (-> axis z) f30-0)) - (set! (-> quat w) f0-1)) + "Construct a quaternion from a normalized axis and angle." + (let* ((half-angle (/ angle 2)) + (s (sin half-angle)) + (c (cos half-angle))) + (set! (-> quat x) (* (-> axis x) s)) + (set! (-> quat y) (* (-> axis y) s)) + (set! (-> quat z) (* (-> axis z) s)) + (set! (-> quat w) c)) quat) -(defun vector-angle<-quaternion! ((arg0 vector) (arg1 quaternion)) - "Convert the quaternion arg1 to axis-angle form and store in arg0 (angle goes in w)" - (let* ((f0-0 1.0) - (f1-0 1.0) - (f2-0 (-> arg1 w)) - (f30-0 (/ f0-0 (sqrtf (- f1-0 (* f2-0 f2-0))))) - (f0-3 (* 2.0 (acos-rad (-> arg1 w))))) - (set! (-> arg0 x) (* (-> arg1 x) f30-0)) - (set! (-> arg0 y) (* (-> arg1 y) f30-0)) - (set! (-> arg0 z) (* (-> arg1 z) f30-0)) - (set! (-> arg0 w) f0-3)) - arg0) +(defun vector-angle<-quaternion! ((axis-angle vector) (quat quaternion)) + "Convert quaternion into axis-angle with normalized axis and angle in radians in `w`." + (let ((inv-mag (/ 1.0 (sqrtf (- 1.0 (square (-> quat w)))))) + (ang (* 2.0 (acos-rad (-> quat w))))) + (set! (-> axis-angle x) (* (-> quat x) inv-mag)) + (set! (-> axis-angle y) (* (-> quat y) inv-mag)) + (set! (-> axis-angle z) (* (-> quat z) inv-mag)) + (set! (-> axis-angle w) ang)) + axis-angle) -(defun quaternion-zero! ((arg0 quaternion)) - "Set quaternion to all 0's" - (set! (-> arg0 vec quad) (the-as uint128 0)) - arg0) +(defun quaternion-zero! ((quat quaternion)) + "Set all components to zero." + (set! (-> quat vec quad) (the-as uint128 0)) + quat) -(defun quaternion-identity! ((arg0 quaternion)) - "Set quaternion to 0,0,0,1 (identity)" - (set! (-> arg0 vec quad) (the-as uint128 0)) - (set! (-> arg0 w) 1.0) - arg0) +(defun quaternion-identity! ((quat quaternion)) + "Set quaternion to represent an identity rotation (no rotation)" + (set! (-> quat vec quad) (the-as uint128 0)) + (set! (-> quat w) 1.0) + quat) -(defun quaternion-i! ((arg0 quaternion)) - "Create unit i quaternion" - (set! (-> arg0 vec quad) (the-as uint128 0)) - (set! (-> arg0 x) 1.0) - arg0) +(defun quaternion-i! ((quat quaternion)) + "Create unit `i` quaternion." + (set! (-> quat vec quad) (the-as uint128 0)) + (set! (-> quat x) 1.0) + quat) -(defun quaternion-j! ((arg0 quaternion)) - "Create unit j quaternion." - (set! (-> arg0 vec quad) (the-as uint128 0)) - (set! (-> arg0 y) 1.0) - arg0) +(defun quaternion-j! ((quat quaternion)) + "Create unit `j` quaternion." + (set! (-> quat vec quad) (the-as uint128 0)) + (set! (-> quat y) 1.0) + quat) -(defun quaternion-k! ((arg0 quaternion)) - "Create unit k quaternion" - (set! (-> arg0 vec quad) (the-as uint128 0)) - (set! (-> arg0 z) 1.0) - arg0) +(defun quaternion-k! ((quat quaternion)) + "Create unit `k` quaternion." + (set! (-> quat vec quad) (the-as uint128 0)) + (set! (-> quat z) 1.0) + quat) -(defun quaternion-copy! ((arg0 quaternion) (arg1 quaternion)) - "Set arg0 = arg1" - (set! (-> arg0 vec quad) (-> arg1 vec quad)) - arg0) +(defun quaternion-copy! ((dst quaternion) (src quaternion)) + "Copy a quaternion." + (set! (-> dst vec quad) (-> src vec quad)) + dst) -(defun quaternion-set! ((arg0 quaternion) (arg1 float) (arg2 float) (arg3 float) (arg4 float)) - "Set arg0 = [arg1, arg2, arg3, arg4]" - (set! (-> arg0 x) arg1) - (set! (-> arg0 y) arg2) - (set! (-> arg0 z) arg3) - (set! (-> arg0 w) arg4) - arg0) +(defun quaternion-set! ((quat quaternion) (i float) (j float) (k float) (w float)) + "Set quaternion components." + (set! (-> quat x) i) + (set! (-> quat y) j) + (set! (-> quat z) k) + (set! (-> quat w) w) + quat) -(defun quaternion+! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion)) +(defun quaternion+! ((result quaternion) (a quaternion) (b quaternion)) "Add quaternions as vectors." (rlet ((vf1 :class vf) (vf2 :class vf)) - (.lvf vf1 (&-> arg1 vec quad)) - (.lvf vf2 (&-> arg2 vec quad)) + (.lvf vf1 (&-> a vec quad)) + (.lvf vf2 (&-> b vec quad)) (.add.vf vf1 vf1 vf2) - (.svf (&-> arg0 vec quad) vf1) - arg0)) + (.svf (&-> result vec quad) vf1) + result)) -(defun quaternion-! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion)) +(defun quaternion-! ((dest quaternion) (a quaternion) (b quaternion)) "Subtract quaternions as vectors." (rlet ((vf1 :class vf) (vf2 :class vf)) - (.lvf vf1 (&-> arg1 vec quad)) - (.lvf vf2 (&-> arg2 vec quad)) + (.lvf vf1 (&-> a vec quad)) + (.lvf vf2 (&-> b vec quad)) (.sub.vf vf1 vf1 vf2) - (.svf (&-> arg0 vec quad) vf1) - arg0)) + (.svf (&-> dest vec quad) vf1) + dest)) -(defun quaternion-negate! ((arg0 quaternion) (arg1 quaternion)) - "Set arg0 = -arg1." +(defun quaternion-negate! ((dst quaternion) (src quaternion)) + "Flip the sign of all quaternion components." (rlet ((vf1 :class vf) (vf2 :class vf)) - (.lvf vf1 (&-> arg1 vec quad)) - ;;(.sub.vf vf2 vf2 vf2) + (.lvf vf1 (&-> src vec quad)) + ;; og:preserve-this + ;; (.sub.vf vf2 vf2 vf2) (.xor.vf vf2 vf2 vf2) (.sub.vf vf1 vf2 vf1) - (.svf (&-> arg0 vec quad) vf1) - arg0)) + (.svf (&-> dst vec quad) vf1) + dst)) -(defun quaternion-conjugate! ((arg0 quaternion) (arg1 quaternion)) - "Set arg0 to the conjugate of arg1 (negate only ijk). - If arg1 is normalized, this is equivalent to the inverse - NOTE: this gives you the inverse rotation." +(defun quaternion-conjugate! ((dst quaternion) (src quaternion)) + "Set `dst` to the quaternion conjugate (invert i,j, k). This represents the inverse rotation." (rlet ((vf1 :class vf) (vf2 :class vf)) - (.lvf vf1 (&-> arg1 vec quad)) + (.lvf vf1 (&-> src vec quad)) + ;; og:preserve-this ;;(.sub.vf vf2 vf2 vf2) (.xor.vf vf2 vf2 vf2) (.sub.vf.xyz vf2 vf2 vf1) (.add.vf.w vf2 vf2 vf1) - (.svf (&-> arg0 vec quad) vf2) - arg0)) + (.svf (&-> dst vec quad) vf2) + dst)) -(defun quaternion-float*! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Multiply each element" +(defun quaternion-float*! ((dst quaternion) (src quaternion) (x float)) + "Multiply all components by `x`" (rlet ((vf1 :class vf) (vf2 :class vf)) - (.lvf vf1 (&-> arg1 vec quad)) - (.mov vf2 arg2) + (.lvf vf1 (&-> src vec quad)) + (.mov vf2 x) (.mul.x.vf vf1 vf1 vf2) - (.svf (&-> arg0 vec quad) vf1) - arg0)) + (.svf (&-> dst vec quad) vf1) + dst)) -(defun quaternion-float/! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Divide each element" - (let ((f0-1 (/ 1.0 arg2))) (quaternion-float*! arg0 arg1 f0-1)) - arg0) +(defun quaternion-float/! ((dst quaternion) (src quaternion) (x float)) + "Divide each element in the quaternion by `x`." + (let ((inv (/ 1.0 x))) (quaternion-float*! dst src inv)) + dst) -(defun quaternion-norm2 ((arg0 quaternion)) - "Get the squared norm of a quaternion" +(defun quaternion-norm2 ((q quaternion)) + "Get the squared norm of a quaternion." (local-vars (v0-0 float)) (rlet ((acc :class vf) (vf0 :class vf) (vf1 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg0 vec quad)) + (.lvf vf1 (&-> q vec quad)) (.mul.vf vf1 vf1 vf1) (.add.z.vf.w acc vf1 vf1) (.add.mul.y.vf.w acc vf0 vf1 acc) @@ -168,14 +164,14 @@ (.mov v0-0 vf1) v0-0)) -(defun quaternion-norm ((arg0 quaternion)) +(defun quaternion-norm ((q quaternion)) "Get the norm of a quaternion." (local-vars (v1-1 float)) (rlet ((acc :class vf) (vf0 :class vf) (vf1 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg0 vec quad)) + (.lvf vf1 (&-> q vec quad)) (.mul.vf vf1 vf1 vf1) (.add.z.vf.w acc vf1 vf1) (.add.mul.y.vf.w acc vf0 vf1 acc) @@ -184,15 +180,15 @@ (.mov v1-1 vf1) (sqrtf v1-1))) -(defun quaternion-normalize! ((arg0 quaternion)) - "Normalize a quaternion" +(defun quaternion-normalize! ((q quaternion)) + "normalize quaternion in place." (rlet ((acc :class vf) (Q :class vf) (vf0 :class vf) (vf1 :class vf) (vf2 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg0 vec quad)) + (.lvf vf1 (&-> q vec quad)) (.mul.vf vf2 vf1 vf1) (.add.z.vf.w acc vf2 vf2) (.add.mul.y.vf.w acc vf0 vf2 acc) @@ -202,12 +198,12 @@ (.mul.vf vf2 vf1 Q) ;;(.nop.vf) ;;(.nop.vf) - (.svf (&-> arg0 vec quad) vf2) - arg0)) + (.svf (&-> q vec quad) vf2) + q)) -(defun quaternion-inverse! ((arg0 quaternion) (arg1 quaternion)) - "Invert a quaternion. The inverse will satisfy q * q^-1 = identity, even if q is not normalized. - If your quaternion is normalized, it is faster/more accurate to do quaternion-conjugate!" +(defun quaternion-inverse! ((dst quaternion) (src quaternion)) + "Return `q^-1` such that `q * q^-1 = identity`, even if `q` isn't normalized. + If `q` is normalized, it's faster to use `quaternion-conjugate`" (rlet ((acc :class vf) (Q :class vf) (vf0 :class vf) @@ -215,7 +211,7 @@ (vf2 :class vf) (vf3 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg1 vec quad)) + (.lvf vf1 (&-> src vec quad)) (.mul.vf vf2 vf1 vf1) ;; (.sub.vf vf3 vf3 vf3) (.xor.vf vf3 vf3 vf3) @@ -229,10 +225,10 @@ (.mul.vf vf3 vf3 Q) (.nop.vf) (.nop.vf) - (.svf (&-> arg0 vec quad) vf3) - arg0)) + (.svf (&-> dst vec quad) vf3) + dst)) -(defun quaternion-dot ((arg0 quaternion) (arg1 quaternion)) +(defun quaternion-dot ((a quaternion) (b quaternion)) "Treat quaternions as vectors and take the dot product." (local-vars (v0-0 float)) (rlet ((acc :class vf) @@ -240,8 +236,8 @@ (vf1 :class vf) (vf2 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg0 vec quad)) - (.lvf vf2 (&-> arg1 vec quad)) + (.lvf vf1 (&-> a vec quad)) + (.lvf vf2 (&-> b vec quad)) (.mul.vf vf1 vf1 vf2) (.add.z.vf.w acc vf1 vf1) (.add.mul.y.vf.w acc vf0 vf1 acc) @@ -250,8 +246,8 @@ (.mov v0-0 vf1) v0-0)) -(defun quaternion*! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion)) - "Real quaternion multiplication" +(defun quaternion*! ((dst quaternion) (a quaternion) (b quaternion)) + "Real quaternion multiplication." (rlet ((acc :class vf) (vf0 :class vf) (vf1 :class vf) @@ -259,8 +255,8 @@ (vf3 :class vf) (vf4 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg1 vec quad)) - (.lvf vf2 (&-> arg2 vec quad)) + (.lvf vf1 (&-> a vec quad)) + (.lvf vf2 (&-> b vec quad)) (.sub.vf.w vf4 vf0 vf0) (.mul.vf vf3 vf1 vf2) (.outer.product.a.vf acc vf1 vf2) @@ -272,62 +268,61 @@ (.sub.mul.y.vf.w acc vf0 vf3 acc) (.sub.mul.x.vf.w acc vf0 vf3 acc) (.add.mul.w.vf vf1 vf4 vf0 acc) - (.svf (&-> arg0 vec quad) vf1) - arg0)) + (.svf (&-> dst vec quad) vf1) + dst)) -(defun quaternion-right-mult-matrix! ((arg0 matrix) (arg1 quaternion)) - "Place quaternion coefficients into a matrix. - You can convert a quaternion to a matrix by taking the product of this - right-mult and left-mult matrix, but this method is not used. - Instead, quaternion->matrix is a more efficient implementation." - (let ((f3-0 (-> arg1 x)) - (f2-0 (-> arg1 y)) - (f1-0 (-> arg1 z)) - (f0-0 (-> arg1 w))) - (set! (-> arg0 vector 0 x) f0-0) - (set! (-> arg0 vector 0 y) f1-0) - (set! (-> arg0 vector 0 z) (- f2-0)) - (set! (-> arg0 vector 0 w) f3-0) - (set! (-> arg0 vector 1 x) (- f1-0)) - (set! (-> arg0 vector 1 y) f0-0) - (set! (-> arg0 vector 1 z) f3-0) - (set! (-> arg0 vector 1 w) f2-0) - (set! (-> arg0 vector 2 x) f2-0) - (set! (-> arg0 vector 2 y) (- f3-0)) - (set! (-> arg0 vector 2 z) f0-0) - (set! (-> arg0 vector 2 w) f1-0) - (set! (-> arg0 vector 3 x) (- f3-0)) - (set! (-> arg0 vector 3 y) (- f2-0)) - (set! (-> arg0 vector 3 z) (- f1-0)) - (set! (-> arg0 vector 3 w) f0-0)) - arg0) +(defun quaternion-right-mult-matrix! ((dst matrix) (q quaternion)) + "Place quaternion coefficients into a matrix such that the product of `right-mult` and `left-mult` are + an equivalent rotation matrix. This method is not used, `quaternion->matrix` is faster." + (let ((i (-> q x)) + (j (-> q y)) + (k (-> q z)) + (w (-> q w))) + (set! (-> dst vector 0 x) w) + (set! (-> dst vector 0 y) k) + (set! (-> dst vector 0 z) (- j)) + (set! (-> dst vector 0 w) i) + (set! (-> dst vector 1 x) (- k)) + (set! (-> dst vector 1 y) w) + (set! (-> dst vector 1 z) i) + (set! (-> dst vector 1 w) j) + (set! (-> dst vector 2 x) j) + (set! (-> dst vector 2 y) (- i)) + (set! (-> dst vector 2 z) w) + (set! (-> dst vector 2 w) k) + (set! (-> dst vector 3 x) (- i)) + (set! (-> dst vector 3 y) (- j)) + (set! (-> dst vector 3 z) (- k)) + (set! (-> dst vector 3 w) w)) + dst) -(defun quaternion-left-mult-matrix! ((arg0 matrix) (arg1 quaternion)) - "Place quaternion coefficients into a matrix. Unused." - (let ((f2-0 (-> arg1 x)) - (f1-0 (-> arg1 y)) - (f0-0 (-> arg1 z))) - (let ((f3-0 (-> arg1 w))) - (set! (-> arg0 vector 0 x) f2-0) - (set! (-> arg0 vector 0 y) f3-0) - (set! (-> arg0 vector 0 z) (- f0-0)) - (set! (-> arg0 vector 0 w) f1-0) - (set! (-> arg0 vector 1 x) f1-0) - (set! (-> arg0 vector 1 y) f0-0) - (set! (-> arg0 vector 1 z) f3-0) - (set! (-> arg0 vector 1 w) (- f3-0)) - (set! (-> arg0 vector 2 x) f0-0) - (set! (-> arg0 vector 2 y) (- f1-0)) - (set! (-> arg0 vector 2 z) f2-0) - (set! (-> arg0 vector 2 w) f3-0) - (set! (-> arg0 vector 3 x) f3-0)) - (set! (-> arg0 vector 3 y) (- f2-0)) - (set! (-> arg0 vector 3 z) (- f1-0)) - (set! (-> arg0 vector 3 w) (- f0-0))) - arg0) +(defun quaternion-left-mult-matrix! ((dst matrix) (q quaternion)) + "Place quaternion coefficients into a matrix such that the product of `right-mult` and `left-mult` are + an equivalent rotation matrix. This method is not used, `quaternion->matrix` is faster." + (let ((i (-> q x)) + (j (-> q y)) + (k (-> q z))) + (let ((w (-> q w))) + (set! (-> dst vector 0 x) i) + (set! (-> dst vector 0 y) w) + (set! (-> dst vector 0 z) (- k)) + (set! (-> dst vector 0 w) j) + (set! (-> dst vector 1 x) j) + (set! (-> dst vector 1 y) k) + (set! (-> dst vector 1 z) w) + (set! (-> dst vector 1 w) (- w)) + (set! (-> dst vector 2 x) k) + (set! (-> dst vector 2 y) (- j)) + (set! (-> dst vector 2 z) i) + (set! (-> dst vector 2 w) w) + (set! (-> dst vector 3 x) w)) + (set! (-> dst vector 3 y) (- i)) + (set! (-> dst vector 3 z) (- j)) + (set! (-> dst vector 3 w) (- k))) + dst) -(defun quaternion->matrix ((arg0 matrix) (arg1 quaternion)) - "Convert quaternion to matrix." +(defun quaternion->matrix ((dst matrix) (q quaternion)) + "Convert quaternion to rotation matrix." (rlet ((acc :class vf) (vf0 :class vf) (vf1 :class vf) @@ -336,7 +331,7 @@ (vf4 :class vf) (vf5 :class vf)) (init-vf0-vector) - (.lvf vf1 (&-> arg1 vec quad)) + (.lvf vf1 (&-> q vec quad)) (.add.vf vf5 vf1 vf1) (.add.w.vf.x vf2 vf0 vf1) (.add.z.vf.y vf2 vf0 vf1) @@ -359,11 +354,11 @@ (.add.w.vf.x vf2 vf2 vf0) (.add.w.vf.y vf3 vf3 vf0) (.add.w.vf.z vf4 vf4 vf0) - (.svf (&-> arg0 vector 3 quad) vf0) - (.svf (&-> arg0 vector 0 quad) vf2) - (.svf (&-> arg0 vector 1 quad) vf3) - (.svf (&-> arg0 vector 2 quad) vf4) - arg0)) + (.svf (&-> dst vector 3 quad) vf0) + (.svf (&-> dst vector 0 quad) vf2) + (.svf (&-> dst vector 1 quad) vf3) + (.svf (&-> dst vector 2 quad) vf4) + dst)) (defun matrix->quaternion ((arg0 quaternion) (arg1 matrix)) "Convert a rotation matrix to a quaternion." @@ -408,8 +403,9 @@ f0-12))))))) arg0) -(defun matrix-with-scale->quaternion ((arg0 quaternion) (arg1 matrix)) - "Convert a matrix with a rotation and scale into a quaternion (just the rotation)" +(defun matrix-with-scale->quaternion ((dst quaternion) (mat matrix)) + "Convert a matrix which contains scaling in addition to rotation to a quaternion." + (local-vars (a1-4 float)) (rlet ((vf1 :class vf) (vf2 :class vf) (vf3 :class vf) @@ -418,71 +414,62 @@ (vf6 :class vf) (vf7 :class vf)) (let ((v1-0 (new-stack-matrix0))) - ;; compute norm of rows of rotation matrix (vector-dot does only xyz) - (let ((f0-2 (vector-dot (-> arg1 vector 0) (-> arg1 vector 0)))) - (let ((f1-3 (vector-dot (-> arg1 vector 1) (-> arg1 vector 1)))) - (let* ((f2-4 (vector-dot (-> arg1 vector 2) (-> arg1 vector 2))) - ;; compute scaling factors. - (f0-4 (/ 1.0 (sqrtf f0-2))) - (f1-5 (/ 1.0 (sqrtf f1-3))) - (f2-6 (/ 1.0 (sqrtf f2-4)))) - ;; load the origin matrix. - (.lvf vf1 (&-> arg1 vector 0 quad)) - (.lvf vf2 (&-> arg1 vector 1 quad)) - (.lvf vf3 (&-> arg1 vector 2 quad)) - (.lvf vf4 (&-> arg1 vector 3 quad)) - ;; move scaling factors into vector regs - (let ((a1-1 f0-4)) (.mov vf5 a1-1)) - (let ((a1-2 f1-5)) (.mov vf6 a1-2)) - (let ((a1-3 f2-6)) (.mov vf7 a1-3))))) - ;; scale! + (let* ((xx (vector-dot (-> mat vector 0) (-> mat vector 0))) + (yy (vector-dot (-> mat vector 1) (-> mat vector 1))) + (zz (vector-dot (-> mat vector 2) (-> mat vector 2))) + (x-inv (/ 1.0 (sqrtf xx))) + (y-inv (/ 1.0 (sqrtf yy))) + (z-inv (/ 1.0 (sqrtf zz)))) + (.lvf vf1 (&-> mat vector 0 quad)) + (.lvf vf2 (&-> mat vector 1 quad)) + (.lvf vf3 (&-> mat vector 2 quad)) + (.lvf vf4 (&-> mat vector 3 quad)) + (let ((a1-1 x-inv)) (.mov vf5 a1-1)) + (let ((a1-2 y-inv)) (.mov vf6 a1-2)) + (let ((a1-3 z-inv)) (.mov vf7 a1-3))) (.mul.x.vf vf1 vf1 vf5) (.mul.x.vf vf2 vf2 vf6) (.mul.x.vf vf3 vf3 vf7) - ;; store in temp matrix (.svf (&-> v1-0 vector 0 quad) vf1) (.svf (&-> v1-0 vector 1 quad) vf2) (.svf (&-> v1-0 vector 2 quad) vf3) (.svf (&-> v1-0 vector 3 quad) vf4) - ;; and convert! - (matrix->quaternion arg0 v1-0)))) + (.mov a1-4 vf4) + (matrix->quaternion dst v1-0)))) -(defun quaternion-vector-len ((arg0 quaternion)) +(defun quaternion-vector-len ((quat quaternion)) "Assuming quaternion is normalized, get the length of the xyz part." - (let ((f0-0 1.0) - (f1-0 (-> arg0 w))) - ;; sqrt(1 - w^2) = sqrt(x^2 + y^2 + z^2) = length - (sqrtf (- f0-0 (* f1-0 f1-0))))) + (sqrtf (- 1.0 (square (-> quat w))))) -(defun quaternion-log! ((arg0 quaternion) (arg1 quaternion)) - "Take the log of a quaternion. Unused." +(defun quaternion-log! ((dst quaternion) (src quaternion)) + "Get the logarithm of a quaternion. This is unused, but is effectively the rotation vector." (cond - ((= (-> arg1 w) 0.0) - (set! (-> arg0 x) (* 1.5707963 (-> arg1 x))) - (set! (-> arg0 y) (* 1.5707963 (-> arg1 y))) - (set! (-> arg0 z) (* 1.5707963 (-> arg1 z)))) + ((= (-> src w) 0.0) + (set! (-> dst x) (* 1.5707963 (-> src x))) + (set! (-> dst y) (* 1.5707963 (-> src y))) + (set! (-> dst z) (* 1.5707963 (-> src z)))) (else - (let* ((f30-0 (quaternion-vector-len arg1)) - (f0-9 (/ (atan2-rad (-> arg1 w) f30-0) f30-0))) - (set! (-> arg0 x) (* (-> arg1 x) f0-9)) - (set! (-> arg0 y) (* (-> arg1 y) f0-9)) - (set! (-> arg0 z) (* (-> arg1 z) f0-9))))) - arg0) + (let* ((f30-0 (quaternion-vector-len src)) + (f0-9 (/ (atan2-rad (-> src w) f30-0) f30-0))) + (set! (-> dst x) (* (-> src x) f0-9)) + (set! (-> dst y) (* (-> src y) f0-9)) + (set! (-> dst z) (* (-> src z) f0-9))))) + dst) -(defun quaternion-exp! ((arg0 quaternion) (arg1 quaternion)) - "Quaternion exponentiation. Unused" - (let ((f30-0 (vector-length (the-as vector arg1)))) +(defun quaternion-exp! ((dst quaternion) (src quaternion)) + "Inverse of quaternion-log!" + (let ((len (vector-length (-> src vec)))) (cond - ((= f30-0 0.0) (set! (-> arg0 x) 0.0) (set! (-> arg0 y) 0.0) (set! (-> arg0 z) 0.0) (set! (-> arg0 w) 1.0)) + ((= len 0.0) (set! (-> dst x) 0.0) (set! (-> dst y) 0.0) (set! (-> dst z) 0.0) (set! (-> dst w) 1.0)) (else - (let ((s5-0 (new 'stack-no-clear 'vector))) - (sincos-rad! (the-as (pointer float) s5-0) f30-0) - (let ((f0-6 (/ (-> s5-0 x) f30-0))) - (set! (-> arg0 x) (* (-> arg1 x) f0-6)) - (set! (-> arg0 y) (* (-> arg1 y) f0-6)) - (set! (-> arg0 z) (* (-> arg1 z) f0-6))) - (set! (-> arg0 w) (-> s5-0 y)))))) - arg0) + (let ((sincos (new 'stack-no-clear 'vector))) + (sincos-rad! (&-> sincos x) len) + (let ((scaled-sincos (/ (-> sincos x) len))) + (set! (-> dst x) (* (-> src x) scaled-sincos)) + (set! (-> dst y) (* (-> src y) scaled-sincos)) + (set! (-> dst z) (* (-> src z) scaled-sincos))) + (set! (-> dst w) (-> sincos y)))))) + dst) (defun quaternion-slerp! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float)) "Real quaternion slerp. Spherical-linear interpolation is a nice way to interpolate @@ -599,126 +586,124 @@ (.svf (&-> arg0 vec quad) vf7) arg0)) -(defun vector-x-quaternion! ((arg0 vector) (arg1 quaternion)) - "Get the first row of the rotation matrix for this quaternion" - (let ((s5-0 (new-stack-matrix0))) - (quaternion->matrix s5-0 arg1) - (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector)) 0))) - arg0) +(defun vector-x-quaternion! ((vec vector) (quat quaternion)) + "Get the first row of the rotation matrix for this quaternion - the direction of the x axis." + (let ((mat (new-stack-matrix0))) + (quaternion->matrix mat quat) + (set! (-> vec quad) (-> (the-as (pointer uint128) (-> mat vector)) 0))) + vec) -(defun vector-y-quaternion! ((arg0 vector) (arg1 quaternion)) - "Get the second row of the rotation matrix for this quaternion" - (let ((s5-0 (new-stack-matrix0))) - (quaternion->matrix s5-0 arg1) - (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector 1)) 0))) - arg0) +(defun vector-y-quaternion! ((vec vector) (quat quaternion)) + "Get the second row of the rotation matrix for this quaternion - the direction of the y axis." + (let ((mat (new-stack-matrix0))) + (quaternion->matrix mat quat) + (set! (-> vec quad) (-> (the-as (pointer uint128) (-> mat vector 1)) 0))) + vec) -(defun vector-z-quaternion! ((arg0 vector) (arg1 quaternion)) - "Get the third row of the rotation matrix for this quaternion" - (let ((s5-0 (new-stack-matrix0))) - (quaternion->matrix s5-0 arg1) - (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector 2)) 0))) - arg0) +(defun vector-z-quaternion! ((vec vector) (quat quaternion)) + "Get the third row of the rotation matrix for this quaternion - the direction of the z axis." + (let ((mat (new-stack-matrix0))) + (quaternion->matrix mat quat) + (set! (-> vec quad) (-> (the-as (pointer uint128) (-> mat vector 2)) 0))) + vec) -(defun quaternion-y-angle ((arg0 quaternion)) - "Get the y rotation angle. Not very efficient" - (let ((v1-1 (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0))) (atan (-> v1-1 x) (-> v1-1 z)))) +(defun quaternion-y-angle ((quat quaternion)) + "Get the y rotation angle of a quaternion - not very efficient." + (let ((vec (vector-z-quaternion! (new 'stack-no-clear 'vector) quat))) (atan (-> vec x) (-> vec z)))) -(defun quaternion-vector-y-angle ((arg0 quaternion) (arg1 vector)) - "Not sure. Angle between quaternion and axis, projected in xz plane?" - (let ((f30-0 (quaternion-y-angle arg0)) - (f0-2 (atan (-> arg1 x) (-> arg1 z)))) - (deg-diff f30-0 f0-2))) +(defun quaternion-vector-y-angle ((quat quaternion) (dir vector)) + "Angle between quaternion and axis in the xz plane." + (let ((quat-yaw (quaternion-y-angle quat)) + (dir-yaw (atan (-> dir x) (-> dir z)))) + (deg-diff quat-yaw dir-yaw))) -(defun quaternion-rotate-local-x! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along x axis." - (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :x 1.0 :w 1.0) arg2))) - (quaternion-normalize! (quaternion*! arg0 arg1 a2-1)))) +(defun quaternion-rotate-local-x! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along local x-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :x 1.0 :w 1.0) angle))) + (quaternion-normalize! (quaternion*! dst quat new-rot)))) -(defun quaternion-rotate-local-y! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along y axis" - (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) arg2))) - (quaternion-normalize! (quaternion*! arg0 arg1 a2-1)))) +(defun quaternion-rotate-local-y! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along local y-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) angle))) + (quaternion-normalize! (quaternion*! dst quat new-rot)))) -(defun quaternion-rotate-local-z! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along z axis." - (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :z 1.0 :w 1.0) arg2))) - (quaternion-normalize! (quaternion*! arg0 arg1 a2-1)))) +(defun quaternion-rotate-local-z! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along local z-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :z 1.0 :w 1.0) angle))) + (quaternion-normalize! (quaternion*! dst quat new-rot)))) -(defun quaternion-rotate-y! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along y axis (right multiply)" - (let ((a1-2 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) arg2))) - (quaternion-normalize! (quaternion*! arg0 a1-2 arg1)))) +(defun quaternion-rotate-y! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along world y-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) angle))) + (quaternion-normalize! (quaternion*! dst new-rot quat)))) -(defun quaternion-rotate-x! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along x axis. This has a different implementation - from the others for some reason." - (let ((a1-3 (quaternion-vector-angle! (new-stack-quaternion0) (vector-x-quaternion! (new-stack-vector0) arg1) arg2))) - (quaternion-normalize! (quaternion*! arg0 a1-3 arg1)))) +(defun quaternion-rotate-x! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along world x-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (vector-x-quaternion! (new-stack-vector0) quat) angle))) + (quaternion-normalize! (quaternion*! dst new-rot quat)))) -(defun quaternion-rotate-z! ((arg0 quaternion) (arg1 quaternion) (arg2 float)) - "Rotate existing quaternion along z axis. Has the weird implementation too." - (let ((a1-3 (quaternion-vector-angle! (new-stack-quaternion0) (vector-z-quaternion! (new-stack-vector0) arg1) arg2))) - (quaternion-normalize! (quaternion*! arg0 a1-3 arg1)))) +(defun quaternion-rotate-z! ((dst quaternion) (quat quaternion) (angle float)) + "Rotate existing quaternion along world z-axis." + (let ((new-rot (quaternion-vector-angle! (new-stack-quaternion0) (vector-z-quaternion! (new-stack-vector0) quat) angle))) + (quaternion-normalize! (quaternion*! dst new-rot quat)))) -(defun quaternion-delta-y ((arg0 quaternion) (arg1 quaternion)) - "Difference in yaw between two quaternions" - (acos (vector-dot (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0) - (vector-z-quaternion! (new 'stack-no-clear 'vector) arg1)))) +(defun quaternion-delta-y ((a quaternion) (b quaternion)) + "Difference in yaw between two quaternions." + (acos (vector-dot (vector-z-quaternion! (new 'stack-no-clear 'vector) a) (vector-z-quaternion! (new 'stack-no-clear 'vector) b)))) -(defun quaternion-rotate-y-to-vector! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float)) - "Rotate along y so z-axis points to match another. Use arg3 as the max rotation amount." - (let ((s5-0 (new 'stack-no-clear 'quaternion))) +(defun quaternion-rotate-y-to-vector! ((dest quaternion) (src quaternion) (vec vector) (max-angle float)) + "Rotate along world y axis so local z axis points to match another vector. Max rotation of `max-rot`." + (let ((adjust-quat (new 'stack-no-clear 'quaternion))) (let ((t9-0 vector-xz-normalize!) - (a0-1 (new 'stack-no-clear 'vector))) - (set! (-> a0-1 x) (-> arg2 x)) - (set! (-> a0-1 y) 0.0) - (set! (-> a0-1 z) (-> arg2 z)) - (set! (-> a0-1 w) 1.0) - (let ((s0-0 (t9-0 a0-1 1.0))) - (quaternion-from-two-vectors-max-angle! s5-0 - (vector-z-quaternion! (the-as vector (new 'stack-no-clear 'quaternion)) arg1) - s0-0 - arg3))) - (quaternion-normalize! (quaternion*! arg0 s5-0 arg1)))) + (xz-dir (new 'stack-no-clear 'vector))) + (set! (-> xz-dir x) (-> vec x)) + (set! (-> xz-dir y) 0.0) + (set! (-> xz-dir z) (-> vec z)) + (set! (-> xz-dir w) 1.0) + (let ((xz-unit-dir (t9-0 xz-dir 1.0))) + (quaternion-from-two-vectors-max-angle! adjust-quat + (vector-z-quaternion! (-> (new 'stack-no-clear 'quaternion) vec) src) + xz-unit-dir + max-angle))) + (quaternion-normalize! (quaternion*! dest adjust-quat src)))) -(defun vector-rotate-y! ((arg0 vector) (arg1 vector) (arg2 float)) - "Rotate vector along y axis. Not very efficient." - (let ((a1-2 (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) (new 'static 'vector :y 1.0 :w 1.0) arg2)) - (s4-0 (new 'stack-no-clear 'matrix))) - (quaternion->matrix s4-0 a1-2) - (vector-matrix*! arg0 arg1 s4-0))) +(defun vector-rotate-y! ((out vector) (in vector) (angle float)) + "Rotate a vector along the y-axis, not very efficient." + (let ((rot-quat (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) (new 'static 'vector :y 1.0 :w 1.0) angle)) + (rot-mat (new 'stack-no-clear 'matrix))) + (quaternion->matrix rot-mat rot-quat) + (vector-matrix*! out in rot-mat))) ;; note that these kind of assume a rotation ordering where you can yaw as much as you want, ;; but if you pitch 180 degrees everything is bad. -(defun vector-y-angle ((arg0 vector)) +(defun vector-y-angle ((vec vector)) "Get the yaw angle of a vector." - (atan (-> arg0 x) (-> arg0 z))) + (atan (-> vec x) (-> vec z))) -(defun vector-x-angle ((arg0 vector)) +(defun vector-x-angle ((vec vector)) "Get the pitch angle of a vector." - (atan (-> arg0 y) (vector-xz-length arg0))) + (atan (-> vec y) (vector-xz-length vec))) -(defun quaterion<-rotate-y-vector ((arg0 quaternion) (arg1 vector)) - "Create a quaternion representing only the yaw of the given vector" - (quaternion-vector-angle! arg0 (new 'static 'vector :y 1.0 :w 1.0) (vector-y-angle arg1))) +(defun quaterion<-rotate-y-vector ((quat quaternion) (forward vector)) + "Create a quaternion representing only the yaw of the given vector." + (quaternion-vector-angle! quat (new 'static 'vector :y 1.0 :w 1.0) (vector-y-angle forward))) -(defun-debug quaternion-validate ((arg0 quaternion)) - "Verify that a quaternion is valid, print an error if not." +(defun-debug quaternion-validate ((quat quaternion)) + "Verify quaternion has valid norm, print an error if not." (with-pp - (let ((f0-0 (quaternion-norm arg0))) - (when (or (< 1.01 f0-0) (< f0-0 0.99)) - (format #t "WARNING: bad quaternion (magnitude ~F) process is " f0-0) + (let ((norm (quaternion-norm quat))) + (when (or (< 1.01 norm) (< norm 0.99)) + (format #t "WARNING: bad quaternion (magnitude ~F) process is " norm) (if (and pp (type-type? (-> pp type) process-tree)) (format #t "~A~%" (-> pp name)) (format #t "#f~%")))) 0 (none))) -(defun quaternion-xz-angle ((arg0 quaternion)) - "yet another function to compute the yaw of a quaternion. This is a particularly inefficient version." - (let ((gp-0 (new 'stack-no-clear 'matrix)) - (s5-0 (new 'stack-no-clear 'vector))) - (quaternion->matrix gp-0 arg0) - (set-vector! s5-0 0.0 0.0 1.0 1.0) - (vector-matrix*! s5-0 s5-0 gp-0) - (vector-y-angle s5-0))) +(defun quaternion-xz-angle ((quat quaternion)) + "Calculate the \"yaw\" of the quaternion. This is particularly inefficient." + (let ((mat (new 'stack-no-clear 'matrix)) + (forward (new 'stack-no-clear 'vector))) + (quaternion->matrix mat quat) + (set-vector! forward 0.0 0.0 1.0 1.0) + (vector-matrix*! forward forward mat) + (vector-y-angle forward))) diff --git a/goal_src/jak1/engine/math/transformq-h.gc b/goal_src/jak1/engine/math/transformq-h.gc index 5168ad808e..eb32243000 100644 --- a/goal_src/jak1/engine/math/transformq-h.gc +++ b/goal_src/jak1/engine/math/transformq-h.gc @@ -58,10 +58,10 @@ (set-and-limit-velocity (_type_ int vector float) trsqv) (get-quaternion (_type_) quaternion))) -(defmethod global-y-angle-to-point ((this trsqv) (arg0 vector)) - "Get the angle in the xz plane from the position of this trsqv to the point arg0." - (vector-y-angle (vector-! (new 'stack-no-clear 'vector) arg0 (-> this trans)))) +(defmethod global-y-angle-to-point ((this trsqv) (pt vector)) + "Get the angle in the xz plane from the position of this trsqv to the point." + (vector-y-angle (vector-! (new 'stack-no-clear 'vector) pt (-> this trans)))) -(defmethod relative-y-angle-to-point ((this trsqv) (arg0 vector)) - "Get the y angle between the current orientation and arg0." - (deg-diff (y-angle this) (vector-y-angle (vector-! (new 'stack-no-clear 'vector) arg0 (-> this trans))))) +(defmethod relative-y-angle-to-point ((this trsqv) (pt vector)) + "Get the y angle between the current orientation and the point." + (deg-diff (y-angle this) (vector-y-angle (vector-! (new 'stack-no-clear 'vector) pt (-> this trans)))))