diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index f02987c..e9a2aa8 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/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) diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 6261448..59c307d 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -110,7 +110,7 @@ namespace DotRecast.Core y = b; z = c; } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Set(float[] @in) { @@ -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), + }; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 53cdfc6..70607d3 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/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; } diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 3170387..478a7ea 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/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; } diff --git a/src/DotRecast.Detour.Crowd/PathCorridor.cs b/src/DotRecast.Detour.Crowd/PathCorridor.cs index d3f72ee..3eacd36 100644 --- a/src/DotRecast.Detour.Crowd/PathCorridor.cs +++ b/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 rc = navquery.Raycast(m_path[0], m_pos, goal, filter, 0, 0); if (rc.Succeeded()) diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 452bdd0..5b167eb 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/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); diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 0f1e61f..94700bd 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/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 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) {