diff --git a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs index cbb4841..68d981b 100644 --- a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs @@ -155,44 +155,53 @@ namespace DotRecast.Detour.Crowd } } - SweepCircleCircleResult SweepCircleCircle(RcVec3f c0, float r0, RcVec3f v, RcVec3f c1, float r1) + private bool SweepCircleCircle(RcVec3f c0, float r0, RcVec3f v, RcVec3f c1, float r1, out float tmin, out float tmax) { const float EPS = 0.0001f; + + tmin = 0; + tmax = 0; + RcVec3f s = c1.Subtract(c0); float r = r0 + r1; float c = s.Dot2D(s) - r * r; float a = v.Dot2D(v); if (a < EPS) - return new SweepCircleCircleResult(false, 0f, 0f); // not moving + return false; // not moving // Overlap, calc time to exit. float b = v.Dot2D(s); float d = b * b - a * c; if (d < 0.0f) - return new SweepCircleCircleResult(false, 0f, 0f); // no intersection. + return false; // no intersection. + a = 1.0f / a; float rd = (float)Math.Sqrt(d); - return new SweepCircleCircleResult(true, (b - rd) * a, (b + rd) * a); + + tmin = (b - rd) * a; + tmax = (b + rd) * a; + + return true; } - IsectRaySegResult IsectRaySeg(RcVec3f ap, RcVec3f u, RcVec3f bp, RcVec3f bq) + private bool IsectRaySeg(RcVec3f ap, RcVec3f u, RcVec3f bp, RcVec3f bq, ref float t) { RcVec3f v = bq.Subtract(bp); RcVec3f w = ap.Subtract(bp); float d = RcVec3f.Perp2D(u, v); if (Math.Abs(d) < 1e-6f) - return new IsectRaySegResult(false, 0f); + return false; d = 1.0f / d; - float t = RcVec3f.Perp2D(v, w) * d; + t = RcVec3f.Perp2D(v, w) * d; if (t < 0 || t > 1) - return new IsectRaySegResult(false, 0f); + return false; float s = RcVec3f.Perp2D(u, w) * d; if (s < 0 || s > 1) - return new IsectRaySegResult(false, 0f); + return false; - return new IsectRaySegResult(true, t); + return true; } /** @@ -237,10 +246,8 @@ namespace DotRecast.Detour.Crowd side += Clamp(Math.Min(cir.dp.Dot2D(vab) * 0.5f + 0.5f, cir.np.Dot2D(vab) * 2), 0.0f, 1.0f); nside++; - SweepCircleCircleResult sres = SweepCircleCircle(pos, rad, vab, cir.p, cir.rad); - if (!sres.intersection) + if (!SweepCircleCircle(pos, rad, vab, cir.p, cir.rad, out var htmin, out var htmax)) continue; - float htmin = sres.htmin, htmax = sres.htmax; // Handle overlapping obstacles. if (htmin < 0.0f && htmax > 0.0f) @@ -281,11 +288,8 @@ namespace DotRecast.Detour.Crowd } else { - var ires = IsectRaySeg(pos, vcand, seg.p, seg.q); - if (!ires.result) + if (!IsectRaySeg(pos, vcand, seg.p, seg.q, ref htmin)) continue; - - htmin = ires.htmin; } // Avoid less when facing walls. diff --git a/src/DotRecast.Detour.Crowd/IsectRaySegResult.cs b/src/DotRecast.Detour.Crowd/IsectRaySegResult.cs deleted file mode 100644 index de269f3..0000000 --- a/src/DotRecast.Detour.Crowd/IsectRaySegResult.cs +++ /dev/null @@ -1,14 +0,0 @@ -namespace DotRecast.Detour.Crowd -{ - public struct IsectRaySegResult - { - public readonly bool result; - public readonly float htmin; - - public IsectRaySegResult(bool result, float htmin) - { - this.result = result; - this.htmin = htmin; - } - } -} \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/SweepCircleCircleResult.cs b/src/DotRecast.Detour.Crowd/SweepCircleCircleResult.cs deleted file mode 100644 index 5bce8e5..0000000 --- a/src/DotRecast.Detour.Crowd/SweepCircleCircleResult.cs +++ /dev/null @@ -1,36 +0,0 @@ -/* -Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -recast4j copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org -DotRecast Copyright (c) 2023 Choi Ikpil ikpil@naver.com - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any damages -arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, subject to the following restrictions: -1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -namespace DotRecast.Detour.Crowd -{ - public struct SweepCircleCircleResult - { - public readonly bool intersection; - public readonly float htmin; - public readonly float htmax; - - public SweepCircleCircleResult(bool intersection, float htmin, float htmax) - { - this.intersection = intersection; - this.htmin = htmin; - this.htmax = htmax; - } - } -} \ No newline at end of file diff --git a/test/DotRecast.Detour.Crowd.Test/Crowd4VelocityTest.cs b/test/DotRecast.Detour.Crowd.Test/Crowd4VelocityTest.cs index 85f335c..4056ace 100644 --- a/test/DotRecast.Detour.Crowd.Test/Crowd4VelocityTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/Crowd4VelocityTest.cs @@ -100,7 +100,7 @@ public class Crowd4VelocityTest : AbstractCrowdTest public void TestAgent1Quality3TVTA() { int updateFlags = DtCrowdAgentParams.DT_CROWD_ANTICIPATE_TURNS | DtCrowdAgentParams.DT_CROWD_OPTIMIZE_VIS - | DtCrowdAgentParams.DT_CROWD_OPTIMIZE_TOPO | DtCrowdAgentParams.DT_CROWD_OBSTACLE_AVOIDANCE; + | DtCrowdAgentParams.DT_CROWD_OPTIMIZE_TOPO | DtCrowdAgentParams.DT_CROWD_OBSTACLE_AVOIDANCE; AddAgentGrid(2, 0.3f, updateFlags, 3, endPoss[0]); SetMoveTarget(endPoss[4], false); @@ -121,4 +121,4 @@ public class Crowd4VelocityTest : AbstractCrowdTest Assert.That(ag.nvel.z, Is.EqualTo(EXPECTED_A1Q3TVTA[i][5]).Within(0.001f)); } } -} +} \ No newline at end of file