a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests 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;
}
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);

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

@ -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<long> resultRef, ref List<long> resultParent, ref List<float> 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;

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

@ -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<StraightPathItem> m_straightPath;
private List<long> m_polys;
private bool m_hitResult;
private List<long> m_parent;
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 DtStatus m_pathFindStatus = DtStatus.DT_FAILURE;
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 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<long>();
var parentRefs = new List<long>();
@ -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();

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 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;
}
}