bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

float[12] to RcVec3f[4]

This commit is contained in:
ikpil 2023-06-30 00:12:36 +09:00
parent 52fbaeac7f
commit 601526e053
4 changed files with 51 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/DotRecast.Detour/DetourCommon.cs
View File

@ -339,7 +339,7 @@ namespace DotRecast.Detour
return dx * dx + dz * dz; 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(); IntersectResult result = new IntersectResult();
const float EPS = 0.000001f; const float EPS = 0.000001f;
@ -348,8 +348,8 @@ namespace DotRecast.Detour
var p0v = p0; var p0v = p0;
for (int i = 0, j = nverts - 1; i < nverts; j = i++) for (int i = 0, j = nverts - 1; i < nverts; j = i++)
{ {
RcVec3f vpj = RcVec3f.Of(verts, j * 3); RcVec3f vpj = verts[j];
RcVec3f vpi = RcVec3f.Of(verts, i * 3); RcVec3f vpi = verts[i];
var edge = vpi.Subtract(vpj); var edge = vpi.Subtract(vpj);
var diff = p0v.Subtract(vpj); var diff = p0v.Subtract(vpj);
float n = RcVec3f.Perp2D(edge, diff); float n = RcVec3f.Perp2D(edge, diff);

24
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -2198,7 +2198,7 @@ namespace DotRecast.Detour
hit = new DtRaycastHit(); 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 curPos = RcVec3f.Zero;
RcVec3f lastPos = RcVec3f.Zero; RcVec3f lastPos = RcVec3f.Zero;
@ -2227,7 +2227,7 @@ namespace DotRecast.Detour
int nv = 0; int nv = 0;
for (int i = 0; i < poly.vertCount; ++i) 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++; nv++;
} }
@ -2366,8 +2366,8 @@ namespace DotRecast.Detour
// and correct the height (since the raycast moves in 2d) // and correct the height (since the raycast moves in 2d)
lastPos = curPos; lastPos = curPos;
curPos = RcVec3f.Mad(startPos, dir, hit.t); curPos = RcVec3f.Mad(startPos, dir, hit.t);
var e1 = RcVec3f.Of(verts, iresult.segMax * 3); var e1 = verts[iresult.segMax];
var e2 = RcVec3f.Of(verts, ((iresult.segMax + 1) % nv) * 3); var e2 = verts[(iresult.segMax + 1) % nv];
var eDir = e2.Subtract(e1); var eDir = e2.Subtract(e1);
var diff = curPos.Subtract(e1); var diff = curPos.Subtract(e1);
float s = Sqr(eDir.x) > Sqr(eDir.z) ? diff.x / eDir.x : diff.z / eDir.z; 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. // Calculate hit normal.
int a = iresult.segMax; int a = iresult.segMax;
int b = iresult.segMax + 1 < nv ? iresult.segMax + 1 : 0; int b = iresult.segMax + 1 < nv ? iresult.segMax + 1 : 0;
int va = a * 3; // int va = a * 3;
int vb = b * 3; // int vb = b * 3;
float dx = verts[vb] - verts[va]; float dx = verts[b].x - verts[a].x;
float dz = verts[vb + 2] - verts[va + 2]; float dz = verts[b].z - verts[a].x;
hit.hitNormal.x = dz; hit.hitNormal.x = dz;
hit.hitNormal.y = 0; hit.hitNormal.y = 0;
hit.hitNormal.z = -dx; hit.hitNormal.z = -dx;
@ -2620,7 +2620,7 @@ namespace DotRecast.Detour
/// @param[out] resultCount The number of polygons found. /// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold. /// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query. /// @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<long> resultRef, ref List<long> resultParent, ref List<float> resultCost) ref List<long> resultRef, ref List<long> resultParent, ref List<float> resultCost)
{ {
resultRef.Clear(); resultRef.Clear();
@ -2628,7 +2628,7 @@ namespace DotRecast.Detour
resultCost.Clear(); resultCost.Clear();
// Validate input // Validate input
int nverts = verts.Length / 3; int nverts = verts.Length;
if (!m_nav.IsValidPolyRef(startRef) || null == verts || nverts < 3 || null == filter) if (!m_nav.IsValidPolyRef(startRef) || null == verts || nverts < 3 || null == filter)
{ {
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
@ -2640,9 +2640,7 @@ namespace DotRecast.Detour
RcVec3f centerPos = RcVec3f.Zero; RcVec3f centerPos = RcVec3f.Zero;
for (int i = 0; i < nverts; ++i) for (int i = 0; i < nverts; ++i)
{ {
centerPos.x += verts[i * 3]; centerPos += verts[i];
centerPos.y += verts[i * 3 + 1];
centerPos.z += verts[i * 3 + 2];
} }
float scale = 1.0f / nverts; float scale = 1.0f / nverts;

42
src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs
View File

@ -22,19 +22,23 @@ public class TestNavmeshTool : IRcTool
private bool m_eposSet; private bool m_eposSet;
private RcVec3f m_spos; private RcVec3f m_spos;
private RcVec3f m_epos; 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_startRef;
private long m_endRef; 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 RcVec3f m_hitPos;
private float m_distanceToWall;
private RcVec3f m_hitNormal; private RcVec3f m_hitNormal;
private bool m_hitResult;
private float m_distanceToWall;
private List<StraightPathItem> m_straightPath; private List<StraightPathItem> m_straightPath;
private List<long> m_polys; private List<long> m_polys;
private bool m_hitResult;
private List<long> m_parent; private List<long> m_parent;
private float m_neighbourhoodRadius; private float m_neighbourhoodRadius;
private readonly float[] m_queryPoly = new float[12]; private readonly RcVec3f[] m_queryPoly = new RcVec3f[4];
private List<RcVec3f> m_smoothPath; private List<RcVec3f> m_smoothPath;
private DtStatus m_pathFindStatus = DtStatus.DT_FAILURE; private DtStatus m_pathFindStatus = DtStatus.DT_FAILURE;
private readonly List<RcVec3f> randomPoints = new(); private readonly List<RcVec3f> randomPoints = new();
@ -293,21 +297,21 @@ public class TestNavmeshTool : IRcTool
float nz = -(m_epos.x - m_spos.x) * 0.25f; float nz = -(m_epos.x - m_spos.x) * 0.25f;
float agentHeight = _impl.GetSample() != null ? _impl.GetSample().GetSettings().agentHeight : 0; float agentHeight = _impl.GetSample() != null ? _impl.GetSample().GetSettings().agentHeight : 0;
m_queryPoly[0] = m_spos.x + nx * 1.2f; m_queryPoly[0].x = m_spos.x + nx * 1.2f;
m_queryPoly[1] = m_spos.y + agentHeight / 2; m_queryPoly[0].y = m_spos.y + agentHeight / 2;
m_queryPoly[2] = m_spos.z + nz * 1.2f; m_queryPoly[0].z = m_spos.z + nz * 1.2f;
m_queryPoly[3] = m_spos.x - nx * 1.3f; m_queryPoly[1].x = m_spos.x - nx * 1.3f;
m_queryPoly[4] = m_spos.y + agentHeight / 2; m_queryPoly[1].y = m_spos.y + agentHeight / 2;
m_queryPoly[5] = m_spos.z - nz * 1.3f; m_queryPoly[1].z = m_spos.z - nz * 1.3f;
m_queryPoly[6] = m_epos.x - nx * 0.8f; m_queryPoly[2].x = m_epos.x - nx * 0.8f;
m_queryPoly[7] = m_epos.y + agentHeight / 2; m_queryPoly[2].y = m_epos.y + agentHeight / 2;
m_queryPoly[8] = m_epos.z - nz * 0.8f; m_queryPoly[2].z = m_epos.z - nz * 0.8f;
m_queryPoly[9] = m_epos.x + nx; m_queryPoly[3].x = m_epos.x + nx;
m_queryPoly[10] = m_epos.y + agentHeight / 2; m_queryPoly[3].y = m_epos.y + agentHeight / 2;
m_queryPoly[11] = m_epos.z + nz; m_queryPoly[3].z = m_epos.z + nz;
var refs = new List<long>(); var refs = new List<long>();
var parentRefs = new List<long>(); var parentRefs = new List<long>();
@ -659,8 +663,8 @@ public class TestNavmeshTool : IRcTool
dd.Begin(LINES, 2.0f); dd.Begin(LINES, 2.0f);
for (int i = 0, j = 3; i < 4; j = i++) 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[j].x, m_queryPoly[j].y, m_queryPoly[j].z, col);
dd.Vertex(m_queryPoly[i * 3], m_queryPoly[i * 3 + 1], m_queryPoly[i * 3 + 2], col); dd.Vertex(m_queryPoly[i].x, m_queryPoly[i].y, m_queryPoly[i].z, col);
} }
dd.End(); dd.End();

28
test/DotRecast.Detour.Test/FindPolysAroundShapeTest.cs
View File

@ -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 nx = (m_epos.z - m_spos.z) * 0.25f;
float nz = -(m_epos.x - m_spos.x) * 0.25f; float nz = -(m_epos.x - m_spos.x) * 0.25f;
float agentHeight = 2.0f; float agentHeight = 2.0f;
float[] m_queryPoly = new float[12]; RcVec3f[] m_queryPoly = new RcVec3f[4];
m_queryPoly[0] = m_spos.x + nx * 1.2f; m_queryPoly[0].x = m_spos.x + nx * 1.2f;
m_queryPoly[1] = m_spos.y + agentHeight / 2; m_queryPoly[0].y = m_spos.y + agentHeight / 2;
m_queryPoly[2] = m_spos.z + nz * 1.2f; m_queryPoly[0].z = m_spos.z + nz * 1.2f;
m_queryPoly[3] = m_spos.x - nx * 1.3f; m_queryPoly[1].x = m_spos.x - nx * 1.3f;
m_queryPoly[4] = m_spos.y + agentHeight / 2; m_queryPoly[1].y = m_spos.y + agentHeight / 2;
m_queryPoly[5] = m_spos.z - nz * 1.3f; m_queryPoly[1].z = m_spos.z - nz * 1.3f;
m_queryPoly[6] = m_epos.x - nx * 0.8f; m_queryPoly[2].x = m_epos.x - nx * 0.8f;
m_queryPoly[7] = m_epos.y + agentHeight / 2; m_queryPoly[2].y = m_epos.y + agentHeight / 2;
m_queryPoly[8] = m_epos.z - nz * 0.8f; m_queryPoly[2].z = m_epos.z - nz * 0.8f;
m_queryPoly[9] = m_epos.x + nx; m_queryPoly[3].x = m_epos.x + nx;
m_queryPoly[10] = m_epos.y + agentHeight / 2; m_queryPoly[3].y = m_epos.y + agentHeight / 2;
m_queryPoly[11] = m_epos.z + nz; m_queryPoly[3].z = m_epos.z + nz;
return m_queryPoly; return m_queryPoly;
} }
} }