Vector3f inline

src/DotRecast.Core/RcMath.cs

@@ -37,7 +37,7 @@ namespace DotRecast.Core
             float totd = 0;
             for (int i = 0; i < npath - 1; ++i)
             {
-                totd += (float)Math.Sqrt(VDistSqr(path, i * 3, (i + 1) * 3));
+                totd += (float)Math.Sqrt(Vector3f.DistSqr(path, i * 3, (i + 1) * 3));
             }
 
             return totd;
@@ -65,41 +65,6 @@ namespace DotRecast.Core
         }
 
 
-        public static float VDistSqr(Vector3f v1, float[] v2, int i)
-        {
-            float dx = v2[i] - v1.x;
-            float dy = v2[i + 1] - v1.y;
-            float dz = v2[i + 2] - v1.z;
-            return dx * dx + dy * dy + dz * dz;
-        }
-
-        public static float VDistSqr(Vector3f v1, Vector3f v2)
-        {
-            float dx = v2.x - v1.x;
-            float dy = v2.y - v1.y;
-            float dz = v2.z - v1.z;
-            return dx * dx + dy * dy + dz * dz;
-        }
-
-        public static float VDistSqr(float[] v, int i, int j)
-        {
-            float dx = v[i] - v[j];
-            float dy = v[i + 1] - v[j + 1];
-            float dz = v[i + 2] - v[j + 2];
-            return dx * dx + dy * dy + dz * dz;
-        }
-
-        /// Returns the distance between two points.
-        /// @param[in] v1 A point. [(x, y, z)]
-        /// @param[in] v2 A point. [(x, y, z)]
-        /// @return The distance between the two points.
-        public static float VDistSqr(float[] v1, float[] v2)
-        {
-            float dx = v2[0] - v1[0];
-            float dy = v2[1] - v1[1];
-            float dz = v2[2] - v1[2];
-            return dx * dx + dy * dy + dz * dz;
-        }
 
 
         /// Derives the square of the scalar length of the vector. (len * len)
@@ -111,49 +76,7 @@ namespace DotRecast.Core
         }
 
 
-        /// Derives the distance between the specified points on the xz-plane.
-        /// @param[in] v1 A point. [(x, y, z)]
-        /// @param[in] v2 A point. [(x, y, z)]
-        /// @return The distance between the point on the xz-plane.
-        ///
-        /// The vectors are projected onto the xz-plane, so the y-values are
-        /// ignored.
-        public static float VDist2D(float[] v1, float[] v2)
-        {
-            float dx = v2[0] - v1[0];
-            float dz = v2[2] - v1[2];
-            return (float)Math.Sqrt(dx * dx + dz * dz);
-        }
 
-        public static float VDist2D(Vector3f v1, Vector3f v2)
-        {
-            float dx = v2.x - v1.x;
-            float dz = v2.z - v1.z;
-            return (float)Math.Sqrt(dx * dx + dz * dz);
-        }
-
-
-        public static float VDist2DSqr(float[] v1, float[] v2)
-        {
-            float dx = v2[0] - v1[0];
-            float dz = v2[2] - v1[2];
-            return dx * dx + dz * dz;
-        }
-
-        public static float VDist2DSqr(Vector3f v1, Vector3f v2)
-        {
-            float dx = v2.x - v1.x;
-            float dz = v2.z - v1.z;
-            return dx * dx + dz * dz;
-        }
-
-
-        public static float VDist2DSqr(Vector3f p, float[] verts, int i)
-        {
-            float dx = verts[i] - p.x;
-            float dz = verts[i + 2] - p.z;
-            return dx * dx + dz * dz;
-        }
 
         /// Normalizes the vector.
         /// @param[in,out] v The vector to normalize. [(x, y, z)]
@@ -183,7 +106,7 @@ namespace DotRecast.Core
 
         public static bool VEqual(Vector3f p0, Vector3f p1, float thresholdSqr)
         {
-            float d = VDistSqr(p0, p1);
+            float d = Vector3f.DistSqr(p0, p1);
             return d < thresholdSqr;
         }
 

src/DotRecast.Core/Vector3f.cs

@@ -357,5 +357,94 @@ namespace DotRecast.Core
                 verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t
             );
         }
+
+
+        /// Returns the distance between two points.
+        /// @param[in] v1 A point. [(x, y, z)]
+        /// @param[in] v2 A point. [(x, y, z)]
+        /// @return The distance between the two points.
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float DistSqr(Vector3f v1, float[] v2, int i)
+        {
+            float dx = v2[i] - v1.x;
+            float dy = v2[i + 1] - v1.y;
+            float dz = v2[i + 2] - v1.z;
+            return dx * dx + dy * dy + dz * dz;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float DistSqr(Vector3f v1, Vector3f v2)
+        {
+            float dx = v2.x - v1.x;
+            float dy = v2.y - v1.y;
+            float dz = v2.z - v1.z;
+            return dx * dx + dy * dy + dz * dz;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float DistSqr(float[] v, int i, int j)
+        {
+            float dx = v[i] - v[j];
+            float dy = v[i + 1] - v[j + 1];
+            float dz = v[i + 2] - v[j + 2];
+            return dx * dx + dy * dy + dz * dz;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float DistSqr(float[] v1, float[] v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dy = v2[1] - v1[1];
+            float dz = v2[2] - v1[2];
+            return dx * dx + dy * dy + dz * dz;
+        }
+
+        /// Derives the distance between the specified points on the xz-plane.
+        /// @param[in] v1 A point. [(x, y, z)]
+        /// @param[in] v2 A point. [(x, y, z)]
+        /// @return The distance between the point on the xz-plane.
+        ///
+        /// The vectors are projected onto the xz-plane, so the y-values are
+        /// ignored.
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2D(float[] v1, float[] v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dz = v2[2] - v1[2];
+            return (float)Math.Sqrt(dx * dx + dz * dz);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2D(Vector3f v1, Vector3f v2)
+        {
+            float dx = v2.x - v1.x;
+            float dz = v2.z - v1.z;
+            return (float)Math.Sqrt(dx * dx + dz * dz);
+        }
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2DSqr(float[] v1, float[] v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dz = v2[2] - v1[2];
+            return dx * dx + dz * dz;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2DSqr(Vector3f v1, Vector3f v2)
+        {
+            float dx = v2.x - v1.x;
+            float dz = v2.z - v1.z;
+            return dx * dx + dz * dz;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2DSqr(Vector3f p, float[] verts, int i)
+        {
+            float dx = verts[i] - p.x;
+            float dz = verts[i + 2] - p.z;
+            return dx * dx + dz * dz;
+        }
     }
 }

src/DotRecast.Detour.Crowd/Crowd.cs

@@ -890,7 +890,7 @@ namespace DotRecast.Detour.Crowd
                 // Update the collision boundary after certain distance has been passed or
                 // if it has become invalid.
                 float updateThr = ag.option.collisionQueryRange * 0.25f;
-                if (VDist2DSqr(ag.npos, ag.boundary.GetCenter()) > Sqr(updateThr)
+                if (Vector3f.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > Sqr(updateThr)
                     || !ag.boundary.IsValid(_navQuery, _filters[ag.option.queryFilterType]))
                 {
                     ag.boundary.Update(ag.corridor.GetFirstPoly(), ag.npos, ag.option.collisionQueryRange, _navQuery,
@@ -1018,7 +1018,7 @@ namespace DotRecast.Detour.Crowd
                         anim.polyRef = refs[1];
                         anim.active = true;
                         anim.t = 0.0f;
-                        anim.tmax = (VDist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
+                        anim.tmax = (Vector3f.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
 
                         ag.state = CrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH;
                         ag.corners.Clear();

src/DotRecast.Detour.Crowd/CrowdAgent.cs

@@ -139,7 +139,7 @@ namespace DotRecast.Detour.Crowd
                 : false;
             if (offMeshConnection)
             {
-                float distSq = VDist2DSqr(npos, corners[corners.Count - 1].GetPos());
+                float distSq = Vector3f.Dist2DSqr(npos, corners[corners.Count - 1].GetPos());
                 if (distSq < radius * radius)
                     return true;
             }
@@ -154,7 +154,7 @@ namespace DotRecast.Detour.Crowd
 
             bool endOfPath = ((corners[corners.Count - 1].GetFlags() & NavMeshQuery.DT_STRAIGHTPATH_END) != 0) ? true : false;
             if (endOfPath)
-                return Math.Min(VDist2D(npos, corners[corners.Count - 1].GetPos()), range);
+                return Math.Min(Vector3f.Dist2D(npos, corners[corners.Count - 1].GetPos()), range);
 
             return range;
         }

src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs

@@ -209,8 +209,8 @@ namespace DotRecast.Detour.Crowd
             float minPenalty, ObstacleAvoidanceDebugData debug)
         {
             // penalty for straying away from the desired and current velocities
-            float vpen = m_params.weightDesVel * (VDist2D(vcand, dvel) * m_invVmax);
+            float vpen = m_params.weightDesVel * (Vector3f.Dist2D(vcand, dvel) * m_invVmax);
-            float vcpen = m_params.weightCurVel * (VDist2D(vcand, vel) * m_invVmax);
+            float vcpen = m_params.weightCurVel * (Vector3f.Dist2D(vcand, vel) * m_invVmax);
 
             // find the threshold hit time to bail out based on the early out penalty
             // (see how the penalty is calculated below to understnad)

src/DotRecast.Detour.Crowd/PathCorridor.cs

@@ -246,7 +246,7 @@ namespace DotRecast.Detour.Crowd
                 foreach (StraightPathItem spi in path)
                 {
                     if ((spi.GetFlags() & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
-                        || VDist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST)
+                        || Vector3f.Dist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST)
                     {
                         break;
                     }
@@ -302,7 +302,7 @@ namespace DotRecast.Detour.Crowd
         public void OptimizePathVisibility(Vector3f next, float pathOptimizationRange, NavMeshQuery navquery, IQueryFilter filter)
         {
             // Clamp the ray to max distance.
-            float dist = VDist2D(m_pos, next);
+            float dist = Vector3f.Dist2D(m_pos, next);
 
             // If too close to the goal, do not try to optimize.
             if (dist < 0.01f)

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

@@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink
             Func<Vector3f, float, Tuple<bool, float>> heightFunc)
         {
             float cs = acfg.cellSize;
-            float dist = (float)Math.Sqrt(VDist2DSqr(es.start.p, es.start.q));
+            float dist = (float)Math.Sqrt(Vector3f.Dist2DSqr(es.start.p, es.start.q));
             int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs));
             SampleGroundSegment(heightFunc, es.start, ngsamples);
             foreach (GroundSegment end in es.end)

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

@@ -59,7 +59,7 @@ namespace DotRecast.Detour.Extras.Jumplink
                 GroundSegment end = es.end[js.groundSegment];
                 Vector3f ep = end.gsamples[js.startSample].p;
                 Vector3f eq = end.gsamples[js.startSample + js.samples - 1].p;
-                float d = Math.Min(VDist2DSqr(sp, sq), VDist2DSqr(ep, eq));
+                float d = Math.Min(Vector3f.Dist2DSqr(sp, sq), Vector3f.Dist2DSqr(ep, eq));
                 if (d >= 4 * acfg.agentRadius * acfg.agentRadius)
                 {
                     JumpLink link = new JumpLink();

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

@@ -35,7 +35,7 @@ namespace DotRecast.Detour.Extras.Jumplink
         private bool SampleTrajectory(JumpLinkBuilderConfig acfg, Heightfield solid, Vector3f pa, Vector3f pb, Trajectory tra)
         {
             float cs = Math.Min(acfg.cellSize, acfg.cellHeight);
-            float d = VDist2D(pa, pb) + Math.Abs(pa.y - pb.y);
+            float d = Vector3f.Dist2D(pa, pb) + Math.Abs(pa.y - pb.y);
             int nsamples = Math.Max(2, (int)Math.Ceiling(d / cs));
             for (int i = 0; i < nsamples; ++i)
             {

src/DotRecast.Detour/LegacyNavMeshQuery.cs

@@ -317,7 +317,7 @@ namespace DotRecast.Detour
                 bool tryLOS = false;
                 if ((m_query.options & DT_FINDPATH_ANY_ANGLE) != 0)
                 {
-                    if ((parentRef != 0) && (VDistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
+                    if ((parentRef != 0) && (Vector3f.DistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
                     {
                         tryLOS = true;
                     }

src/DotRecast.Detour/NavMeshQuery.cs

@@ -853,7 +853,7 @@ namespace DotRecast.Detour
                 if ((options & DT_FINDPATH_ANY_ANGLE) != 0)
                 {
                     if ((parentRef != 0) && (raycastLimitSqr >= float.MaxValue
-                                             || VDistSqr(parentNode.pos, bestNode.pos) < raycastLimitSqr))
+                                             || Vector3f.DistSqr(parentNode.pos, bestNode.pos) < raycastLimitSqr))
                     {
                         tryLOS = true;
                     }
@@ -1179,7 +1179,7 @@ namespace DotRecast.Detour
                 if ((m_query.options & DT_FINDPATH_ANY_ANGLE) != 0)
                 {
                     if ((parentRef != 0) && (m_query.raycastLimitSqr >= float.MaxValue
-                                             || VDistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
+                                             || Vector3f.DistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
                     {
                         tryLOS = true;
                     }

src/DotRecast.Detour/StrictPolygonByCircleConstraint.cs

@@ -19,7 +19,7 @@ namespace DotRecast.Detour
             int outsideVertex = -1;
             for (int pv = 0; pv < verts.Length; pv += 3)
             {
-                if (VDist2DSqr(center, verts, pv) > radiusSqr)
+                if (Vector3f.Dist2DSqr(center, verts, pv) > radiusSqr)
                 {
                     outsideVertex = pv;
                     break;

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

@@ -80,7 +80,7 @@ public class ConvexVolumeTool : Tool
             // Create
 
             // If clicked on that last pt, create the shape.
-            if (pts.Count > 0 && RcMath.VDistSqr(p, Vector3f.Of(pts[pts.Count - 3], pts[pts.Count - 2], pts[pts.Count - 1])) < 0.2f * 0.2f)
+            if (pts.Count > 0 && Vector3f.DistSqr(p, Vector3f.Of(pts[pts.Count - 3], pts[pts.Count - 2], pts[pts.Count - 1])) < 0.2f * 0.2f)
             {
                 if (hull.Count > 2)
                 {

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

@@ -189,7 +189,7 @@ public class CrowdProfilingTool
                     bool valid = true;
                     foreach (FindRandomPointResult zone in zones)
                     {
-                        if (RcMath.VDistSqr(zone.GetRandomPt(), result.result.GetRandomPt()) < zoneSeparation)
+                        if (Vector3f.DistSqr(zone.GetRandomPt(), result.result.GetRandomPt()) < zoneSeparation)
                         {
                             valid = false;
                             break;
@@ -313,7 +313,7 @@ public class CrowdProfilingTool
         List<FindRandomPointResult> potentialTargets = new();
         foreach (FindRandomPointResult zone in zones)
         {
-            if (RcMath.VDistSqr(zone.GetRandomPt(), ag.npos) > zoneRadius * zoneRadius)
+            if (Vector3f.DistSqr(zone.GetRandomPt(), ag.npos) > zoneRadius * zoneRadius)
             {
                 potentialTargets.Add(zone);
             }

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

@@ -436,7 +436,7 @@ public class JumpLinkBuilderTool : Tool
         {
             Vector3f p = link.startSamples[i].p;
             Vector3f q = link.endSamples[i].p;
-            if (i == 0 || VDist2D(prev, p) > agentRadius)
+            if (i == 0 || Vector3f.Dist2D(prev, p) > agentRadius)
             {
                 geom.AddOffMeshConnection(p, q, agentRadius, false, area, flags);
                 prev = p;

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

@@ -57,7 +57,7 @@ public class OffMeshConnectionTool : Tool
             DemoOffMeshConnection nearestConnection = null;
             foreach (DemoOffMeshConnection offMeshCon in geom.GetOffMeshConnections())
             {
-                float d = Math.Min(RcMath.VDistSqr(p, offMeshCon.verts, 0), RcMath.VDistSqr(p, offMeshCon.verts, 3));
+                float d = Math.Min(Vector3f.DistSqr(p, offMeshCon.verts, 0), Vector3f.DistSqr(p, offMeshCon.verts, 3));
                 if (d < nearestDist && Math.Sqrt(d) < sample.GetSettingsUI().GetAgentRadius())
                 {
                     nearestDist = d;

test/DotRecast.Detour.Test/RandomPointTest.cs

@@ -99,7 +99,7 @@ public class RandomPointTest : AbstractDetourTest
             Result<FindRandomPointResult> result = query.FindRandomPointWithinCircle(point.GetRandomRef(), point.GetRandomPt(),
                 radius, filter, f);
             Assert.That(result.Failed(), Is.False);
-            float distance = VDist2D(point.GetRandomPt(), result.result.GetRandomPt());
+            float distance = Vector3f.Dist2D(point.GetRandomPt(), result.result.GetRandomPt());
             Assert.That(distance <= radius, Is.True);
             point = result.result;
         }