From ffa984539251676304b1d2365f1e5643a89ddf6c Mon Sep 17 00:00:00 2001 From: mrb0nk500 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') 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