diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 3c4f06d..e722d45 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -23,7 +23,6 @@ namespace DotRecast.Core.Numerics { public struct RcVec3f { - public float X; public float Y; public float Z; @@ -103,6 +102,9 @@ namespace DotRecast.Core.Numerics return MathF.Sqrt(lengthSquared); } + /// Derives the square of the scalar length of the vector. (len * len) + /// @param[in] v The vector. [(x, y, z)] + /// @return The square of the scalar length of the vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly float LengthSquared() { @@ -313,29 +315,6 @@ namespace DotRecast.Core.Numerics ); } - - /// 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(RcVec3f 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(RcVec3f v1, RcVec3f 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; - } - /// 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)] @@ -380,15 +359,6 @@ namespace DotRecast.Core.Numerics return u.Z * v.X - u.X * v.Z; } - /// Derives the square of the scalar length of the vector. (len * len) - /// @param[in] v The vector. [(x, y, z)] - /// @return The square of the scalar length of the vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float LenSqr(RcVec3f v) - { - return v.X * v.X + v.Y * v.Y + v.Z * v.Z; - } - /// Checks that the specified vector's components are all finite. /// @param[in] v A point. [(x, y, z)] diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs index 075e479..2f4b20e 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/src/DotRecast.Core/Numerics/RcVecUtils.cs @@ -123,6 +123,19 @@ namespace DotRecast.Core.Numerics v1[2] * vector2.Z; } + /// 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 DistanceSquared(RcVec3f 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; + } + /// Normalizes the vector if the length is greater than zero. /// If the magnitude is zero, the vector is unchanged. /// @param[in,out] v The vector to normalize. [(x, y, z)] diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 39e62c8..5894d11 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -911,7 +911,7 @@ namespace DotRecast.Detour.Crowd } diff.Y = 0; - float distSqr = RcVec3f.LenSqr(diff); + float distSqr = diff.LengthSquared(); if (distSqr > RcMath.Sqr(range)) { continue; @@ -1080,7 +1080,7 @@ namespace DotRecast.Detour.Crowd RcVec3f diff = RcVec3f.Subtract(ag.npos, nei.npos); diff.Y = 0; - float distSqr = RcVec3f.LenSqr(diff); + float distSqr = diff.LengthSquared(); if (distSqr < 0.00001f) { continue; @@ -1103,7 +1103,7 @@ namespace DotRecast.Detour.Crowd // Adjust desired velocity. dvel = RcVec3f.Mad(dvel, disp, 1.0f / w); // Clamp desired velocity to desired speed. - float speedSqr = RcVec3f.LenSqr(dvel); + float speedSqr = dvel.LengthSquared(); float desiredSqr = RcMath.Sqr(ag.desiredSpeed); if (speedSqr > desiredSqr) { @@ -1227,7 +1227,7 @@ namespace DotRecast.Detour.Crowd RcVec3f diff = RcVec3f.Subtract(ag.npos, nei.npos); diff.Y = 0; - float dist = RcVec3f.LenSqr(diff); + float dist = diff.LengthSquared(); if (dist > RcMath.Sqr(ag.option.radius + nei.option.radius)) { continue; diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index d4c7c61..3aed2aa 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs @@ -36,7 +36,7 @@ namespace DotRecast.Detour } else { - d = RcVec3f.LenSqr(diff); + d = diff.LengthSquared(); } if (d < _nearestDistanceSqr) diff --git a/src/DotRecast.Detour/DtNavMesh.cs b/src/DotRecast.Detour/DtNavMesh.cs index 4d06950..f9a56ec 100644 --- a/src/DotRecast.Detour/DtNavMesh.cs +++ b/src/DotRecast.Detour/DtNavMesh.cs @@ -1377,7 +1377,7 @@ namespace DotRecast.Detour } else { - d = RcVec3f.LenSqr(diff); + d = diff.LengthSquared(); } if (d < nearestDistanceSqr) diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 5430041..c86d1c2 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -830,8 +830,8 @@ namespace DotRecast.Detour bool tryLOS = false; if ((options & DtFindPathOptions.DT_FINDPATH_ANY_ANGLE) != 0) { - if ((parentRef != 0) && (raycastLimitSqr >= float.MaxValue - || RcVec3f.DistSqr(parentNode.pos, bestNode.pos) < raycastLimitSqr)) + if ((parentRef != 0) && + (raycastLimitSqr >= float.MaxValue || RcVec3f.DistanceSquared(parentNode.pos, bestNode.pos) < raycastLimitSqr)) { tryLOS = true; } @@ -1148,8 +1148,8 @@ namespace DotRecast.Detour bool tryLOS = false; if ((m_query.options & DtFindPathOptions.DT_FINDPATH_ANY_ANGLE) != 0) { - if ((parentRef != 0) && (m_query.raycastLimitSqr >= float.MaxValue - || RcVec3f.DistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr)) + if ((parentRef != 0) && + (m_query.raycastLimitSqr >= float.MaxValue || RcVec3f.DistanceSquared(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr)) { tryLOS = true; } diff --git a/src/DotRecast.Detour/DtUtils.cs b/src/DotRecast.Detour/DtUtils.cs index 888f079..3367163 100644 --- a/src/DotRecast.Detour/DtUtils.cs +++ b/src/DotRecast.Detour/DtUtils.cs @@ -53,7 +53,7 @@ namespace DotRecast.Detour public static bool VEqual(RcVec3f p0, RcVec3f p1, float thresholdSqr) { - float d = RcVec3f.DistSqr(p0, p1); + float d = RcVec3f.DistanceSquared(p0, p1); return d < thresholdSqr; } diff --git a/src/DotRecast.Recast.Toolset/Tools/RcConvexVolumeTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcConvexVolumeTool.cs index 0271a46..f30be34 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcConvexVolumeTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcConvexVolumeTool.cs @@ -45,7 +45,7 @@ namespace DotRecast.Recast.Toolset.Tools // Create // If clicked on that last pt, create the shape. - if (_pts.Count > 0 && RcVec3f.DistSqr(p, _pts[_pts.Count - 1]) < 0.2f * 0.2f) + if (_pts.Count > 0 && RcVec3f.DistanceSquared(p, _pts[_pts.Count - 1]) < 0.2f * 0.2f) { pts = new List(_pts); hull = new List(_hull); diff --git a/src/DotRecast.Recast.Toolset/Tools/RcCrowdAgentProfilingTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcCrowdAgentProfilingTool.cs index 87f3a50..d5e00b1 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcCrowdAgentProfilingTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcCrowdAgentProfilingTool.cs @@ -107,7 +107,7 @@ namespace DotRecast.Recast.Toolset.Tools bool valid = true; foreach (var zone in _polyPoints) { - if (RcVec3f.DistSqr(zone.pt, randomPt) < zoneSeparation) + if (RcVec3f.DistanceSquared(zone.pt, randomPt) < zoneSeparation) { valid = false; break; @@ -285,7 +285,7 @@ namespace DotRecast.Recast.Toolset.Tools List potentialTargets = new List(); foreach (var zone in _polyPoints) { - if (RcVec3f.DistSqr(zone.pt, ag.npos) > _cfg.zoneRadius * _cfg.zoneRadius) + if (RcVec3f.DistanceSquared(zone.pt, ag.npos) > _cfg.zoneRadius * _cfg.zoneRadius) { potentialTargets.Add(zone); } diff --git a/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs index bd37f29..2d7a960 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs @@ -34,7 +34,7 @@ namespace DotRecast.Recast.Toolset.Tools RcOffMeshConnection nearestConnection = null; foreach (RcOffMeshConnection offMeshCon in geom.GetOffMeshConnections()) { - float d = Math.Min(RcVec3f.DistSqr(p, offMeshCon.verts, 0), RcVec3f.DistSqr(p, offMeshCon.verts, 3)); + float d = Math.Min(RcVecUtils.DistanceSquared(p, offMeshCon.verts, 0), RcVecUtils.DistanceSquared(p, offMeshCon.verts, 3)); if (d < nearestDist && Math.Sqrt(d) < settings.agentRadius) { nearestDist = d; diff --git a/test/DotRecast.Core.Test/Vector3Tests.cs b/test/DotRecast.Core.Test/Vector3Tests.cs index 46a511a..a2f0d85 100644 --- a/test/DotRecast.Core.Test/Vector3Tests.cs +++ b/test/DotRecast.Core.Test/Vector3Tests.cs @@ -115,11 +115,11 @@ public class Vector3Tests 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()); float d3 = Vector3.Dot(v1, v2); - + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); var v22 = new RcVec3f(v2.X, v2.Y, v2.Z); var d33 = RcVec3f.Dot(v11, v22); - + Assert.That(d3, Is.EqualTo(d33)); } @@ -140,7 +140,29 @@ public class Vector3Tests Assert.That(d3, Is.EqualTo(d33)); Assert.That(d4, Is.EqualTo(d44)); } - + + [Test] + [Repeat(100000)] + public void TestVectorMinMax() + { + // Min + // Max + } + + [Test] + [Repeat(100000)] + public void TestVectorIsFinite() + { + // IsFinite + } + + [Test] + [Repeat(100000)] + public void TestVectorPerp2D() + { + // Perp2D + } + // [Test] // [Repeat(100000)] // public void TestVectorLerp()