diff options
Diffstat (limited to 'src/Dolphin/mtx')
| -rw-r--r-- | src/Dolphin/mtx/vec.c | 38 |
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; } |