fast inv_sqrt()

8 files changed, 56 insertions, 43 deletions

diff --git a/inc/shared/matrix.h b/inc/shared/matrix.h
index 7574edf..d762324 100644
--- a/inc/shared/matrix.h
+++ b/inc/shared/matrix.h
@@ -115,8 +115,10 @@ typedef struct {
 #define Vector4Set(v, a, b, c, d)   ((v)[0]=(a),(v)[1]=(b),(v)[2]=(c),(v)[3]=(d))
 
 void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up);
-vec_t VectorNormalize(vec3_t v);        // returns vector length
-vec_t VectorNormalize2(vec3_t v, vec3_t out);
+
+void VectorNormalize(vec3_t v);
+void VectorNormalize2(vec3_t v, vec3_t out);
+vec_t VectorNormalizeLen(vec3_t v);        // returns vector length
 mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b);
 
 static inline void AnglesToAxis(vec3_t angles, vec3_t axis[3])
diff --git a/src/baseq2/g_func.c b/src/baseq2/g_func.c
index b08a5f2..026fd05 100644
--- a/src/baseq2/g_func.c
+++ b/src/baseq2/g_func.c
@@ -111,7 +111,7 @@ void Move_Calc(edict_t *ent, vec3_t dest, void(*func)(edict_t*))
 {
     VectorClear(ent->velocity);
     VectorSubtract(dest, ent->s.origin, ent->moveinfo.dir);
-    ent->moveinfo.remaining_distance = VectorNormalize(ent->moveinfo.dir);
+    ent->moveinfo.remaining_distance = VectorNormalizeLen(ent->moveinfo.dir);
     ent->moveinfo.endfunc = func;
 
     if (ent->moveinfo.speed == ent->moveinfo.accel && ent->moveinfo.speed == ent->moveinfo.decel) {
diff --git a/src/client/effects.c b/src/client/effects.c
index 02250a6..54c67f2 100644
--- a/src/client/effects.c
+++ b/src/client/effects.c
@@ -1149,7 +1149,7 @@ void CL_BlasterTrail(vec3_t start, vec3_t end)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -1195,7 +1195,7 @@ void CL_QuadTrail(vec3_t start, vec3_t end)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -1240,7 +1240,7 @@ void CL_FlagTrail(vec3_t start, vec3_t end, int color)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -1287,7 +1287,7 @@ void CL_DiminishingTrail(vec3_t start, vec3_t end, centity_t *old, int flags)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 0.5;
     VectorScale(vec, dec, vec);
@@ -1375,7 +1375,7 @@ void CL_RocketTrail(vec3_t start, vec3_t end, centity_t *old)
     // fire
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 1;
     VectorScale(vec, dec, vec);
@@ -1426,7 +1426,7 @@ void CL_OldRailTrail(void)
 
     VectorCopy(te.pos1, move);
     VectorSubtract(te.pos2, te.pos1, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     MakeNormalVectors(vec, right, up);
 
@@ -1502,7 +1502,7 @@ void CL_BubbleTrail(vec3_t start, vec3_t end)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 32;
     VectorScale(vec, dec, vec);
@@ -1732,7 +1732,7 @@ void CL_TeleportParticles(vec3_t org)
                 dir[1] = i * 8;
                 dir[2] = k * 8;
 
-                VectorNormalize(dir);
+                VectorNormalizeLen(dir);
                 vel = 50 + (rand() & 63);
                 VectorScale(dir, vel, p->vel);
 
diff --git a/src/client/newfx.c b/src/client/newfx.c
index 3e8e720..c3d5d02 100644
--- a/src/client/newfx.c
+++ b/src/client/newfx.c
@@ -71,7 +71,7 @@ void CL_DebugTrail(vec3_t start, vec3_t end)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     MakeNormalVectors(vec, right, up);
 
@@ -112,7 +112,7 @@ void CL_SmokeTrail(vec3_t start, vec3_t end, int colorStart, int colorRun, int s
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     VectorScale(vec, spacing, vec);
 
@@ -150,7 +150,7 @@ void CL_ForceWall(vec3_t start, vec3_t end, int color)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     VectorScale(vec, 4, vec);
 
@@ -236,7 +236,7 @@ void CL_BubbleTrail2(vec3_t start, vec3_t end, int dist)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = dist;
     VectorScale(vec, dec, vec);
@@ -284,7 +284,7 @@ void CL_Heatbeam(vec3_t start, vec3_t forward)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     ltime = (float) cl.time / 1000.0;
     start_pt = fmod(ltime * 96.0, step);
@@ -428,7 +428,7 @@ void CL_TrackerTrail(vec3_t start, vec3_t end, int particleColor)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     VectorCopy(vec, forward);
     vectoangles2(forward, angle_dir);
@@ -654,7 +654,7 @@ void CL_TagTrail(vec3_t start, vec3_t end, int color)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -806,7 +806,7 @@ void CL_BlasterTrail2(vec3_t start, vec3_t end)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -852,7 +852,7 @@ void CL_IonripperTrail(vec3_t start, vec3_t ent)
 
     VectorCopy(start, move);
     VectorSubtract(ent, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
@@ -911,7 +911,7 @@ void CL_TrapParticles(entity_t *ent)
 
     VectorCopy(start, move);
     VectorSubtract(end, start, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     dec = 5;
     VectorScale(vec, 5, vec);
diff --git a/src/client/sound/main.c b/src/client/sound/main.c
index 776c3d9..0cbeff0 100644
--- a/src/client/sound/main.c
+++ b/src/client/sound/main.c
@@ -640,7 +640,7 @@ static void S_SpatializeOrigin(const vec3_t origin, float master_vol, float dist
 // calculate stereo seperation and distance attenuation
     VectorSubtract(origin, listener_origin, source_vec);
 
-    dist = VectorNormalize(source_vec);
+    dist = VectorNormalizeLen(source_vec);
     dist -= SOUND_FULLVOLUME;
     if (dist < 0)
         dist = 0;           // close enough to be at full volume
diff --git a/src/client/tent.c b/src/client/tent.c
index fbf1b10..10a1ba2 100644
--- a/src/client/tent.c
+++ b/src/client/tent.c
@@ -523,7 +523,7 @@ static void CL_AddBeams(void)
         vectoangles2(dist, angles);
 
         // add new entities for the beams
-        d = VectorNormalize(dist);
+        d = VectorNormalizeLen(dist);
         if (b->model == cl_mod_lightning) {
             model_length = 35.0;
             d -= 20.0; // correction so it doesn't end in middle of tesla
@@ -667,7 +667,7 @@ static void CL_AddPlayerBeams(void)
         }
 
         // add new entities for the beams
-        d = VectorNormalize(dist);
+        d = VectorNormalizeLen(dist);
         model_length = 32.0;
         steps = ceil(d / model_length);
         len = (d - model_length) / (steps - 1);
@@ -855,7 +855,7 @@ static void CL_RailSpiral(void)
 
     VectorCopy(te.pos1, move);
     VectorSubtract(te.pos2, te.pos1, vec);
-    len = VectorNormalize(vec);
+    len = VectorNormalizeLen(vec);
 
     MakeNormalVectors(vec, right, up);
 
diff --git a/src/common/pmove.c b/src/common/pmove.c
index 636ef83..7163d85 100644
--- a/src/common/pmove.c
+++ b/src/common/pmove.c
@@ -477,7 +477,7 @@ static void PM_WaterMove(void)
     PM_AddCurrents(wishvel);
 
     VectorCopy(wishvel, wishdir);
-    wishspeed = VectorNormalize(wishdir);
+    wishspeed = VectorNormalizeLen(wishdir);
 
     if (wishspeed > pmp->maxspeed) {
         VectorScale(wishvel, pmp->maxspeed / wishspeed, wishvel);
@@ -524,7 +524,7 @@ static void PM_AirMove(void)
     PM_AddCurrents(wishvel);
 
     VectorCopy(wishvel, wishdir);
-    wishspeed = VectorNormalize(wishdir);
+    wishspeed = VectorNormalizeLen(wishdir);
 
 //
 // clamp to server defined max speed
@@ -832,7 +832,7 @@ static void PM_FlyMove(void)
     wishvel[2] += pm->cmd.upmove;
 
     VectorCopy(wishvel, wishdir);
-    wishspeed = VectorNormalize(wishdir);
+    wishspeed = VectorNormalizeLen(wishdir);
 
     //
     // clamp to server defined max speed
diff --git a/src/shared/matrix.c b/src/shared/matrix.c
index 24682b2..be89c5f 100644
--- a/src/shared/matrix.c
+++ b/src/shared/matrix.c
@@ -35,29 +35,40 @@ void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
     }
 }
 
-vec_t VectorNormalize(vec3_t v)
+static float inv_sqrt(float n)
 {
-    float length = VectorLength(v);
+	float x2 = n * 0.5F;
+	union {
+	    float   f;
+	    int	    i;
+	} y;
 
-    if (length)
-	VectorScale(v, 1 / length, v);
+	/* magic number for doubles: 0x5fe6eb50c7b537a9 */
 
-    return length;
+	y.f = n;
+	y.i = 0x5f375a86 - (y.i >> 1);
+	y.f = y.f * (1.5F - (x2 * y.f * y.f));
+	y.f = y.f * (1.5F - (x2 * y.f * y.f));
+
+	return y.f;
+}
+
+void VectorNormalize(vec3_t v)
+{
+    VectorScale(v, inv_sqrt(VectorLengthSquared(v)), v);
 }
 
-vec_t VectorNormalize2(vec3_t v, vec3_t out)
+void VectorNormalize2(vec3_t v, vec3_t out)
 {
-    float    length, ilength;
+    VectorScale(v, inv_sqrt(VectorLengthSquared(v)), out);
+}
 
-    length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
-    length = sqrt(length);         // FIXME
+vec_t VectorNormalizeLen(vec3_t v)
+{
+    float length = VectorLength(v);
 
-    if (length) {
-        ilength = 1 / length;
-        out[0] = v[0] * ilength;
-        out[1] = v[1] * ilength;
-        out[2] = v[2] * ilength;
-    }
+    if (length)
+	VectorScale(v, 1 / length, v);
 
     return length;
 }