From ffa984539251676304b1d2365f1e5643a89ddf6c Mon Sep 17 00:00:00 2001
From: mrb0nk500 <b0nk@b0nk.xyz>
Date: Thu, 2 Feb 2023 11:32:37 -0400
Subject: mtx: Decompile `PSVECDistance()`
---
src/Dolphin/mtx/vec.c | 38 +++++++++++++++++++++++++++++++++++++-
1 file changed, 37 insertions(+), 1 deletion(-)
(limited to 'src/Dolphin/mtx')
diff --git a/src/Dolphin/mtx/vec.c b/src/Dolphin/mtx/vec.c
index af42c0b..e5ca7f9 100644
--- a/src/Dolphin/mtx/vec.c
+++ b/src/Dolphin/mtx/vec.c
@@ -142,5 +142,41 @@ asm void PSVECCrossProduct
f32 PSVECDistance(const register Vec *a, const register Vec *b)
{
- return 0.0f;
+ register f32 v0_yz, v1_yz, v0_xy, v1_xy;
+ register f32 dist_yz, dist_xy, square_dist, ret_dist;
+ register f32 n_0, n_1;
+ register f32 half, three, zero;
+
+ asm
+ {
+ psq_l v0_yz, 4(a), 0, 0
+ psq_l v1_yz, 4(b), 0, 0
+ ps_sub dist_yz, v0_yz, v1_yz
+
+ psq_l v0_xy, 4(a), 0, 0
+ psq_l v1_xy, 4(b), 0, 0
+ ps_sub dist_yz, dist_yz, dist_yz
+ ps_sub dist_xy, v0_xy, v1_xy
+ }
+ half = 0.5f;
+ asm
+ {
+ ps_madd square_dist, dist_xy, dist_xy, dist_yz
+ fsubs zero, half, half
+ ps_sum0 square_dist, square_dist, dist_yz, dist_yz
+ fcmpu cr0, zero, square_dist
+ beq- __exit
+ }
+ three = 3.0f;
+ asm
+ {
+ frsqrte ret_dist, square_dist
+ fmuls n_0, ret_dist, ret_dist
+ fmuls n_1, ret_dist, half
+ fnmsubs n_0, n_0, square_dist, three
+ fmuls ret_dist, n_0, n_1
+ fmuls square_dist, square_dist, ret_dist
+ __exit:
+ }
+ return square_dist;
}
--
cgit v1.2.3-13-gbd6f