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

...

This commit is contained in:
ikpil 2023-03-29 01:52:26 +09:00
parent 5f6a1ac070
commit 254ec50436
57 changed files with 715 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using System.Numerics;
namespace DotRecast.Core
{
@ -108,6 +109,16 @@ namespace DotRecast.Core
return dest.ToArray();
}
public static Vector3f vMad(Vector3f v1, Vector3f v2, float s)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] + v2[0] * s;
dest[1] = v1[1] + v2[1] * s;
dest[2] = v1[2] + v2[2] * s;
return dest;
}
/// Performs a linear interpolation between two vectors. (@p v1 toward @p
/// v2)
/// @param[out] dest The result vector. [(x, y, x)]
@ -132,13 +143,23 @@ namespace DotRecast.Core
return dest.ToArray();
}
public static float[] vSub(VectorPtr v1, VectorPtr v2)
public static Vector3f vLerp(Vector3f v1, Vector3f v2, float t)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] + (v2[0] - v1[0]) * t;
dest[1] = v1[1] + (v2[1] - v1[1]) * t;
dest[2] = v1[2] + (v2[2] - v1[2]) * t;
return dest;
}
public static Vector3f vSub(VectorPtr v1, VectorPtr v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1.get(0) - v2.get(0);
dest[1] = v1.get(1) - v2.get(1);
dest[2] = v1.get(2) - v2.get(2);
return dest.ToArray();
return dest;
}
public static float[] vSub(float[] v1, float[] v2)
@ -150,15 +171,35 @@ namespace DotRecast.Core
return dest.ToArray();
}
public static float[] vSub(Vector3f v1, Vector3f v2)
public static Vector3f vSub(Vector3f v1, Vector3f v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] - v2[0];
dest[1] = v1[1] - v2[1];
dest[2] = v1[2] - v2[2];
return dest.ToArray();
return dest;
}
public static Vector3f vSub(Vector3f v1, VectorPtr v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] - v2.get(0);
dest[1] = v1[1] - v2.get(1);
dest[2] = v1[2] - v2.get(2);
return dest;
}
public static Vector3f vSub(Vector3f v1, float[] v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] - v2[0];
dest[1] = v1[1] - v2[1];
dest[2] = v1[2] - v2[2];
return dest;
}
public static float[] vAdd(float[] v1, float[] v2)
{
@ -169,6 +210,16 @@ namespace DotRecast.Core
return dest.ToArray();
}
public static Vector3f vAdd(Vector3f v1, Vector3f v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] + v2[0];
dest[1] = v1[1] + v2[1];
dest[2] = v1[2] + v2[2];
return dest;
}
public static float[] vCopy(float[] @in)
{
float[] @out = new float[3];
@ -300,6 +351,15 @@ namespace DotRecast.Core
return dx * dx + dy * dy + dz * dz;
}
public static float vDistSqr(Vector3f v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dy = v2[1] - v1[1];
float dz = v2[2] - v1[2];
return dx * dx + dy * dy + dz * dz;
}
public static float vDistSqr(Vector3f v1, Vector3f v2)
{
float dx = v2[0] - v1[0];
@ -317,11 +377,23 @@ namespace DotRecast.Core
return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
}
public static float vLenSqr(Vector3f v)
{
return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
}
public static float vLen(float[] v)
{
return (float)Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
public static float vLen(Vector3f v)
{
return (float)Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
public static float vDist(float[] v1, float[] verts, int i)
{
float dx = verts[i] - v1[0];
@ -345,6 +417,14 @@ namespace DotRecast.Core
return (float)Math.Sqrt(dx * dx + dz * dz);
}
public static float vDist2D(Vector3f v1, Vector3f v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return (float)Math.Sqrt(dx * dx + dz * dz);
}
public static float vDist2DSqr(float[] v1, float[] v2)
{
float dx = v2[0] - v1[0];
@ -352,7 +432,15 @@ namespace DotRecast.Core
return dx * dx + dz * dz;
}
public static float vDist2DSqr(float[] p, float[] verts, int i)
public static float vDist2DSqr(Vector3f v1, Vector3f v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return dx * dx + dz * dz;
}
public static float vDist2DSqr(Vector3f p, float[] verts, int i)
{
float dx = verts[i] - p[0];
float dz = verts[i + 2] - p[2];
@ -396,12 +484,25 @@ namespace DotRecast.Core
return vEqual(p0, p1, EQUAL_THRESHOLD);
}
public static bool vEqual(Vector3f p0, Vector3f p1)
{
return vEqual(p0, p1, EQUAL_THRESHOLD);
}
public static bool vEqual(float[] p0, float[] p1, float thresholdSqr)
{
float d = vDistSqr(p0, p1);
return d < thresholdSqr;
}
public static bool vEqual(Vector3f p0, Vector3f p1, float thresholdSqr)
{
float d = vDistSqr(p0, p1);
return d < thresholdSqr;
}
/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v)
/// @param[in] u A vector [(x, y, z)]
/// @param[in] v A vector [(x, y, z)]
@ -414,6 +515,12 @@ namespace DotRecast.Core
return u[0] * v[0] + u[2] * v[2];
}
public static float vDot2D(Vector3f u, Vector3f v)
{
return u[0] * v[0] + u[2] * v[2];
}
public static float vDot2D(float[] u, float[] v, int vi)
{
return u[0] * v[vi] + u[2] * v[vi + 2];
@ -431,6 +538,12 @@ namespace DotRecast.Core
return u[2] * v[0] - u[0] * v[2];
}
public static float vPerp2D(Vector3f u, Vector3f v)
{
return u[2] * v[0] - u[0] * v[2];
}
/// @}
/// @name Computational geometry helper functions.
/// @{
@ -500,7 +613,16 @@ namespace DotRecast.Core
return overlap;
}
public static Tuple<float, float> distancePtSegSqr2D(float[] pt, float[] p, float[] q)
public static bool overlapBounds(Vector3f amin, Vector3f amax, Vector3f bmin, Vector3f bmax)
{
bool overlap = true;
overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
return overlap;
}
public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q)
{
float pqx = q[0] - p[0];
float pqz = q[2] - p[2];
@ -527,11 +649,11 @@ namespace DotRecast.Core
return Tuple.Create(dx * dx + dz * dz, t);
}
public static float? closestHeightPointTriangle(float[] p, float[] a, float[] b, float[] c)
public static float? closestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c)
{
float[] v0 = vSub(c, a);
float[] v1 = vSub(b, a);
float[] v2 = vSub(p, a);
Vector3f v0 = vSub(c, a);
Vector3f v1 = vSub(b, a);
Vector3f v2 = vSub(p, a);
// Compute scaled barycentric coordinates
float denom = v0[0] * v1[2] - v0[2] * v1[0];
@ -563,7 +685,7 @@ namespace DotRecast.Core
/// @par
///
/// All points are projected onto the xz-plane, so the y-values are ignored.
public static bool pointInPolygon(float[] pt, float[] verts, int nverts)
public static bool pointInPolygon(Vector3f pt, float[] verts, int nverts)
{
// TODO: Replace pnpoly with triArea2D tests?
int i, j;
@ -582,7 +704,7 @@ namespace DotRecast.Core
return c;
}
public static bool distancePtPolyEdgesSqr(float[] pt, float[] verts, int nverts, float[] ed, float[] et)
public static bool distancePtPolyEdgesSqr(Vector3f pt, float[] verts, int nverts, float[] ed, float[] et)
{
// TODO: Replace pnpoly with triArea2D tests?
int i, j;
@ -668,7 +790,7 @@ namespace DotRecast.Core
// Returns a random point in a convex polygon.
// Adapted from Graphics Gems article.
public static float[] randomPointInConvexPoly(float[] pts, int npts, float[] areas, float s, float t)
public static Vector3f randomPointInConvexPoly(float[] pts, int npts, float[] areas, float s, float t)
{
// Calc triangle araes
float areasum = 0.0f;
@ -705,11 +827,11 @@ namespace DotRecast.Core
int pb = (tri - 1) * 3;
int pc = tri * 3;
return new float[]
return new Vector3f()
{
a * pts[pa] + b * pts[pb] + c * pts[pc],
a * pts[pa + 1] + b * pts[pb + 1] + c * pts[pc + 1],
a * pts[pa + 2] + b * pts[pb + 2] + c * pts[pc + 2]
x = a * pts[pa] + b * pts[pb] + c * pts[pc],
y = a * pts[pa + 1] + b * pts[pb + 1] + c * pts[pc + 1],
z = a * pts[pa + 2] + b * pts[pb + 2] + c * pts[pc + 2]
};
}
@ -753,18 +875,18 @@ namespace DotRecast.Core
public int segMax = -1;
}
public static IntersectResult intersectSegmentPoly2D(float[] p0, float[] p1, float[] verts, int nverts)
public static IntersectResult intersectSegmentPoly2D(Vector3f p0, Vector3f p1, float[] verts, int nverts)
{
IntersectResult result = new IntersectResult();
float EPS = 0.000001f;
float[] dir = vSub(p1, p0);
var dir = vSub(p1, p0);
VectorPtr p0v = new VectorPtr(p0);
var p0v = p0;
for (int i = 0, j = nverts - 1; i < nverts; j = i++)
{
VectorPtr vpj = new VectorPtr(verts, j * 3);
float[] edge = vSub(new VectorPtr(verts, i * 3), vpj);
float[] diff = vSub(p0v, vpj);
var edge = vSub(new VectorPtr(verts, i * 3), vpj);
var diff = vSub(p0v, vpj);
float n = vPerp2D(edge, diff);
float d = vPerp2D(dir, edge);
if (Math.Abs(d) < EPS)
@ -815,7 +937,7 @@ namespace DotRecast.Core
return result;
}
public static Tuple<float, float> distancePtSegSqr2D(float[] pt, float[] verts, int p, int q)
public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q)
{
float pqx = verts[q + 0] - verts[p + 0];
float pqz = verts[q + 2] - verts[p + 2];
@ -852,6 +974,12 @@ namespace DotRecast.Core
return a[0] * b[2] - a[2] * b[0];
}
public static float vperpXZ(Vector3f a, Vector3f b)
{
return a[0] * b[2] - a[2] * b[0];
}
public static Tuple<float, float>? intersectSegSeg2D(float[] ap, float[] aq, float[] bp, float[] bq)
{
float[] u = vSub(aq, ap);
@ -870,9 +998,9 @@ namespace DotRecast.Core
public static Tuple<float, float>? intersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq)
{
float[] u = vSub(aq, ap);
float[] v = vSub(bq, bp);
float[] w = vSub(ap, bp);
Vector3f u = vSub(aq, ap);
Vector3f v = vSub(bq, bp);
Vector3f w = vSub(ap, bp);
float d = vperpXZ(u, v);
if (Math.Abs(d) < 1e-6f)
{
@ -894,6 +1022,16 @@ namespace DotRecast.Core
return @out;
}
public static Vector3f vScale(Vector3f @in, float scale)
{
var @out = new Vector3f();
@out[0] = @in[0] * scale;
@out[1] = @in[1] * scale;
@out[2] = @in[2] * scale;
return @out;
}
/// Checks that the specified vector's components are all finite.
/// @param[in] v A point. [(x, y, z)]
/// @return True if all of the point's components are finite, i.e. not NaN
@ -903,11 +1041,21 @@ namespace DotRecast.Core
return float.IsFinite(v[0]) && float.IsFinite(v[1]) && float.IsFinite(v[2]);
}
public static bool vIsFinite(Vector3f v)
{
return float.IsFinite(v[0]) && float.IsFinite(v[1]) && float.IsFinite(v[2]);
}
/// Checks that the specified vector's 2D components are finite.
/// @param[in] v A point. [(x, y, z)]
public static bool vIsFinite2D(float[] v)
{
return float.IsFinite(v[0]) && float.IsFinite(v[2]);
}
public static bool vIsFinite2D(Vector3f v)
{
return float.IsFinite(v[0]) && float.IsFinite(v[2]);
}
}
}

46
src/DotRecast.Core/Vector2f.cs Normal file
View File

@ -0,0 +1,46 @@
using System;
namespace DotRecast.Core
{
public struct Vector2f
{
public float x;
public float y;
public float this[int index]
{
get => GetElement(index);
set => SetElement(index, value);
}
public float GetElement(int index)
{
switch (index)
{
case 0: return x;
case 1: return y;
default: throw new IndexOutOfRangeException($"{index}");
}
}
public void SetElement(int index, float value)
{
switch (index)
{
case 0:
x = value;
break;
case 1:
y = value;
break;
default: throw new IndexOutOfRangeException($"{index}-{value}");
}
}
public float[] ToArray()
{
return new float[] { x, y };
}
}
}

36
src/DotRecast.Detour.Crowd/Crowd.cs
View File

@ -163,7 +163,7 @@ namespace DotRecast.Detour.Crowd
public Crowd(CrowdConfig config, NavMesh nav, Func<int, QueryFilter> queryFilterFactory)
{
_config = config;
vSet(m_ext, config.maxAgentRadius * 2.0f, config.maxAgentRadius * 1.5f, config.maxAgentRadius * 2.0f);
vSet(ref m_ext, config.maxAgentRadius * 2.0f, config.maxAgentRadius * 1.5f, config.maxAgentRadius * 2.0f);
m_obstacleQuery = new ObstacleAvoidanceQuery(config.maxObstacleAvoidanceCircles, config.maxObstacleAvoidanceSegments);
@ -236,7 +236,7 @@ namespace DotRecast.Detour.Crowd
* The configutation of the agent.
* @return The newly created agent object
*/
public CrowdAgent addAgent(float[] pos, CrowdAgentParams option)
public CrowdAgent addAgent(Vector3f pos, CrowdAgentParams option)
{
CrowdAgent ag = new CrowdAgent(agentId.GetAndIncrement());
m_agents.Add(ag);
@ -245,7 +245,7 @@ namespace DotRecast.Detour.Crowd
// Find nearest position on navmesh and place the agent there.
Result<FindNearestPolyResult> nearestPoly = navQuery.findNearestPoly(pos, m_ext, m_filters[ag.option.queryFilterType]);
float[] nearest = nearestPoly.succeeded() ? nearestPoly.result.getNearestPos() : pos;
var nearest = nearestPoly.succeeded() ? nearestPoly.result.getNearestPos() : pos;
long refs = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L;
ag.corridor.reset(refs, nearest);
ag.boundary.reset();
@ -254,10 +254,10 @@ namespace DotRecast.Detour.Crowd
ag.topologyOptTime = 0;
ag.targetReplanTime = 0;
vSet(ag.dvel, 0, 0, 0);
vSet(ag.nvel, 0, 0, 0);
vSet(ag.vel, 0, 0, 0);
vCopy(ag.npos, nearest);
vSet(ref ag.dvel, 0, 0, 0);
vSet(ref ag.nvel, 0, 0, 0);
vSet(ref ag.vel, 0, 0, 0);
vCopy(ref ag.npos, nearest);
ag.desiredSpeed = 0;
@ -286,7 +286,7 @@ namespace DotRecast.Detour.Crowd
m_agents.Remove(agent);
}
private bool requestMoveTargetReplan(CrowdAgent ag, long refs, float[] pos)
private bool requestMoveTargetReplan(CrowdAgent ag, long refs, Vector3f pos)
{
ag.setTarget(refs, pos);
ag.targetReplan = true;
@ -304,7 +304,7 @@ namespace DotRecast.Detour.Crowd
/// The position will be constrained to the surface of the navigation mesh.
///
/// The request will be processed during the next #update().
public bool requestMoveTarget(CrowdAgent agent, long refs, float[] pos)
public bool requestMoveTarget(CrowdAgent agent, long refs, Vector3f pos)
{
if (refs == 0)
{
@ -325,7 +325,7 @@ namespace DotRecast.Detour.Crowd
{
// Initialize request.
agent.targetRef = 0;
vCopy(agent.targetPos, vel);
vCopy(ref agent.targetPos, vel);
agent.targetPathQueryResult = null;
agent.targetReplan = false;
agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY;
@ -340,8 +340,8 @@ namespace DotRecast.Detour.Crowd
{
// Initialize request.
agent.targetRef = 0;
vSet(agent.targetPos, 0, 0, 0);
vSet(agent.dvel, 0, 0, 0);
vSet(ref agent.targetPos, 0, 0, 0);
vSet(ref agent.dvel, 0, 0, 0);
agent.targetPathQueryResult = null;
agent.targetReplan = false;
agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE;
@ -358,7 +358,7 @@ namespace DotRecast.Detour.Crowd
return new List<CrowdAgent>(m_agents);
}
public float[] getQueryExtents()
public Vector3f getQueryExtents()
{
return m_ext;
}
@ -455,7 +455,7 @@ namespace DotRecast.Detour.Crowd
// First check that the current location is valid.
Vector3f agentPos = new Vector3f();
long agentRef = ag.corridor.getFirstPoly();
vCopy(agentPos, ag.npos);
vCopy(ref agentPos, ag.npos);
if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType]))
{
// Current location is not valid, try to reposition.
@ -465,7 +465,7 @@ namespace DotRecast.Detour.Crowd
agentRef = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L;
if (nearestPoly.succeeded())
{
vCopy(agentPos, nearestPoly.result.getNearestPos());
vCopy(ref agentPos, nearestPoly.result.getNearestPos());
}
if (agentRef == 0)
@ -1008,10 +1008,10 @@ namespace DotRecast.Detour.Crowd
// Adjust the path over the off-mesh connection.
long[] refs = new long[2];
if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, anim.startPos,
anim.endPos, navQuery))
if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, ref anim.startPos,
ref anim.endPos, navQuery))
{
vCopy(anim.initPos, ag.npos);
vCopy(ref anim.initPos, ag.npos);
anim.polyRef = refs[1];
anim.active = true;
anim.t = 0.0f;

25
src/DotRecast.Detour.Crowd/CrowdAgent.cs
View File

@ -20,6 +20,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour.Crowd
{
@ -138,7 +139,7 @@ namespace DotRecast.Detour.Crowd
{
// Fake dynamic constraint.
float maxDelta = option.maxAcceleration * dt;
float[] dv = vSub(nvel, vel);
Vector3f dv = vSub(nvel, vel);
float ds = vLen(dv);
if (ds > maxDelta)
dv = vScale(dv, maxDelta / ds);
@ -148,7 +149,7 @@ namespace DotRecast.Detour.Crowd
if (vLen(vel) > 0.0001f)
npos = vMad(npos, vel, dt);
else
vSet(vel, 0, 0, 0);
vSet(ref vel, 0, 0, 0);
}
public bool overOffmeshConnection(float radius)
@ -182,18 +183,18 @@ namespace DotRecast.Detour.Crowd
return range;
}
public float[] calcSmoothSteerDirection()
public Vector3f calcSmoothSteerDirection()
{
Vector3f dir = new Vector3f();
if (0 < corners.Count)
{
int ip0 = 0;
int ip1 = Math.Min(1, corners.Count - 1);
float[] p0 = corners[ip0].getPos();
float[] p1 = corners[ip1].getPos();
var p0 = corners[ip0].getPos();
var p1 = corners[ip1].getPos();
float[] dir0 = vSub(p0, npos);
float[] dir1 = vSub(p1, npos);
var dir0 = vSub(p0, npos);
var dir1 = vSub(p1, npos);
dir0[1] = 0;
dir1[1] = 0;
@ -206,29 +207,29 @@ namespace DotRecast.Detour.Crowd
dir[1] = 0;
dir[2] = dir0[2] - dir1[2] * len0 * 0.5f;
vNormalize(dir);
vNormalize(ref dir);
}
return dir;
}
public float[] calcStraightSteerDirection()
public Vector3f calcStraightSteerDirection()
{
Vector3f dir = new Vector3f();
if (0 < corners.Count)
{
dir = vSub(corners[0].getPos(), npos);
dir[1] = 0;
vNormalize(dir);
vNormalize(ref dir);
}
return dir;
}
public void setTarget(long refs, float[] pos)
public void setTarget(long refs, Vector3f pos)
{
targetRef = refs;
vCopy(targetPos, pos);
vCopy(ref targetPos, pos);
targetPathQueryResult = null;
if (targetRef != 0)
{

2
src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour.Crowd
{
public class CrowdAgentAnimation

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

@ -20,6 +20,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour.Crowd
{
@ -94,7 +95,7 @@ namespace DotRecast.Detour.Crowd
}
}
public void update(long refs, float[] pos, float collisionQueryRange, NavMeshQuery navquery, QueryFilter filter)
public void update(long refs, Vector3f pos, float collisionQueryRange, NavMeshQuery navquery, QueryFilter filter)
{
if (refs == 0)
{
@ -102,7 +103,7 @@ namespace DotRecast.Detour.Crowd
return;
}
vCopy(m_center, pos);
vCopy(ref m_center, pos);
// First query non-overlapping polygons.
Result<FindLocalNeighbourhoodResult> res = navquery.findLocalNeighbourhood(refs, pos, collisionQueryRange,
filter);
@ -153,7 +154,7 @@ namespace DotRecast.Detour.Crowd
return true;
}
public float[] getCenter()
public Vector3f getCenter()
{
return m_center;
}

41
src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
using DotRecast.Detour.Crowd.Tracking;
namespace DotRecast.Detour.Crowd
@ -36,31 +37,31 @@ namespace DotRecast.Detour.Crowd
public class ObstacleCircle
{
/** Position of the obstacle */
public readonly Vector3f p = new Vector3f();
public Vector3f p = new Vector3f();
/** Velocity of the obstacle */
public readonly Vector3f vel = new Vector3f();
public Vector3f vel = new Vector3f();
/** Velocity of the obstacle */
public readonly Vector3f dvel = new Vector3f();
public Vector3f dvel = new Vector3f();
/** Radius of the obstacle */
public float rad;
/** Use for side selection during sampling. */
public readonly Vector3f dp = new Vector3f();
public Vector3f dp = new Vector3f();
/** Use for side selection during sampling. */
public readonly Vector3f np = new Vector3f();
public Vector3f np = new Vector3f();
}
public class ObstacleSegment
{
/** End points of the obstacle segment */
public readonly Vector3f p = new Vector3f();
public Vector3f p = new Vector3f();
/** End points of the obstacle segment */
public readonly Vector3f q = new Vector3f();
public Vector3f q = new Vector3f();
public bool touch;
}
@ -152,25 +153,25 @@ namespace DotRecast.Detour.Crowd
m_nsegments = 0;
}
public void addCircle(float[] pos, float rad, float[] vel, float[] dvel)
public void addCircle(Vector3f pos, float rad, Vector3f vel, Vector3f dvel)
{
if (m_ncircles >= m_maxCircles)
return;
ObstacleCircle cir = m_circles[m_ncircles++];
vCopy(cir.p, pos);
vCopy(ref cir.p, pos);
cir.rad = rad;
vCopy(cir.vel, vel);
vCopy(cir.dvel, dvel);
vCopy(ref cir.vel, vel);
vCopy(ref cir.dvel, dvel);
}
public void addSegment(float[] p, float[] q)
public void addSegment(Vector3f p, Vector3f q)
{
if (m_nsegments >= m_maxSegments)
return;
ObstacleSegment seg = m_segments[m_nsegments++];
vCopy(seg.p, p);
vCopy(seg.q, q);
vCopy(ref seg.p, p);
vCopy(ref seg.q, q);
}
public int getObstacleCircleCount()
@ -193,7 +194,7 @@ namespace DotRecast.Detour.Crowd
return m_segments[i];
}
private void prepare(float[] pos, float[] dvel)
private void prepare(Vector3f pos, float[] dvel)
{
// Prepare obstacles
for (int i = 0; i < m_ncircles; ++i)
@ -201,13 +202,13 @@ namespace DotRecast.Detour.Crowd
ObstacleCircle cir = m_circles[i];
// Side
float[] pa = pos;
float[] pb = cir.p;
Vector3f pa = pos;
Vector3f pb = cir.p;
float[] orig = { 0f, 0f, 0f };
Vector3f orig = new Vector3f();
Vector3f dv = new Vector3f();
vCopy(cir.dp, vSub(pb, pa));
vNormalize(cir.dp);
vCopy(ref cir.dp, vSub(pb, pa));
vNormalize(ref cir.dp);
dv = vSub(cir.dvel, dvel);
float a = triArea2D(orig, cir.dp, dv);

41
src/DotRecast.Detour.Crowd/PathCorridor.cs
View File

@ -20,6 +20,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour.Crowd
{
@ -64,8 +65,8 @@ namespace DotRecast.Detour.Crowd
*/
public class PathCorridor
{
private readonly Vector3f m_pos = new Vector3f();
private readonly Vector3f m_target = new Vector3f();
private Vector3f m_pos = new Vector3f();
private Vector3f m_target = new Vector3f();
private List<long> m_path;
protected List<long> mergeCorridorStartMoved(List<long> path, List<long> visited)
@ -205,12 +206,12 @@ namespace DotRecast.Detour.Crowd
* @param pos
* The new position in the corridor. [(x, y, z)]
*/
public void reset(long refs, float[] pos)
public void reset(long refs, Vector3f pos)
{
m_path.Clear();
m_path.Add(refs);
vCopy(m_pos, pos);
vCopy(m_target, pos);
vCopy(ref m_pos, pos);
vCopy(ref m_target, pos);
}
private static readonly float MIN_TARGET_DIST = sqr(0.01f);
@ -297,8 +298,7 @@ namespace DotRecast.Detour.Crowd
* @param filter
* The filter to apply to the operation.
*/
public void optimizePathVisibility(float[] next, float pathOptimizationRange, NavMeshQuery navquery,
QueryFilter filter)
public void optimizePathVisibility(Vector3f next, float pathOptimizationRange, NavMeshQuery navquery, QueryFilter filter)
{
// Clamp the ray to max distance.
float dist = vDist2D(m_pos, next);
@ -314,8 +314,8 @@ namespace DotRecast.Detour.Crowd
dist = Math.Min(dist + 0.01f, pathOptimizationRange);
// Adjust ray length.
float[] delta = vSub(next, m_pos);
float[] goal = vMad(m_pos, delta, pathOptimizationRange / dist);
var delta = vSub(next, m_pos);
Vector3f goal = vMad(m_pos, delta, pathOptimizationRange / dist);
Result<RaycastHit> rc = navquery.raycast(m_path[0], m_pos, goal, filter, 0, 0);
if (rc.succeeded())
@ -363,8 +363,7 @@ namespace DotRecast.Detour.Crowd
return false;
}
public bool moveOverOffmeshConnection(long offMeshConRef, long[] refs, float[] start, float[] end,
NavMeshQuery navquery)
public bool moveOverOffmeshConnection(long offMeshConRef, long[] refs, ref Vector3f start, ref Vector3f end, NavMeshQuery navquery)
{
// Advance the path up to and over the off-mesh connection.
long prevRef = 0, polyRef = m_path[0];
@ -388,12 +387,12 @@ namespace DotRecast.Detour.Crowd
refs[1] = polyRef;
NavMesh nav = navquery.getAttachedNavMesh();
Result<Tuple<float[], float[]>> startEnd = nav.getOffMeshConnectionPolyEndPoints(refs[0], refs[1]);
var startEnd = nav.getOffMeshConnectionPolyEndPoints(refs[0], refs[1]);
if (startEnd.succeeded())
{
vCopy(m_pos, startEnd.result.Item2);
vCopy(start, startEnd.result.Item1);
vCopy(end, startEnd.result.Item2);
vCopy(ref m_pos, startEnd.result.Item2);
vCopy(ref start, startEnd.result.Item1);
vCopy(ref end, startEnd.result.Item2);
return true;
}
@ -423,7 +422,7 @@ namespace DotRecast.Detour.Crowd
* @param filter
* The filter to apply to the operation.
*/
public bool movePosition(float[] npos, NavMeshQuery navquery, QueryFilter filter)
public bool movePosition(Vector3f npos, NavMeshQuery navquery, QueryFilter filter)
{
// Move along navmesh and update new position.
Result<MoveAlongSurfaceResult> masResult = navquery.moveAlongSurface(m_path[0], m_pos, npos, filter);
@ -431,7 +430,7 @@ namespace DotRecast.Detour.Crowd
{
m_path = mergeCorridorStartMoved(m_path, masResult.result.getVisited());
// Adjust the position to stay on top of the navmesh.
vCopy(m_pos, masResult.result.getResultPos());
vCopy(ref m_pos, masResult.result.getResultPos());
Result<float> hr = navquery.getPolyHeight(m_path[0], masResult.result.getResultPos());
if (hr.succeeded())
{
@ -492,15 +491,15 @@ namespace DotRecast.Detour.Crowd
* @param path
* The path corridor.
*/
public void setCorridor(float[] target, List<long> path)
public void setCorridor(Vector3f target, List<long> path)
{
vCopy(m_target, target);
vCopy(ref m_target, target);
m_path = new List<long>(path);
}
public void fixPathStart(long safeRef, float[] safePos)
public void fixPathStart(long safeRef, Vector3f safePos)
{
vCopy(m_pos, safePos);
vCopy(ref m_pos, safePos);
if (m_path.Count < 3 && m_path.Count > 0)
{
long p = m_path[m_path.Count - 1];

2
src/DotRecast.Detour.Crowd/PathQuery.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour.Crowd
{
public class PathQuery

4
src/DotRecast.Detour.Crowd/PathQueue.cs
View File

@ -84,9 +84,9 @@ namespace DotRecast.Detour.Crowd
}
PathQuery q = new PathQuery();
vCopy(q.startPos, startPos);
vCopy(ref q.startPos, startPos);
q.startRef = startRef;
vCopy(q.endPos, endPos);
vCopy(ref q.endPos, endPos);
q.endRef = endRef;
q.result.status = null;
q.filter = filter;

2
src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour.Crowd.Tracking
{
public class CrowdAgentDebugInfo

3
src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Detour.Crowd.Tracking
{
@ -100,7 +101,7 @@ namespace DotRecast.Detour.Crowd.Tracking
return m_nsamples;
}
public float[] getSampleVelocity(int i)
public Vector3f getSampleVelocity(int i)
{
Vector3f vel = new Vector3f();
vel[0] = m_vel[i * 3];

2
src/DotRecast.Detour.Dynamic/DynamicTileCheckpoint.cs
View File

@ -35,7 +35,7 @@ namespace DotRecast.Detour.Dynamic
private Heightfield clone(Heightfield source)
{
Heightfield clone = new Heightfield(source.width, source.height, vCopy(source.bmin), vCopy(source.bmax), source.cs,
Heightfield clone = new Heightfield(source.width, source.height, source.bmin, source.bmax, source.cs,
source.ch, source.borderSize);
for (int z = 0, pz = 0; z < source.height; z++, pz += source.width)
{

6
src/DotRecast.Detour.Dynamic/Io/VoxelTile.cs
View File

@ -30,13 +30,13 @@ namespace DotRecast.Detour.Dynamic.Io
public readonly int borderSize;
public int width;
public int depth;
public readonly float[] boundsMin;
public float[] boundsMax;
public readonly Vector3f boundsMin;
public Vector3f boundsMax;
public float cellSize;
public float cellHeight;
public readonly byte[] spanData;
public VoxelTile(int tileX, int tileZ, int width, int depth, float[] boundsMin, float[] boundsMax, float cellSize,
public VoxelTile(int tileX, int tileZ, int width, int depth, Vector3f boundsMin, Vector3f boundsMax, float cellSize,
float cellHeight, int borderSize, ByteBuffer buffer)
{
this.tileX = tileX;

5
src/DotRecast.Detour.Dynamic/VoxelQuery.cs
View File

@ -17,6 +17,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
using DotRecast.Recast;
namespace DotRecast.Detour.Dynamic
@ -28,12 +29,12 @@ namespace DotRecast.Detour.Dynamic
*/
public class VoxelQuery
{
private readonly float[] origin;
private readonly Vector3f origin;
private readonly float tileWidth;
private readonly float tileDepth;
private readonly Func<int, int, Heightfield> heightfieldProvider;
public VoxelQuery(float[] origin, float tileWidth, float tileDepth, Func<int, int, Heightfield> heightfieldProvider)
public VoxelQuery(Vector3f origin, float tileWidth, float tileDepth, Func<int, int, Heightfield> heightfieldProvider)
{
this.origin = origin;
this.tileWidth = tileWidth;

9
src/DotRecast.Detour.Extras/BVTreeBuilder.cs
View File

@ -16,6 +16,7 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
using static DotRecast.Core.RecastMath;
namespace DotRecast.Detour.Extras
@ -40,12 +41,12 @@ namespace DotRecast.Detour.Extras
it.i = i;
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
vCopy(bmin, data.verts, data.polys[i].verts[0] * 3);
vCopy(bmax, data.verts, data.polys[i].verts[0] * 3);
vCopy(ref bmin, data.verts, data.polys[i].verts[0] * 3);
vCopy(ref bmax, data.verts, data.polys[i].verts[0] * 3);
for (int j = 1; j < data.polys[i].vertCount; j++)
{
vMin(bmin, data.verts, data.polys[i].verts[j] * 3);
vMax(bmax, data.verts, data.polys[i].verts[j] * 3);
vMin(ref bmin, data.verts, data.polys[i].verts[j] * 3);
vMax(ref bmax, data.verts, data.polys[i].verts[j] * 3);
}
it.bmin[0] = clamp((int)((bmin[0] - data.header.bmin[0]) * quantFactor), 0, 0x7fffffff);

7
src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
using DotRecast.Recast;
using static DotRecast.Core.RecastMath;
@ -7,7 +8,7 @@ namespace DotRecast.Detour.Extras.Jumplink
public abstract class AbstractGroundSampler : GroundSampler
{
protected void sampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es,
Func<float[], float, Tuple<bool, float>> heightFunc)
Func<Vector3f, float, Tuple<bool, float>> heightFunc)
{
float cs = acfg.cellSize;
float dist = (float)Math.Sqrt(vDist2DSqr(es.start.p, es.start.q));
@ -21,7 +22,7 @@ namespace DotRecast.Detour.Extras.Jumplink
public abstract void sample(JumpLinkBuilderConfig acfg, RecastBuilderResult result, EdgeSampler es);
protected void sampleGroundSegment(Func<float[], float, Tuple<bool, float>> heightFunc, GroundSegment seg, int nsamples)
protected void sampleGroundSegment(Func<Vector3f, float, Tuple<bool, float>> heightFunc, GroundSegment seg, int nsamples)
{
seg.gsamples = new GroundSample[nsamples];
@ -31,7 +32,7 @@ namespace DotRecast.Detour.Extras.Jumplink
GroundSample s = new GroundSample();
seg.gsamples[i] = s;
float[] pt = vLerp(seg.p, seg.q, u);
Vector3f pt = vLerp(seg.p, seg.q, u);
Tuple<bool, float> height = heightFunc.Invoke(pt, seg.height);
s.p[0] = pt[0];
s.p[1] = height.Item2;

11
src/DotRecast.Detour.Extras/Jumplink/ClimbTrajectory.cs
View File

@ -1,16 +1,17 @@
using System;
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
public class ClimbTrajectory : Trajectory
{
public override float[] apply(float[] start, float[] end, float u)
public override Vector3f apply(Vector3f start, Vector3f end, float u)
{
return new float[]
return new Vector3f()
{
lerp(start[0], end[0], Math.Min(2f * u, 1f)),
lerp(start[1], end[1], Math.Max(0f, 2f * u - 1f)),
lerp(start[2], end[2], Math.Min(2f * u, 1f))
x = lerp(start[0], end[0], Math.Min(2f * u, 1f)),
y = lerp(start[1], end[1], Math.Max(0f, 2f * u - 1f)),
z = lerp(start[2], end[2], Math.Min(2f * u, 1f))
};
}
}

6
src/DotRecast.Detour.Extras/Jumplink/Edge.cs
View File

@ -1,8 +1,10 @@
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
public class Edge
{
public readonly Vector3f sp = new Vector3f();
public readonly Vector3f sq = new Vector3f();
public Vector3f sp = new Vector3f();
public Vector3f sq = new Vector3f();
}
}

3
src/DotRecast.Detour.Extras/Jumplink/EdgeExtractor.cs
View File

@ -1,4 +1,5 @@
using System.Collections.Generic;
using DotRecast.Core;
using DotRecast.Recast;
using static DotRecast.Recast.RecastConstants;
@ -12,7 +13,7 @@ namespace DotRecast.Detour.Extras.Jumplink
List<Edge> edges = new List<Edge>();
if (mesh != null)
{
float[] orig = mesh.bmin;
Vector3f orig = mesh.bmin;
float cs = mesh.cs;
float ch = mesh.ch;
for (int i = 0; i < mesh.npolys; i++)

11
src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs
View File

@ -1,4 +1,5 @@
using System.Collections.Generic;
using DotRecast.Core;
using static DotRecast.Core.RecastMath;
namespace DotRecast.Detour.Extras.Jumplink
@ -16,11 +17,11 @@ namespace DotRecast.Detour.Extras.Jumplink
public EdgeSampler(Edge edge, Trajectory trajectory)
{
this.trajectory = trajectory;
vCopy(ax, vSub(edge.sq, edge.sp));
vNormalize(ax);
vSet(az, ax[2], 0, -ax[0]);
vNormalize(az);
vSet(ay, 0, 1, 0);
vCopy(ref ax, vSub(edge.sq, edge.sp));
vNormalize(ref ax);
vSet(ref az, ax[2], 0, -ax[0]);
vNormalize(ref az);
vSet(ref ay, 0, 1, 0);
}
}
}

41
src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
@ -29,9 +30,9 @@ namespace DotRecast.Detour.Extras.Jumplink
EdgeSampler es = new EdgeSampler(edge, new JumpTrajectory(acfg.jumpHeight));
es.start.height = acfg.agentClimb * 2;
Vector3f offset = new Vector3f();
trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb });
vadd(es.start.p, edge.sp, offset);
vadd(es.start.q, edge.sq, offset);
trans2d(ref offset, es.az, es.ay, new Vector2f { x = acfg.startDistance, y = -acfg.agentClimb, });
vadd(ref es.start.p, edge.sp, offset);
vadd(ref es.start.q, edge.sq, offset);
float dx = acfg.endDistance - 2 * acfg.agentRadius;
float cs = acfg.cellSize;
@ -41,11 +42,11 @@ namespace DotRecast.Detour.Extras.Jumplink
{
float v = (float)j / (float)(nsamples - 1);
float ox = 2 * acfg.agentRadius + dx * v;
trans2d(offset, es.az, es.ay, new float[] { ox, acfg.minHeight });
trans2d(ref offset, es.az, es.ay, new Vector2f { x = ox, y = acfg.minHeight });
GroundSegment end = new GroundSegment();
end.height = acfg.heightRange;
vadd(end.p, edge.sp, offset);
vadd(end.q, edge.sq, offset);
vadd(ref end.p, edge.sp, offset);
vadd(ref end.q, edge.sq, offset);
es.end.Add(end);
}
@ -57,15 +58,15 @@ namespace DotRecast.Detour.Extras.Jumplink
EdgeSampler es = new EdgeSampler(edge, new ClimbTrajectory());
es.start.height = acfg.agentClimb * 2;
Vector3f offset = new Vector3f();
trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb });
vadd(es.start.p, edge.sp, offset);
vadd(es.start.q, edge.sq, offset);
trans2d(ref offset, es.az, es.ay, new Vector2f() { x = acfg.startDistance, y = -acfg.agentClimb });
vadd(ref es.start.p, edge.sp, offset);
vadd(ref es.start.q, edge.sq, offset);
trans2d(offset, es.az, es.ay, new float[] { acfg.endDistance, acfg.minHeight });
trans2d(ref offset, es.az, es.ay, new Vector2f() { x = acfg.endDistance, y = acfg.minHeight });
GroundSegment end = new GroundSegment();
end.height = acfg.heightRange;
vadd(end.p, edge.sp, offset);
vadd(end.q, edge.sq, offset);
vadd(ref end.p, edge.sp, offset);
vadd(ref end.q, edge.sq, offset);
es.end.Add(end);
return es;
}
@ -77,11 +78,27 @@ namespace DotRecast.Detour.Extras.Jumplink
dest[2] = v1[2] + v2[2];
}
private void vadd(ref Vector3f dest, Vector3f v1, Vector3f v2)
{
dest[0] = v1[0] + v2[0];
dest[1] = v1[1] + v2[1];
dest[2] = v1[2] + v2[2];
}
private void trans2d(float[] dst, float[] ax, float[] ay, float[] pt)
{
dst[0] = ax[0] * pt[0] + ay[0] * pt[1];
dst[1] = ax[1] * pt[0] + ay[1] * pt[1];
dst[2] = ax[2] * pt[0] + ay[2] * pt[1];
}
private void trans2d(ref Vector3f dst, Vector3f ax, Vector3f ay, Vector2f pt)
{
dst[0] = ax[0] * pt[0] + ay[0] * pt[1];
dst[1] = ax[1] * pt[0] + ay[1] * pt[1];
dst[2] = ax[2] * pt[0] + ay[2] * pt[1];
}
}
}

4
src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs
View File

@ -1,8 +1,10 @@
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
public class GroundSample
{
public readonly Vector3f p = new Vector3f();
public Vector3f p = new Vector3f();
public bool validTrajectory;
public bool validHeight;
}

6
src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs
View File

@ -1,9 +1,11 @@
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
public class GroundSegment
{
public readonly Vector3f p = new Vector3f();
public readonly Vector3f q = new Vector3f();
public Vector3f p = new Vector3f();
public Vector3f q = new Vector3f();
public GroundSample[] gsamples;
public float height;
}

10
src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs
View File

@ -54,11 +54,11 @@ namespace DotRecast.Detour.Extras.Jumplink
List<JumpLink> links = new List<JumpLink>();
foreach (JumpSegment js in jumpSegments)
{
float[] sp = es.start.gsamples[js.startSample].p;
float[] sq = es.start.gsamples[js.startSample + js.samples - 1].p;
Vector3f sp = es.start.gsamples[js.startSample].p;
Vector3f sq = es.start.gsamples[js.startSample + js.samples - 1].p;
GroundSegment end = es.end[js.groundSegment];
float[] ep = end.gsamples[js.startSample].p;
float[] eq = end.gsamples[js.startSample + js.samples - 1].p;
Vector3f ep = end.gsamples[js.startSample].p;
Vector3f eq = end.gsamples[js.startSample + js.samples - 1].p;
float d = Math.Min(vDist2DSqr(sp, sq), vDist2DSqr(ep, eq));
if (d >= 4 * acfg.agentRadius * acfg.agentRadius)
{
@ -72,7 +72,7 @@ namespace DotRecast.Detour.Extras.Jumplink
for (int j = 0; j < link.nspine; ++j)
{
float u = ((float)j) / (link.nspine - 1);
float[] p = es.trajectory.apply(sp, ep, u);
Vector3f p = es.trajectory.apply(sp, ep, u);
link.spine0[j * 3] = p[0];
link.spine0[j * 3 + 1] = p[1];
link.spine0[j * 3 + 2] = p[2];

10
src/DotRecast.Detour.Extras/Jumplink/JumpTrajectory.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
@ -11,12 +12,13 @@ namespace DotRecast.Detour.Extras.Jumplink
this.jumpHeight = jumpHeight;
}
public override float[] apply(float[] start, float[] end, float u)
public override Vector3f apply(Vector3f start, Vector3f end, float u)
{
return new float[]
return new Vector3f
{
lerp(start[0], end[0], u), interpolateHeight(start[1], end[1], u),
lerp(start[2], end[2], u)
x = lerp(start[0], end[0], u),
y = interpolateHeight(start[1], end[1], u),
z = lerp(start[2], end[2], u)
};
}

7
src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs
View File

@ -15,7 +15,7 @@ namespace DotRecast.Detour.Extras.Jumplink
return true;
}
public float getCost(float[] pa, float[] pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef,
public float getCost(Vector3f pa, Vector3f pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef,
MeshTile curTile, Poly curPoly, long nextRef, MeshTile nextTile, Poly nextPoly)
{
return 0;
@ -69,10 +69,9 @@ namespace DotRecast.Detour.Extras.Jumplink
}
}
private Tuple<bool, float> getNavMeshHeight(NavMeshQuery navMeshQuery, float[] pt, float cs,
float heightRange)
private Tuple<bool, float> getNavMeshHeight(NavMeshQuery navMeshQuery, Vector3f pt, float cs, float heightRange)
{
float[] halfExtents = new float[] { cs, heightRange, cs };
Vector3f halfExtents = new Vector3f { x = cs, y = heightRange, z = cs };
float maxHeight = pt[1] + heightRange;
AtomicBoolean found = new AtomicBoolean();
AtomicFloat minHeight = new AtomicFloat(pt[1]);

3
src/DotRecast.Detour.Extras/Jumplink/Trajectory.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
namespace DotRecast.Detour.Extras.Jumplink
{
@ -9,7 +10,7 @@ namespace DotRecast.Detour.Extras.Jumplink
return u * g + (1f - u) * f;
}
public virtual float[] apply(float[] start, float[] end, float u)
public virtual Vector3f apply(Vector3f start, Vector3f end, float u)
{
throw new NotImplementedException();
}

7
src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
using DotRecast.Recast;
using static DotRecast.Core.RecastMath;
@ -31,7 +32,7 @@ namespace DotRecast.Detour.Extras.Jumplink
}
}
private bool sampleTrajectory(JumpLinkBuilderConfig acfg, Heightfield solid, float[] pa, float[] pb, Trajectory tra)
private bool sampleTrajectory(JumpLinkBuilderConfig acfg, Heightfield solid, Vector3f pa, Vector3f pb, Trajectory tra)
{
float cs = Math.Min(acfg.cellSize, acfg.cellHeight);
float d = vDist2D(pa, pb) + Math.Abs(pa[1] - pb[1]);
@ -39,7 +40,7 @@ namespace DotRecast.Detour.Extras.Jumplink
for (int i = 0; i < nsamples; ++i)
{
float u = (float)i / (float)(nsamples - 1);
float[] p = tra.apply(pa, pb, u);
Vector3f p = tra.apply(pa, pb, u);
if (checkHeightfieldCollision(solid, p[0], p[1] + acfg.groundTolerance, p[1] + acfg.agentHeight, p[2]))
{
return false;
@ -55,7 +56,7 @@ namespace DotRecast.Detour.Extras.Jumplink
int h = solid.height;
float cs = solid.cs;
float ch = solid.ch;
float[] orig = solid.bmin;
Vector3f orig = solid.bmin;
int ix = (int)Math.Floor((x - orig[0]) / cs);
int iz = (int)Math.Floor((z - orig[2]) / cs);

28
src/DotRecast.Detour.TileCache/TileCache.cs
View File

@ -363,7 +363,7 @@ namespace DotRecast.Detour.TileCache
TileCacheObstacle ob = allocObstacle();
ob.type = TileCacheObstacle.TileCacheObstacleType.CYLINDER;
vCopy(ob.pos, pos);
vCopy(ref ob.pos, pos);
ob.radius = radius;
ob.height = height;
@ -376,19 +376,19 @@ namespace DotRecast.Detour.TileCache
TileCacheObstacle ob = allocObstacle();
ob.type = TileCacheObstacle.TileCacheObstacleType.BOX;
vCopy(ob.bmin, bmin);
vCopy(ob.bmax, bmax);
vCopy(ref ob.bmin, bmin);
vCopy(ref ob.bmax, bmax);
return addObstacleRequest(ob).refs;
}
// Box obstacle: can be rotated in Y
public long addBoxObstacle(float[] center, float[] extents, float yRadians)
public long addBoxObstacle(Vector3f center, Vector3f extents, float yRadians)
{
TileCacheObstacle ob = allocObstacle();
ob.type = TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX;
vCopy(ob.center, center);
vCopy(ob.extents, extents);
vCopy(ref ob.center, center);
vCopy(ref ob.extents, extents);
float coshalf = (float)Math.Cos(0.5f * yRadians);
float sinhalf = (float)Math.Sin(-0.5f * yRadians);
ob.rotAux[0] = coshalf * sinhalf;
@ -438,7 +438,7 @@ namespace DotRecast.Detour.TileCache
return o;
}
List<long> queryTiles(float[] bmin, float[] bmax)
List<long> queryTiles(Vector3f bmin, Vector3f bmax)
{
List<long> results = new List<long>();
float tw = m_params.width * m_params.cs;
@ -616,8 +616,7 @@ namespace DotRecast.Detour.TileCache
{
if (ob.type == TileCacheObstacle.TileCacheObstacleType.CYLINDER)
{
builder.markCylinderArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.pos, ob.radius,
ob.height, 0);
builder.markCylinderArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.pos, ob.radius, ob.height, 0);
}
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.BOX)
{
@ -625,8 +624,7 @@ namespace DotRecast.Detour.TileCache
}
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX)
{
builder.markBoxArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.center, ob.extents,
ob.rotAux, 0);
builder.markBoxArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.center, ob.extents, ob.rotAux, 0);
}
}
}
@ -684,7 +682,7 @@ namespace DotRecast.Detour.TileCache
return layer;
}
void calcTightTileBounds(TileCacheLayerHeader header, float[] bmin, float[] bmax)
void calcTightTileBounds(TileCacheLayerHeader header, Vector3f bmin, Vector3f bmax)
{
float cs = m_params.cs;
bmin[0] = header.bmin[0] + header.minx * cs;
@ -695,7 +693,7 @@ namespace DotRecast.Detour.TileCache
bmax[2] = header.bmin[2] + (header.maxy + 1) * cs;
}
void getObstacleBounds(TileCacheObstacle ob, float[] bmin, float[] bmax)
void getObstacleBounds(TileCacheObstacle ob, Vector3f bmin, Vector3f bmax)
{
if (ob.type == TileCacheObstacle.TileCacheObstacleType.CYLINDER)
{
@ -708,8 +706,8 @@ namespace DotRecast.Detour.TileCache
}
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.BOX)
{
vCopy(bmin, ob.bmin);
vCopy(bmax, ob.bmax);
vCopy(ref bmin, ob.bmin);
vCopy(ref bmax, ob.bmax);
}
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX)
{

8
src/DotRecast.Detour.TileCache/TileCacheBuilder.cs
View File

@ -1856,8 +1856,7 @@ namespace DotRecast.Detour.TileCache
return mesh;
}
public void markCylinderArea(TileCacheLayer layer, float[] orig, float cs, float ch, float[] pos, float radius,
float height, int areaId)
public void markCylinderArea(TileCacheLayer layer, Vector3f orig, float cs, float ch, Vector3f pos, float radius, float height, int areaId)
{
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
@ -1918,8 +1917,7 @@ namespace DotRecast.Detour.TileCache
}
}
public void markBoxArea(TileCacheLayer layer, float[] orig, float cs, float ch, float[] bmin, float[] bmax,
int areaId)
public void markBoxArea(TileCacheLayer layer, Vector3f orig, float cs, float ch, Vector3f bmin, Vector3f bmax, int areaId)
{
int w = layer.header.width;
int h = layer.header.height;
@ -2047,7 +2045,7 @@ namespace DotRecast.Detour.TileCache
return layer;
}
public void markBoxArea(TileCacheLayer layer, float[] orig, float cs, float ch, float[] center, float[] extents,
public void markBoxArea(TileCacheLayer layer, Vector3f orig, float cs, float ch, Vector3f center, Vector3f extents,
float[] rotAux, int areaId)
{
int w = layer.header.width;

2
src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour.TileCache
{
public class TileCacheLayerHeader

11
src/DotRecast.Detour.TileCache/TileCacheObstacle.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour.TileCache
{
@ -33,12 +34,12 @@ namespace DotRecast.Detour.TileCache
public readonly int index;
public TileCacheObstacleType type;
public readonly Vector3f pos = new Vector3f();
public readonly Vector3f bmin = new Vector3f();
public readonly Vector3f bmax = new Vector3f();
public Vector3f pos = new Vector3f();
public Vector3f bmin = new Vector3f();
public Vector3f bmax = new Vector3f();
public float radius, height;
public readonly Vector3f center = new Vector3f();
public readonly Vector3f extents = new Vector3f();
public Vector3f center = new Vector3f();
public Vector3f extents = new Vector3f();
public readonly float[] rotAux = new float[2]; // { cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 }
public List<long> touched = new List<long>();
public readonly List<long> pending = new List<long>();

4
src/DotRecast.Detour.TileCache/TileCacheParams.cs
View File

@ -18,11 +18,13 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour.TileCache
{
public class TileCacheParams
{
public readonly Vector3f orig = new Vector3f();
public Vector3f orig = new Vector3f();
public float cs, ch;
public int width, height;
public float walkableHeight;

8
src/DotRecast.Detour/ClosestPointOnPolyResult.cs
View File

@ -18,14 +18,16 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
public class ClosestPointOnPolyResult
{
private readonly bool posOverPoly;
private readonly float[] closest;
private readonly Vector3f closest;
public ClosestPointOnPolyResult(bool posOverPoly, float[] closest)
public ClosestPointOnPolyResult(bool posOverPoly, Vector3f closest)
{
this.posOverPoly = posOverPoly;
this.closest = closest;
@ -38,7 +40,7 @@ namespace DotRecast.Detour
}
/** Returns the closest point on the polygon. [(x, y, z)] */
public float[] getClosest()
public Vector3f getClosest()
{
return closest;
}

4
src/DotRecast.Detour/ConvexConvexIntersection.cs
View File

@ -73,8 +73,8 @@ namespace DotRecast.Detour
vCopy(ref a1, p, 3 * ((ai + n - 1) % n)); // prev a
vCopy(ref b1, q, 3 * ((bi + m - 1) % m)); // prev b
float[] A = vSub(a, a1);
float[] B = vSub(b, b1);
Vector3f A = vSub(a, a1);
Vector3f B = vSub(b, b1);
float cross = B[0] * A[2] - A[0] * B[2]; // triArea2D({0, 0}, A, B);
float aHB = triArea2D(b1, b, a);

3
src/DotRecast.Detour/DefaultQueryFilter.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -85,7 +86,7 @@ namespace DotRecast.Detour
return (poly.flags & m_includeFlags) != 0 && (poly.flags & m_excludeFlags) == 0;
}
public float getCost(float[] pa, float[] pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef,
public float getCost(Vector3f pa, Vector3f pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef,
MeshTile curTile, Poly curPoly, long nextRef, MeshTile nextTile, Poly nextPoly)
{
return vDist(pa, pb) * m_areaCost[curPoly.getArea()];

4
src/DotRecast.Detour/DefaultQueryHeuristic.cs
View File

@ -15,6 +15,8 @@ freely, subject to the following restrictions:
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
using static DotRecast.Core.RecastMath;
namespace DotRecast.Detour
@ -33,7 +35,7 @@ namespace DotRecast.Detour
this.scale = scale;
}
public float getCost(float[] neighbourPos, float[] endPos)
public float getCost(Vector3f neighbourPos, Vector3f endPos)
{
return vDist(neighbourPos, endPos) * scale;
}

12
src/DotRecast.Detour/FindDistanceToWallResult.cs
View File

@ -18,16 +18,18 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
//TODO: (PP) Add comments
public class FindDistanceToWallResult
{
private readonly float distance;
private readonly float[] position;
private readonly float[] normal;
private readonly Vector3f position;
private readonly Vector3f normal;
public FindDistanceToWallResult(float distance, float[] position, float[] normal)
public FindDistanceToWallResult(float distance, Vector3f position, Vector3f normal)
{
this.distance = distance;
this.position = position;
@ -39,12 +41,12 @@ namespace DotRecast.Detour
return distance;
}
public float[] getPosition()
public Vector3f getPosition()
{
return position;
}
public float[] getNormal()
public Vector3f getNormal()
{
return normal;
}

13
src/DotRecast.Detour/FindNearestPolyQuery.cs
View File

@ -1,4 +1,5 @@
using System;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -7,18 +8,18 @@ namespace DotRecast.Detour
public class FindNearestPolyQuery : PolyQuery
{
private readonly NavMeshQuery query;
private readonly float[] center;
private readonly Vector3f center;
private long nearestRef;
private float[] nearestPt;
private Vector3f nearestPt;
private bool overPoly;
private float nearestDistanceSqr;
public FindNearestPolyQuery(NavMeshQuery query, float[] center)
public FindNearestPolyQuery(NavMeshQuery query, Vector3f center)
{
this.query = query;
this.center = center;
nearestDistanceSqr = float.MaxValue;
nearestPt = new float[] { center[0], center[1], center[2] };
nearestPt = center;
}
public void process(MeshTile tile, Poly poly, long refs)
@ -26,12 +27,12 @@ namespace DotRecast.Detour
// Find nearest polygon amongst the nearby polygons.
Result<ClosestPointOnPolyResult> closest = query.closestPointOnPoly(refs, center);
bool posOverPoly = closest.result.isPosOverPoly();
float[] closestPtPoly = closest.result.getClosest();
var closestPtPoly = closest.result.getClosest();
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
float d = 0;
float[] diff = vSub(center, closestPtPoly);
Vector3f diff = vSub(center, closestPtPoly);
if (posOverPoly)
{
d = Math.Abs(diff[1]) - tile.data.header.walkableClimb;

8
src/DotRecast.Detour/FindNearestPolyResult.cs
View File

@ -18,15 +18,17 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
public class FindNearestPolyResult
{
private readonly long nearestRef;
private readonly float[] nearestPos;
private readonly Vector3f nearestPos;
private readonly bool overPoly;
public FindNearestPolyResult(long nearestRef, float[] nearestPos, bool overPoly)
public FindNearestPolyResult(long nearestRef, Vector3f nearestPos, bool overPoly)
{
this.nearestRef = nearestRef;
this.nearestPos = nearestPos;
@ -40,7 +42,7 @@ namespace DotRecast.Detour
}
/** Returns the nearest point on the polygon. [opt] [(x, y, z)]. Unchanged if no polygon is found. */
public float[] getNearestPos()
public Vector3f getNearestPos()
{
return nearestPos;
}

8
src/DotRecast.Detour/FindRandomPointResult.cs
View File

@ -18,15 +18,17 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
//TODO: (PP) Add comments
public class FindRandomPointResult
{
private readonly long randomRef;
private readonly float[] randomPt;
private readonly Vector3f randomPt;
public FindRandomPointResult(long randomRef, float[] randomPt)
public FindRandomPointResult(long randomRef, Vector3f randomPt)
{
this.randomRef = randomRef;
this.randomPt = randomPt;
@ -39,7 +41,7 @@ namespace DotRecast.Detour
}
/// @param[out] randomPt The random location.
public float[] getRandomPt()
public Vector3f getRandomPt()
{
return randomPt;
}

35
src/DotRecast.Detour/LegacyNavMeshQuery.cs
View File

@ -33,18 +33,16 @@ namespace DotRecast.Detour
{
}
public override Result<List<long>> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public override Result<List<long>> findPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
int options, float raycastLimit)
{
return findPath(startRef, endRef, startPos, endPos, filter);
}
public override Result<List<long>> findPath(long startRef, long endRef, float[] startPos, float[] endPos,
QueryFilter filter)
public override Result<List<long>> findPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || null == startPos
|| !vIsFinite(startPos) || null == endPos || !vIsFinite(endPos) || null == filter)
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || !vIsFinite(startPos) || !vIsFinite(endPos) || null == filter)
{
return Results.invalidParam<List<long>>();
}
@ -144,11 +142,11 @@ namespace DotRecast.Detour
// If the node is visited the first time, calculate node position.
if (neighbourNode.flags == 0)
{
Result<float[]> midpod = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
var midpod = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
neighbourTile);
if (!midpod.failed())
{
neighbourNode.pos = Vector3f.Of(midpod.result);
neighbourNode.pos = midpod.result;
}
}
@ -160,9 +158,9 @@ namespace DotRecast.Detour
if (neighbourRef == endRef)
{
// Cost
float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly,
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
float endCost = filter.getCost(neighbourNode.pos.ToArray(), endPos, bestRef, bestTile, bestPoly, neighbourRef,
float endCost = filter.getCost(neighbourNode.pos, endPos, bestRef, bestTile, bestPoly, neighbourRef,
neighbourTile, neighbourPoly, 0L, null, null);
cost = bestNode.cost + curCost + endCost;
@ -171,7 +169,7 @@ namespace DotRecast.Detour
else
{
// Cost
float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly,
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
cost = bestNode.cost + curCost;
heuristic = vDist(neighbourNode.pos, endPos) * H_SCALE;
@ -361,11 +359,11 @@ namespace DotRecast.Detour
// position.
if (neighbourNode.flags == 0)
{
Result<float[]> midpod = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
var midpod = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
neighbourTile);
if (!midpod.failed())
{
neighbourNode.pos = Vector3f.Of(midpod.result);
neighbourNode.pos = midpod.result;
}
}
@ -646,11 +644,10 @@ namespace DotRecast.Detour
return Results.of(status, path);
}
public override Result<FindDistanceToWallResult> findDistanceToWall(long startRef, float[] centerPos, float maxRadius,
QueryFilter filter)
public override Result<FindDistanceToWallResult> findDistanceToWall(long startRef, Vector3f centerPos, float maxRadius, QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == centerPos || !vIsFinite(centerPos) || maxRadius < 0
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(centerPos) || maxRadius < 0
|| !float.IsFinite(maxRadius) || null == filter)
{
return Results.invalidParam<FindDistanceToWallResult>();
@ -660,7 +657,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, centerPos);
vCopy(ref startNode.pos, centerPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
@ -808,7 +805,7 @@ namespace DotRecast.Detour
// Cost
if (neighbourNode.flags == 0)
{
Result<float[]> midPoint = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
var midPoint = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
neighbourTile);
if (midPoint.succeeded())
{
@ -845,11 +842,11 @@ namespace DotRecast.Detour
Vector3f hitNormal = new Vector3f();
if (bestvi != null && bestvj != null)
{
float[] tangent = vSub(bestvi, bestvj);
var tangent = vSub(bestvi, bestvj);
hitNormal[0] = tangent[2];
hitNormal[1] = 0;
hitNormal[2] = -tangent[0];
vNormalize(hitNormal);
vNormalize(ref hitNormal);
}
return Results.success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal));

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

@ -19,24 +19,25 @@ freely, subject to the following restrictions:
*/
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour
{
public class MoveAlongSurfaceResult
{
/** The result position of the mover. [(x, y, z)] */
private readonly float[] resultPos;
private readonly Vector3f resultPos;
/** The reference ids of the polygons visited during the move. */
private readonly List<long> visited;
public MoveAlongSurfaceResult(float[] resultPos, List<long> visited)
public MoveAlongSurfaceResult(Vector3f resultPos, List<long> visited)
{
this.resultPos = resultPos;
this.visited = visited;
}
public float[] getResultPos()
public Vector3f getResultPos()
{
return resultPos;
}

107
src/DotRecast.Detour/NavMesh.cs
View File

@ -21,6 +21,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using System.Collections.Immutable;
using System.Numerics;
using DotRecast.Core;
namespace DotRecast.Detour
@ -212,7 +213,7 @@ namespace DotRecast.Detour
* The world position for the query. [(x, y, z)]
* @return 2-element int array with (tx,ty) tile location
*/
public int[] calcTileLoc(float[] pos)
public int[] calcTileLoc(Vector3f pos)
{
int tx = (int)Math.Floor((pos[0] - m_orig[0]) / m_tileWidth);
int ty = (int)Math.Floor((pos[2] - m_orig[2]) / m_tileHeight);
@ -335,7 +336,7 @@ namespace DotRecast.Detour
// TODO: These methods are duplicates from dtNavMeshQuery, but are needed
// for off-mesh connection finding.
List<long> queryPolygonsInTile(MeshTile tile, float[] qmin, float[] qmax)
List<long> queryPolygonsInTile(MeshTile tile, Vector3f qmin, Vector3f qmax)
{
List<long> polys = new List<long>();
if (tile.data.bvTree != null)
@ -833,7 +834,12 @@ namespace DotRecast.Detour
continue;
}
float[] ext = new float[] { targetCon.rad, target.data.header.walkableClimb, targetCon.rad };
var ext = new Vector3f()
{
x = targetCon.rad,
y = target.data.header.walkableClimb,
z = targetCon.rad
};
// Find polygon to connect to.
Vector3f p = new Vector3f();
@ -847,7 +853,7 @@ namespace DotRecast.Detour
continue;
}
float[] nearestPt = nearest.getNearestPos();
var nearestPt = nearest.getNearestPos();
// findNearestPoly may return too optimistic results, further check
// to make sure.
@ -1059,10 +1065,15 @@ namespace DotRecast.Detour
OffMeshConnection con = tile.data.offMeshCons[i];
Poly poly = tile.data.polys[con.poly];
float[] ext = new float[] { con.rad, tile.data.header.walkableClimb, con.rad };
var ext = new Vector3f()
{
x = con.rad,
y = tile.data.header.walkableClimb,
z = con.rad,
};
// Find polygon to connect to.
FindNearestPolyResult nearestPoly = findNearestPolyInTile(tile, con.pos, ext);
FindNearestPolyResult nearestPoly = findNearestPolyInTile(tile, Vector3f.Of(con.pos), ext);
long refs = nearestPoly.getNearestRef();
if (refs == 0)
{
@ -1070,7 +1081,7 @@ namespace DotRecast.Detour
}
float[] p = con.pos; // First vertex
float[] nearestPt = nearestPoly.getNearestPos();
Vector3f nearestPt = nearestPoly.getNearestPos();
// findNearestPoly may return too optimistic results, further check
// to make sure.
if (sqr(nearestPt[0] - p[0]) + sqr(nearestPt[2] - p[2]) > sqr(con.rad))
@ -1116,15 +1127,15 @@ namespace DotRecast.Detour
* @param pos
* @return
*/
float[] closestPointOnDetailEdges(MeshTile tile, Poly poly, float[] pos, bool onlyBoundary)
Vector3f closestPointOnDetailEdges(MeshTile tile, Poly poly, Vector3f pos, bool onlyBoundary)
{
int ANY_BOUNDARY_EDGE = (DT_DETAIL_EDGE_BOUNDARY << 0) | (DT_DETAIL_EDGE_BOUNDARY << 2)
| (DT_DETAIL_EDGE_BOUNDARY << 4);
int ip = poly.index;
float dmin = float.MaxValue;
float tmin = 0;
float[] pmin = null;
float[] pmax = null;
Vector3f pmin = new Vector3f();
Vector3f pmax = new Vector3f();
if (tile.data.detailMeshes != null)
{
@ -1138,25 +1149,27 @@ namespace DotRecast.Detour
continue;
}
float[][] v = new float[3][];
Vector3f[] v = new Vector3f[3];
for (int j = 0; j < 3; ++j)
{
if (tris[ti + j] < poly.vertCount)
{
int index = poly.verts[tris[ti + j]] * 3;
v[j] = new float[]
v[j] = new Vector3f
{
tile.data.verts[index], tile.data.verts[index + 1],
tile.data.verts[index + 2]
x = tile.data.verts[index],
y = tile.data.verts[index + 1],
z = tile.data.verts[index + 2]
};
}
else
{
int index = (pd.vertBase + (tris[ti + j] - poly.vertCount)) * 3;
v[j] = new float[]
v[j] = new Vector3f
{
tile.data.detailVerts[index], tile.data.detailVerts[index + 1],
tile.data.detailVerts[index + 2]
x = tile.data.detailVerts[index],
y = tile.data.detailVerts[index + 1],
z = tile.data.detailVerts[index + 2]
};
}
}
@ -1186,7 +1199,7 @@ namespace DotRecast.Detour
}
else
{
float[][] v = ArrayUtils.Of<float>(2, 3);
Vector3f[] v = new Vector3f[2];
for (int j = 0; j < poly.vertCount; ++j)
{
int k = (j + 1) % poly.vertCount;
@ -1213,7 +1226,7 @@ namespace DotRecast.Detour
return vLerp(pmin, pmax, tmin);
}
public float? getPolyHeight(MeshTile tile, Poly poly, float[] pos)
public float? getPolyHeight(MeshTile tile, Poly poly, Vector3f pos)
{
// Off-mesh connections do not have detail polys and getting height
// over them does not make sense.
@ -1243,25 +1256,27 @@ namespace DotRecast.Detour
for (int j = 0; j < pd.triCount; ++j)
{
int t = (pd.triBase + j) * 4;
float[][] v = new float[3][];
Vector3f[] v = new Vector3f[3];
for (int k = 0; k < 3; ++k)
{
if (tile.data.detailTris[t + k] < poly.vertCount)
{
int index = poly.verts[tile.data.detailTris[t + k]] * 3;
v[k] = new float[]
v[k] = new Vector3f
{
tile.data.verts[index], tile.data.verts[index + 1],
tile.data.verts[index + 2]
x = tile.data.verts[index],
y = tile.data.verts[index + 1],
z = tile.data.verts[index + 2]
};
}
else
{
int index = (pd.vertBase + (tile.data.detailTris[t + k] - poly.vertCount)) * 3;
v[k] = new float[]
v[k] = new Vector3f
{
tile.data.detailVerts[index], tile.data.detailVerts[index + 1],
tile.data.detailVerts[index + 2]
x = tile.data.detailVerts[index],
y = tile.data.detailVerts[index + 1],
z = tile.data.detailVerts[index + 2]
};
}
}
@ -1275,7 +1290,7 @@ namespace DotRecast.Detour
}
else
{
float[][] v = ArrayUtils.Of<float>(3, 3);
Vector3f[] v = new Vector3f[3];
v[0][0] = tile.data.verts[poly.verts[0] * 3];
v[0][1] = tile.data.verts[poly.verts[0] * 3 + 1];
v[0][2] = tile.data.verts[poly.verts[0] * 3 + 2];
@ -1300,17 +1315,17 @@ namespace DotRecast.Detour
// or larger floating point values) the point is on an edge, so just select
// closest. This should almost never happen so the extra iteration here is
// ok.
float[] closest = closestPointOnDetailEdges(tile, poly, pos, false);
var closest = closestPointOnDetailEdges(tile, poly, pos, false);
return closest[1];
}
public ClosestPointOnPolyResult closestPointOnPoly(long refs, float[] pos)
public ClosestPointOnPolyResult closestPointOnPoly(long refs, Vector3f pos)
{
Tuple<MeshTile, Poly> tileAndPoly = getTileAndPolyByRefUnsafe(refs);
MeshTile tile = tileAndPoly.Item1;
Poly poly = tileAndPoly.Item2;
Vector3f closest = new Vector3f();
vCopy(closest, pos);
vCopy(ref closest, pos);
float? h = getPolyHeight(tile, poly, pos);
if (null != h)
{
@ -1322,9 +1337,9 @@ namespace DotRecast.Detour
if (poly.getType() == Poly.DT_POLYTYPE_OFFMESH_CONNECTION)
{
int i = poly.verts[0] * 3;
float[] v0 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
var v0 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] };
i = poly.verts[1] * 3;
float[] v1 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
var v1 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] };
Tuple<float, float> dt = distancePtSegSqr2D(pos, v0, v1);
return new ClosestPointOnPolyResult(false, vLerp(v0, v1, dt.Item2));
}
@ -1333,12 +1348,12 @@ namespace DotRecast.Detour
return new ClosestPointOnPolyResult(false, closestPointOnDetailEdges(tile, poly, pos, true));
}
FindNearestPolyResult findNearestPolyInTile(MeshTile tile, float[] center, float[] extents)
FindNearestPolyResult findNearestPolyInTile(MeshTile tile, Vector3f center, Vector3f extents)
{
float[] nearestPt = null;
Vector3f nearestPt = new Vector3f();
bool overPoly = false;
float[] bmin = vSub(center, extents);
float[] bmax = vAdd(center, extents);
Vector3f bmin = vSub(center, extents);
Vector3f bmax = vAdd(center, extents);
// Get nearby polygons from proximity grid.
List<long> polys = queryPolygonsInTile(tile, bmin, bmax);
@ -1352,11 +1367,11 @@ namespace DotRecast.Detour
float d;
ClosestPointOnPolyResult cpp = closestPointOnPoly(refs, center);
bool posOverPoly = cpp.isPosOverPoly();
float[] closestPtPoly = cpp.getClosest();
Vector3f closestPtPoly = cpp.getClosest();
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
float[] diff = vSub(center, closestPtPoly);
Vector3f diff = vSub(center, closestPtPoly);
if (posOverPoly)
{
d = Math.Abs(diff[1]) - tile.data.header.walkableClimb;
@ -1501,11 +1516,11 @@ namespace DotRecast.Detour
/// normal polygon at one of its endpoints. This is the polygon identified
/// by
/// the prevRef parameter.
public Result<Tuple<float[], float[]>> getOffMeshConnectionPolyEndPoints(long prevRef, long polyRef)
public Result<Tuple<Vector3f, Vector3f>> getOffMeshConnectionPolyEndPoints(long prevRef, long polyRef)
{
if (polyRef == 0)
{
return Results.invalidParam<Tuple<float[], float[]>>("polyRef = 0");
return Results.invalidParam<Tuple<Vector3f, Vector3f>>("polyRef = 0");
}
// Get current polygon
@ -1515,18 +1530,18 @@ namespace DotRecast.Detour
int ip = saltitip[2];
if (it >= m_maxTiles)
{
return Results.invalidParam<Tuple<float[], float[]>>("Invalid tile ID > max tiles");
return Results.invalidParam<Tuple<Vector3f, Vector3f>>("Invalid tile ID > max tiles");
}
if (m_tiles[it].salt != salt || m_tiles[it].data.header == null)
{
return Results.invalidParam<Tuple<float[], float[]>>("Invalid salt or missing tile header");
return Results.invalidParam<Tuple<Vector3f, Vector3f>>("Invalid salt or missing tile header");
}
MeshTile tile = m_tiles[it];
if (ip >= tile.data.header.polyCount)
{
return Results.invalidParam<Tuple<float[], float[]>>("Invalid poly ID > poly count");
return Results.invalidParam<Tuple<Vector3f, Vector3f>>("Invalid poly ID > poly count");
}
Poly poly = tile.data.polys[ip];
@ -1534,7 +1549,7 @@ namespace DotRecast.Detour
// Make sure that the current poly is indeed off-mesh link.
if (poly.getType() != Poly.DT_POLYTYPE_OFFMESH_CONNECTION)
{
return Results.invalidParam<Tuple<float[], float[]>>("Invalid poly type");
return Results.invalidParam<Tuple<Vector3f, Vector3f>>("Invalid poly type");
}
// Figure out which way to hand out the vertices.
@ -1557,8 +1572,8 @@ namespace DotRecast.Detour
Vector3f startPos = new Vector3f();
Vector3f endPos = new Vector3f();
vCopy(startPos, tile.data.verts, poly.verts[idx0] * 3);
vCopy(endPos, tile.data.verts, poly.verts[idx1] * 3);
vCopy(ref startPos, tile.data.verts, poly.verts[idx0] * 3);
vCopy(ref endPos, tile.data.verts, poly.verts[idx1] * 3);
return Results.success(Tuple.Create(startPos, endPos));
}

10
src/DotRecast.Detour/NavMeshBuilder.cs
View File

@ -254,7 +254,7 @@ namespace DotRecast.Detour
const int XM = 1 << 2;
const int ZM = 1 << 3;
public static int classifyOffMeshPoint(VectorPtr pt, float[] bmin, float[] bmax)
public static int classifyOffMeshPoint(VectorPtr pt, Vector3f bmin, Vector3f bmax)
{
int outcode = 0;
outcode |= (pt.get(0) >= bmax[0]) ? XP : 0;
@ -343,8 +343,8 @@ namespace DotRecast.Detour
hmax += option.walkableClimb;
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
vCopy(bmin, option.bmin);
vCopy(bmax, option.bmax);
vCopy(ref bmin, option.bmin);
vCopy(ref bmax, option.bmax);
bmin[1] = hmin;
bmax[1] = hmax;
@ -467,8 +467,8 @@ namespace DotRecast.Detour
header.polyCount = totPolyCount;
header.vertCount = totVertCount;
header.maxLinkCount = maxLinkCount;
vCopy(header.bmin, option.bmin);
vCopy(header.bmax, option.bmax);
vCopy(ref header.bmin, option.bmin);
vCopy(ref header.bmax, option.bmax);
header.detailMeshCount = option.polyCount;
header.detailVertCount = uniqueDetailVertCount;
header.detailTriCount = detailTriCount;

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

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
/// Represents the source data used to build an navigation mesh tile.
@ -113,10 +115,10 @@ namespace DotRecast.Detour
public int tileLayer;
/// < The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.)
public float[] bmin;
public Vector3f bmin;
/// < The minimum bounds of the tile. [(x, y, z)] [Unit: wu]
public float[] bmax;
public Vector3f bmax;
/// < The maximum bounds of the tile. [(x, y, z)] [Unit: wu]
/// @}

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

@ -21,6 +21,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using System.Collections.Immutable;
using System.Linq;
using DotRecast.Core;
namespace DotRecast.Detour
@ -193,7 +194,7 @@ namespace DotRecast.Detour
float s = frand.frand();
float t = frand.frand();
float[] pt = randomPointInConvexPoly(verts, poly.vertCount, areas, s, t);
var pt = randomPointInConvexPoly(verts, poly.vertCount, areas, s, t);
ClosestPointOnPolyResult closest = closestPointOnPoly(polyRef, pt).result;
return Results.success(new FindRandomPointResult(polyRef, closest.getClosest()));
}
@ -214,7 +215,7 @@ namespace DotRecast.Detour
* Function returning a random number [0..1).
* @return Random location
*/
public Result<FindRandomPointResult> findRandomPointAroundCircle(long startRef, float[] centerPos, float maxRadius,
public Result<FindRandomPointResult> findRandomPointAroundCircle(long startRef, Vector3f centerPos, float maxRadius,
QueryFilter filter, FRand frand)
{
return findRandomPointAroundCircle(startRef, centerPos, maxRadius, filter, frand, PolygonByCircleConstraint.noop());
@ -235,17 +236,17 @@ namespace DotRecast.Detour
* Function returning a random number [0..1).
* @return Random location
*/
public Result<FindRandomPointResult> findRandomPointWithinCircle(long startRef, float[] centerPos, float maxRadius,
public Result<FindRandomPointResult> findRandomPointWithinCircle(long startRef, Vector3f centerPos, float maxRadius,
QueryFilter filter, FRand frand)
{
return findRandomPointAroundCircle(startRef, centerPos, maxRadius, filter, frand, PolygonByCircleConstraint.strict());
}
public Result<FindRandomPointResult> findRandomPointAroundCircle(long startRef, float[] centerPos, float maxRadius,
public Result<FindRandomPointResult> findRandomPointAroundCircle(long startRef, Vector3f centerPos, float maxRadius,
QueryFilter filter, FRand frand, PolygonByCircleConstraint constraint)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == centerPos || !vIsFinite(centerPos) || maxRadius < 0
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(centerPos) || maxRadius < 0
|| !float.IsFinite(maxRadius) || null == filter || null == frand)
{
return Results.invalidParam<FindRandomPointResult>();
@ -361,11 +362,11 @@ namespace DotRecast.Detour
continue;
}
float[] va = portalpoints.result.left;
float[] vb = portalpoints.result.right;
var va = portalpoints.result.left;
var vb = portalpoints.result.right;
// If the circle is not touching the next polygon, skip it.
Tuple<float, float> distseg = distancePtSegSqr2D(centerPos, va, vb);
var distseg = distancePtSegSqr2D(centerPos, va, vb);
float distSqr = distseg.Item1;
if (distSqr > radiusSqr)
{
@ -382,7 +383,7 @@ namespace DotRecast.Detour
// Cost
if (neighbourNode.flags == 0)
{
neighbourNode.pos = Vector3f.Of(vLerp(va, vb, 0.5f));
neighbourNode.pos = vLerp(va, vb, 0.5f);
}
float total = bestNode.total + vDist(bestNode.pos, neighbourNode.pos);
@ -420,7 +421,7 @@ namespace DotRecast.Detour
float t = frand.frand();
float[] areas = new float[randomPolyVerts.Length / 3];
float[] pt = randomPointInConvexPoly(randomPolyVerts, randomPolyVerts.Length / 3, areas, s, t);
Vector3f pt = randomPointInConvexPoly(randomPolyVerts, randomPolyVerts.Length / 3, areas, s, t);
ClosestPointOnPolyResult closest = closestPointOnPoly(randomPolyRef, pt).result;
return Results.success(new FindRandomPointResult(randomPolyRef, closest.getClosest()));
}
@ -440,9 +441,9 @@ namespace DotRecast.Detour
/// @param[out] closest
/// @param[out] posOverPoly
/// @returns The status flags for the query.
public Result<ClosestPointOnPolyResult> closestPointOnPoly(long refs, float[] pos)
public Result<ClosestPointOnPolyResult> closestPointOnPoly(long refs, Vector3f pos)
{
if (!m_nav.isValidPolyRef(refs) || null == pos || !vIsFinite(pos))
if (!m_nav.isValidPolyRef(refs) || !vIsFinite(pos))
{
return Results.invalidParam<ClosestPointOnPolyResult>();
}
@ -467,24 +468,24 @@ namespace DotRecast.Detour
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point. [(x, y, z)]
/// @returns The status flags for the query.
public Result<float[]> closestPointOnPolyBoundary(long refs, float[] pos)
public Result<Vector3f> closestPointOnPolyBoundary(long refs, Vector3f pos)
{
Result<Tuple<MeshTile, Poly>> tileAndPoly = m_nav.getTileAndPolyByRef(refs);
if (tileAndPoly.failed())
{
return Results.of<float[]>(tileAndPoly.status, tileAndPoly.message);
return Results.of<Vector3f>(tileAndPoly.status, tileAndPoly.message);
}
MeshTile tile = tileAndPoly.result.Item1;
Poly poly = tileAndPoly.result.Item2;
if (tile == null)
{
return Results.invalidParam<float[]>("Invalid tile");
return Results.invalidParam<Vector3f>("Invalid tile");
}
if (null == pos || !vIsFinite(pos))
if (!vIsFinite(pos))
{
return Results.invalidParam<float[]>();
return Results.invalidParam<Vector3f>();
}
// Collect vertices.
@ -497,10 +498,10 @@ namespace DotRecast.Detour
Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
}
float[] closest;
Vector3f closest;
if (distancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
closest = vCopy(pos);
closest = pos;
}
else
{
@ -518,7 +519,7 @@ namespace DotRecast.Detour
int va = imin * 3;
int vb = ((imin + 1) % nv) * 3;
closest = vLerp(verts, va, vb, edget[imin]);
closest = Vector3f.Of(vLerp(verts, va, vb, edget[imin]));
}
return Results.success(closest);
@ -534,7 +535,7 @@ namespace DotRecast.Detour
/// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)]
/// @param[out] height The height at the surface of the polygon.
/// @returns The status flags for the query.
public Result<float> getPolyHeight(long refs, float[] pos)
public Result<float> getPolyHeight(long refs, Vector3f pos)
{
Result<Tuple<MeshTile, Poly>> tileAndPoly = m_nav.getTileAndPolyByRef(refs);
if (tileAndPoly.failed())
@ -545,7 +546,7 @@ namespace DotRecast.Detour
MeshTile tile = tileAndPoly.result.Item1;
Poly poly = tileAndPoly.result.Item2;
if (null == pos || !vIsFinite2D(pos))
if (!vIsFinite2D(pos))
{
return Results.invalidParam<float>();
}
@ -556,10 +557,10 @@ namespace DotRecast.Detour
if (poly.getType() == Poly.DT_POLYTYPE_OFFMESH_CONNECTION)
{
int i = poly.verts[0] * 3;
float[] v0 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
var v0 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] };
i = poly.verts[1] * 3;
float[] v1 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
Tuple<float, float> dt = distancePtSegSqr2D(pos, v0, v1);
var v1 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] };
var dt = distancePtSegSqr2D(pos, v0, v1);
return Results.success(v0[1] + (v1[1] - v0[1]) * dt.Item2);
}
@ -579,7 +580,7 @@ namespace DotRecast.Detour
* The polygon filter to apply to the query.
* @return FindNearestPolyResult containing nearestRef, nearestPt and overPoly
*/
public Result<FindNearestPolyResult> findNearestPoly(float[] center, float[] halfExtents, QueryFilter filter)
public Result<FindNearestPolyResult> findNearestPoly(Vector3f center, Vector3f halfExtents, QueryFilter filter)
{
// Get nearby polygons from proximity grid.
FindNearestPolyQuery query = new FindNearestPolyQuery(this, center);
@ -593,13 +594,13 @@ namespace DotRecast.Detour
}
// FIXME: (PP) duplicate?
protected void queryPolygonsInTile(MeshTile tile, float[] qmin, float[] qmax, QueryFilter filter, PolyQuery query)
protected void queryPolygonsInTile(MeshTile tile, Vector3f qmin, Vector3f qmax, QueryFilter filter, PolyQuery query)
{
if (tile.data.bvTree != null)
{
int nodeIndex = 0;
float[] tbmin = tile.data.header.bmin;
float[] tbmax = tile.data.header.bmax;
var tbmin = tile.data.header.bmin;
var tbmax = tile.data.header.bmax;
float qfac = tile.data.header.bvQuantFactor;
// Calculate quantized box
int[] bmin = new int[3];
@ -670,13 +671,13 @@ namespace DotRecast.Detour
// Calc polygon bounds.
int v = p.verts[0] * 3;
vCopy(bmin, tile.data.verts, v);
vCopy(bmax, tile.data.verts, v);
vCopy(ref bmin, tile.data.verts, v);
vCopy(ref bmax, tile.data.verts, v);
for (int j = 1; j < p.vertCount; ++j)
{
v = p.verts[j] * 3;
vMin(bmin, tile.data.verts, v);
vMax(bmax, tile.data.verts, v);
vMin(ref bmin, tile.data.verts, v);
vMax(ref bmax, tile.data.verts, v);
}
if (overlapBounds(qmin, qmax, bmin, bmax))
@ -700,17 +701,16 @@ namespace DotRecast.Detour
* The polygon filter to apply to the query.
* @return The reference ids of the polygons that overlap the query box.
*/
public Status queryPolygons(float[] center, float[] halfExtents, QueryFilter filter, PolyQuery query)
public Status queryPolygons(Vector3f center, Vector3f halfExtents, QueryFilter filter, PolyQuery query)
{
if (null == center || !vIsFinite(center) || null == halfExtents || !vIsFinite(halfExtents)
|| null == filter)
if (!vIsFinite(center) || !vIsFinite(halfExtents) || null == filter)
{
return Status.FAILURE_INVALID_PARAM;
}
// Find tiles the query touches.
float[] bmin = vSub(center, halfExtents);
float[] bmax = vAdd(center, halfExtents);
Vector3f bmin = vSub(center, halfExtents);
Vector3f bmax = vAdd(center, halfExtents);
foreach (var t in queryTiles(center, halfExtents))
{
queryPolygonsInTile(t, bmin, bmax, filter, query);
@ -722,15 +722,15 @@ namespace DotRecast.Detour
/**
* Finds tiles that overlap the search box.
*/
public IList<MeshTile> queryTiles(float[] center, float[] halfExtents)
public IList<MeshTile> queryTiles(Vector3f center, Vector3f halfExtents)
{
if (null == center || !vIsFinite(center) || null == halfExtents || !vIsFinite(halfExtents))
if (!vIsFinite(center) || !vIsFinite(halfExtents))
{
return ImmutableArray<MeshTile>.Empty;
}
float[] bmin = vSub(center, halfExtents);
float[] bmax = vAdd(center, halfExtents);
Vector3f bmin = vSub(center, halfExtents);
Vector3f bmax = vAdd(center, halfExtents);
int[] minxy = m_nav.calcTileLoc(bmin);
int minx = minxy[0];
int miny = minxy[1];
@ -769,24 +769,22 @@ namespace DotRecast.Detour
* The polygon filter to apply to the query.
* @return Found path
*/
public virtual Result<List<long>> findPath(long startRef, long endRef, float[] startPos, float[] endPos,
QueryFilter filter)
public virtual Result<List<long>> findPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter)
{
return findPath(startRef, endRef, startPos, endPos, filter, new DefaultQueryHeuristic(), 0, 0);
}
public virtual Result<List<long>> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public virtual Result<List<long>> findPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
int options, float raycastLimit)
{
return findPath(startRef, endRef, startPos, endPos, filter, new DefaultQueryHeuristic(), options, raycastLimit);
}
public Result<List<long>> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public Result<List<long>> findPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
QueryHeuristic heuristic, int options, float raycastLimit)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || null == startPos
|| !vIsFinite(startPos) || null == endPos || !vIsFinite(endPos) || null == filter)
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || !vIsFinite(startPos) || !vIsFinite(endPos) || null == filter)
{
return Results.invalidParam<List<long>>();
}
@ -814,7 +812,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, startPos);
vCopy(ref startNode.pos, startPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = heuristic.getCost(startPos, endPos);
@ -913,8 +911,8 @@ namespace DotRecast.Detour
}
// If the node is visited the first time, calculate node position.
float[] neighbourPos = neighbourNode.pos;
Result<float[]> midpod = neighbourRef == endRef
var neighbourPos = neighbourNode.pos;
var midpod = neighbourRef == endRef
? getEdgeIntersectionPoint(bestNode.pos, bestRef, bestPoly, bestTile, endPos, neighbourRef,
neighbourPoly, neighbourTile)
: getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile);
@ -1043,19 +1041,19 @@ namespace DotRecast.Detour
* query options (see: #FindPathOptions)
* @return
*/
public Status initSlicedFindPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public Status initSlicedFindPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
int options)
{
return initSlicedFindPath(startRef, endRef, startPos, endPos, filter, options, new DefaultQueryHeuristic(), -1.0f);
}
public Status initSlicedFindPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public Status initSlicedFindPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
int options, float raycastLimit)
{
return initSlicedFindPath(startRef, endRef, startPos, endPos, filter, options, new DefaultQueryHeuristic(), raycastLimit);
}
public Status initSlicedFindPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter,
public Status initSlicedFindPath(long startRef, long endRef, Vector3f startPos, Vector3f endPos, QueryFilter filter,
int options, QueryHeuristic heuristic, float raycastLimit)
{
// Init path state.
@ -1063,16 +1061,15 @@ namespace DotRecast.Detour
m_query.status = Status.FAILURE;
m_query.startRef = startRef;
m_query.endRef = endRef;
vCopy(m_query.startPos, startPos);
vCopy(m_query.endPos, endPos);
vCopy(ref m_query.startPos, startPos);
vCopy(ref m_query.endPos, endPos);
m_query.filter = filter;
m_query.options = options;
m_query.heuristic = heuristic;
m_query.raycastLimitSqr = sqr(raycastLimit);
// Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || null == startPos
|| !vIsFinite(startPos) || null == endPos || !vIsFinite(endPos) || null == filter)
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) || !vIsFinite(startPos) || !vIsFinite(endPos) || null == filter)
{
return Status.FAILURE_INVALID_PARAM;
}
@ -1097,7 +1094,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, startPos);
vCopy(ref startNode.pos, startPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = heuristic.getCost(startPos, endPos);
@ -1243,8 +1240,8 @@ namespace DotRecast.Detour
// If the node is visited the first time, calculate node
// position.
float[] neighbourPos = neighbourNode.pos;
Result<float[]> midpod = neighbourRef == m_query.endRef
var neighbourPos = neighbourNode.pos;
var midpod = neighbourRef == m_query.endRef
? getEdgeIntersectionPoint(bestNode.pos, bestRef, bestPoly, bestTile, m_query.endPos,
neighbourRef, neighbourPoly, neighbourTile)
: getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile);
@ -1448,7 +1445,7 @@ namespace DotRecast.Detour
return Results.of(status, path);
}
protected Status appendVertex(float[] pos, int flags, long refs, List<StraightPathItem> straightPath,
protected Status appendVertex(Vector3f pos, int flags, long refs, List<StraightPathItem> straightPath,
int maxStraightPath)
{
if (straightPath.Count > 0 && vEqual(straightPath[straightPath.Count - 1].pos, pos))
@ -1475,10 +1472,10 @@ namespace DotRecast.Detour
return Status.IN_PROGRESS;
}
protected Status appendPortals(int startIdx, int endIdx, float[] endPos, List<long> path,
protected Status appendPortals(int startIdx, int endIdx, Vector3f endPos, List<long> path,
List<StraightPathItem> straightPath, int maxStraightPath, int options)
{
float[] startPos = straightPath[straightPath.Count - 1].pos;
var startPos = straightPath[straightPath.Count - 1].pos;
// Append or update last vertex
Status stat;
for (int i = startIdx; i < endIdx; i++)
@ -1510,8 +1507,8 @@ namespace DotRecast.Detour
break;
}
float[] left = portals.result.left;
float[] right = portals.result.right;
var left = portals.result.left;
var right = portals.result.right;
if ((options & DT_STRAIGHTPATH_AREA_CROSSINGS) != 0)
{
@ -1527,7 +1524,7 @@ namespace DotRecast.Detour
if (null != interect)
{
float t = interect.Item2;
float[] pt = vLerp(left, right, t);
var pt = vLerp(left, right, t);
stat = appendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath);
if (!stat.isInProgress())
{
@ -1564,31 +1561,31 @@ namespace DotRecast.Detour
/// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0]
/// @param[in] options Query options. (see: #dtStraightPathOptions)
/// @returns The status flags for the query.
public virtual Result<List<StraightPathItem>> findStraightPath(float[] startPos, float[] endPos, List<long> path,
public virtual Result<List<StraightPathItem>> findStraightPath(Vector3f startPos, Vector3f endPos, List<long> path,
int maxStraightPath, int options)
{
List<StraightPathItem> straightPath = new List<StraightPathItem>();
if (null == startPos || !vIsFinite(startPos) || null == endPos || !vIsFinite(endPos)
if (!vIsFinite(startPos) || !vIsFinite(endPos)
|| null == path || 0 == path.Count || path[0] == 0 || maxStraightPath <= 0)
{
return Results.invalidParam<List<StraightPathItem>>();
}
// TODO: Should this be callers responsibility?
Result<float[]> closestStartPosRes = closestPointOnPolyBoundary(path[0], startPos);
Result<Vector3f> closestStartPosRes = closestPointOnPolyBoundary(path[0], startPos);
if (closestStartPosRes.failed())
{
return Results.invalidParam<List<StraightPathItem>>("Cannot find start position");
}
float[] closestStartPos = closestStartPosRes.result;
Result<float[]> closestEndPosRes = closestPointOnPolyBoundary(path[path.Count - 1], endPos);
var closestStartPos = closestStartPosRes.result;
var closestEndPosRes = closestPointOnPolyBoundary(path[path.Count - 1], endPos);
if (closestEndPosRes.failed())
{
return Results.invalidParam<List<StraightPathItem>>("Cannot find end position");
}
float[] closestEndPos = closestEndPosRes.result;
var closestEndPos = closestEndPosRes.result;
// Add start point.
Status stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath);
if (!stat.isInProgress())
@ -1598,9 +1595,9 @@ namespace DotRecast.Detour
if (path.Count > 1)
{
float[] portalApex = vCopy(closestStartPos);
float[] portalLeft = vCopy(portalApex);
float[] portalRight = vCopy(portalApex);
Vector3f portalApex = closestStartPos;
Vector3f portalLeft = portalApex;
Vector3f portalRight = portalApex;
int apexIndex = 0;
int leftIndex = 0;
int rightIndex = 0;
@ -1613,8 +1610,8 @@ namespace DotRecast.Detour
for (int i = 0; i < path.Count; ++i)
{
float[] left;
float[] right;
Vector3f left;
Vector3f right;
int toType;
if (i + 1 < path.Count)
@ -1659,8 +1656,8 @@ namespace DotRecast.Detour
else
{
// End of the path.
left = vCopy(closestEndPos);
right = vCopy(closestEndPos);
left = closestEndPos;
right = closestEndPos;
toType = Poly.DT_POLYTYPE_GROUND;
}
@ -1669,7 +1666,7 @@ namespace DotRecast.Detour
{
if (vEqual(portalApex, portalRight) || triArea2D(portalApex, portalLeft, right) > 0.0f)
{
portalRight = vCopy(right);
portalRight = right;
rightPolyRef = (i + 1 < path.Count) ? path[i + 1] : 0;
rightPolyType = toType;
rightIndex = i;
@ -1687,7 +1684,7 @@ namespace DotRecast.Detour
}
}
portalApex = vCopy(portalLeft);
portalApex = portalLeft;
apexIndex = leftIndex;
int flags = 0;
@ -1709,8 +1706,8 @@ namespace DotRecast.Detour
return Results.success(straightPath);
}
portalLeft = vCopy(portalApex);
portalRight = vCopy(portalApex);
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
@ -1726,7 +1723,7 @@ namespace DotRecast.Detour
{
if (vEqual(portalApex, portalLeft) || triArea2D(portalApex, portalRight, left) < 0.0f)
{
portalLeft = vCopy(left);
portalLeft = left;
leftPolyRef = (i + 1 < path.Count) ? path[i + 1] : 0;
leftPolyType = toType;
leftIndex = i;
@ -1744,7 +1741,7 @@ namespace DotRecast.Detour
}
}
portalApex = vCopy(portalRight);
portalApex = portalRight;
apexIndex = rightIndex;
int flags = 0;
@ -1766,8 +1763,8 @@ namespace DotRecast.Detour
return Results.success(straightPath);
}
portalLeft = vCopy(portalApex);
portalRight = vCopy(portalApex);
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
@ -1822,12 +1819,11 @@ namespace DotRecast.Detour
/// @param[in] endPos The desired end position of the mover. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @returns Path
public Result<MoveAlongSurfaceResult> moveAlongSurface(long startRef, float[] startPos, float[] endPos,
QueryFilter filter)
public Result<MoveAlongSurfaceResult> moveAlongSurface(long startRef, Vector3f startPos, Vector3f endPos, QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == startPos || !vIsFinite(startPos)
|| null == endPos || !vIsFinite(endPos) || null == filter)
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(startPos)
|| !vIsFinite(endPos) || null == filter)
{
return Results.invalidParam<MoveAlongSurfaceResult>();
}
@ -1846,10 +1842,10 @@ namespace DotRecast.Detour
Vector3f bestPos = new Vector3f();
float bestDist = float.MaxValue;
Node bestNode = null;
vCopy(bestPos, startPos);
vCopy(ref bestPos, startPos);
// Search constraints
float[] searchPos = vLerp(startPos, endPos, 0.5f);
var searchPos = vLerp(startPos, endPos, 0.5f);
float searchRadSqr = sqr(vDist(startPos, endPos) / 2.0f + 0.001f);
float[] verts = new float[m_nav.getMaxVertsPerPoly() * 3];
@ -1878,7 +1874,7 @@ namespace DotRecast.Detour
if (pointInPolygon(endPos, verts, nverts))
{
bestNode = curNode;
vCopy(bestPos, endPos);
vCopy(ref bestPos, endPos);
break;
}
@ -1936,7 +1932,7 @@ namespace DotRecast.Detour
if (distSqr < bestDist)
{
// Update nearest distance.
bestPos = vLerp(verts, vj, vi, tseg);
bestPos = Vector3f.Of(vLerp(verts, vj, vi, tseg));
bestDist = distSqr;
bestNode = curNode;
}
@ -2000,12 +1996,12 @@ namespace DotRecast.Detour
public class PortalResult
{
public readonly float[] left;
public readonly float[] right;
public readonly Vector3f left;
public readonly Vector3f right;
public readonly int fromType;
public readonly int toType;
public PortalResult(float[] left, float[] right, int fromType, int toType)
public PortalResult(Vector3f left, Vector3f right, int fromType, int toType)
{
this.left = left;
this.right = right;
@ -2072,8 +2068,8 @@ namespace DotRecast.Detour
if (fromTile.links[i].refs == to)
{
int v = fromTile.links[i].edge;
Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, left, 0, 3);
Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, right, 0, 3);
Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, left.ToArray(), 0, 3);
Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, right.ToArray(), 0, 3);
return Results.success(new PortalResult(left, right, fromType, toType));
}
}
@ -2088,8 +2084,8 @@ namespace DotRecast.Detour
if (toTile.links[i].refs == from)
{
int v = toTile.links[i].edge;
Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, left, 0, 3);
Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, right, 0, 3);
Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, left.ToArray(), 0, 3);
Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, right.ToArray(), 0, 3);
return Results.success(new PortalResult(left, right, fromType, toType));
}
}
@ -2100,8 +2096,8 @@ namespace DotRecast.Detour
// Find portal vertices.
int v0 = fromPoly.verts[link.edge];
int v1 = fromPoly.verts[(link.edge + 1) % fromPoly.vertCount];
Array.Copy(fromTile.data.verts, v0 * 3, left, 0, 3);
Array.Copy(fromTile.data.verts, v1 * 3, right, 0, 3);
Array.Copy(fromTile.data.verts, v0 * 3, left.ToArray(), 0, 3);
Array.Copy(fromTile.data.verts, v1 * 3, right.ToArray(), 0, 3);
// If the link is at tile boundary, dtClamp the vertices to
// the link width.
@ -2113,25 +2109,25 @@ namespace DotRecast.Detour
float s = 1.0f / 255.0f;
float tmin = link.bmin * s;
float tmax = link.bmax * s;
left = vLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = vLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax);
left = Vector3f.Of(vLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin));
right = Vector3f.Of(vLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax));
}
}
return Results.success(new PortalResult(left, right, fromType, toType));
}
protected Result<float[]> getEdgeMidPoint(long from, Poly fromPoly, MeshTile fromTile, long to,
protected Result<Vector3f> getEdgeMidPoint(long from, Poly fromPoly, MeshTile fromTile, long to,
Poly toPoly, MeshTile toTile)
{
Result<PortalResult> ppoints = getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, 0, 0);
if (ppoints.failed())
{
return Results.of<float[]>(ppoints.status, ppoints.message);
return Results.of<Vector3f>(ppoints.status, ppoints.message);
}
float[] left = ppoints.result.left;
float[] right = ppoints.result.right;
var left = ppoints.result.left;
var right = ppoints.result.right;
Vector3f mid = new Vector3f();
mid[0] = (left[0] + right[0]) * 0.5f;
mid[1] = (left[1] + right[1]) * 0.5f;
@ -2139,17 +2135,17 @@ namespace DotRecast.Detour
return Results.success(mid);
}
protected Result<float[]> getEdgeIntersectionPoint(float[] fromPos, long from, Poly fromPoly, MeshTile fromTile,
float[] toPos, long to, Poly toPoly, MeshTile toTile)
protected Result<Vector3f> getEdgeIntersectionPoint(Vector3f fromPos, long from, Poly fromPoly, MeshTile fromTile,
Vector3f toPos, long to, Poly toPoly, MeshTile toTile)
{
Result<PortalResult> ppoints = getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, 0, 0);
if (ppoints.failed())
{
return Results.of<float[]>(ppoints.status, ppoints.message);
return Results.of<Vector3f>(ppoints.status, ppoints.message);
}
float[] left = ppoints.result.left;
float[] right = ppoints.result.right;
Vector3f left = ppoints.result.left;
Vector3f right = ppoints.result.right;
float t = 0.5f;
Tuple<float, float> interect = intersectSegSeg2D(fromPos, toPos, left, right);
if (null != interect)
@ -2157,7 +2153,7 @@ namespace DotRecast.Detour
t = clamp(interect.Item2, 0.1f, 0.9f);
}
float[] pt = vLerp(left, right, t);
Vector3f pt = vLerp(left, right, t);
return Results.success(pt);
}
@ -2215,12 +2211,11 @@ namespace DotRecast.Detour
/// @param[out] pathCount The number of visited polygons. [opt]
/// @param[in] maxPath The maximum number of polygons the @p path array can hold.
/// @returns The status flags for the query.
public Result<RaycastHit> raycast(long startRef, float[] startPos, float[] endPos, QueryFilter filter, int options,
public Result<RaycastHit> raycast(long startRef, Vector3f startPos, Vector3f endPos, QueryFilter filter, int options,
long prevRef)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == startPos || !vIsFinite(startPos)
|| null == endPos || !vIsFinite(endPos) || null == filter
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(startPos) || !vIsFinite(endPos) || null == filter
|| (prevRef != 0 && !m_nav.isValidPolyRef(prevRef)))
{
return Results.invalidParam<RaycastHit>();
@ -2233,7 +2228,7 @@ namespace DotRecast.Detour
Vector3f curPos = new Vector3f(), lastPos = new Vector3f();
vCopy(ref curPos, startPos);
float[] dir = vSub(endPos, startPos);
var dir = vSub(endPos, startPos);
MeshTile prevTile, tile, nextTile;
Poly prevPoly, poly, nextPoly;
@ -2400,12 +2395,12 @@ namespace DotRecast.Detour
{
// compute the intersection point at the furthest end of the polygon
// and correct the height (since the raycast moves in 2d)
vCopy(lastPos, curPos);
vCopy(ref lastPos, curPos);
curPos = vMad(startPos, dir, hit.t);
VectorPtr e1 = new VectorPtr(verts, iresult.segMax * 3);
VectorPtr e2 = new VectorPtr(verts, ((iresult.segMax + 1) % nv) * 3);
float[] eDir = vSub(e2, e1);
float[] diff = vSub(new VectorPtr(curPos), e1);
Vector3f eDir = vSub(e2, e1);
Vector3f diff = vSub(curPos, e1);
float s = sqr(eDir[0]) > sqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
curPos[1] = e1.get(1) + eDir[1] * s;
@ -2427,7 +2422,7 @@ namespace DotRecast.Detour
hit.hitNormal[0] = dz;
hit.hitNormal[1] = 0;
hit.hitNormal[2] = -dx;
vNormalize(hit.hitNormal);
vNormalize(ref hit.hitNormal);
return Results.success(hit);
}
@ -2487,12 +2482,11 @@ namespace DotRecast.Detour
/// @param[out] resultCount The number of polygons found. [opt]
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
public Result<FindPolysAroundResult> findPolysAroundCircle(long startRef, float[] centerPos, float radius,
QueryFilter filter)
public Result<FindPolysAroundResult> findPolysAroundCircle(long startRef, Vector3f centerPos, float radius, QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == centerPos || !vIsFinite(centerPos) || radius < 0
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(centerPos) || radius < 0
|| !float.IsFinite(radius) || null == filter)
{
return Results.invalidParam<FindPolysAroundResult>();
@ -2506,7 +2500,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, centerPos);
vCopy(ref startNode.pos, centerPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
@ -2578,8 +2572,8 @@ namespace DotRecast.Detour
continue;
}
float[] va = pp.result.left;
float[] vb = pp.result.right;
var va = pp.result.left;
var vb = pp.result.right;
// If the circle is not touching the next polygon, skip it.
Tuple<float, float> distseg = distancePtSegSqr2D(centerPos, va, vb);
@ -2696,7 +2690,7 @@ namespace DotRecast.Detour
centerPos[2] *= scale;
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, centerPos);
vCopy(ref startNode.pos, centerPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
@ -2766,8 +2760,8 @@ namespace DotRecast.Detour
continue;
}
float[] va = pp.result.left;
float[] vb = pp.result.right;
var va = pp.result.left;
var vb = pp.result.right;
// If the poly is not touching the edge to the next polygon, skip the connection it.
IntersectResult ir = intersectSegmentPoly2D(va, vb, verts, nverts);
@ -2857,11 +2851,11 @@ 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 Result<FindLocalNeighbourhoodResult> findLocalNeighbourhood(long startRef, float[] centerPos, float radius,
public Result<FindLocalNeighbourhoodResult> findLocalNeighbourhood(long startRef, Vector3f centerPos, float radius,
QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == centerPos || !vIsFinite(centerPos) || radius < 0
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(centerPos) || radius < 0
|| !float.IsFinite(radius) || null == filter)
{
return Results.invalidParam<FindLocalNeighbourhoodResult>();
@ -2942,8 +2936,8 @@ namespace DotRecast.Detour
continue;
}
float[] va = pp.result.left;
float[] vb = pp.result.right;
var va = pp.result.left;
var vb = pp.result.right;
// If the circle is not touching the next polygon, skip it.
Tuple<float, float> distseg = distancePtSegSqr2D(centerPos, va, vb);
@ -3207,11 +3201,11 @@ namespace DotRecast.Detour
/// @param[out] hitNormal The normalized ray formed from the wall point to the
/// source point. [(x, y, z)]
/// @returns The status flags for the query.
public virtual Result<FindDistanceToWallResult> findDistanceToWall(long startRef, float[] centerPos, float maxRadius,
public virtual Result<FindDistanceToWallResult> findDistanceToWall(long startRef, Vector3f centerPos, float maxRadius,
QueryFilter filter)
{
// Validate input
if (!m_nav.isValidPolyRef(startRef) || null == centerPos || !vIsFinite(centerPos) || maxRadius < 0
if (!m_nav.isValidPolyRef(startRef) || !vIsFinite(centerPos) || maxRadius < 0
|| !float.IsFinite(maxRadius) || null == filter)
{
return Results.invalidParam<FindDistanceToWallResult>();
@ -3221,7 +3215,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, centerPos);
vCopy(ref startNode.pos, centerPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
@ -3369,7 +3363,7 @@ namespace DotRecast.Detour
// Cost
if (neighbourNode.flags == 0)
{
Result<float[]> midPoint = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
var midPoint = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
neighbourTile);
if (midPoint.succeeded())
{
@ -3406,11 +3400,11 @@ namespace DotRecast.Detour
Vector3f hitNormal = new Vector3f();
if (bestvi != null && bestvj != null)
{
float[] tangent = vSub(bestvi, bestvj);
var tangent = vSub(bestvi, bestvj);
hitNormal[0] = tangent[2];
hitNormal[1] = 0;
hitNormal[2] = -tangent[0];
vNormalize(hitNormal);
vNormalize(ref hitNormal);
}
return Results.success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal));

9
src/DotRecast.Detour/PolygonByCircleConstraint.cs
View File

@ -17,6 +17,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -24,7 +25,7 @@ namespace DotRecast.Detour
public interface PolygonByCircleConstraint
{
float[] aply(float[] polyVerts, float[] circleCenter, float radius);
float[] aply(float[] polyVerts, Vector3f circleCenter, float radius);
public static PolygonByCircleConstraint noop()
{
@ -38,7 +39,7 @@ namespace DotRecast.Detour
public class NoOpPolygonByCircleConstraint : PolygonByCircleConstraint
{
public float[] aply(float[] polyVerts, float[] circleCenter, float radius)
public float[] aply(float[] polyVerts, Vector3f circleCenter, float radius)
{
return polyVerts;
}
@ -52,7 +53,7 @@ namespace DotRecast.Detour
private const int CIRCLE_SEGMENTS = 12;
private static float[] unitCircle;
public float[] aply(float[] verts, float[] center, float radius)
public float[] aply(float[] verts, Vector3f center, float radius)
{
float radiusSqr = radius * radius;
int outsideVertex = -1;
@ -82,7 +83,7 @@ namespace DotRecast.Detour
return intersection;
}
private float[] circle(float[] center, float radius)
private float[] circle(Vector3f center, float radius)
{
if (unitCircle == null)
{

4
src/DotRecast.Detour/QueryFilter.cs
View File

@ -18,13 +18,15 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
public interface QueryFilter
{
bool passFilter(long refs, MeshTile tile, Poly poly);
float getCost(float[] pa, float[] pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef, MeshTile curTile,
float getCost(Vector3f pa, Vector3f pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef, MeshTile curTile,
Poly curPoly, long nextRef, MeshTile nextTile, Poly nextPoly);
}
}

4
src/DotRecast.Detour/QueryHeuristic.cs
View File

@ -16,10 +16,12 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
public interface QueryHeuristic
{
float getCost(float[] neighbourPos, float[] endPos);
float getCost(Vector3f neighbourPos, Vector3f endPos);
}
}

2
src/DotRecast.Detour/RaycastHit.cs
View File

@ -32,7 +32,7 @@ namespace DotRecast.Detour
public float t;
/** hitNormal The normal of the nearest wall hit. [(x, y, z)] */
public readonly Vector3f hitNormal = new Vector3f();
public Vector3f hitNormal = new Vector3f();
/** Visited polygons. */
public readonly List<long> path = new List<long>();

10
src/DotRecast.Detour/StraightPathItem.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
using static DotRecast.Core.RecastMath;
@ -25,18 +27,18 @@ namespace DotRecast.Detour
//TODO: (PP) Add comments
public class StraightPathItem
{
public float[] pos;
public Vector3f pos;
public int flags;
public long refs;
public StraightPathItem(float[] pos, int flags, long refs)
public StraightPathItem(Vector3f pos, int flags, long refs)
{
this.pos = vCopy(pos);
this.pos = pos;
this.flags = flags;
this.refs = refs;
}
public float[] getPos()
public Vector3f getPos()
{
return pos;
}

8
src/DotRecast.Recast/Heightfield.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Recast
{
/** Represents a heightfield layer within a layer set. */
@ -30,10 +32,10 @@ namespace DotRecast.Recast
public readonly int height;
/** The minimum bounds in world space. [(x, y, z)] */
public readonly float[] bmin;
public readonly Vector3f bmin;
/** The maximum bounds in world space. [(x, y, z)] */
public readonly float[] bmax;
public Vector3f bmax;
/** The size of each cell. (On the xz-plane.) */
public readonly float cs;
@ -47,7 +49,7 @@ namespace DotRecast.Recast
/** Border size in cell units */
public readonly int borderSize;
public Heightfield(int width, int height, float[] bmin, float[] bmax, float cs, float ch, int borderSize)
public Heightfield(int width, int height, Vector3f bmin, Vector3f bmax, float cs, float ch, int borderSize)
{
this.width = width;
this.height = height;

2
src/DotRecast.Recast/RecastFilledVolumeRasterization.cs
View File

@ -786,7 +786,7 @@ namespace DotRecast.Recast
return dx * dx + dy * dy + dz * dz;
}
private static bool overlapBounds(float[] amin, float[] amax, float[] bounds)
private static bool overlapBounds(Vector3f amin, Vector3f amax, float[] bounds)
{
bool overlap = true;
overlap = (amin[0] > bounds[3] || amax[0] < bounds[0]) ? false : overlap;

6
src/DotRecast.Recast/RecastRasterization.cs
View File

@ -50,7 +50,7 @@ namespace DotRecast.Recast
return overlap;
}
private static bool overlapBounds(float[] amin, float[] amax, Vector3f bmin, Vector3f bmax)
private static bool overlapBounds(Vector3f amin, Vector3f amax, Vector3f bmin, Vector3f bmax)
{
bool overlap = true;
overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
@ -255,8 +255,8 @@ namespace DotRecast.Recast
* @param flagMergeThreshold
* The threshold in which area flags will be merged
*/
private static void rasterizeTri(float[] verts, int v0, int v1, int v2, int area, Heightfield hf, float[] hfBBMin,
float[] hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold)
private static void rasterizeTri(float[] verts, int v0, int v1, int v2, int area, Heightfield hf, Vector3f hfBBMin,
Vector3f hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold)
{
Vector3f tmin = new Vector3f();
Vector3f tmax = new Vector3f();

2
src/DotRecast.Recast/RecastVoxelization.cs
View File

@ -29,7 +29,7 @@ namespace DotRecast.Recast
RecastConfig cfg = builderCfg.cfg;
// Allocate voxel heightfield where we rasterize our input data to.
Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin.ToArray(), builderCfg.bmax.ToArray(), cfg.cs, cfg.ch, cfg.borderSize);
Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin, builderCfg.bmax, cfg.cs, cfg.ch, cfg.borderSize);
// Allocate array that can hold triangle area types.
// If you have multiple meshes you need to process, allocate