diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 099f792..50c4351 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/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 diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs index 7ebeb70..c7aed7d 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/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; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 5894d11..12af0bc 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/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(); diff --git a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs index f0a93ce..fd88848 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs +++ b/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; } diff --git a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs index 7df5ed4..1b6e240 100644 --- a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs +++ b/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 vcpen = m_params.weightCurVel * (RcVec3f.Dist2D(vcand, vel) * m_invVmax); + float vpen = m_params.weightDesVel * (RcVecUtils.Dist2D(vcand, dvel) * 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) diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 62b009a..142c960 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/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) diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 3034d2c..8efafd2 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 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); diff --git a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs index e9391cf..22f4fa3 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]; 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(); diff --git a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs index 9521643..dc2dc6f 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, 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) { diff --git a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs index f6c0470..bf5266a 100644 --- a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs +++ b/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; diff --git a/src/DotRecast.Detour/DtUtils.cs b/src/DotRecast.Detour/DtUtils.cs index 3367163..0405c3e 100644 --- a/src/DotRecast.Detour/DtUtils.cs +++ b/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 d = RcVec3f.Perp2D(dir, edge); + float n = RcVecUtils.Perp2D(edge, diff); + float d = RcVecUtils.Perp2D(dir, edge); if (MathF.Abs(d) < EPS) { // S is nearly parallel to this edge diff --git a/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs index 3dbf430..105ca24 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs +++ b/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; diff --git a/test/DotRecast.Detour.Test/RandomPointTest.cs b/test/DotRecast.Detour.Test/RandomPointTest.cs index 2db417e..7905c54 100644 --- a/test/DotRecast.Detour.Test/RandomPointTest.cs +++ b/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;