refactor: RcVec3f.Normalize now corresponds to System.Numerics.Vector3.Normalize in the switching build

src/DotRecast.Core/Numerics/RcVec3f.cs

@@ -23,6 +23,8 @@ namespace DotRecast.Core.Numerics
 {
     public struct RcVec3f
     {
+        public const float EPSILON = 1e-6f;
+
         public float X;
         public float Y;
         public float Z;
@@ -148,21 +150,6 @@ namespace DotRecast.Core.Numerics
             return hash;
         }
 
-        /// Normalizes the vector.
-        /// @param[in,out] v The vector to normalize. [(x, y, z)]
-        [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;
-            }
-        }
-
-        public const float EPSILON = 1e-6f;
 
         /// Normalizes the vector if the length is greater than zero.
         /// If the magnitude is zero, the vector is unchanged.
@@ -535,13 +522,23 @@ namespace DotRecast.Core.Numerics
             dest.Z = v1.X * v2.Y - v1.Y * v2.X;
         }
 
+        /// Normalizes the vector.
+        /// @param[in,out] v The vector to normalize. [(x, y, z)]
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Normalize(ref RcVec3f v)
+        public static RcVec3f Normalize(RcVec3f v)
         {
-            float d = (float)(1.0f / Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z));
+            float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z)));
-            v.X *= d;
+
-            v.Y *= d;
+            if (d != 0)
-            v.Z *= d;
+            {
+                return new RcVec3f(
+                    v.X *= d,
+                    v.Y *= d,
+                    v.Z *= d
+                );
+            }
+
+            return v;
         }
     }
 }

src/DotRecast.Core/Numerics/RcVecExtensions.cs

@@ -29,6 +29,7 @@ namespace DotRecast.Core.Numerics
             }
         }
 
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static RcVec3f Scale(this RcVec3f v, float scale)
         {

src/DotRecast.Detour.Crowd/DtCrowdAgent.cs

@@ -180,7 +180,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;
-                dir.Normalize();
+                dir = RcVec3f.Normalize(dir);
             }
 
             return dir;
@@ -193,7 +193,7 @@ namespace DotRecast.Detour.Crowd
             {
                 dir = RcVec3f.Subtract(corners[0].pos, npos);
                 dir.Y = 0;
-                dir.Normalize();
+                dir = RcVec3f.Normalize(dir);
             }
 
             return dir;

src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs

@@ -127,7 +127,7 @@ namespace DotRecast.Detour.Crowd
                 RcVec3f orig = new RcVec3f();
                 RcVec3f dv = new RcVec3f();
                 cir.dp = RcVec3f.Subtract(pb, pa);
-                cir.dp.Normalize();
+                cir.dp = RcVec3f.Normalize(cir.dp);
                 dv = RcVec3f.Subtract(cir.dvel, dvel);
 
                 float a = DtUtils.TriArea2D(orig, cir.dp, dv);

src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs

@@ -76,11 +76,12 @@ namespace DotRecast.Detour.Dynamic.Colliders
                 new RcVec3f(up.X, up.Y, up.Z),
                 RcVec3f.Zero
             };
-            RcVec3f.Normalize(ref halfEdges[1]);
+            
+            halfEdges[1] = RcVec3f.Normalize(halfEdges[1]);
             RcVec3f.Cross(ref halfEdges[0], up, forward);
-            RcVec3f.Normalize(ref halfEdges[0]);
+            halfEdges[0] = RcVec3f.Normalize(halfEdges[0]);
             RcVec3f.Cross(ref halfEdges[2], halfEdges[0], up);
-            RcVec3f.Normalize(ref halfEdges[2]);
+            halfEdges[2] = RcVec3f.Normalize(halfEdges[2]);
             halfEdges[0].X *= extent.X;
             halfEdges[0].Y *= extent.X;
             halfEdges[0].Z *= extent.X;

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

@@ -17,9 +17,11 @@ namespace DotRecast.Detour.Extras.Jumplink
         {
             this.trajectory = trajectory;
             ax = RcVec3f.Subtract(edge.sq, edge.sp);
-            ax.Normalize();
+            ax = RcVec3f.Normalize(ax);
+            
             az = new RcVec3f(ax.Z, 0, -ax.X);
-            az.Normalize();
+            az = RcVec3f.Normalize(az);
+            
             ay = new RcVec3f(0, 1, 0);
         }
     }

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -2363,10 +2363,7 @@ namespace DotRecast.Detour
                     // int vb = b * 3;
                     float dx = verts[b].X - verts[a].X;
                     float dz = verts[b].Z - verts[a].X;
-                    hit.hitNormal.X = dz;
+                    hit.hitNormal = RcVec3f.Normalize(new RcVec3f(dz, 0, -dx));
-                    hit.hitNormal.Y = 0;
-                    hit.hitNormal.Z = -dx;
-                    hit.hitNormal.Normalize();
                     return DtStatus.DT_SUCCSESS;
                 }
 
@@ -3302,10 +3299,7 @@ namespace DotRecast.Detour
             if (hasBestV)
             {
                 var tangent = RcVec3f.Subtract(bestvi, bestvj);
-                hitNormal.X = tangent.Z;
+                hitNormal = RcVec3f.Normalize(new RcVec3f(tangent.Z, 0, -tangent.X));
-                hitNormal.Y = 0;
-                hitNormal.Z = -tangent.X;
-                hitNormal.Normalize();
             }
 
             hitDist = (float)Math.Sqrt(radiusSqr);

src/DotRecast.Recast.Demo/RecastDemo.cs

@@ -786,8 +786,10 @@ public class RecastDemo : IRecastDemoChannel
         }
 
         RcVec3f rayDir = new RcVec3f(rayEnd.X - rayStart.X, rayEnd.Y - rayStart.Y, rayEnd.Z - rayStart.Z);
+        rayDir = RcVec3f.Normalize(rayDir);
+        
         ISampleTool raySampleTool = toolset.GetTool();
-        rayDir.Normalize();
+        
         if (raySampleTool != null)
         {
             Logger.Information($"click ray - tool({raySampleTool.GetTool().GetName()}) rayStart({rayStart.X:0.#},{rayStart.Y:0.#},{rayStart.Z:0.#}) pos({rayDir.X:0.#},{rayDir.Y:0.#},{rayDir.Z:0.#}) shift({processHitTestShift})");

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

@@ -50,7 +50,7 @@ public static class GizmoRenderer
         normal.X = e0.Y * e1.Z - e0.Z * e1.Y;
         normal.Y = e0.Z * e1.X - e0.X * e1.Z;
         normal.Z = e0.X * e1.Y - e0.Y * e1.X;
-        RcVec3f.Normalize(ref normal);
+        normal = RcVec3f.Normalize(normal);
         float c = Math.Clamp(0.57735026f * (normal.X + normal.Y + normal.Z), -1, 1);
         int col = DebugDraw.DuLerpCol(
             DebugDraw.DuRGBA(32, 32, 0, 160),

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

@@ -510,7 +510,7 @@ public class TestNavmeshSampleTool : ISampleTool
                                 RcVec3f delta = RcVec3f.Subtract(s3, s.vmin);
                                 RcVec3f p0 = RcVec3f.Mad(s.vmin, delta, 0.5f);
                                 RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X);
-                                norm.Normalize();
+                                norm = RcVec3f.Normalize(norm);
                                 RcVec3f p1 = RcVec3f.Mad(p0, norm, agentRadius * 0.5f);
                                 // Skip backfacing segments.
                                 if (segmentRefs[j] != 0)

src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs

@@ -22,11 +22,11 @@ namespace DotRecast.Recast.Toolset.Gizmos
             RcVec3f axis = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z);
             RcVec3f[] normals = new RcVec3f[3];
             normals[1] = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z);
-            RcVec3f.Normalize(ref normals[1]);
+            normals[1] = RcVec3f.Normalize(normals[1]);
             normals[0] = GetSideVector(axis);
             normals[2] = RcVec3f.Zero;
             RcVec3f.Cross(ref normals[2], normals[0], normals[1]);
-            RcVec3f.Normalize(ref normals[2]);
+            normals[2] = RcVec3f.Normalize(normals[2]);
             triangles = GenerateSphericalTriangles();
             var trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X);
             var trY = new RcVec3f(normals[0].Y, normals[1].Y, normals[2].Y);
@@ -48,7 +48,7 @@ namespace DotRecast.Recast.Toolset.Gizmos
                 v.X = vertices[i] - center[0];
                 v.Y = vertices[i + 1] - center[1];
                 v.Z = vertices[i + 2] - center[2];
-                RcVec3f.Normalize(ref v);
+                v = RcVec3f.Normalize(v);
                 gradient[i / 3] = Math.Clamp(0.57735026f * (v.X + v.Y + v.Z), -1, 1);
             }
         }
@@ -64,7 +64,7 @@ namespace DotRecast.Recast.Toolset.Gizmos
             RcVec3f forward = new RcVec3f();
             RcVec3f.Cross(ref forward, side, axis);
             RcVec3f.Cross(ref side, axis, forward);
-            RcVec3f.Normalize(ref side);
+            side = RcVec3f.Normalize(side);
             return side;
         }
     }

src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs

@@ -22,11 +22,11 @@ namespace DotRecast.Recast.Toolset.Gizmos
             RcVec3f axis = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z);
             RcVec3f[] normals = new RcVec3f[3];
             normals[1] = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z);
-            RcVec3f.Normalize(ref normals[1]);
+            normals[1] = RcVec3f.Normalize(normals[1]);
             normals[0] = GetSideVector(axis);
             normals[2] = RcVec3f.Zero;
             RcVec3f.Cross(ref normals[2], normals[0], normals[1]);
-            RcVec3f.Normalize(ref normals[2]);
+            normals[2] = RcVec3f.Normalize(normals[2]);
             triangles = GenerateCylindricalTriangles();
             RcVec3f trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X);
             RcVec3f trY = new RcVec3f(normals[0].Y, normals[1].Y, normals[2].Y);
@@ -53,7 +53,7 @@ namespace DotRecast.Recast.Toolset.Gizmos
                     v.X = vertices[i] - center.X;
                     v.Y = vertices[i + 1] - center.Y;
                     v.Z = vertices[i + 2] - center.Z;
-                    RcVec3f.Normalize(ref v);
+                    v = RcVec3f.Normalize(v);
                     gradient[i / 3] = Math.Clamp(0.57735026f * (v.X + v.Y + v.Z), -1, 1);
                 }
             }
@@ -70,7 +70,7 @@ namespace DotRecast.Recast.Toolset.Gizmos
             RcVec3f forward = new RcVec3f();
             RcVec3f.Cross(ref forward, side, axis);
             RcVec3f.Cross(ref side, axis, forward);
-            RcVec3f.Normalize(ref side);
+            side = RcVec3f.Normalize(side);
             return side;
         }
     }

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

@@ -296,7 +296,7 @@ namespace DotRecast.Recast.Toolset.Tools
         {
             RcVec3f vel = RcVec3f.Subtract(tgt, pos);
             vel.Y = 0.0f;
-            vel.Normalize();
+            vel = RcVec3f.Normalize(vel);
             return vel.Scale(speed);
         }
 

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

@@ -183,7 +183,8 @@ namespace DotRecast.Recast.Toolset.Tools
                 0.01f + (float)random.NextDouble(),
                 (1f - 2 * (float)random.NextDouble())
             );
-            a.Normalize();
+            a = RcVec3f.Normalize(a);
+            
             float len = 1f + (float)random.NextDouble() * 20f;
             a.X *= len;
             a.Y *= len;
@@ -214,7 +215,7 @@ namespace DotRecast.Recast.Toolset.Tools
         {
             float radius = 0.7f + (float)random.NextDouble() * 4f;
             RcVec3f a = new RcVec3f(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble());
-            a.Normalize();
+            a = RcVec3f.Normalize(a);
             float len = 2f + (float)random.NextDouble() * 20f;
             a[0] *= len;
             a[1] *= len;
@@ -233,7 +234,8 @@ namespace DotRecast.Recast.Toolset.Tools
             RcVec3f baseCenter = new RcVec3f(p.X, p.Y + 3, p.Z);
             RcVec3f baseUp = new RcVec3f(0, 1, 0);
             RcVec3f forward = new RcVec3f((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
-            forward.Normalize();
+            forward = RcVec3f.Normalize(forward);
+            
             RcVec3f side = RcVec3f.Cross(forward, baseUp);
             DtBoxCollider @base = new DtBoxCollider(baseCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(baseUp, forward, baseExtent),
                 SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb);

src/DotRecast.Recast/RcCommons.cs

@@ -143,7 +143,7 @@ namespace DotRecast.Recast
             RcVec3f.Sub(ref e0, verts, v1 * 3, v0 * 3);
             RcVec3f.Sub(ref e1, verts, v2 * 3, v0 * 3);
             RcVec3f.Cross(ref norm, e0, e1);
-            RcVec3f.Normalize(ref norm);
+            norm = RcVec3f.Normalize(norm);
         }
 
 

src/DotRecast.Recast/RcFilledVolumeRasterization.cs

@@ -79,9 +79,9 @@ namespace DotRecast.Recast
                 new RcVec3f(halfEdges[1].X, halfEdges[1].Y, halfEdges[1].Z),
                 new RcVec3f(halfEdges[2].X, halfEdges[2].Y, halfEdges[2].Z),
             };
-            RcVec3f.Normalize(ref normals[0]);
+            normals[0] = RcVec3f.Normalize(normals[0]);
-            RcVec3f.Normalize(ref normals[1]);
+            normals[1] = RcVec3f.Normalize(normals[1]);
-            RcVec3f.Normalize(ref normals[2]);
+            normals[2] = RcVec3f.Normalize(normals[2]);
 
             float[] vertices = new float[8 * 3];
             float[] bounds = new float[]

test/DotRecast.Core.Test/Vector3Tests.cs

@@ -0,0 +1,78 @@
+using System;
+using System.Numerics;
+using DotRecast.Core.Numerics;
+using NUnit.Framework;
+
+namespace DotRecast.Core.Test;
+
+public class Vector3Tests
+{
+    [Test]
+    [Repeat(10000)]
+    public void TestVectorLength()
+    {
+        var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+
+        Assert.That(v1.Length(), Is.EqualTo(v11.Length()));
+        Assert.That(v1.LengthSquared(), Is.EqualTo(v11.LengthSquared()));
+    }
+
+    [Test]
+    [Repeat(10000)]
+    public void TestVectorSubtract()
+    {
+        var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v3 = Vector3.Subtract(v1, v2);
+        var v4 = v1 - v2;
+        Assert.That(v3, Is.EqualTo(v4));
+
+        var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+        var v22 = new RcVec3f(v2.X, v2.Y, v2.Z);
+        var v33 = RcVec3f.Subtract(v11, v22);
+        var v44 = v11 - v22;
+        Assert.That(v33, Is.EqualTo(v44));
+
+        Assert.That(v3.X, Is.EqualTo(v33.X));
+        Assert.That(v3.Y, Is.EqualTo(v33.Y));
+        Assert.That(v3.Z, Is.EqualTo(v33.Z));
+    }
+
+
+    [Test]
+    [Repeat(10000)]
+    public void TestVectorAdd()
+    {
+        var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v3 = Vector3.Add(v1, v2);
+        var v4 = v1 + v2;
+        Assert.That(v3, Is.EqualTo(v4));
+
+        var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+        var v22 = new RcVec3f(v2.X, v2.Y, v2.Z);
+        var v33 = RcVec3f.Add(v11, v22);
+        var v44 = v11 + v22;
+        Assert.That(v33, Is.EqualTo(v44));
+
+        Assert.That(v3.X, Is.EqualTo(v33.X));
+        Assert.That(v3.Y, Is.EqualTo(v33.Y));
+        Assert.That(v3.Z, Is.EqualTo(v33.Z));
+    }
+
+    [Test]
+    [Repeat(10000)]
+    public void TestVectorNormalize()
+    {
+        var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle());
+        var v2 = Vector3.Normalize(v1);
+
+        var v11 = new RcVec3f(v1.X, v1.Y, v1.Z);
+        var v22 = RcVec3f.Normalize(v11);
+
+        Assert.That(v2.X, Is.EqualTo(v22.X).Within(0.000001d));
+        Assert.That(v2.Y, Is.EqualTo(v22.Y).Within(0.000001d));
+        Assert.That(v2.Z, Is.EqualTo(v22.Z).Within(0.000001d));
+    }
+}

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

@@ -21,13 +21,10 @@ freely, subject to the following restrictions:
 using System;
 using System.Collections.Generic;
 using DotRecast.Core.Numerics;
-
 using NUnit.Framework;
 
 namespace DotRecast.Detour.Crowd.Test;
 
-
-
 [Parallelizable]
 public class AbstractCrowdTest
 {
@@ -155,7 +152,7 @@ public class AbstractCrowdTest
     {
         RcVec3f vel = RcVec3f.Subtract(tgt, pos);
         vel.Y = 0.0f;
-        vel.Normalize();
+        vel = RcVec3f.Normalize(vel);
         vel = vel.Scale(speed);
         return vel;
     }