RcMath VNormalize -> Vector3f.Normalize

src/DotRecast.Core/RcMath.cs

@@ -304,17 +304,6 @@ namespace DotRecast.Core
             }
         }
 
-        public static void VNormalize(ref Vector3f v)
-        {
-            float d = (float)(1.0f / Math.Sqrt(Sqr(v.x) + Sqr(v.y) + Sqr(v.z)));
-            if (d != 0)
-            {
-                v.x *= d;
-                v.y *= d;
-                v.z *= d;
-            }
-        }
-
 
         /// Performs a 'sloppy' colocation check of the specified points.
         /// @param[in] p0 A point. [(x, y, z)]

src/DotRecast.Core/Vector3f.cs

@@ -172,5 +172,17 @@ namespace DotRecast.Core
                 (v1.x * v2.y) - (v1.y * v2.x)
             );
         }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void Normalize()
+        {
+            float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(x) + RcMath.Sqr(y) + RcMath.Sqr(z)));
+            if (d != 0)
+            {
+                x *= d;
+                y *= d;
+                z *= d;
+            }
+        }
     }
 }

src/DotRecast.Detour.Crowd/CrowdAgent.cs

@@ -182,8 +182,7 @@ namespace DotRecast.Detour.Crowd
                 dir.x = dir0.x - dir1.x * len0 * 0.5f;
                 dir.y = 0;
                 dir.z = dir0.z - dir1.z * len0 * 0.5f;
-
-                VNormalize(ref dir);
+                dir.Normalize();
             }
 
             return dir;
@@ -196,7 +195,7 @@ namespace DotRecast.Detour.Crowd
             {
                 dir = corners[0].GetPos().Subtract(npos);
                 dir.y = 0;
-                VNormalize(ref dir);
+                dir.Normalize();
             }
 
             return dir;

src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs

@@ -128,7 +128,7 @@ namespace DotRecast.Detour.Crowd
                 Vector3f orig = new Vector3f();
                 Vector3f dv = new Vector3f();
                 cir.dp = pb.Subtract(pa);
-                VNormalize(ref cir.dp);
+                cir.dp.Normalize();
                 dv = cir.dvel.Subtract(dvel);
 
                 float a = TriArea2D(orig, cir.dp, dv);

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

@@ -18,9 +18,9 @@ namespace DotRecast.Detour.Extras.Jumplink
         {
             this.trajectory = trajectory;
             ax = edge.sq.Subtract(edge.sp);
-            VNormalize(ref ax);
+            ax.Normalize();
             VSet(ref az, ax.z, 0, -ax.x);
-            VNormalize(ref az);
+            az.Normalize();
             VSet(ref ay, 0, 1, 0);
         }
     }

src/DotRecast.Detour/LegacyNavMeshQuery.cs

@@ -848,7 +848,7 @@ namespace DotRecast.Detour
                 hitNormal.x = tangent.z;
                 hitNormal.y = 0;
                 hitNormal.z = -tangent.x;
-                VNormalize(ref hitNormal);
+                hitNormal.Normalize();
             }
 
             return Results.Success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal));

src/DotRecast.Detour/NavMeshQuery.cs

@@ -2400,7 +2400,7 @@ namespace DotRecast.Detour
                     hit.hitNormal.x = dz;
                     hit.hitNormal.y = 0;
                     hit.hitNormal.z = -dx;
-                    VNormalize(ref hit.hitNormal);
+                    hit.hitNormal.Normalize();
                     return Results.Success(hit);
                 }
 
@@ -3372,7 +3372,7 @@ namespace DotRecast.Detour
                 hitNormal.x = tangent.z;
                 hitNormal.y = 0;
                 hitNormal.z = -tangent.x;
-                VNormalize(ref hitNormal);
+                hitNormal.Normalize();
             }
 
             return Results.Success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal));

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

@@ -314,7 +314,7 @@ public class CrowdTool : Tool
     {
         Vector3f vel = tgt.Subtract(pos);
         vel.y = 0.0f;
-        VNormalize(ref vel);
+        vel.Normalize();
         return VScale(vel, speed);
     }
 

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

@@ -201,7 +201,7 @@ public class DynamicUpdateTool : Tool
             0.01f + (float)random.NextDouble(),
             (1f - 2 * (float)random.NextDouble())
         );
-        VNormalize(ref a);
+        a.Normalize();
         float len = 1f + (float)random.NextDouble() * 20f;
         a.x *= len;
         a.y *= len;
@@ -247,7 +247,7 @@ public class DynamicUpdateTool : Tool
         Vector3f baseCenter = Vector3f.Of(p.x, p.y + 3, p.z);
         Vector3f baseUp = Vector3f.Of(0, 1, 0);
         Vector3f forward = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
-        VNormalize(ref forward);
+        forward.Normalize();
         Vector3f side = Vector3f.Cross(forward, baseUp);
         BoxCollider @base = new BoxCollider(baseCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(baseUp, forward, baseExtent),
             SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb);

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

@@ -858,7 +858,7 @@ public class TestNavmeshTool : Tool
                                 Vector3f delta = s3.Subtract(s.vmin);
                                 Vector3f p0 = VMad(s.vmin, delta, 0.5f);
                                 Vector3f norm = Vector3f.Of(delta.z, 0, -delta.x);
-                                VNormalize(ref norm);
+                                norm.Normalize();
                                 Vector3f p1 = VMad(p0, norm, agentRadius * 0.5f);
                                 // Skip backfacing segments.
                                 if (wallSegments.GetSegmentRef(j) != 0)

test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs

@@ -155,7 +155,7 @@ public class AbstractCrowdTest
     {
         Vector3f vel = tgt.Subtract(pos);
         vel.y = 0.0f;
-        VNormalize(ref vel);
+        vel.Normalize();
         vel = VScale(vel, speed);
         return vel;
     }