From 0c37c9eedbeea1f0ffb1e362d20a6137d26a9916 Mon Sep 17 00:00:00 2001 From: ikpil Date: Sun, 14 May 2023 16:57:57 +0900 Subject: [PATCH] remove VSub -> Vector3f.Subtract --- src/DotRecast.Core/Intersections.cs | 8 +++--- src/DotRecast.Core/RcMath.cs | 28 ++++++------------- src/DotRecast.Core/Vector3f.cs | 18 +++++++++++- src/DotRecast.Detour.Crowd/Crowd.cs | 6 ++-- src/DotRecast.Detour.Crowd/CrowdAgent.cs | 8 +++--- .../ObstacleAvoidanceQuery.cs | 16 +++++------ src/DotRecast.Detour.Crowd/PathCorridor.cs | 2 +- .../Jumplink/EdgeSampler.cs | 2 +- .../ConvexConvexIntersection.cs | 4 +-- src/DotRecast.Detour/FindNearestPolyQuery.cs | 2 +- src/DotRecast.Detour/LegacyNavMeshQuery.cs | 2 +- src/DotRecast.Detour/NavMesh.cs | 4 +-- src/DotRecast.Detour/NavMeshQuery.cs | 12 ++++---- src/DotRecast.Recast.Demo/Tools/CrowdTool.cs | 2 +- .../Tools/TestNavmeshTool.cs | 4 +-- .../AbstractCrowdTest.cs | 2 +- 16 files changed, 63 insertions(+), 57 deletions(-) diff --git a/src/DotRecast.Core/Intersections.cs b/src/DotRecast.Core/Intersections.cs index 4d4024c..f3d16b7 100644 --- a/src/DotRecast.Core/Intersections.cs +++ b/src/DotRecast.Core/Intersections.cs @@ -27,9 +27,9 @@ namespace DotRecast.Core public static float? IntersectSegmentTriangle(Vector3f sp, Vector3f sq, Vector3f a, Vector3f b, Vector3f c) { float v, w; - Vector3f ab = VSub(b, a); - Vector3f ac = VSub(c, a); - Vector3f qp = VSub(sp, sq); + Vector3f ab = b.Subtract(a); + Vector3f ac = c.Subtract(a); + Vector3f qp = sp.Subtract(sq); // Compute triangle normal. Can be precalculated or cached if // intersecting multiple segments against the same triangle @@ -46,7 +46,7 @@ namespace DotRecast.Core // Compute intersection t value of pq with plane of triangle. A ray // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay // dividing by d until intersection has been found to pierce triangle - Vector3f ap = VSub(sp, a); + Vector3f ap = sp.Subtract(a); float t = VDot(ap, norm); if (t < 0.0f) { diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index dc3cbf2..8ab2793 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -142,16 +142,6 @@ namespace DotRecast.Core ); } - - public static Vector3f VSub(Vector3f v1, Vector3f v2) - { - return new Vector3f( - v1.x - v2.x, - v1.y - v2.y, - v1.z - v2.z - ); - } - public static Vector3f VAdd(Vector3f v1, Vector3f v2) { return new Vector3f( @@ -517,9 +507,9 @@ namespace DotRecast.Core public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c) { - Vector3f v0 = VSub(c, a); - Vector3f v1 = VSub(b, a); - Vector3f v2 = VSub(p, a); + Vector3f v0 = c.Subtract(a); + Vector3f v1 = b.Subtract(a); + Vector3f v2 = p.Subtract(a); // Compute scaled barycentric coordinates float denom = v0.x * v1.z - v0.z * v1.x; @@ -737,15 +727,15 @@ namespace DotRecast.Core { IntersectResult result = new IntersectResult(); float EPS = 0.000001f; - var dir = VSub(p1, p0); + var dir = p1.Subtract(p0); var p0v = p0; for (int i = 0, j = nverts - 1; i < nverts; j = i++) { Vector3f vpj = Vector3f.Of(verts, j * 3); Vector3f vpi = Vector3f.Of(verts, i * 3); - var edge = VSub(vpi, vpj); - var diff = VSub(p0v, vpj); + var edge = vpi.Subtract(vpj); + var diff = p0v.Subtract(vpj); float n = VPerp2D(edge, diff); float d = VPerp2D(dir, edge); if (Math.Abs(d) < EPS) @@ -868,9 +858,9 @@ namespace DotRecast.Core public static Tuple? IntersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq) { - Vector3f u = VSub(aq, ap); - Vector3f v = VSub(bq, bp); - Vector3f w = VSub(ap, bp); + Vector3f u = aq.Subtract(ap); + Vector3f v = bq.Subtract(bp); + Vector3f w = ap.Subtract(bp); float d = VperpXZ(u, v); if (Math.Abs(d) < 1e-6f) { diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 2a0ccc7..0d0d2d9 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -17,6 +17,7 @@ freely, subject to the following restrictions: */ using System; +using System.Runtime.CompilerServices; namespace DotRecast.Core { @@ -93,6 +94,16 @@ namespace DotRecast.Core } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3f Subtract(Vector3f right) + { + return new Vector3f( + x - right.x, + y - right.y, + z - right.z + ); + } + public override bool Equals(object obj) { if (!(obj is Vector3f)) @@ -126,6 +137,11 @@ namespace DotRecast.Core { return !left.Equals(right); } - + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3f operator -(Vector3f left, Vector3f right) + { + return left.Subtract(right); + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 28f1a8e..5500201 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -917,7 +917,7 @@ namespace DotRecast.Detour.Crowd } // Check for overlap. - Vector3f diff = VSub(pos, ag.npos); + Vector3f diff = pos.Subtract(ag.npos); if (Math.Abs(diff.y) >= (height + ag.option.height) / 2.0f) { continue; @@ -1091,7 +1091,7 @@ namespace DotRecast.Detour.Crowd { CrowdAgent nei = ag.neis[j].agent; - Vector3f diff = VSub(ag.npos, nei.npos); + Vector3f diff = ag.npos.Subtract(nei.npos); diff.y = 0; float distSqr = VLenSqr(diff); @@ -1245,7 +1245,7 @@ namespace DotRecast.Detour.Crowd { CrowdAgent nei = ag.neis[j].agent; long idx1 = nei.idx; - Vector3f diff = VSub(ag.npos, nei.npos); + Vector3f diff = ag.npos.Subtract(nei.npos); diff.y = 0; float dist = VLenSqr(diff); diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 6ada1c7..79af187 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -115,7 +115,7 @@ namespace DotRecast.Detour.Crowd { // Fake dynamic constraint. float maxDelta = option.maxAcceleration * dt; - Vector3f dv = VSub(nvel, vel); + Vector3f dv = nvel.Subtract(vel); float ds = VLen(dv); if (ds > maxDelta) dv = VScale(dv, maxDelta / ds); @@ -169,8 +169,8 @@ namespace DotRecast.Detour.Crowd var p0 = corners[ip0].GetPos(); var p1 = corners[ip1].GetPos(); - var dir0 = VSub(p0, npos); - var dir1 = VSub(p1, npos); + var dir0 = p0.Subtract(npos); + var dir1 = p1.Subtract(npos); dir0.y = 0; dir1.y = 0; @@ -194,7 +194,7 @@ namespace DotRecast.Detour.Crowd Vector3f dir = new Vector3f(); if (0 < corners.Count) { - dir = VSub(corners[0].GetPos(), npos); + dir = corners[0].GetPos().Subtract(npos); dir.y = 0; VNormalize(ref dir); } diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index e16307e..8785d6f 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -127,9 +127,9 @@ namespace DotRecast.Detour.Crowd Vector3f orig = new Vector3f(); Vector3f dv = new Vector3f(); - cir.dp = VSub(pb, pa); + cir.dp = pb.Subtract(pa); VNormalize(ref cir.dp); - dv = VSub(cir.dvel, dvel); + dv = cir.dvel.Subtract(dvel); float a = TriArea2D(orig, cir.dp, dv); if (a < 0.01f) @@ -158,7 +158,7 @@ namespace DotRecast.Detour.Crowd SweepCircleCircleResult SweepCircleCircle(Vector3f c0, float r0, Vector3f v, Vector3f c1, float r1) { const float EPS = 0.0001f; - Vector3f s = VSub(c1, c0); + Vector3f s = c1.Subtract(c0); float r = r0 + r1; float c = VDot2D(s, s) - r * r; float a = VDot2D(v, v); @@ -177,8 +177,8 @@ namespace DotRecast.Detour.Crowd IsectRaySegResult IsectRaySeg(Vector3f ap, Vector3f u, Vector3f bp, Vector3f bq) { - Vector3f v = VSub(bq, bp); - Vector3f w = VSub(ap, bp); + Vector3f v = bq.Subtract(bp); + Vector3f w = ap.Subtract(bp); float d = VPerp2D(u, v); if (Math.Abs(d) < 1e-6f) return new IsectRaySegResult(false, 0f); @@ -230,8 +230,8 @@ namespace DotRecast.Detour.Crowd // RVO Vector3f vab = VScale(vcand, 2); - vab = VSub(vab, vel); - vab = VSub(vab, cir.vel); + vab = vab.Subtract(vel); + vab = vab.Subtract(cir.vel); // Side side += Clamp(Math.Min(VDot2D(cir.dp, vab) * 0.5f + 0.5f, VDot2D(cir.np, vab) * 2), 0.0f, 1.0f); @@ -269,7 +269,7 @@ namespace DotRecast.Detour.Crowd if (seg.touch) { // Special case when the agent is very close to the segment. - Vector3f sdir = VSub(seg.q, seg.p); + Vector3f sdir = seg.q.Subtract(seg.p); Vector3f snorm = new Vector3f(); snorm.x = -sdir.z; snorm.z = sdir.x; diff --git a/src/DotRecast.Detour.Crowd/PathCorridor.cs b/src/DotRecast.Detour.Crowd/PathCorridor.cs index d1f3aa4..fdada17 100644 --- a/src/DotRecast.Detour.Crowd/PathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/PathCorridor.cs @@ -315,7 +315,7 @@ namespace DotRecast.Detour.Crowd dist = Math.Min(dist + 0.01f, pathOptimizationRange); // Adjust ray length. - var delta = VSub(next, m_pos); + var delta = next.Subtract(m_pos); Vector3f goal = VMad(m_pos, delta, pathOptimizationRange / dist); Result rc = navquery.Raycast(m_path[0], m_pos, goal, filter, 0, 0); diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs index d9f0ef9..ecea8a6 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs @@ -17,7 +17,7 @@ namespace DotRecast.Detour.Extras.Jumplink public EdgeSampler(JumpEdge edge, Trajectory trajectory) { this.trajectory = trajectory; - ax = VSub(edge.sq, edge.sp); + ax = edge.sq.Subtract(edge.sp); VNormalize(ref ax); VSet(ref az, ax.z, 0, -ax.x); VNormalize(ref az); diff --git a/src/DotRecast.Detour/ConvexConvexIntersection.cs b/src/DotRecast.Detour/ConvexConvexIntersection.cs index a693ccb..9f06247 100644 --- a/src/DotRecast.Detour/ConvexConvexIntersection.cs +++ b/src/DotRecast.Detour/ConvexConvexIntersection.cs @@ -59,8 +59,8 @@ namespace DotRecast.Detour VCopy(ref a1, p, 3 * ((ai + n - 1) % n)); // prev a VCopy(ref b1, q, 3 * ((bi + m - 1) % m)); // prev b - Vector3f A = VSub(a, a1); - Vector3f B = VSub(b, b1); + Vector3f A = a.Subtract(a1); + Vector3f B = b.Subtract(b1); float cross = B.x * A.z - A.x * B.z; // TriArea2D({0, 0}, A, B); float aHB = TriArea2D(b1, b, a); diff --git a/src/DotRecast.Detour/FindNearestPolyQuery.cs b/src/DotRecast.Detour/FindNearestPolyQuery.cs index 440e7ec..27267c8 100644 --- a/src/DotRecast.Detour/FindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/FindNearestPolyQuery.cs @@ -33,7 +33,7 @@ namespace DotRecast.Detour // If a point is directly over a polygon and closer than // climb height, favor that instead of straight line nearest point. float d = 0; - Vector3f diff = VSub(center, closestPtPoly); + Vector3f diff = center.Subtract(closestPtPoly); if (posOverPoly) { d = Math.Abs(diff.y) - tile.data.header.walkableClimb; diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index 80a9bd9..33e3713 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -844,7 +844,7 @@ namespace DotRecast.Detour Vector3f hitNormal = new Vector3f(); if (bestvi != null && bestvj != null) { - var tangent = VSub(bestvi.Value, bestvj.Value); + var tangent = bestvi.Value.Subtract(bestvj.Value); hitNormal.x = tangent.z; hitNormal.y = 0; hitNormal.z = -tangent.x; diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index 844aef9..16013a6 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -1355,7 +1355,7 @@ namespace DotRecast.Detour { Vector3f nearestPt = new Vector3f(); bool overPoly = false; - Vector3f bmin = VSub(center, extents); + Vector3f bmin = center.Subtract(extents); Vector3f bmax = VAdd(center, extents); // Get nearby polygons from proximity grid. @@ -1374,7 +1374,7 @@ namespace DotRecast.Detour // If a point is directly over a polygon and closer than // climb height, favor that instead of straight line nearest point. - Vector3f diff = VSub(center, closestPtPoly); + Vector3f diff = center.Subtract(closestPtPoly); if (posOverPoly) { d = Math.Abs(diff.y) - tile.data.header.walkableClimb; diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 6d80252..cdbb82b 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -690,7 +690,7 @@ namespace DotRecast.Detour } // Find tiles the query touches. - Vector3f bmin = VSub(center, halfExtents); + Vector3f bmin = center.Subtract(halfExtents); Vector3f bmax = VAdd(center, halfExtents); foreach (var t in QueryTiles(center, halfExtents)) { @@ -710,7 +710,7 @@ namespace DotRecast.Detour return ImmutableArray.Empty; } - Vector3f bmin = VSub(center, halfExtents); + Vector3f bmin = center.Subtract(halfExtents); Vector3f bmax = VAdd(center, halfExtents); int[] minxy = m_nav.CalcTileLoc(bmin); int minx = minxy[0]; @@ -2210,7 +2210,7 @@ namespace DotRecast.Detour Vector3f lastPos = Vector3f.Zero; curPos = startPos; - var dir = VSub(endPos, startPos); + var dir = endPos.Subtract(startPos); MeshTile prevTile, tile, nextTile; Poly prevPoly, poly, nextPoly; @@ -2377,8 +2377,8 @@ namespace DotRecast.Detour curPos = VMad(startPos, dir, hit.t); var e1 = Vector3f.Of(verts, iresult.segMax * 3); var e2 = Vector3f.Of(verts, ((iresult.segMax + 1) % nv) * 3); - var eDir = VSub(e2, e1); - var diff = VSub(curPos, e1); + var eDir = e2.Subtract(e1); + var diff = curPos.Subtract(e1); float s = Sqr(eDir.x) > Sqr(eDir.z) ? diff.x / eDir.x : diff.z / eDir.z; curPos.y = e1.y + eDir.y * s; @@ -3368,7 +3368,7 @@ namespace DotRecast.Detour Vector3f hitNormal = new Vector3f(); if (bestvi != null && bestvj != null) { - var tangent = VSub(bestvi.Value, bestvj.Value); + var tangent = bestvi.Value.Subtract(bestvj.Value); hitNormal.x = tangent.z; hitNormal.y = 0; hitNormal.z = -tangent.x; diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs index 18dcee8..598093c 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs @@ -312,7 +312,7 @@ public class CrowdTool : Tool private Vector3f CalcVel(Vector3f pos, Vector3f tgt, float speed) { - Vector3f vel = VSub(tgt, pos); + Vector3f vel = tgt.Subtract(pos); vel.y = 0.0f; VNormalize(ref vel); return VScale(vel, speed); diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index de0fcbf..13b0804 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -225,7 +225,7 @@ public class TestNavmeshTool : Tool : false; // Find movement delta. - Vector3f delta = VSub(steerTarget.steerPos, iterPos); + Vector3f delta = steerTarget.steerPos.Subtract(iterPos); float len = (float)Math.Sqrt(VDot(delta, delta)); // If the steer target is end of path or off-mesh link, do not move past the location. if ((endOfPath || offMeshConnection) && len < STEP_SIZE) @@ -855,7 +855,7 @@ public class TestNavmeshTool : Tool continue; } - Vector3f delta = VSub(s3, s.vmin); + Vector3f delta = s3.Subtract(s.vmin); Vector3f p0 = VMad(s.vmin, delta, 0.5f); Vector3f norm = Vector3f.Of(delta.z, 0, -delta.x); VNormalize(ref norm); diff --git a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs index 4ca7668..44feb56 100644 --- a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs @@ -153,7 +153,7 @@ public class AbstractCrowdTest protected Vector3f CalcVel(Vector3f pos, Vector3f tgt, float speed) { - Vector3f vel = VSub(tgt, pos); + Vector3f vel = tgt.Subtract(pos); vel.y = 0.0f; VNormalize(ref vel); vel = VScale(vel, speed);