bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases 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

@ -881,6 +881,34 @@ namespace DotRecast.Core
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)
{
float pqx = verts[q + 0] - verts[p + 0];

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();
}
protected void addSegment(float dist, float[] s)
protected void addSegment(float dist, SegmentVert s)
{
// Insert neighbour based on the distance.
Segment seg = new Segment();
seg.s[0].x = s[0];
seg.s[0].y = s[1];
seg.s[0].z = s[2];
seg.s[1].x = s[3];
seg.s[1].y = s[4];
seg.s[1].z = s[5];
seg.s[0] = s.vmin;
seg.s[1] = s.vmax;
//Array.Copy(s, seg.s, 6);
seg.d = dist;
if (0 == m_segs.Count)
@ -126,7 +122,7 @@ namespace DotRecast.Detour.Crowd
GetPolyWallSegmentsResult gpws = result.result;
for (int k = 0; k < gpws.getSegmentRefs().Count; ++k)
{
float[] s = gpws.getSegmentVerts()[k];
SegmentVert s = gpws.getSegmentVerts()[k];
// Skip too distant segments.
Tuple<float, float> distseg = distancePtSegSqr2D(pos, s, 0, 3);
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 DotRecast.Core;
namespace DotRecast.Detour
{
public class GetPolyWallSegmentsResult
{
private readonly List<float[]> segmentVerts;
private readonly List<SegmentVert> segmentVerts;
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.segmentRefs = segmentRefs;
}
public List<float[]> getSegmentVerts()
public List<SegmentVert> getSegmentVerts()
{
return segmentVerts;
}

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

@ -3096,7 +3096,7 @@ namespace DotRecast.Detour
Poly poly = tileAndPoly.result.Item2;
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);
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 ivi = poly.verts[i] * 3;
float[] seg = new float[6];
Array.Copy(tile.data.verts, ivj, seg, 0, 3);
Array.Copy(tile.data.verts, ivi, seg, 3, 3);
var seg = new SegmentVert();
vCopy(ref seg.vmin, tile.data.verts, ivj);
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);
segmentRefs.Add(neiRef);
continue;
@ -3168,15 +3170,9 @@ namespace DotRecast.Detour
{
float tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f;
float[] seg = new float[6];
var vmin = vLerp(tile.data.verts, vj, vi, tmin);
var 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];
var seg = new SegmentVert();
seg.vmin = vLerp(tile.data.verts, vj, vi, tmin);
seg.vmax = vLerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
segmentRefs.Add(ints[k].refs);
}
@ -3188,15 +3184,9 @@ namespace DotRecast.Detour
{
float tmin = imin / 255.0f;
float tmax = imax / 255.0f;
float[] seg = new float[6];
var vmin = vLerp(tile.data.verts, vj, vi, tmin);
var 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];
var seg = new SegmentVert();
seg.vmin = vLerp(tile.data.verts, vj, vi, tmin);
seg.vmax = vLerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
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;
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]);
// Skip too distant segments.
Tuple<float, float> distSqr = distancePtSegSqr2D(m_spos, s, 0, 3);
@ -854,8 +854,8 @@ public class TestNavmeshTool : Tool
continue;
}
Vector3f delta = vSub(s3, s);
Vector3f p0 = vMad(s, delta, 0.5f);
Vector3f delta = vSub(s3, s.vmin);
Vector3f p0 = vMad(s.vmin, delta, 0.5f);
Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]);
vNormalize(ref norm);
Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f);
@ -869,7 +869,7 @@ public class TestNavmeshTool : Tool
else
{
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);
}