summaryrefslogtreecommitdiff
path: root/src/Dolphin/mtx
diff options
context:
space:
mode:
Diffstat (limited to 'src/Dolphin/mtx')
-rw-r--r--src/Dolphin/mtx/vec.c38
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;
}