diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 50c4351..6b6cf1e 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -263,62 +263,6 @@ namespace DotRecast.Core.Numerics (vector1.Z * vector2.Z); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float PerpXZ(RcVec3f a, RcVec3f b) - { - 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 RcVec3f Mad(RcVec3f v1, RcVec3f v2, float s) - { - return new RcVec3f() - { - X = v1.X + (v2.X * s), - Y = v1.Y + (v2.Y * s), - Z = v1.Z + (v2.Z * s), - }; - } - - /// Performs a linear interpolation between two vectors. (@p v1 toward @p - /// v2) - /// @param[out] dest The result vector. [(x, y, x)] - /// @param[in] v1 The starting vector. - /// @param[in] v2 The destination vector. - /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static RcVec3f Lerp(float[] verts, int v1, int v2, float t) - { - return new RcVec3f( - verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t, - verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t, - verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t - ); - } - - /// 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 - /// or any of the infinities. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool IsFinite(RcVec3f v) - { - return float.IsFinite(v.X) && float.IsFinite(v.Y) && float.IsFinite(v.Z); - } - - /// Checks that the specified vector's 2D components are finite. - /// @param[in] v A point. [(x, y, z)] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool IsFinite2D(RcVec3f v) - { - return float.IsFinite(v.X) && float.IsFinite(v.Z); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void CopyTo(float[] array) { diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs index c7aed7d..7e82f77 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/src/DotRecast.Core/Numerics/RcVecUtils.cs @@ -219,5 +219,61 @@ namespace DotRecast.Core.Numerics { 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 + /// or any of the infinities. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsFinite(RcVec3f v) + { + return float.IsFinite(v.X) && float.IsFinite(v.Y) && float.IsFinite(v.Z); + } + + /// Checks that the specified vector's 2D components are finite. + /// @param[in] v A point. [(x, y, z)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsFinite2D(RcVec3f v) + { + return float.IsFinite(v.X) && float.IsFinite(v.Z); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float PerpXZ(RcVec3f a, RcVec3f b) + { + return (a.X * b.Z) - (a.Z * b.X); + } + + /// Performs a linear interpolation between two vectors. (@p v1 toward @p + /// v2) + /// @param[out] dest The result vector. [(x, y, x)] + /// @param[in] v1 The starting vector. + /// @param[in] v2 The destination vector. + /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static RcVec3f Lerp(float[] verts, int v1, int v2, float t) + { + return new RcVec3f( + verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t, + verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t, + verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t + ); + } + + /// 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 RcVec3f Mad(RcVec3f v1, RcVec3f v2, float s) + { + return new RcVec3f() + { + 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/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 12af0bc..7431c47 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -1094,14 +1094,14 @@ namespace DotRecast.Detour.Crowd float dist = MathF.Sqrt(distSqr); float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist)); - disp = RcVec3f.Mad(disp, diff, weight / dist); + disp = RcVecUtils.Mad(disp, diff, weight / dist); w += 1.0f; } if (w > 0.0001f) { // Adjust desired velocity. - dvel = RcVec3f.Mad(dvel, disp, 1.0f / w); + dvel = RcVecUtils.Mad(dvel, disp, 1.0f / w); // Clamp desired velocity to desired speed. float speedSqr = dvel.LengthSquared(); float desiredSqr = RcMath.Sqr(ag.desiredSpeed); @@ -1254,7 +1254,7 @@ namespace DotRecast.Detour.Crowd pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor; } - ag.disp = RcVec3f.Mad(ag.disp, diff, pen); + ag.disp = RcVecUtils.Mad(ag.disp, diff, pen); w += 1.0f; } diff --git a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs index fd88848..032d92d 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs @@ -121,7 +121,7 @@ namespace DotRecast.Detour.Crowd // Integrate if (vel.Length() > 0.0001f) - npos = RcVec3f.Mad(npos, vel, dt); + npos = RcVecUtils.Mad(npos, vel, dt); else vel = RcVec3f.Zero; } diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 142c960..1fcd3f2 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -193,7 +193,7 @@ namespace DotRecast.Detour.Crowd // Adjust ray length. var delta = RcVec3f.Subtract(next, m_pos); - RcVec3f goal = RcVec3f.Mad(m_pos, delta, pathOptimizationRange / dist); + RcVec3f goal = RcVecUtils.Mad(m_pos, delta, pathOptimizationRange / dist); var status = navquery.Raycast(m_path[0], m_pos, goal, filter, 0, 0, out var rayHit); if (status.Succeeded()) diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index e72b4bb..eae3a56 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -205,7 +205,7 @@ namespace DotRecast.Detour randomPt = centerPos; // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0 + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || maxRadius < 0 || !float.IsFinite(maxRadius) || null == filter || null == frand) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; @@ -406,7 +406,7 @@ namespace DotRecast.Detour closest = pos; posOverPoly = false; - if (!m_nav.IsValidPolyRef(refs) || !RcVec3f.IsFinite(pos)) + if (!m_nav.IsValidPolyRef(refs) || !RcVecUtils.IsFinite(pos)) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } @@ -441,7 +441,7 @@ namespace DotRecast.Detour return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } - if (tile == null || !RcVec3f.IsFinite(pos)) + if (tile == null || !RcVecUtils.IsFinite(pos)) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } @@ -476,7 +476,7 @@ namespace DotRecast.Detour int va = imin * 3; int vb = ((imin + 1) % nv) * 3; - closest = RcVec3f.Lerp(verts, va, vb, edget[imin]); + closest = RcVecUtils.Lerp(verts, va, vb, edget[imin]); } return DtStatus.DT_SUCCSESS; @@ -502,7 +502,7 @@ namespace DotRecast.Detour return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } - if (!RcVec3f.IsFinite2D(pos)) + if (!RcVecUtils.IsFinite2D(pos)) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } @@ -673,7 +673,7 @@ namespace DotRecast.Detour */ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, IDtPolyQuery query) { - if (!RcVec3f.IsFinite(center) || !RcVec3f.IsFinite(halfExtents) || null == filter) + if (!RcVecUtils.IsFinite(center) || !RcVecUtils.IsFinite(halfExtents) || null == filter) { return DtStatus.DT_INVALID_PARAM; } @@ -694,7 +694,7 @@ namespace DotRecast.Detour */ public IList QueryTiles(RcVec3f center, RcVec3f halfExtents) { - if (!RcVec3f.IsFinite(center) || !RcVec3f.IsFinite(halfExtents)) + if (!RcVecUtils.IsFinite(center) || !RcVecUtils.IsFinite(halfExtents)) { return RcImmutableArray.Empty; } @@ -744,7 +744,7 @@ namespace DotRecast.Detour path.Clear(); // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == filter) + if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == filter) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } @@ -1020,7 +1020,7 @@ namespace DotRecast.Detour m_query.raycastLimitSqr = RcMath.Sqr(raycastLimit); // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == filter) + if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == filter) { return DtStatus.DT_INVALID_PARAM; } @@ -1514,7 +1514,7 @@ namespace DotRecast.Detour ref List straightPath, int maxStraightPath, int options) { - if (!RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == straightPath + if (!RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == straightPath || null == path || 0 == path.Count || path[0] == 0 || maxStraightPath <= 0) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; @@ -1779,8 +1779,8 @@ namespace DotRecast.Detour visited.Clear(); // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(startPos) - || !RcVec3f.IsFinite(endPos) || null == filter) + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(startPos) + || !RcVecUtils.IsFinite(endPos) || null == filter) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } @@ -1883,7 +1883,7 @@ namespace DotRecast.Detour if (distSqr < bestDist) { // Update nearest distance. - bestPos = RcVec3f.Lerp(verts, vj, vi, tseg); + bestPos = RcVecUtils.Lerp(verts, vj, vi, tseg); bestDist = distSqr; bestNode = curNode; } @@ -2062,8 +2062,8 @@ namespace DotRecast.Detour float s = 1.0f / 255.0f; float tmin = link.bmin * s; float tmax = link.bmax * s; - left = RcVec3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); - right = RcVec3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); + left = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); + right = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); } } @@ -2165,7 +2165,7 @@ namespace DotRecast.Detour hit = null; // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == filter || (prevRef != 0 && !m_nav.IsValidPolyRef(prevRef))) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; @@ -2340,7 +2340,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 = RcVec3f.Mad(startPos, dir, hit.t); + curPos = RcVecUtils.Mad(startPos, dir, hit.t); var e1 = verts[segMax]; var e2 = verts[(segMax + 1) % nv]; var eDir = RcVec3f.Subtract(e2, e1); @@ -2434,7 +2434,7 @@ namespace DotRecast.Detour } // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || radius < 0 + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || radius < 0 || !float.IsFinite(radius) || null == filter) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; @@ -2777,7 +2777,7 @@ namespace DotRecast.Detour ref List resultRef, ref List resultParent) { // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || radius < 0 + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || radius < 0 || !float.IsFinite(radius) || null == filter || null == resultRef || null == resultParent) { @@ -3055,8 +3055,8 @@ namespace DotRecast.Detour float tmin = ints[k].tmin / 255.0f; float tmax = ints[k].tmax / 255.0f; var seg = new RcSegmentVert(); - seg.vmin = RcVec3f.Lerp(tile.data.verts, vj, vi, tmin); - seg.vmax = RcVec3f.Lerp(tile.data.verts, vj, vi, tmax); + seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(ints[k].refs); } @@ -3069,8 +3069,8 @@ namespace DotRecast.Detour float tmin = imin / 255.0f; float tmax = imax / 255.0f; var seg = new RcSegmentVert(); - seg.vmin = RcVec3f.Lerp(tile.data.verts, vj, vi, tmin); - seg.vmax = RcVec3f.Lerp(tile.data.verts, vj, vi, tmax); + seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(0L); } @@ -3109,7 +3109,7 @@ namespace DotRecast.Detour hitNormal = RcVec3f.Zero; // Validate input - if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0 + if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || maxRadius < 0 || !float.IsFinite(maxRadius) || null == filter) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; diff --git a/src/DotRecast.Detour/DtUtils.cs b/src/DotRecast.Detour/DtUtils.cs index 0405c3e..3fc81f3 100644 --- a/src/DotRecast.Detour/DtUtils.cs +++ b/src/DotRecast.Detour/DtUtils.cs @@ -425,14 +425,14 @@ namespace DotRecast.Detour RcVec3f u = RcVec3f.Subtract(aq, ap); RcVec3f v = RcVec3f.Subtract(bq, bp); RcVec3f w = RcVec3f.Subtract(ap, bp); - float d = RcVec3f.PerpXZ(u, v); + float d = RcVecUtils.PerpXZ(u, v); if (MathF.Abs(d) < 1e-6f) { return false; } - s = RcVec3f.PerpXZ(v, w) / d; - t = RcVec3f.PerpXZ(u, w) / d; + s = RcVecUtils.PerpXZ(v, w) / d; + t = RcVecUtils.PerpXZ(u, w) / d; return true; } diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index 3be0a7b..7e26eca 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -508,10 +508,10 @@ public class TestNavmeshSampleTool : ISampleTool } RcVec3f delta = RcVec3f.Subtract(s3, s.vmin); - RcVec3f p0 = RcVec3f.Mad(s.vmin, delta, 0.5f); + RcVec3f p0 = RcVecUtils.Mad(s.vmin, delta, 0.5f); RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X); norm = RcVec3f.Normalize(norm); - RcVec3f p1 = RcVec3f.Mad(p0, norm, agentRadius * 0.5f); + RcVec3f p1 = RcVecUtils.Mad(p0, norm, agentRadius * 0.5f); // Skip backfacing segments. if (segmentRefs[j] != 0) { diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index d688ef4..a7da3d9 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -85,7 +85,7 @@ namespace DotRecast.Recast.Toolset.Tools len = STEP_SIZE / len; } - RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len); + RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); // Move navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, ref visited); diff --git a/test/DotRecast.Core.Test/Vector3Tests.cs b/test/DotRecast.Core.Test/Vector3Tests.cs index 0a18fe6..37d1798 100644 --- a/test/DotRecast.Core.Test/Vector3Tests.cs +++ b/test/DotRecast.Core.Test/Vector3Tests.cs @@ -172,13 +172,6 @@ public class Vector3Tests // IsFinite } - [Test] - [Repeat(100000)] - public void TestVectorPerp2D() - { - // Perp2D - } - // [Test] // [Repeat(100000)] // public void TestVectorLerp()