refactor: RcVecUtils.SafeNormalize

src/DotRecast.Core/Numerics/RcVec3f.cs

@@ -23,7 +23,6 @@ namespace DotRecast.Core.Numerics
 {
     public struct RcVec3f
     {
-        public const float EPSILON = 1e-6f;
 
         public float X;
         public float Y;
@@ -144,29 +143,10 @@ namespace DotRecast.Core.Numerics
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public override int GetHashCode()
         {
-            int hash = X.GetHashCode();
+            return HashCode.Combine(X, Y, Z);
-            hash = RcHashCodes.CombineHashCodes(hash, Y.GetHashCode());
-            hash = RcHashCodes.CombineHashCodes(hash, Z.GetHashCode());
-            return hash;
         }
 
 
-        /// Normalizes the vector if the length is greater than zero.
-        /// If the magnitude is zero, the vector is unchanged.
-        /// @param[in,out]	v	The vector to normalize. [(x, y, z)]
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void SafeNormalize()
-        {
-            float sqMag = RcMath.Sqr(X) + RcMath.Sqr(Y) + RcMath.Sqr(Z);
-            if (sqMag > EPSILON)
-            {
-                float inverseMag = 1.0f / (float)Math.Sqrt(sqMag);
-                X *= inverseMag;
-                Y *= inverseMag;
-                Z *= inverseMag;
-            }
-        }
-
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void Min(float[] @in, int i)
         {
@@ -368,7 +348,7 @@ namespace DotRecast.Core.Numerics
         {
             float dx = v2.X - v1.X;
             float dz = v2.Z - v1.Z;
-            return (float)Math.Sqrt(dx * dx + dz * dz);
+            return (float)MathF.Sqrt(dx * dx + dz * dz);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -457,18 +437,13 @@ namespace DotRecast.Core.Numerics
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static RcVec3f Normalize(RcVec3f v)
         {
-            float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z)));
+            float d = 1.0f / MathF.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z));
 
-            if (d != 0)
+            return new RcVec3f(
-            {
+                v.X *= d,
-                return new RcVec3f(
+                v.Y *= d,
-                    v.X *= d,
+                v.Z *= d
-                    v.Y *= d,
+            );
-                    v.Z *= d
-                );
-            }
-
-            return v;
         }
     }
 }

src/DotRecast.Core/Numerics/RcVecUtils.cs

@@ -5,6 +5,8 @@ namespace DotRecast.Core.Numerics
 {
     public static class RcVecUtils
     {
+        public const float EPSILON = 1e-6f;
+        
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static float Get(this RcVec2f v, int i)
         {
@@ -120,5 +122,25 @@ namespace DotRecast.Core.Numerics
                    v1[1] * vector2.Y +
                    v1[2] * vector2.Z;
         }
+        
+        /// Normalizes the vector if the length is greater than zero.
+        /// If the magnitude is zero, the vector is unchanged.
+        /// @param[in,out]	v	The vector to normalize. [(x, y, z)]
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static RcVec3f SafeNormalize(RcVec3f v)
+        {
+            float sqMag = RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z);
+            if (sqMag > EPSILON)
+            {
+                float inverseMag = 1.0f / MathF.Sqrt(sqMag);
+                return new RcVec3f(
+                    v.X *= inverseMag,
+                    v.Y *= inverseMag,
+                    v.Z *= inverseMag
+                );
+            }
+
+            return v;
+        }
     }
 }

src/DotRecast.Detour.Crowd/DtCrowd.cs

@@ -1091,7 +1091,7 @@ namespace DotRecast.Detour.Crowd
                             continue;
                         }
 
-                        float dist = (float)Math.Sqrt(distSqr);
+                        float dist = MathF.Sqrt(distSqr);
                         float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist));
 
                         disp = RcVec3f.Mad(disp, diff, weight / dist);
@@ -1233,7 +1233,7 @@ namespace DotRecast.Detour.Crowd
                             continue;
                         }
 
-                        dist = (float)Math.Sqrt(dist);
+                        dist = MathF.Sqrt(dist);
                         float pen = (ag.option.radius + nei.option.radius) - dist;
                         if (dist < 0.0001f)
                         {

src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs

@@ -175,7 +175,7 @@ namespace DotRecast.Detour.Crowd
                 return false; // no intersection.
 
             a = 1.0f / a;
-            float rd = (float)Math.Sqrt(d);
+            float rd = MathF.Sqrt(d);
 
             tmin = (b - rd) * a;
             tmax = (b + rd) * a;
@@ -364,7 +364,7 @@ namespace DotRecast.Detour.Crowd
         // vector normalization that ignores the y-component.
         void DtNormalize2D(float[] v)
         {
-            float d = (float)Math.Sqrt(v[0] * v[0] + v[2] * v[2]);
+            float d = MathF.Sqrt(v[0] * v[0] + v[2] * v[2]);
             if (d == 0)
                 return;
             d = 1.0f / d;

src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs

@@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink
         protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc)
         {
             float cs = acfg.cellSize;
-            float dist = (float)Math.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q));
+            float dist = MathF.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q));
             int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs));
 
             SampleGroundSegment(heightFunc, es.start, ngsamples);

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -3302,7 +3302,7 @@ namespace DotRecast.Detour
                 hitNormal = RcVec3f.Normalize(new RcVec3f(tangent.Z, 0, -tangent.X));
             }
 
-            hitDist = (float)Math.Sqrt(radiusSqr);
+            hitDist = MathF.Sqrt(radiusSqr);
 
             return status;
         }

src/DotRecast.Detour/DtUtils.cs

@@ -193,7 +193,7 @@ namespace DotRecast.Detour
                 acc += dacc;
             }
 
-            float v = (float)Math.Sqrt(t);
+            float v = MathF.Sqrt(t);
 
             float a = 1 - v;
             float b = (1 - u) * v;

src/DotRecast.Recast.Demo/Draw/DebugDraw.cs

@@ -335,7 +335,7 @@ public class DebugDraw
         float dx = x1 - x0;
         float dy = y1 - y0;
         float dz = z1 - z0;
-        float len = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
+        float len = MathF.Sqrt(dx * dx + dy * dy + dz * dz);
         RcVec3f prev = new RcVec3f();
         EvalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, ref prev);
         for (int i = 1; i <= NUM_ARC_PTS; ++i)
@@ -677,7 +677,7 @@ public class DebugDraw
 
     private void NormalizePlane(float px, float py, float pz, float pw, ref float[] plane)
     {
-        float length = (float)Math.Sqrt(px * px + py * py + pz * pz);
+        float length = MathF.Sqrt(px * px + py * py + pz * pz);
         if (length != 0)
         {
             length = 1f / length;

src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs

@@ -424,7 +424,7 @@ public class TestNavmeshSampleTool : ISampleTool
                 dd.DepthMask(false);
                 float dx = m_epos.X - m_spos.X;
                 float dz = m_epos.Z - m_spos.Z;
-                float dist = (float)Math.Sqrt(dx * dx + dz * dz);
+                float dist = MathF.Sqrt(dx * dx + dz * dz);
                 dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
                 dd.DepthMask(true);
             }
@@ -566,7 +566,7 @@ public class TestNavmeshSampleTool : ISampleTool
                 dd.DepthMask(false);
                 float dx = m_epos.X - m_spos.X;
                 float dz = m_epos.Z - m_spos.Z;
-                float dist = (float)Math.Sqrt(dx * dx + dz * dz);
+                float dist = MathF.Sqrt(dx * dx + dz * dz);
                 dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
                 dd.DepthMask(true);
             }

src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs

@@ -101,7 +101,7 @@ namespace DotRecast.Recast.Toolset.Geom
                 normals[i] = e0.Y * e1.Z - e0.Z * e1.Y;
                 normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z;
                 normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X;
-                float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
+                float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
                 if (d > 0)
                 {
                     d = 1.0f / d;

src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs

@@ -74,7 +74,7 @@ namespace DotRecast.Recast.Toolset.Tools
 
                 // Find movement delta.
                 RcVec3f delta = RcVec3f.Subtract(steerPos, iterPos);
-                float len = (float)Math.Sqrt(RcVec3f.Dot(delta, delta));
+                float len = MathF.Sqrt(RcVec3f.Dot(delta, delta));
                 // If the steer target is end of path or off-mesh link, do not move past the location.
                 if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                 {
@@ -329,7 +329,7 @@ namespace DotRecast.Recast.Toolset.Tools
 
             float dx = epos.X - spos.X;
             float dz = epos.Z - spos.Z;
-            float dist = (float)Math.Sqrt(dx * dx + dz * dz);
+            float dist = MathF.Sqrt(dx * dx + dz * dz);
 
             List<long> tempResultRefs = new List<long>();
             List<long> tempParentRefs = new List<long>();
@@ -417,7 +417,7 @@ namespace DotRecast.Recast.Toolset.Tools
 
             float dx = epos.X - spos.X;
             float dz = epos.Z - spos.Z;
-            float dist = (float)Math.Sqrt(dx * dx + dz * dz);
+            float dist = MathF.Sqrt(dx * dx + dz * dz);
 
             IDtPolygonByCircleConstraint constraint = constrainByCircle
                 ? DtStrictDtPolygonByCircleConstraint.Shared

src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs

@@ -163,7 +163,7 @@ namespace DotRecast.Recast.Geom
                 normals[i] = e0.Y * e1.Z - e0.Z * e1.Y;
                 normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z;
                 normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X;
-                float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
+                float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
                 if (d > 0)
                 {
                     d = 1.0f / d;

src/DotRecast.Recast/RcAreas.cs

@@ -760,12 +760,12 @@ namespace DotRecast.Recast
                 // From A to B on the x/z plane
                 RcVec3f prevSegmentDir = RcVec3f.Subtract(vertB, vertA);
                 prevSegmentDir.Y = 0; // Squash onto x/z plane
-                prevSegmentDir.SafeNormalize();
+                prevSegmentDir = RcVecUtils.SafeNormalize(prevSegmentDir);
 
                 // From B to C on the x/z plane
                 RcVec3f currSegmentDir = RcVec3f.Subtract(vertC, vertB);
                 currSegmentDir.Y = 0; // Squash onto x/z plane
-                currSegmentDir.SafeNormalize();
+                currSegmentDir = RcVecUtils.SafeNormalize(currSegmentDir);
 
                 // The y component of the cross product of the two normalized segment directions.
                 // The X and Z components of the cross product are both zero because the two
@@ -792,7 +792,7 @@ namespace DotRecast.Recast
                 bool bevel = cornerMiterSqMag * MITER_LIMIT * MITER_LIMIT < 1.0f;
 
                 // Scale the corner miter so it's proportional to how much the corner should be offset compared to the edges.
-                if (cornerMiterSqMag > RcVec3f.EPSILON)
+                if (cornerMiterSqMag > RcVecUtils.EPSILON)
                 {
                     float scale = 1.0f / cornerMiterSqMag;
                     cornerMiterX *= scale;

src/DotRecast.Recast/RcFilledVolumeRasterization.cs

@@ -253,7 +253,7 @@ namespace DotRecast.Recast
                 return null;
             }
 
-            float discrSqrt = (float)Math.Sqrt(discr);
+            float discrSqrt = MathF.Sqrt(discr);
             float tmin = -b - discrSqrt;
             float tmax = -b + discrSqrt;
 
@@ -346,7 +346,7 @@ namespace DotRecast.Recast
             float discr = b * b - c;
             if (discr > EPSILON)
             {
-                float discrSqrt = (float)Math.Sqrt(discr);
+                float discrSqrt = MathF.Sqrt(discr);
                 float t1 = -b - discrSqrt;
                 float t2 = -b + discrSqrt;
                 if (t1 <= 1 && t2 >= 0)
@@ -454,7 +454,7 @@ namespace DotRecast.Recast
                 return null; // No real roots; no intersection
             }
 
-            float discSqrt = (float)Math.Sqrt(discr);
+            float discSqrt = MathF.Sqrt(discr);
             float t1 = (-b - discSqrt) / a;
             float t2 = (-b + discSqrt) / a;
 

src/DotRecast.Recast/RcMeshDetails.cs

@@ -60,7 +60,7 @@ namespace DotRecast.Recast
 
         private static float Vdist2(float[] verts, int p, int q)
         {
-            return (float)Math.Sqrt(VdistSq2(verts, p, q));
+            return MathF.Sqrt(VdistSq2(verts, p, q));
         }
 
         private static float VdistSq2(float[] p, float[] q)
@@ -88,17 +88,17 @@ namespace DotRecast.Recast
 
         private static float Vdist2(float[] p, float[] q)
         {
-            return (float)Math.Sqrt(VdistSq2(p, q));
+            return MathF.Sqrt(VdistSq2(p, q));
         }
 
         private static float Vdist2(RcVec3f p, RcVec3f q)
         {
-            return (float)Math.Sqrt(VdistSq2(p, q));
+            return MathF.Sqrt(VdistSq2(p, q));
         }
 
         private static float Vdist2(float[] p, RcVec3f q)
         {
-            return (float)Math.Sqrt(VdistSq2(p, q));
+            return MathF.Sqrt(VdistSq2(p, q));
         }
 
 
@@ -119,12 +119,12 @@ namespace DotRecast.Recast
 
         private static float Vdist2(float[] p, float[] verts, int q)
         {
-            return (float)Math.Sqrt(VdistSq2(p, verts, q));
+            return MathF.Sqrt(VdistSq2(p, verts, q));
         }
 
         private static float Vdist2(RcVec3f p, float[] verts, int q)
         {
-            return (float)Math.Sqrt(VdistSq2(p, verts, q));
+            return MathF.Sqrt(VdistSq2(p, verts, q));
         }
 
 
@@ -743,7 +743,7 @@ namespace DotRecast.Recast
                 minDist = Math.Min(minDist, maxEdgeDist);
             }
 
-            return (float)Math.Sqrt(minDist);
+            return MathF.Sqrt(minDist);
         }
 
         private static void TriangulateHull(int nverts, float[] verts, int nhull, int[] hull, int nin, List<int> tris)
@@ -890,7 +890,7 @@ namespace DotRecast.Recast
                     float dx = @in[vi + 0] - @in[vj + 0];
                     float dy = @in[vi + 1] - @in[vj + 1];
                     float dz = @in[vi + 2] - @in[vj + 2];
-                    float d = (float)Math.Sqrt(dx * dx + dz * dz);
+                    float d = MathF.Sqrt(dx * dx + dz * dz);
                     int nn = 1 + (int)Math.Floor(d / sampleDist);
                     if (nn >= MAX_VERTS_PER_EDGE)
                     {