diff --git a/src/DotRecast.Core/RecastMath.cs b/src/DotRecast.Core/RecastMath.cs index bd30b71..1a56217 100644 --- a/src/DotRecast.Core/RecastMath.cs +++ b/src/DotRecast.Core/RecastMath.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using System.Numerics; namespace DotRecast.Core { @@ -107,6 +108,16 @@ namespace DotRecast.Core dest[2] = v1[2] + v2[2] * s; 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) @@ -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,14 +171,34 @@ 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) @@ -168,6 +209,16 @@ namespace DotRecast.Core dest[2] = v1[2] + v2[2]; 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) { @@ -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]; @@ -316,11 +376,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) { @@ -344,6 +416,14 @@ namespace DotRecast.Core float dz = v2[2] - v1[2]; 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) { @@ -351,8 +431,16 @@ namespace DotRecast.Core float dz = v2[2] - v1[2]; return dx * dx + dz * dz; } + + 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(float[] p, float[] verts, int i) + + public static float vDist2DSqr(Vector3f p, float[] verts, int i) { float dx = verts[i] - p[0]; float dz = verts[i + 2] - p[2]; @@ -395,12 +483,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)] @@ -413,6 +514,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) { @@ -430,6 +537,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. @@ -499,8 +612,17 @@ namespace DotRecast.Core overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; return overlap; } + + 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 distancePtSegSqr2D(float[] pt, float[] p, float[] q) + public static Tuple 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 distancePtSegSqr2D(float[] pt, float[] verts, int p, int q) + public static Tuple 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]; @@ -851,6 +973,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? intersectSegSeg2D(float[] ap, float[] aq, float[] bp, float[] bq) { @@ -870,9 +998,9 @@ namespace DotRecast.Core public static Tuple? 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) { @@ -893,6 +1021,16 @@ namespace DotRecast.Core @out[2] = @in[2] * scale; 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)] @@ -902,6 +1040,11 @@ 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)] @@ -909,5 +1052,10 @@ namespace DotRecast.Core { return float.IsFinite(v[0]) && float.IsFinite(v[2]); } + + public static bool vIsFinite2D(Vector3f v) + { + return float.IsFinite(v[0]) && float.IsFinite(v[2]); + } } } \ No newline at end of file diff --git a/src/DotRecast.Core/Vector2f.cs b/src/DotRecast.Core/Vector2f.cs new file mode 100644 index 0000000..b4b18ae --- /dev/null +++ b/src/DotRecast.Core/Vector2f.cs @@ -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 }; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 381d16a..21945b7 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -163,7 +163,7 @@ namespace DotRecast.Detour.Crowd public Crowd(CrowdConfig config, NavMesh nav, Func 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 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(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; diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 5e240f1..317afac 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -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) { diff --git a/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs b/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs index da1b66c..377b5e1 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs @@ -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 diff --git a/src/DotRecast.Detour.Crowd/LocalBoundary.cs b/src/DotRecast.Detour.Crowd/LocalBoundary.cs index 20e6d95..e1a7011 100644 --- a/src/DotRecast.Detour.Crowd/LocalBoundary.cs +++ b/src/DotRecast.Detour.Crowd/LocalBoundary.cs @@ -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 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; } diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index f1c2415..cf3f116 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -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); diff --git a/src/DotRecast.Detour.Crowd/PathCorridor.cs b/src/DotRecast.Detour.Crowd/PathCorridor.cs index e52810c..9d9288c 100644 --- a/src/DotRecast.Detour.Crowd/PathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/PathCorridor.cs @@ -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 m_path; protected List mergeCorridorStartMoved(List path, List 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 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> 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 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 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 path) + public void setCorridor(Vector3f target, List path) { - vCopy(m_target, target); + vCopy(ref m_target, target); m_path = new List(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]; diff --git a/src/DotRecast.Detour.Crowd/PathQuery.cs b/src/DotRecast.Detour.Crowd/PathQuery.cs index f106012..077cbb7 100644 --- a/src/DotRecast.Detour.Crowd/PathQuery.cs +++ b/src/DotRecast.Detour.Crowd/PathQuery.cs @@ -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 diff --git a/src/DotRecast.Detour.Crowd/PathQueue.cs b/src/DotRecast.Detour.Crowd/PathQueue.cs index 9b3c6ed..0aa1b21 100644 --- a/src/DotRecast.Detour.Crowd/PathQueue.cs +++ b/src/DotRecast.Detour.Crowd/PathQueue.cs @@ -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; diff --git a/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs b/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs index cbe9c41..2831fda 100644 --- a/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs +++ b/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs @@ -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 diff --git a/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs b/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs index 6ff1b10..0774fd7 100644 --- a/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs +++ b/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs @@ -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]; diff --git a/src/DotRecast.Detour.Dynamic/DynamicTileCheckpoint.cs b/src/DotRecast.Detour.Dynamic/DynamicTileCheckpoint.cs index 32e812d..7d5e32f 100644 --- a/src/DotRecast.Detour.Dynamic/DynamicTileCheckpoint.cs +++ b/src/DotRecast.Detour.Dynamic/DynamicTileCheckpoint.cs @@ -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) { diff --git a/src/DotRecast.Detour.Dynamic/Io/VoxelTile.cs b/src/DotRecast.Detour.Dynamic/Io/VoxelTile.cs index 1a71fcb..046ee71 100644 --- a/src/DotRecast.Detour.Dynamic/Io/VoxelTile.cs +++ b/src/DotRecast.Detour.Dynamic/Io/VoxelTile.cs @@ -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; diff --git a/src/DotRecast.Detour.Dynamic/VoxelQuery.cs b/src/DotRecast.Detour.Dynamic/VoxelQuery.cs index 99a9a74..39bf31f 100644 --- a/src/DotRecast.Detour.Dynamic/VoxelQuery.cs +++ b/src/DotRecast.Detour.Dynamic/VoxelQuery.cs @@ -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 heightfieldProvider; - public VoxelQuery(float[] origin, float tileWidth, float tileDepth, Func heightfieldProvider) + public VoxelQuery(Vector3f origin, float tileWidth, float tileDepth, Func heightfieldProvider) { this.origin = origin; this.tileWidth = tileWidth; diff --git a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs index 65dd210..2289089 100644 --- a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs +++ b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs @@ -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); diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 2dc884b..6b82441 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs @@ -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> heightFunc) + Func> 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> heightFunc, GroundSegment seg, int nsamples) + protected void sampleGroundSegment(Func> 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 height = heightFunc.Invoke(pt, seg.height); s.p[0] = pt[0]; s.p[1] = height.Item2; diff --git a/src/DotRecast.Detour.Extras/Jumplink/ClimbTrajectory.cs b/src/DotRecast.Detour.Extras/Jumplink/ClimbTrajectory.cs index 289740b..80841cf 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/ClimbTrajectory.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/ClimbTrajectory.cs @@ -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)) }; } } diff --git a/src/DotRecast.Detour.Extras/Jumplink/Edge.cs b/src/DotRecast.Detour.Extras/Jumplink/Edge.cs index c5e95bf..8a7313a 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/Edge.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/Edge.cs @@ -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(); } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeExtractor.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeExtractor.cs index 0bbab0d..343f669 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeExtractor.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeExtractor.cs @@ -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 edges = new List(); 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++) diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs index 13d7d85..dcce58a 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs @@ -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); } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs index e8238d6..78735f8 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs @@ -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; } @@ -76,6 +77,14 @@ namespace DotRecast.Detour.Extras.Jumplink dest[1] = v1[1] + v2[1]; 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) { @@ -83,5 +92,13 @@ namespace DotRecast.Detour.Extras.Jumplink 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]; + } + } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs b/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs index e308e78..918a7d4 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs @@ -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; } diff --git a/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs b/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs index 2e505df..8836b5f 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs @@ -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; } diff --git a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs index 8e62eb9..a4f326c 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs @@ -54,11 +54,11 @@ namespace DotRecast.Detour.Extras.Jumplink List links = new List(); 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]; diff --git a/src/DotRecast.Detour.Extras/Jumplink/JumpTrajectory.cs b/src/DotRecast.Detour.Extras/Jumplink/JumpTrajectory.cs index beccc51..1e8a450 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/JumpTrajectory.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/JumpTrajectory.cs @@ -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) }; } diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index 3b15018..30ca9ba 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs @@ -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 getNavMeshHeight(NavMeshQuery navMeshQuery, float[] pt, float cs, - float heightRange) + private Tuple 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]); diff --git a/src/DotRecast.Detour.Extras/Jumplink/Trajectory.cs b/src/DotRecast.Detour.Extras/Jumplink/Trajectory.cs index dd37c4d..e6dd8bc 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/Trajectory.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/Trajectory.cs @@ -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(); } diff --git a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs index 4833bd5..1761ed9 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs @@ -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); diff --git a/src/DotRecast.Detour.TileCache/TileCache.cs b/src/DotRecast.Detour.TileCache/TileCache.cs index bdd5f46..5efc943 100644 --- a/src/DotRecast.Detour.TileCache/TileCache.cs +++ b/src/DotRecast.Detour.TileCache/TileCache.cs @@ -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 queryTiles(float[] bmin, float[] bmax) + List queryTiles(Vector3f bmin, Vector3f bmax) { List results = new List(); 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) { diff --git a/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs b/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs index 7429c59..07d5c38 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs @@ -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; diff --git a/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs b/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs index 50267ed..0f5d2f1 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs @@ -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 diff --git a/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs b/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs index 7909bb9..3e31b85 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs @@ -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 touched = new List(); public readonly List pending = new List(); diff --git a/src/DotRecast.Detour.TileCache/TileCacheParams.cs b/src/DotRecast.Detour.TileCache/TileCacheParams.cs index 229a27a..714b7a1 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheParams.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheParams.cs @@ -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; diff --git a/src/DotRecast.Detour/ClosestPointOnPolyResult.cs b/src/DotRecast.Detour/ClosestPointOnPolyResult.cs index 2d43325..1aad484 100644 --- a/src/DotRecast.Detour/ClosestPointOnPolyResult.cs +++ b/src/DotRecast.Detour/ClosestPointOnPolyResult.cs @@ -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; } diff --git a/src/DotRecast.Detour/ConvexConvexIntersection.cs b/src/DotRecast.Detour/ConvexConvexIntersection.cs index 450b69c..b375ab6 100644 --- a/src/DotRecast.Detour/ConvexConvexIntersection.cs +++ b/src/DotRecast.Detour/ConvexConvexIntersection.cs @@ -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); diff --git a/src/DotRecast.Detour/DefaultQueryFilter.cs b/src/DotRecast.Detour/DefaultQueryFilter.cs index bde3d2f..9138eb8 100644 --- a/src/DotRecast.Detour/DefaultQueryFilter.cs +++ b/src/DotRecast.Detour/DefaultQueryFilter.cs @@ -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()]; diff --git a/src/DotRecast.Detour/DefaultQueryHeuristic.cs b/src/DotRecast.Detour/DefaultQueryHeuristic.cs index 3d7ac48..3570bcc 100644 --- a/src/DotRecast.Detour/DefaultQueryHeuristic.cs +++ b/src/DotRecast.Detour/DefaultQueryHeuristic.cs @@ -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; } diff --git a/src/DotRecast.Detour/FindDistanceToWallResult.cs b/src/DotRecast.Detour/FindDistanceToWallResult.cs index e528554..040b846 100644 --- a/src/DotRecast.Detour/FindDistanceToWallResult.cs +++ b/src/DotRecast.Detour/FindDistanceToWallResult.cs @@ -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; } diff --git a/src/DotRecast.Detour/FindNearestPolyQuery.cs b/src/DotRecast.Detour/FindNearestPolyQuery.cs index 29e827e..14f2c3b 100644 --- a/src/DotRecast.Detour/FindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/FindNearestPolyQuery.cs @@ -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 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; diff --git a/src/DotRecast.Detour/FindNearestPolyResult.cs b/src/DotRecast.Detour/FindNearestPolyResult.cs index a8239a3..908c722 100644 --- a/src/DotRecast.Detour/FindNearestPolyResult.cs +++ b/src/DotRecast.Detour/FindNearestPolyResult.cs @@ -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; } diff --git a/src/DotRecast.Detour/FindRandomPointResult.cs b/src/DotRecast.Detour/FindRandomPointResult.cs index 32d1114..1f1e61b 100644 --- a/src/DotRecast.Detour/FindRandomPointResult.cs +++ b/src/DotRecast.Detour/FindRandomPointResult.cs @@ -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; } diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index ca3c075..a7011ff 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -33,18 +33,16 @@ namespace DotRecast.Detour { } - public override Result> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter, + public override Result> 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> findPath(long startRef, long endRef, float[] startPos, float[] endPos, - QueryFilter filter) + public override Result> 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>(); } @@ -144,11 +142,11 @@ namespace DotRecast.Detour // If the node is visited the first time, calculate node position. if (neighbourNode.flags == 0) { - Result 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 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 findDistanceToWall(long startRef, float[] centerPos, float maxRadius, - QueryFilter filter) + public override Result 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(); @@ -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 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)); diff --git a/src/DotRecast.Detour/MoveAlongSurfaceResult.cs b/src/DotRecast.Detour/MoveAlongSurfaceResult.cs index d4fa663..f805ede 100644 --- a/src/DotRecast.Detour/MoveAlongSurfaceResult.cs +++ b/src/DotRecast.Detour/MoveAlongSurfaceResult.cs @@ -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 visited; - public MoveAlongSurfaceResult(float[] resultPos, List visited) + public MoveAlongSurfaceResult(Vector3f resultPos, List visited) { this.resultPos = resultPos; this.visited = visited; } - public float[] getResultPos() + public Vector3f getResultPos() { return resultPos; } diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index ae15590..64306bd 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -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 queryPolygonsInTile(MeshTile tile, float[] qmin, float[] qmax) + List queryPolygonsInTile(MeshTile tile, Vector3f qmin, Vector3f qmax) { List polys = new List(); 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(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(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 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 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 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> getOffMeshConnectionPolyEndPoints(long prevRef, long polyRef) + public Result> getOffMeshConnectionPolyEndPoints(long prevRef, long polyRef) { if (polyRef == 0) { - return Results.invalidParam>("polyRef = 0"); + return Results.invalidParam>("polyRef = 0"); } // Get current polygon @@ -1515,18 +1530,18 @@ namespace DotRecast.Detour int ip = saltitip[2]; if (it >= m_maxTiles) { - return Results.invalidParam>("Invalid tile ID > max tiles"); + return Results.invalidParam>("Invalid tile ID > max tiles"); } if (m_tiles[it].salt != salt || m_tiles[it].data.header == null) { - return Results.invalidParam>("Invalid salt or missing tile header"); + return Results.invalidParam>("Invalid salt or missing tile header"); } MeshTile tile = m_tiles[it]; if (ip >= tile.data.header.polyCount) { - return Results.invalidParam>("Invalid poly ID > poly count"); + return Results.invalidParam>("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>("Invalid poly type"); + return Results.invalidParam>("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)); } diff --git a/src/DotRecast.Detour/NavMeshBuilder.cs b/src/DotRecast.Detour/NavMeshBuilder.cs index 1390216..f8227c0 100644 --- a/src/DotRecast.Detour/NavMeshBuilder.cs +++ b/src/DotRecast.Detour/NavMeshBuilder.cs @@ -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; diff --git a/src/DotRecast.Detour/NavMeshDataCreateParams.cs b/src/DotRecast.Detour/NavMeshDataCreateParams.cs index d9c4f62..dfeeda5 100644 --- a/src/DotRecast.Detour/NavMeshDataCreateParams.cs +++ b/src/DotRecast.Detour/NavMeshDataCreateParams.cs @@ -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] /// @} diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index edd5e2c..992bdab 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -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 findRandomPointAroundCircle(long startRef, float[] centerPos, float maxRadius, + public Result 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 findRandomPointWithinCircle(long startRef, float[] centerPos, float maxRadius, + public Result findRandomPointWithinCircle(long startRef, Vector3f centerPos, float maxRadius, QueryFilter filter, FRand frand) { return findRandomPointAroundCircle(startRef, centerPos, maxRadius, filter, frand, PolygonByCircleConstraint.strict()); } - public Result findRandomPointAroundCircle(long startRef, float[] centerPos, float maxRadius, + public Result 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(); @@ -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 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 closestPointOnPoly(long refs, float[] pos) + public Result closestPointOnPoly(long refs, Vector3f pos) { - if (!m_nav.isValidPolyRef(refs) || null == pos || !vIsFinite(pos)) + if (!m_nav.isValidPolyRef(refs) || !vIsFinite(pos)) { return Results.invalidParam(); } @@ -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 closestPointOnPolyBoundary(long refs, float[] pos) + public Result closestPointOnPolyBoundary(long refs, Vector3f pos) { Result> tileAndPoly = m_nav.getTileAndPolyByRef(refs); if (tileAndPoly.failed()) { - return Results.of(tileAndPoly.status, tileAndPoly.message); + return Results.of(tileAndPoly.status, tileAndPoly.message); } MeshTile tile = tileAndPoly.result.Item1; Poly poly = tileAndPoly.result.Item2; if (tile == null) { - return Results.invalidParam("Invalid tile"); + return Results.invalidParam("Invalid tile"); } - if (null == pos || !vIsFinite(pos)) + if (!vIsFinite(pos)) { - return Results.invalidParam(); + return Results.invalidParam(); } // 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 getPolyHeight(long refs, float[] pos) + public Result getPolyHeight(long refs, Vector3f pos) { Result> 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(); } @@ -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 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 findNearestPoly(float[] center, float[] halfExtents, QueryFilter filter) + public Result 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 queryTiles(float[] center, float[] halfExtents) + public IList queryTiles(Vector3f center, Vector3f halfExtents) { - if (null == center || !vIsFinite(center) || null == halfExtents || !vIsFinite(halfExtents)) + if (!vIsFinite(center) || !vIsFinite(halfExtents)) { return ImmutableArray.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> findPath(long startRef, long endRef, float[] startPos, float[] endPos, - QueryFilter filter) + public virtual Result> 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> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter, + public virtual Result> 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> findPath(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter, + public Result> 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>(); } @@ -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 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 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 straightPath, + protected Status appendVertex(Vector3f pos, int flags, long refs, List 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 path, + protected Status appendPortals(int startIdx, int endIdx, Vector3f endPos, List path, List 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> findStraightPath(float[] startPos, float[] endPos, List path, + public virtual Result> findStraightPath(Vector3f startPos, Vector3f endPos, List path, int maxStraightPath, int options) { List straightPath = new List(); - 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>(); } // TODO: Should this be callers responsibility? - Result closestStartPosRes = closestPointOnPolyBoundary(path[0], startPos); + Result closestStartPosRes = closestPointOnPolyBoundary(path[0], startPos); if (closestStartPosRes.failed()) { return Results.invalidParam>("Cannot find start position"); } - float[] closestStartPos = closestStartPosRes.result; - Result 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>("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 moveAlongSurface(long startRef, float[] startPos, float[] endPos, - QueryFilter filter) + public Result 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(); } @@ -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 getEdgeMidPoint(long from, Poly fromPoly, MeshTile fromTile, long to, + protected Result getEdgeMidPoint(long from, Poly fromPoly, MeshTile fromTile, long to, Poly toPoly, MeshTile toTile) { Result ppoints = getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, 0, 0); if (ppoints.failed()) { - return Results.of(ppoints.status, ppoints.message); + return Results.of(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 getEdgeIntersectionPoint(float[] fromPos, long from, Poly fromPoly, MeshTile fromTile, - float[] toPos, long to, Poly toPoly, MeshTile toTile) + protected Result getEdgeIntersectionPoint(Vector3f fromPos, long from, Poly fromPoly, MeshTile fromTile, + Vector3f toPos, long to, Poly toPoly, MeshTile toTile) { Result ppoints = getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, 0, 0); if (ppoints.failed()) { - return Results.of(ppoints.status, ppoints.message); + return Results.of(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 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 raycast(long startRef, float[] startPos, float[] endPos, QueryFilter filter, int options, + public Result 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(); @@ -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 findPolysAroundCircle(long startRef, float[] centerPos, float radius, - QueryFilter filter) + public Result 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(); @@ -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 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 findLocalNeighbourhood(long startRef, float[] centerPos, float radius, + public Result 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(); @@ -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 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 findDistanceToWall(long startRef, float[] centerPos, float maxRadius, + public virtual Result 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(); @@ -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 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)); diff --git a/src/DotRecast.Detour/PolygonByCircleConstraint.cs b/src/DotRecast.Detour/PolygonByCircleConstraint.cs index 828ebe8..f6a3036 100644 --- a/src/DotRecast.Detour/PolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/PolygonByCircleConstraint.cs @@ -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) { diff --git a/src/DotRecast.Detour/QueryFilter.cs b/src/DotRecast.Detour/QueryFilter.cs index 28f151f..926cdbf 100644 --- a/src/DotRecast.Detour/QueryFilter.cs +++ b/src/DotRecast.Detour/QueryFilter.cs @@ -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); } } \ No newline at end of file diff --git a/src/DotRecast.Detour/QueryHeuristic.cs b/src/DotRecast.Detour/QueryHeuristic.cs index 77b6927..f62342e 100644 --- a/src/DotRecast.Detour/QueryHeuristic.cs +++ b/src/DotRecast.Detour/QueryHeuristic.cs @@ -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); } } \ No newline at end of file diff --git a/src/DotRecast.Detour/RaycastHit.cs b/src/DotRecast.Detour/RaycastHit.cs index f56b3b0..ddcde1d 100644 --- a/src/DotRecast.Detour/RaycastHit.cs +++ b/src/DotRecast.Detour/RaycastHit.cs @@ -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 path = new List(); diff --git a/src/DotRecast.Detour/StraightPathItem.cs b/src/DotRecast.Detour/StraightPathItem.cs index c414ac1..fcc1cd1 100644 --- a/src/DotRecast.Detour/StraightPathItem.cs +++ b/src/DotRecast.Detour/StraightPathItem.cs @@ -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; } diff --git a/src/DotRecast.Recast/Heightfield.cs b/src/DotRecast.Recast/Heightfield.cs index f9ebcb0..497b07c 100644 --- a/src/DotRecast.Recast/Heightfield.cs +++ b/src/DotRecast.Recast/Heightfield.cs @@ -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; diff --git a/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs b/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs index ae8540b..fbb2d18 100644 --- a/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs @@ -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; diff --git a/src/DotRecast.Recast/RecastRasterization.cs b/src/DotRecast.Recast/RecastRasterization.cs index fe6283d..6c1d498 100644 --- a/src/DotRecast.Recast/RecastRasterization.cs +++ b/src/DotRecast.Recast/RecastRasterization.cs @@ -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(); diff --git a/src/DotRecast.Recast/RecastVoxelization.cs b/src/DotRecast.Recast/RecastVoxelization.cs index a863920..1eb869f 100644 --- a/src/DotRecast.Recast/RecastVoxelization.cs +++ b/src/DotRecast.Recast/RecastVoxelization.cs @@ -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