...

src/DotRecast.Core/RecastMath.cs

@@ -19,6 +19,7 @@ freely, subject to the following restrictions:
 */
 
 using System;
+using System.Numerics;
 
 namespace DotRecast.Core
 {
@@ -108,6 +109,16 @@ namespace DotRecast.Core
             return dest.ToArray();
         }
         
+        public static Vector3f vMad(Vector3f v1, Vector3f v2, float s)
+        {
+            Vector3f dest = new Vector3f();
+            dest[0] = v1[0] + v2[0] * s;
+            dest[1] = v1[1] + v2[1] * s;
+            dest[2] = v1[2] + v2[2] * s;
+            return dest;
+        }
+
+
         /// Performs a linear interpolation between two vectors. (@p v1 toward @p
         /// v2)
         /// @param[out] dest The result vector. [(x, y, x)]
@@ -132,13 +143,23 @@ namespace DotRecast.Core
             return dest.ToArray();
         }
         
-        public static float[] vSub(VectorPtr v1, VectorPtr v2)
+        public static Vector3f vLerp(Vector3f v1, Vector3f v2, float t)
+        {
+            Vector3f dest = new Vector3f();
+            dest[0] = v1[0] + (v2[0] - v1[0]) * t;
+            dest[1] = v1[1] + (v2[1] - v1[1]) * t;
+            dest[2] = v1[2] + (v2[2] - v1[2]) * t;
+            return dest;
+        }
+
+        
+        public static Vector3f vSub(VectorPtr v1, VectorPtr v2)
         {
             Vector3f dest = new Vector3f();
             dest[0] = v1.get(0) - v2.get(0);
             dest[1] = v1.get(1) - v2.get(1);
             dest[2] = v1.get(2) - v2.get(2);
-            return dest.ToArray();
+            return dest;
         }
 
         public static float[] vSub(float[] v1, float[] v2)
@@ -150,15 +171,35 @@ namespace DotRecast.Core
             return dest.ToArray();
         }
         
-        public static float[] vSub(Vector3f v1, Vector3f v2)
+        public static Vector3f vSub(Vector3f v1, Vector3f v2)
         {
             Vector3f dest = new Vector3f();
             dest[0] = v1[0] - v2[0];
             dest[1] = v1[1] - v2[1];
             dest[2] = v1[2] - v2[2];
-            return dest.ToArray();
+            return dest;
         }
         
+        public static Vector3f vSub(Vector3f v1, VectorPtr v2)
+        {
+            Vector3f dest = new Vector3f();
+            dest[0] = v1[0] - v2.get(0);
+            dest[1] = v1[1] - v2.get(1);
+            dest[2] = v1[2] - v2.get(2);
+            return dest;
+        }
+
+        
+        public static Vector3f vSub(Vector3f v1, float[] v2)
+        {
+            Vector3f dest = new Vector3f();
+            dest[0] = v1[0] - v2[0];
+            dest[1] = v1[1] - v2[1];
+            dest[2] = v1[2] - v2[2];
+            return dest;
+        }
+
+
 
         public static float[] vAdd(float[] v1, float[] v2)
         {
@@ -169,6 +210,16 @@ namespace DotRecast.Core
             return dest.ToArray();
         }
         
+        public static Vector3f vAdd(Vector3f v1, Vector3f v2)
+        {
+            Vector3f dest = new Vector3f();
+            dest[0] = v1[0] + v2[0];
+            dest[1] = v1[1] + v2[1];
+            dest[2] = v1[2] + v2[2];
+            return dest;
+        }
+
+
         public static float[] vCopy(float[] @in)
         {
             float[] @out = new float[3];
@@ -300,6 +351,15 @@ namespace DotRecast.Core
             return dx * dx + dy * dy + dz * dz;
         }
         
+        public static float vDistSqr(Vector3f v1, float[] v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dy = v2[1] - v1[1];
+            float dz = v2[2] - v1[2];
+            return dx * dx + dy * dy + dz * dz;
+        }
+
+        
         public static float vDistSqr(Vector3f v1, Vector3f v2)
         {
             float dx = v2[0] - v1[0];
@@ -317,11 +377,23 @@ namespace DotRecast.Core
             return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
         }
         
+        public static float vLenSqr(Vector3f v)
+        {
+            return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+        }
+
+
         public static float vLen(float[] v)
         {
             return (float)Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
         }
         
+        public static float vLen(Vector3f v)
+        {
+            return (float)Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+        }
+
+
         public static float vDist(float[] v1, float[] verts, int i)
         {
             float dx = verts[i] - v1[0];
@@ -345,6 +417,14 @@ namespace DotRecast.Core
             return (float)Math.Sqrt(dx * dx + dz * dz);
         }
         
+        public static float vDist2D(Vector3f v1, Vector3f v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dz = v2[2] - v1[2];
+            return (float)Math.Sqrt(dx * dx + dz * dz);
+        }
+
+
         public static float vDist2DSqr(float[] v1, float[] v2)
         {
             float dx = v2[0] - v1[0];
@@ -352,7 +432,15 @@ namespace DotRecast.Core
             return dx * dx + dz * dz;
         }
         
-        public static float vDist2DSqr(float[] p, float[] verts, int i)
+        public static float vDist2DSqr(Vector3f v1, Vector3f v2)
+        {
+            float dx = v2[0] - v1[0];
+            float dz = v2[2] - v1[2];
+            return dx * dx + dz * dz;
+        }
+
+
+        public static float vDist2DSqr(Vector3f p, float[] verts, int i)
         {
             float dx = verts[i] - p[0];
             float dz = verts[i + 2] - p[2];
@@ -396,12 +484,25 @@ namespace DotRecast.Core
             return vEqual(p0, p1, EQUAL_THRESHOLD);
         }
         
+        public static bool vEqual(Vector3f p0, Vector3f p1)
+        {
+            return vEqual(p0, p1, EQUAL_THRESHOLD);
+        }
+
+
         public static bool vEqual(float[] p0, float[] p1, float thresholdSqr)
         {
             float d = vDistSqr(p0, p1);
             return d < thresholdSqr;
         }
         
+        public static bool vEqual(Vector3f p0, Vector3f p1, float thresholdSqr)
+        {
+            float d = vDistSqr(p0, p1);
+            return d < thresholdSqr;
+        }
+
+
         /// Derives the dot product of two vectors on the xz-plane. (@p u . @p v)
         /// @param[in] u A vector [(x, y, z)]
         /// @param[in] v A vector [(x, y, z)]
@@ -414,6 +515,12 @@ namespace DotRecast.Core
             return u[0] * v[0] + u[2] * v[2];
         }
         
+        public static float vDot2D(Vector3f u, Vector3f v)
+        {
+            return u[0] * v[0] + u[2] * v[2];
+        }
+
+
         public static float vDot2D(float[] u, float[] v, int vi)
         {
             return u[0] * v[vi] + u[2] * v[vi + 2];
@@ -431,6 +538,12 @@ namespace DotRecast.Core
             return u[2] * v[0] - u[0] * v[2];
         }
         
+        public static float vPerp2D(Vector3f u, Vector3f v)
+        {
+            return u[2] * v[0] - u[0] * v[2];
+        }
+        
+
         /// @}
         /// @name Computational geometry helper functions.
         /// @{
@@ -500,7 +613,16 @@ namespace DotRecast.Core
             return overlap;
         }
         
-        public static Tuple<float, float> distancePtSegSqr2D(float[] pt, float[] p, float[] q)
+        public static bool overlapBounds(Vector3f amin, Vector3f amax, Vector3f bmin, Vector3f bmax)
+        {
+            bool overlap = true;
+            overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
+            overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
+            overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
+            return overlap;
+        }
+
+        public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q)
         {
             float pqx = q[0] - p[0];
             float pqz = q[2] - p[2];
@@ -527,11 +649,11 @@ namespace DotRecast.Core
             return Tuple.Create(dx * dx + dz * dz, t);
         }
 
-        public static float? closestHeightPointTriangle(float[] p, float[] a, float[] b, float[] c)
+        public static float? closestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c)
         {
-            float[] v0 = vSub(c, a);
+            Vector3f v0 = vSub(c, a);
-            float[] v1 = vSub(b, a);
+            Vector3f v1 = vSub(b, a);
-            float[] v2 = vSub(p, 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],
+                x = a * pts[pa] + b * pts[pb] + c * pts[pc],
-                a * pts[pa + 1] + b * pts[pb + 1] + c * pts[pc + 1],
+                y = a * pts[pa + 1] + b * pts[pb + 1] + c * pts[pc + 1],
-                a * pts[pa + 2] + b * pts[pb + 2] + c * pts[pc + 2]
+                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);
+                var edge = vSub(new VectorPtr(verts, i * 3), vpj);
-                float[] diff = vSub(p0v, vpj);
+                var diff = vSub(p0v, vpj);
                 float n = vPerp2D(edge, diff);
                 float d = vPerp2D(dir, edge);
                 if (Math.Abs(d) < EPS)
@@ -815,7 +937,7 @@ namespace DotRecast.Core
             return result;
         }
 
-        public static Tuple<float, float> distancePtSegSqr2D(float[] pt, float[] verts, int p, int q)
+        public static Tuple<float, float> distancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q)
         {
             float pqx = verts[q + 0] - verts[p + 0];
             float pqz = verts[q + 2] - verts[p + 2];
@@ -852,6 +974,12 @@ namespace DotRecast.Core
             return a[0] * b[2] - a[2] * b[0];
         }
         
+        public static float vperpXZ(Vector3f a, Vector3f b)
+        {
+            return a[0] * b[2] - a[2] * b[0];
+        }
+
+
         public static Tuple<float, float>? intersectSegSeg2D(float[] ap, float[] aq, float[] bp, float[] bq)
         {
             float[] u = vSub(aq, ap);
@@ -870,9 +998,9 @@ namespace DotRecast.Core
         
         public static Tuple<float, float>? intersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq)
         {
-            float[] u = vSub(aq, ap);
+            Vector3f u = vSub(aq, ap);
-            float[] v = vSub(bq, bp);
+            Vector3f v = vSub(bq, bp);
-            float[] w = vSub(ap, bp);
+            Vector3f w = vSub(ap, bp);
             float d = vperpXZ(u, v);
             if (Math.Abs(d) < 1e-6f)
             {
@@ -894,6 +1022,16 @@ namespace DotRecast.Core
             return @out;
         }
         
+        public static Vector3f vScale(Vector3f @in, float scale)
+        {
+            var @out = new Vector3f();
+            @out[0] = @in[0] * scale;
+            @out[1] = @in[1] * scale;
+            @out[2] = @in[2] * scale;
+            return @out;
+        }
+
+
         /// Checks that the specified vector's components are all finite.
         /// @param[in] v A point. [(x, y, z)]
         /// @return True if all of the point's components are finite, i.e. not NaN
@@ -903,11 +1041,21 @@ namespace DotRecast.Core
             return float.IsFinite(v[0]) && float.IsFinite(v[1]) && float.IsFinite(v[2]);
         }
         
+        public static bool vIsFinite(Vector3f v)
+        {
+            return float.IsFinite(v[0]) && float.IsFinite(v[1]) && float.IsFinite(v[2]);
+        }
+
         /// Checks that the specified vector's 2D components are finite.
         /// @param[in] v A point. [(x, y, z)]
         public static bool vIsFinite2D(float[] v)
         {
             return float.IsFinite(v[0]) && float.IsFinite(v[2]);
         }
+
+        public static bool vIsFinite2D(Vector3f v)
+        {
+            return float.IsFinite(v[0]) && float.IsFinite(v[2]);
+        }
     }
 }

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

src/DotRecast.Detour.Crowd/Crowd.cs

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

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();
+                var p0 = corners[ip0].getPos();
-                float[] p1 = corners[ip1].getPos();
+                var p1 = corners[ip1].getPos();
 
-                float[] dir0 = vSub(p0, npos);
+                var dir0 = vSub(p0, npos);
-                float[] dir1 = vSub(p1, 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)
             {

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

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<FindLocalNeighbourhoodResult> res = navquery.findLocalNeighbourhood(refs, pos, collisionQueryRange,
                 filter);
@@ -153,7 +154,7 @@ namespace DotRecast.Detour.Crowd
             return true;
         }
 
-        public float[] getCenter()
+        public Vector3f getCenter()
         {
             return m_center;
         }

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(ref cir.vel, vel);
-            vCopy(cir.dvel, dvel);
+            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(ref seg.p, p);
-            vCopy(seg.q, q);
+            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;
+                Vector3f pa = pos;
-                float[] pb = cir.p;
+                Vector3f pb = cir.p;
 
-                float[] orig = { 0f, 0f, 0f };
+                Vector3f orig = new Vector3f();
                 Vector3f dv = new Vector3f();
-                vCopy(cir.dp, vSub(pb, pa));
+                vCopy(ref cir.dp, vSub(pb, pa));
-                vNormalize(cir.dp);
+                vNormalize(ref cir.dp);
                 dv = vSub(cir.dvel, dvel);
 
                 float a = triArea2D(orig, cir.dp, dv);

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

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

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;

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

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

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)
             {

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 readonly Vector3f boundsMin;
-        public float[] boundsMax;
+        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;

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<int, int, Heightfield> heightfieldProvider;
 
-        public VoxelQuery(float[] origin, float tileWidth, float tileDepth, Func<int, int, Heightfield> heightfieldProvider)
+        public VoxelQuery(Vector3f origin, float tileWidth, float tileDepth, Func<int, int, Heightfield> heightfieldProvider)
         {
             this.origin = origin;
             this.tileWidth = tileWidth;

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(ref bmin, data.verts, data.polys[i].verts[0] * 3);
-                vCopy(bmax, 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);
+                    vMin(ref bmin, data.verts, data.polys[i].verts[j] * 3);
-                    vMax(bmax, 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);

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<float[], float, Tuple<bool, float>> heightFunc)
+            Func<Vector3f, float, Tuple<bool, float>> heightFunc)
         {
             float cs = acfg.cellSize;
             float dist = (float)Math.Sqrt(vDist2DSqr(es.start.p, es.start.q));
@@ -21,7 +22,7 @@ namespace DotRecast.Detour.Extras.Jumplink
 
         public abstract void sample(JumpLinkBuilderConfig acfg, RecastBuilderResult result, EdgeSampler es);
 
-        protected void sampleGroundSegment(Func<float[], float, Tuple<bool, float>> heightFunc, GroundSegment seg, int nsamples)
+        protected void sampleGroundSegment(Func<Vector3f, float, Tuple<bool, float>> heightFunc, GroundSegment seg, int nsamples)
         {
             seg.gsamples = new GroundSample[nsamples];
 
@@ -31,7 +32,7 @@ namespace DotRecast.Detour.Extras.Jumplink
 
                 GroundSample s = new GroundSample();
                 seg.gsamples[i] = s;
-                float[] pt = vLerp(seg.p, seg.q, u);
+                Vector3f pt = vLerp(seg.p, seg.q, u);
                 Tuple<bool, float> height = heightFunc.Invoke(pt, seg.height);
                 s.p[0] = pt[0];
                 s.p[1] = height.Item2;

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)),
+                x = lerp(start[0], end[0], Math.Min(2f * u, 1f)),
-                lerp(start[1], end[1], Math.Max(0f, 2f * u - 1f)),
+                y = lerp(start[1], end[1], Math.Max(0f, 2f * u - 1f)),
-                lerp(start[2], end[2], Math.Min(2f * u, 1f))
+                z = lerp(start[2], end[2], Math.Min(2f * u, 1f))
             };
         }
     }

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 Vector3f sp = new Vector3f();
-        public readonly Vector3f sq = new Vector3f();
+        public Vector3f sq = new Vector3f();
     }
 }

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<Edge> edges = new List<Edge>();
             if (mesh != null)
             {
-                float[] orig = mesh.bmin;
+                Vector3f orig = mesh.bmin;
                 float cs = mesh.cs;
                 float ch = mesh.ch;
                 for (int i = 0; i < mesh.npolys; i++)

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));
+            vCopy(ref ax, vSub(edge.sq, edge.sp));
-            vNormalize(ax);
+            vNormalize(ref ax);
-            vSet(az, ax[2], 0, -ax[0]);
+            vSet(ref az, ax[2], 0, -ax[0]);
-            vNormalize(az);
+            vNormalize(ref az);
-            vSet(ay, 0, 1, 0);
+            vSet(ref ay, 0, 1, 0);
         }
     }
 }

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 });
+            trans2d(ref offset, es.az, es.ay, new Vector2f { x = acfg.startDistance, y = -acfg.agentClimb, });
-            vadd(es.start.p, edge.sp, offset);
+            vadd(ref es.start.p, edge.sp, offset);
-            vadd(es.start.q, edge.sq, 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(ref end.p, edge.sp, offset);
-                vadd(end.q, edge.sq, 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 });
+            trans2d(ref offset, es.az, es.ay, new Vector2f() { x = acfg.startDistance, y = -acfg.agentClimb });
-            vadd(es.start.p, edge.sp, offset);
+            vadd(ref es.start.p, edge.sp, offset);
-            vadd(es.start.q, edge.sq, 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(ref end.p, edge.sp, offset);
-            vadd(end.q, edge.sq, offset);
+            vadd(ref end.q, edge.sq, offset);
             es.end.Add(end);
             return es;
         }
@@ -77,11 +78,27 @@ namespace DotRecast.Detour.Extras.Jumplink
             dest[2] = v1[2] + v2[2];
         }
         
+        private void vadd(ref Vector3f dest, Vector3f v1, Vector3f v2)
+        {
+            dest[0] = v1[0] + v2[0];
+            dest[1] = v1[1] + v2[1];
+            dest[2] = v1[2] + v2[2];
+        }
+
+
         private void trans2d(float[] dst, float[] ax, float[] ay, float[] pt)
         {
             dst[0] = ax[0] * pt[0] + ay[0] * pt[1];
             dst[1] = ax[1] * pt[0] + ay[1] * pt[1];
             dst[2] = ax[2] * pt[0] + ay[2] * pt[1];
         }
+        
+        private void trans2d(ref Vector3f dst, Vector3f ax, Vector3f ay, Vector2f pt)
+        {
+            dst[0] = ax[0] * pt[0] + ay[0] * pt[1];
+            dst[1] = ax[1] * pt[0] + ay[1] * pt[1];
+            dst[2] = ax[2] * pt[0] + ay[2] * pt[1];
+        }
+
     }
 }

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

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 Vector3f p = new Vector3f();
-        public readonly Vector3f q = new Vector3f();
+        public Vector3f q = new Vector3f();
         public GroundSample[] gsamples;
         public float height;
     }

src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs

@@ -54,11 +54,11 @@ namespace DotRecast.Detour.Extras.Jumplink
             List<JumpLink> links = new List<JumpLink>();
             foreach (JumpSegment js in jumpSegments)
             {
-                float[] sp = es.start.gsamples[js.startSample].p;
+                Vector3f sp = es.start.gsamples[js.startSample].p;
-                float[] sq = es.start.gsamples[js.startSample + js.samples - 1].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;
+                Vector3f ep = end.gsamples[js.startSample].p;
-                float[] eq = end.gsamples[js.startSample + js.samples - 1].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];

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),
+                x = lerp(start[0], end[0], u), 
-                lerp(start[2], end[2], u)
+                y = interpolateHeight(start[1], end[1], u),
+                z = lerp(start[2], end[2], u)
             };
         }
 

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<bool, float> getNavMeshHeight(NavMeshQuery navMeshQuery, float[] pt, float cs,
+        private Tuple<bool, float> getNavMeshHeight(NavMeshQuery navMeshQuery, Vector3f pt, float cs, float heightRange)
-            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]);

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

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

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(ref ob.bmin, bmin);
-            vCopy(ob.bmax, bmax);
+            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(ref ob.center, center);
-            vCopy(ob.extents, extents);
+            vCopy(ref ob.extents, extents);
             float coshalf = (float)Math.Cos(0.5f * yRadians);
             float sinhalf = (float)Math.Sin(-0.5f * yRadians);
             ob.rotAux[0] = coshalf * sinhalf;
@@ -438,7 +438,7 @@ namespace DotRecast.Detour.TileCache
             return o;
         }
 
-        List<long> queryTiles(float[] bmin, float[] bmax)
+        List<long> queryTiles(Vector3f bmin, Vector3f bmax)
         {
             List<long> results = new List<long>();
             float tw = m_params.width * m_params.cs;
@@ -616,8 +616,7 @@ namespace DotRecast.Detour.TileCache
                 {
                     if (ob.type == TileCacheObstacle.TileCacheObstacleType.CYLINDER)
                     {
-                        builder.markCylinderArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.pos, ob.radius,
+                        builder.markCylinderArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.pos, ob.radius, ob.height, 0);
-                            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,
+                        builder.markBoxArea(layer, tile.header.bmin, m_params.cs, m_params.ch, ob.center, ob.extents, ob.rotAux, 0);
-                            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(ref bmin, ob.bmin);
-                vCopy(bmax, ob.bmax);
+                vCopy(ref bmax, ob.bmax);
             }
             else if (ob.type == TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX)
             {

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,
+        public void markCylinderArea(TileCacheLayer layer, Vector3f orig, float cs, float ch, Vector3f pos, float radius, float height, int areaId)
-            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,
+        public void markBoxArea(TileCacheLayer layer, Vector3f orig, float cs, float ch, Vector3f bmin, Vector3f bmax, int areaId)
-            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;

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

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 Vector3f pos = new Vector3f();
-        public readonly Vector3f bmin = new Vector3f();
+        public Vector3f bmin = new Vector3f();
-        public readonly Vector3f bmax = new Vector3f();
+        public Vector3f bmax = new Vector3f();
         public float radius, height;
-        public readonly Vector3f center = new Vector3f();
+        public Vector3f center = new Vector3f();
-        public readonly Vector3f extents = new Vector3f();
+        public Vector3f extents = new Vector3f();
         public readonly float[] rotAux = new float[2]; // { cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 }
         public List<long> touched = new List<long>();
         public readonly List<long> pending = new List<long>();

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;

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

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);
+                Vector3f A = vSub(a, a1);
-                float[] B = vSub(b, b1);
+                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);

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()];

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

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 Vector3f position;
-        private readonly float[] normal;
+        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;
         }

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<ClosestPointOnPolyResult> closest = query.closestPointOnPoly(refs, center);
             bool posOverPoly = closest.result.isPosOverPoly();
-            float[] closestPtPoly = closest.result.getClosest();
+            var closestPtPoly = closest.result.getClosest();
 
             // If a point is directly over a polygon and closer than
             // climb height, favor that instead of straight line nearest point.
             float d = 0;
-            float[] diff = vSub(center, closestPtPoly);
+            Vector3f diff = vSub(center, closestPtPoly);
             if (posOverPoly)
             {
                 d = Math.Abs(diff[1]) - tile.data.header.walkableClimb;

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

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

src/DotRecast.Detour/LegacyNavMeshQuery.cs

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

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<long> visited;
 
-        public MoveAlongSurfaceResult(float[] resultPos, List<long> visited)
+        public MoveAlongSurfaceResult(Vector3f resultPos, List<long> visited)
         {
             this.resultPos = resultPos;
             this.visited = visited;
         }
 
-        public float[] getResultPos()
+        public Vector3f getResultPos()
         {
             return resultPos;
         }

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

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(ref bmin, option.bmin);
-                vCopy(bmax, option.bmax);
+                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(ref header.bmin, option.bmin);
-            vCopy(header.bmax, option.bmax);
+            vCopy(ref header.bmax, option.bmax);
             header.detailMeshCount = option.polyCount;
             header.detailVertCount = uniqueDetailVertCount;
             header.detailTriCount = detailTriCount;

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]
         /// @}

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

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)
                 {

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

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

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<long> path = new List<long>();

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

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;

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;

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,
+        private static void rasterizeTri(float[] verts, int v0, int v1, int v2, int area, Heightfield hf, Vector3f hfBBMin,
-            float[] hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold)
+            Vector3f hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold)
         {
             Vector3f tmin = new Vector3f();
             Vector3f tmax = new Vector3f();

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