move VMad -> Vector3f.Mad

src/DotRecast.Core/RcMath.cs

@@ -91,19 +91,6 @@ namespace DotRecast.Core
         }
 
 
-        /// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
-        /// @param[out] dest The result vector. [(x, y, z)]
-        /// @param[in] v1 The base vector. [(x, y, z)]
-        /// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)]
-        /// @param[in] s The amount to scale @p v2 by before adding to @p v1.
-        public static Vector3f VMad(Vector3f v1, Vector3f v2, float s)
-        {
-            Vector3f dest = new Vector3f();
-            dest.x = v1.x + v2.x * s;
-            dest.y = v1.y + v2.y * s;
-            dest.z = v1.z + v2.z * s;
-            return dest;
-        }
 
         /// Performs a linear interpolation between two vectors. (@p v1 toward @p
         /// v2)

src/DotRecast.Core/Vector3f.cs

@@ -309,5 +309,21 @@ namespace DotRecast.Core
         {
             return (a.x * b.z) - (a.z * b.x);
         }
+
+        /// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
+        /// @param[out] dest The result vector. [(x, y, z)]
+        /// @param[in] v1 The base vector. [(x, y, z)]
+        /// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)]
+        /// @param[in] s The amount to scale @p v2 by before adding to @p v1.
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3f Mad(Vector3f v1, Vector3f v2, float s)
+        {
+            return new Vector3f()
+            {
+                x = v1.x + (v2.x * s),
+                y = v1.y + (v2.y * s),
+                z = v1.z + (v2.z * s),
+            };
+        }
     }
 }

src/DotRecast.Detour.Crowd/Crowd.cs

@@ -1108,14 +1108,14 @@ namespace DotRecast.Detour.Crowd
                         float dist = (float)Math.Sqrt(distSqr);
                         float weight = separationWeight * (1.0f - Sqr(dist * invSeparationDist));
 
-                        disp = VMad(disp, diff, weight / dist);
+                        disp = Vector3f.Mad(disp, diff, weight / dist);
                         w += 1.0f;
                     }
 
                     if (w > 0.0001f)
                     {
                         // Adjust desired velocity.
-                        dvel = VMad(dvel, disp, 1.0f / w);
+                        dvel = Vector3f.Mad(dvel, disp, 1.0f / w);
                         // Clamp desired velocity to desired speed.
                         float speedSqr = VLenSqr(dvel);
                         float desiredSqr = Sqr(ag.desiredSpeed);
@@ -1275,7 +1275,7 @@ namespace DotRecast.Detour.Crowd
                             pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor;
                         }
 
-                        ag.disp = VMad(ag.disp, diff, pen);
+                        ag.disp = Vector3f.Mad(ag.disp, diff, pen);
 
                         w += 1.0f;
                     }

src/DotRecast.Detour.Crowd/CrowdAgent.cs

@@ -123,7 +123,7 @@ namespace DotRecast.Detour.Crowd
 
             // Integrate
             if (vel.Length() > 0.0001f)
-                npos = VMad(npos, vel, dt);
+                npos = Vector3f.Mad(npos, vel, dt);
             else
                 vel = Vector3f.Zero;
         }

src/DotRecast.Detour.Crowd/PathCorridor.cs

@@ -316,7 +316,7 @@ namespace DotRecast.Detour.Crowd
 
             // Adjust ray length.
             var delta = next.Subtract(m_pos);
-            Vector3f goal = VMad(m_pos, delta, pathOptimizationRange / dist);
+            Vector3f goal = Vector3f.Mad(m_pos, delta, pathOptimizationRange / dist);
 
             Result<RaycastHit> rc = navquery.Raycast(m_path[0], m_pos, goal, filter, 0, 0);
             if (rc.Succeeded())

src/DotRecast.Detour/NavMeshQuery.cs

@@ -2373,7 +2373,7 @@ namespace DotRecast.Detour
                     // compute the intersection point at the furthest end of the polygon
                     // and correct the height (since the raycast moves in 2d)
                     lastPos = curPos;
-                    curPos = VMad(startPos, dir, hit.t);
+                    curPos = Vector3f.Mad(startPos, dir, hit.t);
                     var e1 = Vector3f.Of(verts, iresult.segMax * 3);
                     var e2 = Vector3f.Of(verts, ((iresult.segMax + 1) % nv) * 3);
                     var eDir = e2.Subtract(e1);

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

@@ -237,7 +237,7 @@ public class TestNavmeshTool : Tool
                             len = STEP_SIZE / len;
                         }
 
-                        Vector3f moveTgt = VMad(iterPos, delta, len);
+                        Vector3f moveTgt = Vector3f.Mad(iterPos, delta, len);
                         // Move
                         Result<MoveAlongSurfaceResult> result = m_navQuery.MoveAlongSurface(polys[0], iterPos,
                             moveTgt, m_filter);
@@ -856,10 +856,10 @@ public class TestNavmeshTool : Tool
                                 }
 
                                 Vector3f delta = s3.Subtract(s.vmin);
-                                Vector3f p0 = VMad(s.vmin, delta, 0.5f);
+                                Vector3f p0 = Vector3f.Mad(s.vmin, delta, 0.5f);
                                 Vector3f norm = Vector3f.Of(delta.z, 0, -delta.x);
                                 norm.Normalize();
-                                Vector3f p1 = VMad(p0, norm, agentRadius * 0.5f);
+                                Vector3f p1 = Vector3f.Mad(p0, norm, agentRadius * 0.5f);
                                 // Skip backfacing segments.
                                 if (wallSegments.GetSegmentRef(j) != 0)
                                 {