a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests Activity

add SegmentVert

This commit is contained in:
ikpil 2023-04-22 13:43:24 +09:00
parent f4406b05e0
commit 2300779b72
6 changed files with 81 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/DotRecast.Core/RecastMath.cs
View File

@ -880,6 +880,34 @@ namespace DotRecast.Core
result.intersects = true; result.intersects = true;
return result; return result;
} }
public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, SegmentVert verts, int p, int q)
{
float pqx = verts[q + 0] - verts[p + 0];
float pqz = verts[q + 2] - verts[p + 2];
float dx = pt[0] - verts[p + 0];
float dz = pt[2] - verts[p + 2];
float d = pqx * pqx + pqz * pqz;
float t = pqx * dx + pqz * dz;
if (d > 0)
{
t /= d;
}
if (t < 0)
{
t = 0;
}
else if (t > 1)
{
t = 1;
}
dx = verts[p + 0] + t * pqx - pt[0];
dz = verts[p + 2] + t * pqz - pt[2];
return Tuple.Create(dx * dx + dz * dz, t);
}
public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q) public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q)
{ {

29
src/DotRecast.Core/SegmentVert.cs Normal file
View File

@ -0,0 +1,29 @@
using System;
namespace DotRecast.Core
{
public struct SegmentVert
{
public Vector3f vmin;
public Vector3f vmax;
public float this[int index]
{
get => GetElement(index);
}
public float GetElement(int index)
{
switch (index)
{
case 0: return vmin.x;
case 1: return vmin.y;
case 2: return vmin.z;
case 3: return vmax.x;
case 4: return vmax.y;
case 5: return vmax.z;
default: throw new IndexOutOfRangeException($"{index}");
}
}
}
}

12
src/DotRecast.Detour.Crowd/LocalBoundary.cs
View File

@ -55,16 +55,12 @@ namespace DotRecast.Detour.Crowd
m_segs.Clear(); m_segs.Clear();
} }
protected void addSegment(float dist, float[] s) protected void addSegment(float dist, SegmentVert s)
{ {
// Insert neighbour based on the distance. // Insert neighbour based on the distance.
Segment seg = new Segment(); Segment seg = new Segment();
seg.s[0].x = s[0]; seg.s[0] = s.vmin;
seg.s[0].y = s[1]; seg.s[1] = s.vmax;
seg.s[0].z = s[2];
seg.s[1].x = s[3];
seg.s[1].y = s[4];
seg.s[1].z = s[5];
//Array.Copy(s, seg.s, 6); //Array.Copy(s, seg.s, 6);
seg.d = dist; seg.d = dist;
if (0 == m_segs.Count) if (0 == m_segs.Count)
@ -126,7 +122,7 @@ namespace DotRecast.Detour.Crowd
GetPolyWallSegmentsResult gpws = result.result; GetPolyWallSegmentsResult gpws = result.result;
for (int k = 0; k < gpws.getSegmentRefs().Count; ++k) for (int k = 0; k < gpws.getSegmentRefs().Count; ++k)
{ {
float[] s = gpws.getSegmentVerts()[k]; SegmentVert s = gpws.getSegmentVerts()[k];
// Skip too distant segments. // Skip too distant segments.
Tuple<float, float> distseg = distancePtSegSqr2D(pos, s, 0, 3); Tuple<float, float> distseg = distancePtSegSqr2D(pos, s, 0, 3);
if (distseg.Item1 > sqr(collisionQueryRange)) if (distseg.Item1 > sqr(collisionQueryRange))

7
src/DotRecast.Detour/GetPolyWallSegmentsResult.cs
View File

@ -19,21 +19,22 @@ freely, subject to the following restrictions:
*/ */
using System.Collections.Generic; using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour namespace DotRecast.Detour
{ {
public class GetPolyWallSegmentsResult public class GetPolyWallSegmentsResult
{ {
private readonly List<float[]> segmentVerts; private readonly List<SegmentVert> segmentVerts;
private readonly List<long> segmentRefs; private readonly List<long> segmentRefs;
public GetPolyWallSegmentsResult(List<float[]> segmentVerts, List<long> segmentRefs) public GetPolyWallSegmentsResult(List<SegmentVert> segmentVerts, List<long> segmentRefs)
{ {
this.segmentVerts = segmentVerts; this.segmentVerts = segmentVerts;
this.segmentRefs = segmentRefs; this.segmentRefs = segmentRefs;
} }
public List<float[]> getSegmentVerts() public List<SegmentVert> getSegmentVerts()
{ {
return segmentVerts; return segmentVerts;
} }

34
src/DotRecast.Detour/NavMeshQuery.cs
View File

@ -3096,7 +3096,7 @@ namespace DotRecast.Detour
Poly poly = tileAndPoly.result.Item2; Poly poly = tileAndPoly.result.Item2;
List<long> segmentRefs = new List<long>(); List<long> segmentRefs = new List<long>();
List<float[]> segmentVerts = new List<float[]>(); List<SegmentVert> segmentVerts = new List<SegmentVert>();
List<SegInterval> ints = new List<SegInterval>(16); List<SegInterval> ints = new List<SegInterval>(16);
for (int i = 0, j = poly.vertCount - 1; i < poly.vertCount; j = i++) for (int i = 0, j = poly.vertCount - 1; i < poly.vertCount; j = i++)
@ -3146,9 +3146,11 @@ namespace DotRecast.Detour
int ivj = poly.verts[j] * 3; int ivj = poly.verts[j] * 3;
int ivi = poly.verts[i] * 3; int ivi = poly.verts[i] * 3;
float[] seg = new float[6]; var seg = new SegmentVert();
Array.Copy(tile.data.verts, ivj, seg, 0, 3); vCopy(ref seg.vmin, tile.data.verts, ivj);
Array.Copy(tile.data.verts, ivi, seg, 3, 3); vCopy(ref seg.vmax, tile.data.verts, ivi);
// Array.Copy(tile.data.verts, ivj, seg, 0, 3);
// Array.Copy(tile.data.verts, ivi, seg, 3, 3);
segmentVerts.Add(seg); segmentVerts.Add(seg);
segmentRefs.Add(neiRef); segmentRefs.Add(neiRef);
continue; continue;
@ -3168,15 +3170,9 @@ namespace DotRecast.Detour
{ {
float tmin = ints[k].tmin / 255.0f; float tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f; float tmax = ints[k].tmax / 255.0f;
float[] seg = new float[6]; var seg = new SegmentVert();
var vmin = vLerp(tile.data.verts, vj, vi, tmin); seg.vmin = vLerp(tile.data.verts, vj, vi, tmin);
var vmax = vLerp(tile.data.verts, vj, vi, tmax); seg.vmax = vLerp(tile.data.verts, vj, vi, tmax);
seg[0] = vmin[0];
seg[1] = vmin[1];
seg[2] = vmin[2];
seg[3] = vmax[0];
seg[4] = vmax[1];
seg[5] = vmax[2];
segmentVerts.Add(seg); segmentVerts.Add(seg);
segmentRefs.Add(ints[k].refs); segmentRefs.Add(ints[k].refs);
} }
@ -3188,15 +3184,9 @@ namespace DotRecast.Detour
{ {
float tmin = imin / 255.0f; float tmin = imin / 255.0f;
float tmax = imax / 255.0f; float tmax = imax / 255.0f;
float[] seg = new float[6]; var seg = new SegmentVert();
var vmin = vLerp(tile.data.verts, vj, vi, tmin); seg.vmin = vLerp(tile.data.verts, vj, vi, tmin);
var vmax = vLerp(tile.data.verts, vj, vi, tmax); seg.vmax = vLerp(tile.data.verts, vj, vi, tmax);
seg[0] = vmin[0];
seg[1] = vmin[1];
seg[2] = vmin[2];
seg[3] = vmax[0];
seg[4] = vmax[1];
seg[5] = vmax[2];
segmentVerts.Add(seg); segmentVerts.Add(seg);
segmentRefs.Add(0L); segmentRefs.Add(0L);
} }

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

@ -845,7 +845,7 @@ public class TestNavmeshTool : Tool
GetPolyWallSegmentsResult wallSegments = result.result; GetPolyWallSegmentsResult wallSegments = result.result;
for (int j = 0; j < wallSegments.getSegmentVerts().Count; ++j) for (int j = 0; j < wallSegments.getSegmentVerts().Count; ++j)
{ {
float[] s = wallSegments.getSegmentVerts()[j]; SegmentVert s = wallSegments.getSegmentVerts()[j];
Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]); Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]);
// Skip too distant segments. // Skip too distant segments.
Tuple<float, float> distSqr = distancePtSegSqr2D(m_spos, s, 0, 3); Tuple<float, float> distSqr = distancePtSegSqr2D(m_spos, s, 0, 3);
@ -854,8 +854,8 @@ public class TestNavmeshTool : Tool
continue; continue;
} }
Vector3f delta = vSub(s3, s); Vector3f delta = vSub(s3, s.vmin);
Vector3f p0 = vMad(s, delta, 0.5f); Vector3f p0 = vMad(s.vmin, delta, 0.5f);
Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]); Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]);
vNormalize(ref norm); vNormalize(ref norm);
Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f); Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f);
@ -869,7 +869,7 @@ public class TestNavmeshTool : Tool
else else
{ {
int col = duRGBA(192, 32, 16, 192); int col = duRGBA(192, 32, 16, 192);
if (triArea2D(m_spos, s, s3) < 0.0f) if (triArea2D(m_spos, s.vmin, s3) < 0.0f)
{ {
col = duRGBA(96, 32, 16, 192); col = duRGBA(96, 32, 16, 192);
} }