refactor: RcVec3f.Dist2D, Dist2DSqr, Perp2D

src/DotRecast.Core/Numerics/RcVec3f.cs

@@ -301,51 +301,6 @@ namespace DotRecast.Core.Numerics
             );
         }
 
-        /// 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(RcVec3f v1, RcVec3f v2)
-        {
-            float dx = v2.X - v1.X;
-            float dz = v2.Z - v1.Z;
-            return (float)MathF.Sqrt(dx * dx + dz * dz);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static float Dist2DSqr(RcVec3f v1, RcVec3f 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(RcVec3f p, float[] verts, int i)
-        {
-            float dx = verts[i] - p.X;
-            float dz = verts[i + 2] - p.Z;
-            return dx * dx + dz * dz;
-        }
-
-        /// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz)
-        /// @param[in] u The LHV vector [(x, y, z)]
-        /// @param[in] v The RHV vector [(x, y, z)]
-        /// @return The dot product on the xz-plane.
-        ///
-        /// The vectors are projected onto the xz-plane, so the y-values are
-        /// ignored.
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static float Perp2D(RcVec3f u, RcVec3f v)
-        {
-            return u.Z * v.X - u.X * v.Z;
-        }
-
-
         /// Checks that the specified vector's components are all finite.
         /// @param[in] v A point. [(x, y, z)]
         /// @return True if all of the point's components are finite, i.e. not NaN

src/DotRecast.Core/Numerics/RcVecUtils.cs

@@ -175,5 +175,49 @@ namespace DotRecast.Core.Numerics
                 (v.Z > @in[i + 2]) ? v.Z : @in[i + 2]
             );
         }
+
+        /// 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(RcVec3f v1, RcVec3f v2)
+        {
+            float dx = v2.X - v1.X;
+            float dz = v2.Z - v1.Z;
+            return (float)MathF.Sqrt(dx * dx + dz * dz);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Dist2DSqr(RcVec3f v1, RcVec3f 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(RcVec3f p, float[] verts, int i)
+        {
+            float dx = verts[i] - p.X;
+            float dz = verts[i + 2] - p.Z;
+            return dx * dx + dz * dz;
+        }
+
+        /// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz)
+        /// @param[in] u The LHV vector [(x, y, z)]
+        /// @param[in] v The RHV vector [(x, y, z)]
+        /// @return The dot product on the xz-plane.
+        ///
+        /// The vectors are projected onto the xz-plane, so the y-values are
+        /// ignored.
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static float Perp2D(RcVec3f u, RcVec3f v)
+        {
+            return u.Z * v.X - u.X * v.Z;
+        }
     }
 }

src/DotRecast.Detour.Crowd/DtCrowd.cs

@@ -877,7 +877,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 (RcVec3f.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr)
+                if (RcVecUtils.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr)
                     || !ag.boundary.IsValid(_navQuery, _filters[ag.option.queryFilterType]))
                 {
                     ag.boundary.Update(ag.corridor.GetFirstPoly(), ag.npos, ag.option.collisionQueryRange, _navQuery,
@@ -1005,7 +1005,7 @@ namespace DotRecast.Detour.Crowd
                         anim.polyRef = refs[1];
                         anim.active = true;
                         anim.t = 0.0f;
-                        anim.tmax = (RcVec3f.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
+                        anim.tmax = (RcVecUtils.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
 
                         ag.state = DtCrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH;
                         ag.corners.Clear();

src/DotRecast.Detour.Crowd/DtCrowdAgent.cs

@@ -137,7 +137,7 @@ namespace DotRecast.Detour.Crowd
                 : false;
             if (offMeshConnection)
             {
-                float distSq = RcVec3f.Dist2DSqr(npos, corners[corners.Count - 1].pos);
+                float distSq = RcVecUtils.Dist2DSqr(npos, corners[corners.Count - 1].pos);
                 if (distSq < radius * radius)
                     return true;
             }
@@ -152,7 +152,7 @@ namespace DotRecast.Detour.Crowd
 
             bool endOfPath = ((corners[corners.Count - 1].flags & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0) ? true : false;
             if (endOfPath)
-                return Math.Min(RcVec3f.Dist2D(npos, corners[corners.Count - 1].pos), range);
+                return Math.Min(RcVecUtils.Dist2D(npos, corners[corners.Count - 1].pos), range);
 
             return range;
         }

src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs

@@ -187,16 +187,16 @@ namespace DotRecast.Detour.Crowd
         {
             RcVec3f v = RcVec3f.Subtract(bq, bp);
             RcVec3f w = RcVec3f.Subtract(ap, bp);
-            float d = RcVec3f.Perp2D(u, v);
+            float d = RcVecUtils.Perp2D(u, v);
             if (MathF.Abs(d) < 1e-6f)
                 return false;
 
             d = 1.0f / d;
-            t = RcVec3f.Perp2D(v, w) * d;
+            t = RcVecUtils.Perp2D(v, w) * d;
             if (t < 0 || t > 1)
                 return false;
 
-            float s = RcVec3f.Perp2D(u, w) * d;
+            float s = RcVecUtils.Perp2D(u, w) * d;
             if (s < 0 || s > 1)
                 return false;
 
@@ -217,8 +217,8 @@ namespace DotRecast.Detour.Crowd
             float minPenalty, DtObstacleAvoidanceDebugData debug)
         {
             // penalty for straying away from the desired and current velocities
-            float vpen = m_params.weightDesVel * (RcVec3f.Dist2D(vcand, dvel) * m_invVmax);
+            float vpen = m_params.weightDesVel * (RcVecUtils.Dist2D(vcand, dvel) * m_invVmax);
-            float vcpen = m_params.weightCurVel * (RcVec3f.Dist2D(vcand, vel) * m_invVmax);
+            float vcpen = m_params.weightCurVel * (RcVecUtils.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 understand)

src/DotRecast.Detour.Crowd/DtPathCorridor.cs

@@ -123,7 +123,7 @@ namespace DotRecast.Detour.Crowd
                 foreach (DtStraightPath spi in corners)
                 {
                     if ((spi.flags & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
-                        || RcVec3f.Dist2DSqr(spi.pos, m_pos) > MIN_TARGET_DIST)
+                        || RcVecUtils.Dist2DSqr(spi.pos, m_pos) > MIN_TARGET_DIST)
                     {
                         break;
                     }
@@ -179,7 +179,7 @@ namespace DotRecast.Detour.Crowd
         public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, DtNavMeshQuery navquery, IDtQueryFilter filter)
         {
             // Clamp the ray to max distance.
-            float dist = RcVec3f.Dist2D(m_pos, next);
+            float dist = RcVecUtils.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
         protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc)
         {
             float cs = acfg.cellSize;
-            float dist = MathF.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q));
+            float dist = MathF.Sqrt(RcVecUtils.Dist2DSqr(es.start.p, es.start.q));
             int ngsamples = Math.Max(2, (int)MathF.Ceiling(dist / cs));
 
             SampleGroundSegment(heightFunc, es.start, ngsamples);

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

@@ -59,7 +59,7 @@ namespace DotRecast.Detour.Extras.Jumplink
                 GroundSegment end = es.end[js.groundSegment];
                 RcVec3f ep = end.gsamples[js.startSample].p;
                 RcVec3f eq = end.gsamples[js.startSample + js.samples - 1].p;
-                float d = Math.Min(RcVec3f.Dist2DSqr(sp, sq), RcVec3f.Dist2DSqr(ep, eq));
+                float d = Math.Min(RcVecUtils.Dist2DSqr(sp, sq), RcVecUtils.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, RcHeightfield solid, RcVec3f pa, RcVec3f pb, Trajectory tra)
         {
             float cs = Math.Min(acfg.cellSize, acfg.cellHeight);
-            float d = RcVec3f.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y);
+            float d = RcVecUtils.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y);
             int nsamples = Math.Max(2, (int)MathF.Ceiling(d / cs));
             for (int i = 0; i < nsamples; ++i)
             {

src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs

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

src/DotRecast.Detour/DtUtils.cs

@@ -362,8 +362,8 @@ namespace DotRecast.Detour
                 RcVec3f vpi = verts[i];
                 var edge = RcVec3f.Subtract(vpi, vpj);
                 var diff = RcVec3f.Subtract(p0v, vpj);
-                float n = RcVec3f.Perp2D(edge, diff);
+                float n = RcVecUtils.Perp2D(edge, diff);
-                float d = RcVec3f.Perp2D(dir, edge);
+                float d = RcVecUtils.Perp2D(dir, edge);
                 if (MathF.Abs(d) < EPS)
                 {
                     // S is nearly parallel to this edge

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

@@ -116,7 +116,7 @@ namespace DotRecast.Recast.Toolset.Tools
             {
                 RcVec3f p = link.startSamples[i].p;
                 RcVec3f q = link.endSamples[i].p;
-                if (i == 0 || RcVec3f.Dist2D(prev, p) > agentRadius)
+                if (i == 0 || RcVecUtils.Dist2D(prev, p) > agentRadius)
                 {
                     geom.AddOffMeshConnection(p, q, agentRadius, false, area, flags);
                     prev = p;

test/DotRecast.Detour.Test/RandomPointTest.cs

@@ -103,7 +103,7 @@ public class RandomPointTest : AbstractDetourTest
             var status = query.FindRandomPointWithinCircle(randomRef, randomPt, radius, filter, f, out var nextRandomRef, out var nextRandomPt);
             Assert.That(status.Failed(), Is.False);
 
-            float distance = RcVec3f.Dist2D(randomPt, nextRandomPt);
+            float distance = RcVecUtils.Dist2D(randomPt, nextRandomPt);
             Assert.That(distance <= radius, Is.True);
 
             randomRef = nextRandomRef;