diff --git a/src/DotRecast.Detour/DetourCommon.cs b/src/DotRecast.Detour/DetourCommon.cs index d06d914..81a7ff3 100644 --- a/src/DotRecast.Detour/DetourCommon.cs +++ b/src/DotRecast.Detour/DetourCommon.cs @@ -339,7 +339,7 @@ namespace DotRecast.Detour return dx * dx + dz * dz; } - public static IntersectResult IntersectSegmentPoly2D(RcVec3f p0, RcVec3f p1, float[] verts, int nverts) + public static IntersectResult IntersectSegmentPoly2D(RcVec3f p0, RcVec3f p1, RcVec3f[] verts, int nverts) { IntersectResult result = new IntersectResult(); const float EPS = 0.000001f; @@ -348,8 +348,8 @@ namespace DotRecast.Detour var p0v = p0; for (int i = 0, j = nverts - 1; i < nverts; j = i++) { - RcVec3f vpj = RcVec3f.Of(verts, j * 3); - RcVec3f vpi = RcVec3f.Of(verts, i * 3); + RcVec3f vpj = verts[j]; + RcVec3f vpi = verts[i]; var edge = vpi.Subtract(vpj); var diff = p0v.Subtract(vpj); float n = RcVec3f.Perp2D(edge, diff); diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 685c722..f0fdd6d 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -2198,7 +2198,7 @@ namespace DotRecast.Detour hit = new DtRaycastHit(); - float[] verts = new float[m_nav.GetMaxVertsPerPoly() * 3 + 3]; + RcVec3f[] verts = new RcVec3f[m_nav.GetMaxVertsPerPoly() + 1]; RcVec3f curPos = RcVec3f.Zero; RcVec3f lastPos = RcVec3f.Zero; @@ -2227,7 +2227,7 @@ namespace DotRecast.Detour int nv = 0; for (int i = 0; i < poly.vertCount; ++i) { - Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, nv * 3, 3); + verts[nv] = RcVec3f.Of(tile.data.verts, poly.verts[i] * 3); nv++; } @@ -2366,8 +2366,8 @@ namespace DotRecast.Detour // and correct the height (since the raycast moves in 2d) lastPos = curPos; curPos = RcVec3f.Mad(startPos, dir, hit.t); - var e1 = RcVec3f.Of(verts, iresult.segMax * 3); - var e2 = RcVec3f.Of(verts, ((iresult.segMax + 1) % nv) * 3); + var e1 = verts[iresult.segMax]; + var e2 = verts[(iresult.segMax + 1) % nv]; 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; @@ -2384,10 +2384,10 @@ namespace DotRecast.Detour // Calculate hit normal. int a = iresult.segMax; int b = iresult.segMax + 1 < nv ? iresult.segMax + 1 : 0; - int va = a * 3; - int vb = b * 3; - float dx = verts[vb] - verts[va]; - float dz = verts[vb + 2] - verts[va + 2]; + // int va = a * 3; + // int vb = b * 3; + float dx = verts[b].x - verts[a].x; + float dz = verts[b].z - verts[a].x; hit.hitNormal.x = dz; hit.hitNormal.y = 0; hit.hitNormal.z = -dx; @@ -2620,7 +2620,7 @@ namespace DotRecast.Detour /// @param[out] resultCount The number of polygons found. /// @param[in] maxResult The maximum number of polygons the result arrays can hold. /// @returns The status flags for the query. - public DtStatus FindPolysAroundShape(long startRef, float[] verts, IDtQueryFilter filter, + public DtStatus FindPolysAroundShape(long startRef, RcVec3f[] verts, IDtQueryFilter filter, ref List resultRef, ref List resultParent, ref List resultCost) { resultRef.Clear(); @@ -2628,7 +2628,7 @@ namespace DotRecast.Detour resultCost.Clear(); // Validate input - int nverts = verts.Length / 3; + int nverts = verts.Length; if (!m_nav.IsValidPolyRef(startRef) || null == verts || nverts < 3 || null == filter) { return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; @@ -2640,9 +2640,7 @@ namespace DotRecast.Detour RcVec3f centerPos = RcVec3f.Zero; for (int i = 0; i < nverts; ++i) { - centerPos.x += verts[i * 3]; - centerPos.y += verts[i * 3 + 1]; - centerPos.z += verts[i * 3 + 2]; + centerPos += verts[i]; } float scale = 1.0f / nverts; diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 0465604..d07c04f 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -22,19 +22,23 @@ public class TestNavmeshTool : IRcTool private bool m_eposSet; private RcVec3f m_spos; private RcVec3f m_epos; - private readonly DtQueryDefaultFilter m_filter; - private readonly RcVec3f m_polyPickExt = RcVec3f.Of(2, 4, 2); private long m_startRef; private long m_endRef; + + private readonly DtQueryDefaultFilter m_filter; + private readonly RcVec3f m_polyPickExt = RcVec3f.Of(2, 4, 2); + + // for hit private RcVec3f m_hitPos; - private float m_distanceToWall; private RcVec3f m_hitNormal; + private bool m_hitResult; + + private float m_distanceToWall; private List m_straightPath; private List m_polys; - private bool m_hitResult; private List m_parent; private float m_neighbourhoodRadius; - private readonly float[] m_queryPoly = new float[12]; + private readonly RcVec3f[] m_queryPoly = new RcVec3f[4]; private List m_smoothPath; private DtStatus m_pathFindStatus = DtStatus.DT_FAILURE; private readonly List randomPoints = new(); @@ -293,21 +297,21 @@ public class TestNavmeshTool : IRcTool float nz = -(m_epos.x - m_spos.x) * 0.25f; float agentHeight = _impl.GetSample() != null ? _impl.GetSample().GetSettings().agentHeight : 0; - m_queryPoly[0] = m_spos.x + nx * 1.2f; - m_queryPoly[1] = m_spos.y + agentHeight / 2; - m_queryPoly[2] = m_spos.z + nz * 1.2f; + m_queryPoly[0].x = m_spos.x + nx * 1.2f; + m_queryPoly[0].y = m_spos.y + agentHeight / 2; + m_queryPoly[0].z = m_spos.z + nz * 1.2f; - m_queryPoly[3] = m_spos.x - nx * 1.3f; - m_queryPoly[4] = m_spos.y + agentHeight / 2; - m_queryPoly[5] = m_spos.z - nz * 1.3f; + m_queryPoly[1].x = m_spos.x - nx * 1.3f; + m_queryPoly[1].y = m_spos.y + agentHeight / 2; + m_queryPoly[1].z = m_spos.z - nz * 1.3f; - m_queryPoly[6] = m_epos.x - nx * 0.8f; - m_queryPoly[7] = m_epos.y + agentHeight / 2; - m_queryPoly[8] = m_epos.z - nz * 0.8f; + m_queryPoly[2].x = m_epos.x - nx * 0.8f; + m_queryPoly[2].y = m_epos.y + agentHeight / 2; + m_queryPoly[2].z = m_epos.z - nz * 0.8f; - m_queryPoly[9] = m_epos.x + nx; - m_queryPoly[10] = m_epos.y + agentHeight / 2; - m_queryPoly[11] = m_epos.z + nz; + m_queryPoly[3].x = m_epos.x + nx; + m_queryPoly[3].y = m_epos.y + agentHeight / 2; + m_queryPoly[3].z = m_epos.z + nz; var refs = new List(); var parentRefs = new List(); @@ -659,8 +663,8 @@ public class TestNavmeshTool : IRcTool dd.Begin(LINES, 2.0f); for (int i = 0, j = 3; i < 4; j = i++) { - dd.Vertex(m_queryPoly[j * 3], m_queryPoly[j * 3 + 1], m_queryPoly[j * 3 + 2], col); - dd.Vertex(m_queryPoly[i * 3], m_queryPoly[i * 3 + 1], m_queryPoly[i * 3 + 2], col); + dd.Vertex(m_queryPoly[j].x, m_queryPoly[j].y, m_queryPoly[j].z, col); + dd.Vertex(m_queryPoly[i].x, m_queryPoly[i].y, m_queryPoly[i].z, col); } dd.End(); diff --git a/test/DotRecast.Detour.Test/FindPolysAroundShapeTest.cs b/test/DotRecast.Detour.Test/FindPolysAroundShapeTest.cs index 8935105..1c00f36 100644 --- a/test/DotRecast.Detour.Test/FindPolysAroundShapeTest.cs +++ b/test/DotRecast.Detour.Test/FindPolysAroundShapeTest.cs @@ -158,28 +158,28 @@ public class FindPolysAroundShapeTest : AbstractDetourTest } } - private float[] GetQueryPoly(RcVec3f m_spos, RcVec3f m_epos) + private RcVec3f[] GetQueryPoly(RcVec3f m_spos, RcVec3f m_epos) { float nx = (m_epos.z - m_spos.z) * 0.25f; float nz = -(m_epos.x - m_spos.x) * 0.25f; float agentHeight = 2.0f; - float[] m_queryPoly = new float[12]; - m_queryPoly[0] = m_spos.x + nx * 1.2f; - m_queryPoly[1] = m_spos.y + agentHeight / 2; - m_queryPoly[2] = m_spos.z + nz * 1.2f; + RcVec3f[] m_queryPoly = new RcVec3f[4]; + m_queryPoly[0].x = m_spos.x + nx * 1.2f; + m_queryPoly[0].y = m_spos.y + agentHeight / 2; + m_queryPoly[0].z = m_spos.z + nz * 1.2f; - m_queryPoly[3] = m_spos.x - nx * 1.3f; - m_queryPoly[4] = m_spos.y + agentHeight / 2; - m_queryPoly[5] = m_spos.z - nz * 1.3f; + m_queryPoly[1].x = m_spos.x - nx * 1.3f; + m_queryPoly[1].y = m_spos.y + agentHeight / 2; + m_queryPoly[1].z = m_spos.z - nz * 1.3f; - m_queryPoly[6] = m_epos.x - nx * 0.8f; - m_queryPoly[7] = m_epos.y + agentHeight / 2; - m_queryPoly[8] = m_epos.z - nz * 0.8f; + m_queryPoly[2].x = m_epos.x - nx * 0.8f; + m_queryPoly[2].y = m_epos.y + agentHeight / 2; + m_queryPoly[2].z = m_epos.z - nz * 0.8f; - m_queryPoly[9] = m_epos.x + nx; - m_queryPoly[10] = m_epos.y + agentHeight / 2; - m_queryPoly[11] = m_epos.z + nz; + m_queryPoly[3].x = m_epos.x + nx; + m_queryPoly[3].y = m_epos.y + agentHeight / 2; + m_queryPoly[3].z = m_epos.z + nz; return m_queryPoly; } } \ No newline at end of file