mtx: Decompile `PSVECDistance()`

1 files changed, 37 insertions, 1 deletions

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;
 }