2 files changed, 44 insertions, 0 deletions
diff --git a/include/JSystem/JGeometry.h b/include/JSystem/JGeometry.h
index dd984bcc85..8f37b01fc9 100644
--- a/include/JSystem/JGeometry.h
+++ b/include/JSystem/JGeometry.h
@@ -3,6 +3,8 @@
 
 #include "dolphin/mtx/vec.h"
 #include "dolphin/types.h"
+#include "MSL_C/float.h"
+#include "MSL_C/math.h"
 
 namespace JGeometry {
 
@@ -46,6 +48,11 @@ inline void setTVec3f(const f32* vec_a, f32* vec_b) {
     };
 }
 
+inline float fsqrt_step(float mag) {
+    f32 root = __frsqrte(mag);
+    return 0.5f * root * (3.0f - mag * (root * root));
+}
+
 template <>
 struct TVec3<f32> {
     f32 x;
@@ -110,6 +117,38 @@ struct TVec3<f32> {
         };
         return *this;
     }
+
+    f32 squared() {
+        return C_VECSquareMag((Vec*)&x);
+    }
+
+    void normalize() {
+        f32 sq = squared();
+        if (sq <= FLT_EPSILON * 32.0f) {
+            return;
+        }
+        f32 norm;
+        if (sq <= 0.0f) {
+            norm = sq;
+        } else {
+            norm = fsqrt_step(sq);
+        }
+        scale(norm);
+    }
+
+    void scale(register f32 sc) {
+        register f32 z;
+        register f32 x_y;
+        register f32* dst = &x;
+        asm {
+            psq_l    x_y, 0(dst),  0, 0
+            psq_l    z,   8(dst),  1, 0
+            ps_muls0 x_y,    x_y, sc
+            psq_st   x_y, 0(dst),  0, 0
+            ps_muls0 x_y,       z, sc
+            psq_st   x_y,  8(dst),  1, 0
+        };
+    }
 };
 
 template <typename T>
diff --git a/include/JSystem/JParticle/JPAParticle.h b/include/JSystem/JParticle/JPAParticle.h
index ed4fe9142a..3d27dca8e4 100644
--- a/include/JSystem/JParticle/JPAParticle.h
+++ b/include/JSystem/JParticle/JPAParticle.h
@@ -75,6 +75,11 @@ public:
 
 template <class T>
 struct JPANode {
+    JPANode() {
+        mpPrev = NULL;
+        mpNext = NULL;
+    }
+    ~JPANode() {}
     JPANode<T>* mpPrev;
     JPANode<T>* mpNext;
     T mData;