From b1f29a0812bfdf1b4afb9fbe50f8f4d219974de6 Mon Sep 17 00:00:00 2001 From: ikpil Date: Thu, 18 May 2023 08:25:57 +0900 Subject: [PATCH] move RcMath VDist, VLen -> Vector3f.Distance, Length --- src/DotRecast.Core/RcMath.cs | 38 ------------------- src/DotRecast.Core/Vector3f.cs | 31 ++++++++++++--- src/DotRecast.Detour.Crowd/Crowd.cs | 2 +- src/DotRecast.Detour.Crowd/CrowdAgent.cs | 8 ++-- src/DotRecast.Detour/DefaultQueryFilter.cs | 2 +- src/DotRecast.Detour/DefaultQueryHeuristic.cs | 2 +- src/DotRecast.Detour/LegacyNavMeshQuery.cs | 8 ++-- src/DotRecast.Detour/NavMeshQuery.cs | 6 +-- .../Tools/Gizmos/CapsuleGizmo.cs | 2 +- .../Tools/Gizmos/CylinderGizmo.cs | 2 +- 10 files changed, 42 insertions(+), 59 deletions(-) diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index 5a70cc3..23d11aa 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -172,19 +172,6 @@ namespace DotRecast.Core } - /// 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 VDist(Vector3f v1, Vector3f v2) - { - float dx = v2.x - v1.x; - float dy = v2.y - v1.y; - float dz = v2.z - v1.z; - return (float)Math.Sqrt(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)] @@ -201,37 +188,12 @@ namespace DotRecast.Core /// 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. - public static float VLenSqr(float[] v) - { - return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; - } - public static float VLenSqr(Vector3f v) { return v.x * v.x + v.y * v.y + v.z * v.z; } - public static float VLen(float[] v) - { - return (float)Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); - } - - public static float VLen(Vector3f v) - { - return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z); - } - - - public static float VDist(float[] v1, float[] verts, int i) - { - float dx = verts[i] - v1[0]; - float dy = verts[i + 1] - v1[1]; - float dz = verts[i + 2] - v1[2]; - return (float)Math.Sqrt(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)] diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index bffe563..64c7e63 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -93,9 +93,15 @@ namespace DotRecast.Core default: throw new IndexOutOfRangeException($"{index}-{value}"); } } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3f Subtract(Vector3f right) + public readonly float Length() + { + return (float)Math.Sqrt(x * x + y * y + z * z); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly Vector3f Subtract(Vector3f right) { return new Vector3f( x - right.x, @@ -105,7 +111,7 @@ namespace DotRecast.Core } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Vector3f Add(Vector3f v2) + public readonly Vector3f Add(Vector3f v2) { return new Vector3f( x + v2.x, @@ -122,13 +128,13 @@ namespace DotRecast.Core /// The vectors are projected onto the xz-plane, so the y-values are /// ignored. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public float Dot2D(Vector3f v) + public readonly float Dot2D(Vector3f v) { return x * v.x + z * v.z; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public float Dot2D(float[] v, int vi) + public readonly float Dot2D(float[] v, int vi) { return x * v[vi] + z * v[vi + 2]; } @@ -210,5 +216,20 @@ namespace DotRecast.Core v1.z + (v2.z - v1.z) * 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 Distance(Vector3f v1, Vector3f v2) + { + float dx = v2.x - v1.x; + float dy = v2.y - v1.y; + float dz = v2.z - v1.z; + return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); + } + + } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 5d1a493..f4e6700 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -1055,7 +1055,7 @@ namespace DotRecast.Detour.Crowd if (ag.targetState == MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { dvel = ag.targetPos; - ag.desiredSpeed = VLen(ag.targetPos); + ag.desiredSpeed = ag.targetPos.Length(); } else { diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 910d2b6..c09d826 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -116,13 +116,13 @@ namespace DotRecast.Detour.Crowd // Fake dynamic constraint. float maxDelta = option.maxAcceleration * dt; Vector3f dv = nvel.Subtract(vel); - float ds = VLen(dv); + float ds = dv.Length(); if (ds > maxDelta) dv = VScale(dv, maxDelta / ds); vel = vel.Add(dv); // Integrate - if (VLen(vel) > 0.0001f) + if (vel.Length() > 0.0001f) npos = VMad(npos, vel, dt); else vel = Vector3f.Zero; @@ -174,8 +174,8 @@ namespace DotRecast.Detour.Crowd dir0.y = 0; dir1.y = 0; - float len0 = VLen(dir0); - float len1 = VLen(dir1); + float len0 = dir0.Length(); + float len1 = dir1.Length(); if (len1 > 0.001f) dir1 = VScale(dir1, 1.0f / len1); diff --git a/src/DotRecast.Detour/DefaultQueryFilter.cs b/src/DotRecast.Detour/DefaultQueryFilter.cs index c390a10..9ed6caf 100644 --- a/src/DotRecast.Detour/DefaultQueryFilter.cs +++ b/src/DotRecast.Detour/DefaultQueryFilter.cs @@ -89,7 +89,7 @@ namespace DotRecast.Detour public float GetCost(Vector3f pa, Vector3f pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef, MeshTile curTile, Poly curPoly, long nextRef, MeshTile nextTile, Poly nextPoly) { - return VDist(pa, pb) * m_areaCost[curPoly.GetArea()]; + return Vector3f.Distance(pa, pb) * m_areaCost[curPoly.GetArea()]; } public int GetIncludeFlags() diff --git a/src/DotRecast.Detour/DefaultQueryHeuristic.cs b/src/DotRecast.Detour/DefaultQueryHeuristic.cs index e79ac6f..b0d72fc 100644 --- a/src/DotRecast.Detour/DefaultQueryHeuristic.cs +++ b/src/DotRecast.Detour/DefaultQueryHeuristic.cs @@ -37,7 +37,7 @@ namespace DotRecast.Detour public float GetCost(Vector3f neighbourPos, Vector3f endPos) { - return VDist(neighbourPos, endPos) * scale; + return Vector3f.Distance(neighbourPos, endPos) * scale; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index e830013..640afed 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -62,7 +62,7 @@ namespace DotRecast.Detour startNode.pos = startPos; startNode.pidx = 0; startNode.cost = 0; - startNode.total = VDist(startPos, endPos) * H_SCALE; + startNode.total = Vector3f.Distance(startPos, endPos) * H_SCALE; startNode.id = startRef; startNode.flags = Node.DT_NODE_OPEN; m_openList.Push(startNode); @@ -173,7 +173,7 @@ namespace DotRecast.Detour float curCost = filter.GetCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); cost = bestNode.cost + curCost; - heuristic = VDist(neighbourNode.pos, endPos) * H_SCALE; + heuristic = Vector3f.Distance(neighbourNode.pos, endPos) * H_SCALE; } float total = cost + heuristic; @@ -410,7 +410,7 @@ namespace DotRecast.Detour } else { - heuristic = VDist(neighbourNode.pos, m_query.endPos) * H_SCALE; + heuristic = Vector3f.Distance(neighbourNode.pos, m_query.endPos) * H_SCALE; } float total = cost + heuristic; @@ -815,7 +815,7 @@ namespace DotRecast.Detour } } - float total = bestNode.total + VDist(bestNode.pos, neighbourNode.pos); + float total = bestNode.total + Vector3f.Distance(bestNode.pos, neighbourNode.pos); // The node is already in open list and the new result is worse, skip. if ((neighbourNode.flags & Node.DT_NODE_OPEN) != 0 && total >= neighbourNode.total) diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index e34542a..4dfcb2f 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -367,7 +367,7 @@ namespace DotRecast.Detour neighbourNode.pos = Vector3f.Lerp(va, vb, 0.5f); } - float total = bestNode.total + VDist(bestNode.pos, neighbourNode.pos); + float total = bestNode.total + Vector3f.Distance(bestNode.pos, neighbourNode.pos); // The node is already in open list and the new result is worse, skip. if ((neighbourNode.flags & Node.DT_NODE_OPEN) != 0 && total >= neighbourNode.total) @@ -1827,7 +1827,7 @@ namespace DotRecast.Detour // Search constraints var searchPos = Vector3f.Lerp(startPos, endPos, 0.5f); - float searchRadSqr = Sqr(VDist(startPos, endPos) / 2.0f + 0.001f); + float searchRadSqr = Sqr(Vector3f.Distance(startPos, endPos) / 2.0f + 0.001f); float[] verts = new float[m_nav.GetMaxVertsPerPoly() * 3]; @@ -3339,7 +3339,7 @@ namespace DotRecast.Detour } } - float total = bestNode.total + VDist(bestNode.pos, neighbourNode.pos); + float total = bestNode.total + Vector3f.Distance(bestNode.pos, neighbourNode.pos); // The node is already in open list and the new result is worse, skip. if ((neighbourNode.flags & Node.DT_NODE_OPEN) != 0 && total >= neighbourNode.total) diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs index 52170b8..581f563 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs @@ -33,7 +33,7 @@ public class CapsuleGizmo : IColliderGizmo var trY = Vector3f.Of(normals[0].y, normals[1].y, normals[2].y); var trZ = Vector3f.Of(normals[0].z, normals[1].z, normals[2].z); float[] spVertices = GenerateSphericalVertices(); - float halfLength = 0.5f * VLen(axis); + float halfLength = 0.5f * axis.Length(); vertices = new float[spVertices.Length]; gradient = new float[spVertices.Length / 3]; Vector3f v = new Vector3f(); diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs index ed7e136..e1e134b 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs @@ -33,7 +33,7 @@ public class CylinderGizmo : IColliderGizmo Vector3f trY = Vector3f.Of(normals[0].y, normals[1].y, normals[2].y); Vector3f trZ = Vector3f.Of(normals[0].z, normals[1].z, normals[2].z); vertices = GenerateCylindricalVertices(); - float halfLength = 0.5f * VLen(axis); + float halfLength = 0.5f * axis.Length(); gradient = new float[vertices.Length / 3]; Vector3f v = new Vector3f(); for (int i = 0; i < vertices.Length; i += 3)