From 425e1887f6b1327528afe8b9b0f3712eeaf7c748 Mon Sep 17 00:00:00 2001 From: ikpil Date: Wed, 24 May 2023 00:06:28 +0900 Subject: [PATCH] Vector3f inline --- src/DotRecast.Core/RcMath.cs | 81 +---------------- src/DotRecast.Core/Vector3f.cs | 91 ++++++++++++++++++- src/DotRecast.Detour.Crowd/Crowd.cs | 4 +- src/DotRecast.Detour.Crowd/CrowdAgent.cs | 4 +- .../ObstacleAvoidanceQuery.cs | 4 +- src/DotRecast.Detour.Crowd/PathCorridor.cs | 4 +- .../Jumplink/AbstractGroundSampler.cs | 2 +- .../Jumplink/JumpLinkBuilder.cs | 2 +- .../Jumplink/TrajectorySampler.cs | 2 +- src/DotRecast.Detour/LegacyNavMeshQuery.cs | 2 +- src/DotRecast.Detour/NavMeshQuery.cs | 4 +- .../StrictPolygonByCircleConstraint.cs | 2 +- .../Tools/ConvexVolumeTool.cs | 2 +- .../Tools/CrowdProfilingTool.cs | 4 +- .../Tools/JumpLinkBuilderTool.cs | 2 +- .../Tools/OffMeshConnectionTool.cs | 2 +- test/DotRecast.Detour.Test/RandomPointTest.cs | 2 +- 17 files changed, 113 insertions(+), 101 deletions(-) diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index 3aeb85d..61b349f 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/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; } diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 2b2bdf2..89690b2 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -217,7 +217,7 @@ namespace DotRecast.Core z *= d; } } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Min(float[] @in, int i) { @@ -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; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 70607d3..899b836 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/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(); diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 478a7ea..260c476 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/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; } diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index 3ebb436..9e90130 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/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 vcpen = m_params.weightCurVel * (VDist2D(vcand, vel) * m_invVmax); + float vpen = m_params.weightDesVel * (Vector3f.Dist2D(vcand, dvel) * 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) diff --git a/src/DotRecast.Detour.Crowd/PathCorridor.cs b/src/DotRecast.Detour.Crowd/PathCorridor.cs index 3eacd36..4e92dbc 100644 --- a/src/DotRecast.Detour.Crowd/PathCorridor.cs +++ b/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) diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 52bb347..e938929 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs @@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink Func> 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) diff --git a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs index e70cfc8..b561927 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs +++ b/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(); diff --git a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs index 9b2cedf..5c2bc5a 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs +++ b/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) { diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index 640afed..e370c1a 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/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; } diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 3dec3db..2a89642 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/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; } diff --git a/src/DotRecast.Detour/StrictPolygonByCircleConstraint.cs b/src/DotRecast.Detour/StrictPolygonByCircleConstraint.cs index 72eaf74..3e4e223 100644 --- a/src/DotRecast.Detour/StrictPolygonByCircleConstraint.cs +++ b/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; diff --git a/src/DotRecast.Recast.Demo/Tools/ConvexVolumeTool.cs b/src/DotRecast.Recast.Demo/Tools/ConvexVolumeTool.cs index 6137c64..8ab21bf 100644 --- a/src/DotRecast.Recast.Demo/Tools/ConvexVolumeTool.cs +++ b/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) { diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs index 1b075c8..481af62 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs +++ b/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 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); } diff --git a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs b/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs index 1b2f97a..3ff8955 100644 --- a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs +++ b/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; diff --git a/src/DotRecast.Recast.Demo/Tools/OffMeshConnectionTool.cs b/src/DotRecast.Recast.Demo/Tools/OffMeshConnectionTool.cs index 4f8fd42..951d142 100644 --- a/src/DotRecast.Recast.Demo/Tools/OffMeshConnectionTool.cs +++ b/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; diff --git a/test/DotRecast.Detour.Test/RandomPointTest.cs b/test/DotRecast.Detour.Test/RandomPointTest.cs index e797f61..20aef6c 100644 --- a/test/DotRecast.Detour.Test/RandomPointTest.cs +++ b/test/DotRecast.Detour.Test/RandomPointTest.cs @@ -99,7 +99,7 @@ public class RandomPointTest : AbstractDetourTest Result 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; }