float[] => vector3f

src/DotRecast.Core/RecastMath.cs

@@ -45,16 +45,16 @@ namespace DotRecast.Core
         }
 
 
-        public static float[] vCross(float[] v1, float[] v2)
+        public static Vector3f vCross(Vector3f v1, Vector3f v2)
         {
             Vector3f dest = new Vector3f();
             dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
             dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
             dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
-            return dest.ToArray();
+            return dest;
         }
         
-        public static float vDot(float[] v1, float[] v2)
+        public static float vDot(Vector3f v1, Vector3f v2)
         {
             return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
         }

src/DotRecast.Detour.Dynamic/Colliders/BoxCollider.cs

@@ -17,23 +17,24 @@ freely, subject to the following restrictions:
 */
 
 using System;
+using DotRecast.Core;
 using DotRecast.Recast;
 
 namespace DotRecast.Detour.Dynamic.Colliders
 {
     public class BoxCollider : AbstractCollider
     {
-        private readonly float[] center;
+        private readonly Vector3f center;
         private readonly float[][] halfEdges;
 
-        public BoxCollider(float[] center, float[][] halfEdges, int area, float flagMergeThreshold) :
+        public BoxCollider(Vector3f center, float[][] halfEdges, int area, float flagMergeThreshold) :
             base(area, flagMergeThreshold, bounds(center, halfEdges))
         {
             this.center = center;
             this.halfEdges = halfEdges;
         }
 
-        private static float[] bounds(float[] center, float[][] halfEdges)
+        private static float[] bounds(Vector3f center, float[][] halfEdges)
         {
             float[] bounds = new float[]
             {
@@ -65,14 +66,19 @@ namespace DotRecast.Detour.Dynamic.Colliders
                 telemetry);
         }
 
-        public static float[][] getHalfEdges(float[] up, float[] forward, float[] extent)
+        public static Vector3f[] getHalfEdges(Vector3f up, Vector3f forward, float[] extent)
         {
-            float[][] halfEdges = new float[][] { new float[3], new float[] { up[0], up[1], up[2] }, new float[3] };
+            Vector3f[] halfEdges =
-            RecastVectors.normalize(halfEdges[1]);
+            {
-            RecastVectors.cross(halfEdges[0], up, forward);
+                Vector3f.Zero,
-            RecastVectors.normalize(halfEdges[0]);
+                Vector3f.Of(up[0], up[1], up[2]),
-            RecastVectors.cross(halfEdges[2], halfEdges[0], up);
+                Vector3f.Zero
-            RecastVectors.normalize(halfEdges[2]);
+            };
+            RecastVectors.normalize(ref halfEdges[1]);
+            RecastVectors.cross(ref halfEdges[0], up, forward);
+            RecastVectors.normalize(ref halfEdges[0]);
+            RecastVectors.cross(ref halfEdges[2], halfEdges[0], up);
+            RecastVectors.normalize(ref halfEdges[2]);
             halfEdges[0][0] *= extent[0];
             halfEdges[0][1] *= extent[0];
             halfEdges[0][2] *= extent[0];

src/DotRecast.Recast.Demo/Draw/GLU.cs

@@ -67,7 +67,7 @@ public class GLU
         matrix[15] = 0.0f;
     }
 
-    public static int glhUnProjectf(float winx, float winy, float winz, float[] modelview, float[] projection, int[] viewport, float[] objectCoordinate)
+    public static int glhUnProjectf(float winx, float winy, float winz, float[] modelview, float[] projection, int[] viewport, ref Vector3f objectCoordinate)
     {
         // Transformation matrices
         float[] m = new float[16], A = new float[16];

src/DotRecast.Recast.Demo/Draw/NavMeshRenderer.cs

@@ -20,6 +20,7 @@ freely, subject to the following restrictions:
 
 using System.Collections.Generic;
 using System.Linq;
+using DotRecast.Core;
 using DotRecast.Detour;
 using DotRecast.Recast.Demo.Builder;
 using DotRecast.Recast.Demo.Geom;
@@ -200,8 +201,8 @@ public class NavMeshRenderer
     private void drawGeomBounds(DemoInputGeomProvider geom)
     {
         // Draw bounds
-        float[] bmin = geom.getMeshBoundsMin();
+        Vector3f bmin = geom.getMeshBoundsMin();
-        float[] bmax = geom.getMeshBoundsMax();
+        Vector3f bmax = geom.getMeshBoundsMax();
         debugDraw.debugDrawBoxWire(bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2],
             DebugDraw.duRGBA(255, 255, 255, 128), 1.0f);
         debugDraw.begin(DebugDrawPrimitives.POINTS, 5.0f);

src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs

@@ -71,25 +71,25 @@ public class DemoInputGeomProvider : InputGeomProvider
         this.faces = faces;
         normals = new float[faces.Length];
         calculateNormals();
-        bmin = new float[3];
+        bmin = Vector3f.Zero;
-        bmax = new float[3];
+        bmax = Vector3f.Zero;
-        RecastVectors.copy(bmin, vertices, 0);
+        RecastVectors.copy(ref bmin, vertices, 0);
-        RecastVectors.copy(bmax, vertices, 0);
+        RecastVectors.copy(ref bmax, vertices, 0);
         for (int i = 1; i < vertices.Length / 3; i++)
         {
-            RecastVectors.min(bmin, vertices, i * 3);
+            RecastVectors.min(ref bmin, vertices, i * 3);
-            RecastVectors.max(bmax, vertices, i * 3);
+            RecastVectors.max(ref bmax, vertices, i * 3);
         }
 
         chunkyTriMesh = new ChunkyTriMesh(vertices, faces, faces.Length / 3, 256);
     }
 
-    public float[] getMeshBoundsMin()
+    public Vector3f getMeshBoundsMin()
     {
         return bmin;
     }
 
-    public float[] getMeshBoundsMax()
+    public Vector3f getMeshBoundsMax()
     {
         return bmax;
     }
@@ -138,7 +138,7 @@ public class DemoInputGeomProvider : InputGeomProvider
         return offMeshConnections;
     }
 
-    public void addOffMeshConnection(float[] start, float[] end, float radius, bool bidir, int area, int flags)
+    public void addOffMeshConnection(Vector3f start, Vector3f end, float radius, bool bidir, int area, int flags)
     {
         offMeshConnections.Add(new DemoOffMeshConnection(start, end, radius, bidir, area, flags));
     }
@@ -149,7 +149,7 @@ public class DemoInputGeomProvider : InputGeomProvider
         offMeshConnections.RemoveAll(filter); // TODO : 확인 필요
     }
 
-    public float? raycastMesh(float[] src, Vector3f dst)
+    public float? raycastMesh(Vector3f src, Vector3f dst)
     {
         // Prune hit ray.
         float[] btminmax = Intersections.intersectSegmentAABB(src, dst, bmin, bmax);
@@ -179,21 +179,21 @@ public class DemoInputGeomProvider : InputGeomProvider
             int[] tris = chunk.tris;
             for (int j = 0; j < chunk.tris.Length; j += 3)
             {
-                float[] v1 = new float[]
+                Vector3f v1 = Vector3f.Of(
-                {
+                    vertices[tris[j] * 3],
-                    vertices[tris[j] * 3], vertices[tris[j] * 3 + 1],
+                    vertices[tris[j] * 3 + 1],
                     vertices[tris[j] * 3 + 2]
-                };
+                );
-                float[] v2 = new float[]
+                Vector3f v2 = Vector3f.Of(
-                {
+                    vertices[tris[j + 1] * 3], 
-                    vertices[tris[j + 1] * 3], vertices[tris[j + 1] * 3 + 1],
+                    vertices[tris[j + 1] * 3 + 1],
                     vertices[tris[j + 1] * 3 + 2]
-                };
+                );
-                float[] v3 = new float[]
+                Vector3f v3 = Vector3f.Of(
-                {
+                    vertices[tris[j + 2] * 3], 
-                    vertices[tris[j + 2] * 3], vertices[tris[j + 2] * 3 + 1],
+                    vertices[tris[j + 2] * 3 + 1],
                     vertices[tris[j + 2] * 3 + 2]
-                };
+                );
                 float? t = Intersections.intersectSegmentTriangle(src, dst, v1, v2, v3);
                 if (null != t)
                 {

src/DotRecast.Recast.Demo/Geom/DemoOffMeshConnection.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.Demo.Geom;
 
 public class DemoOffMeshConnection
@@ -28,7 +30,7 @@ public class DemoOffMeshConnection
     public readonly int area;
     public readonly int flags;
 
-    public DemoOffMeshConnection(float[] start, float[] end, float radius, bool bidir, int area, int flags)
+    public DemoOffMeshConnection(Vector3f start, Vector3f end, float radius, bool bidir, int area, int flags)
     {
         verts = new float[6];
         verts[0] = start[0];

src/DotRecast.Recast.Demo/Geom/Intersections.cs

@@ -24,20 +24,20 @@ namespace DotRecast.Recast.Demo.Geom;
 
 public class Intersections
 {
-    public static float? intersectSegmentTriangle(float[] sp, float[] sq, float[] a, float[] b, float[] c)
+    public static float? intersectSegmentTriangle(Vector3f sp, Vector3f sq, Vector3f a, Vector3f b, Vector3f c)
     {
         float v, w;
-        float[] ab = vSub(b, a);
+        Vector3f ab = vSub(b, a);
-        float[] ac = vSub(c, a);
+        Vector3f ac = vSub(c, a);
-        float[] qp = vSub(sp, sq);
+        Vector3f qp = vSub(sp, sq);
 
         // Compute triangle normal. Can be precalculated or cached if
         // intersecting multiple segments against the same triangle
-        float[] norm = RecastMath.vCross(ab, ac);
+        Vector3f norm = vCross(ab, ac);
 
         // Compute denominator d. If d <= 0, segment is parallel to or points
         // away from triangle, so exit early
-        float d = RecastMath.vDot(qp, norm);
+        float d = vDot(qp, norm);
         if (d <= 0.0f)
         {
             return null;
@@ -46,8 +46,8 @@ public class Intersections
         // Compute intersection t value of pq with plane of triangle. A ray
         // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
         // dividing by d until intersection has been found to pierce triangle
-        float[] ap = vSub(sp, a);
+        Vector3f ap = vSub(sp, a);
-        float t = RecastMath.vDot(ap, norm);
+        float t = vDot(ap, norm);
         if (t < 0.0f)
         {
             return null;
@@ -59,14 +59,14 @@ public class Intersections
         }
 
         // Compute barycentric coordinate components and test if within bounds
-        float[] e = RecastMath.vCross(qp, ap);
+        Vector3f e = vCross(qp, ap);
-        v = RecastMath.vDot(ac, e);
+        v = vDot(ac, e);
         if (v < 0.0f || v > d)
         {
             return null;
         }
 
-        w = -RecastMath.vDot(ab, e);
+        w = -vDot(ab, e);
         if (w < 0.0f || v + w > d)
         {
             return null;
@@ -78,7 +78,7 @@ public class Intersections
         return t;
     }
 
-    public static float[] intersectSegmentAABB(float[] sp, Vector3f sq, Vector3f amin, Vector3f amax)
+    public static float[] intersectSegmentAABB(Vector3f sp, Vector3f sq, Vector3f amin, Vector3f amax)
     {
         float EPS = 1e-6f;
 

src/DotRecast.Recast.Demo/Geom/NavMeshRaycast.cs

@@ -26,7 +26,7 @@ namespace DotRecast.Recast.Demo.Geom;
  */
 public class NavMeshRaycast
 {
-    public static float? raycast(NavMesh mesh, float[] src, float[] dst)
+    public static float? raycast(NavMesh mesh, Vector3f src, Vector3f dst)
     {
         for (int t = 0; t < mesh.getMaxTiles(); ++t)
         {
@@ -44,7 +44,7 @@ public class NavMeshRaycast
         return null;
     }
 
-    private static float? raycast(MeshTile tile, float[] sp, float[] sq)
+    private static float? raycast(MeshTile tile, Vector3f sp, Vector3f sq)
     {
         for (int i = 0; i < tile.data.header.polyCount; ++i)
         {
@@ -58,7 +58,7 @@ public class NavMeshRaycast
 
             if (pd != null)
             {
-                float[][] verts = ArrayUtils.Of<float>(3, 3);
+                Vector3f[] verts = new Vector3f[3];
                 for (int j = 0; j < pd.triCount; ++j)
                 {
                     int t = (pd.triBase + j) * 4;

src/DotRecast.Recast.Demo/Geom/NavMeshUtils.cs

@@ -17,16 +17,17 @@ freely, subject to the following restrictions:
 */
 
 using System;
+using DotRecast.Core;
 using DotRecast.Detour;
 
 namespace DotRecast.Recast.Demo.Geom;
 
 public class NavMeshUtils
 {
-    public static float[][] getNavMeshBounds(NavMesh mesh)
+    public static Vector3f[] getNavMeshBounds(NavMesh mesh)
     {
-        float[] bmin = new float[] { float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity };
+        Vector3f bmin = Vector3f.Of(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
-        float[] bmax = new float[] { float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity };
+        Vector3f bmax = Vector3f.Of(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
         for (int t = 0; t < mesh.getMaxTiles(); ++t)
         {
             MeshTile tile = mesh.getTile(t);
@@ -44,6 +45,6 @@ public class NavMeshUtils
             }
         }
 
-        return new float[][] { bmin, bmax };
+        return new[] { bmin, bmax };
     }
 }

src/DotRecast.Recast.Demo/Geom/PolyMeshRaycast.cs

@@ -23,7 +23,7 @@ namespace DotRecast.Recast.Demo.Geom;
 
 public class PolyMeshRaycast
 {
-    public static float? raycast(IList<RecastBuilderResult> results, float[] src, float[] dst)
+    public static float? raycast(IList<RecastBuilderResult> results, Vector3f src, Vector3f dst)
     {
         foreach (RecastBuilderResult result in results)
         {
@@ -40,7 +40,7 @@ public class PolyMeshRaycast
         return null;
     }
 
-    private static float? raycast(PolyMesh poly, PolyMeshDetail meshDetail, float[] sp, float[] sq)
+    private static float? raycast(PolyMesh poly, PolyMeshDetail meshDetail, Vector3f sp, Vector3f sq)
     {
         if (meshDetail != null)
         {
@@ -54,7 +54,7 @@ public class PolyMeshRaycast
                 int tris = btris * 4;
                 for (int j = 0; j < ntris; ++j)
                 {
-                    float[][] vs = ArrayUtils.Of<float>(3, 3);
+                    Vector3f[] vs = new Vector3f[3];
                     for (int k = 0; k < 3; ++k)
                     {
                         vs[k][0] = meshDetail.verts[verts + meshDetail.tris[tris + j * 4 + k] * 3];

src/DotRecast.Recast.Demo/RecastDemo.cs

@@ -96,8 +96,8 @@ public class RecastDemo
     private readonly float[] cameraEulers = { 45, -45 };
     private Vector3f cameraPos = Vector3f.Of(0, 0, 0);
 
-    private readonly Vector3f rayStart = new Vector3f();
+    private Vector3f rayStart = new Vector3f();
-    private readonly Vector3f rayEnd = new Vector3f();
+    private Vector3f rayEnd = new Vector3f();
 
     private float[] projectionMatrix = new float[16];
     private float[] modelviewMatrix = new float[16];
@@ -112,7 +112,7 @@ public class RecastDemo
 
     private int[] viewport;
     private bool markerPositionSet;
-    private readonly Vector3f markerPosition = new Vector3f();
+    private Vector3f markerPosition = new Vector3f();
     private ToolsView toolsUI;
     private RcSettingsView settingsUI;
     private long prevFrameTime;
@@ -479,8 +479,8 @@ public class RecastDemo
        */
         if (sample.getInputGeom() != null)
         {
-            float[] bmin = sample.getInputGeom().getMeshBoundsMin();
+            Vector3f bmin = sample.getInputGeom().getMeshBoundsMin();
-            float[] bmax = sample.getInputGeom().getMeshBoundsMax();
+            Vector3f bmax = sample.getInputGeom().getMeshBoundsMax();
             int[] voxels = Recast.calcGridSize(bmin, bmax, settingsUI.getCellSize());
             settingsUI.setVoxels(voxels);
             settingsUI.setTiles(tileNavMeshBuilder.getTiles(sample.getInputGeom(), settingsUI.getCellSize(), settingsUI.getTileSize()));
@@ -612,8 +612,8 @@ public class RecastDemo
 
         if (!_mouseOverMenu)
         {
-            GLU.glhUnProjectf(mousePos[0], viewport[3] - 1 - mousePos[1], 0.0f, modelviewMatrix, projectionMatrix, viewport, rayStart);
+            GLU.glhUnProjectf(mousePos[0], viewport[3] - 1 - mousePos[1], 0.0f, modelviewMatrix, projectionMatrix, viewport, ref rayStart);
-            GLU.glhUnProjectf(mousePos[0], viewport[3] - 1 - mousePos[1], 1.0f, modelviewMatrix, projectionMatrix, viewport, rayEnd);
+            GLU.glhUnProjectf(mousePos[0], viewport[3] - 1 - mousePos[1], 1.0f, modelviewMatrix, projectionMatrix, viewport, ref rayEnd);
 
             // Hit test mesh.
             DemoInputGeomProvider inputGeom = sample.getInputGeom();
@@ -681,18 +681,18 @@ public class RecastDemo
 
         if (sample.isChanged())
         {
-            float[] bmin = null;
+            Vector3f? bminN = null;
-            float[] bmax = null;
+            Vector3f? bmaxN = null;
             if (sample.getInputGeom() != null)
             {
-                bmin = sample.getInputGeom().getMeshBoundsMin();
+                bminN = sample.getInputGeom().getMeshBoundsMin();
-                bmax = sample.getInputGeom().getMeshBoundsMax();
+                bmaxN = sample.getInputGeom().getMeshBoundsMax();
             }
             else if (sample.getNavMesh() != null)
             {
-                float[][] bounds = NavMeshUtils.getNavMeshBounds(sample.getNavMesh());
+                Vector3f[] bounds = NavMeshUtils.getNavMeshBounds(sample.getNavMesh());
-                bmin = bounds[0];
+                bminN = bounds[0];
-                bmax = bounds[1];
+                bmaxN = bounds[1];
             }
             else if (0 < sample.getRecastResults().Count)
             {
@@ -700,23 +700,32 @@ public class RecastDemo
                 {
                     if (result.getSolidHeightfield() != null)
                     {
-                        if (bmin == null)
+                        if (bminN == null)
                         {
-                            bmin = new float[] { float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity };
+                            bminN = Vector3f.Of(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
-                            bmax = new float[] { float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity };
+                            bmaxN = Vector3f.Of(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
                         }
 
-                        for (int i = 0; i < 3; i++)
+                            bminN = Vector3f.Of(
-                        {
+                                Math.Min(bminN.Value[0], result.getSolidHeightfield().bmin[0]),
-                            bmin[i] = Math.Min(bmin[i], result.getSolidHeightfield().bmin[i]);
+                                Math.Min(bminN.Value[1], result.getSolidHeightfield().bmin[1]),
-                            bmax[i] = Math.Max(bmax[i], result.getSolidHeightfield().bmax[i]);
+                                Math.Min(bminN.Value[2], result.getSolidHeightfield().bmin[2])
-                        }
+                            );
+                            
+                            bmaxN = Vector3f.Of(
+                                Math.Max(bmaxN.Value[0], result.getSolidHeightfield().bmax[0]),
+                                Math.Max(bmaxN.Value[1], result.getSolidHeightfield().bmax[1]),
+                                Math.Max(bmaxN.Value[2], result.getSolidHeightfield().bmax[2])
+                            );
                     }
                 }
             }
 
-            if (bmin != null && bmax != null)
+            if (bminN != null && bmaxN != null)
             {
+                Vector3f bmin = bminN.Value;
+                Vector3f bmax = bmaxN.Value;
+
                 camr = (float)(Math.Sqrt(
                                    sqr(bmax[0] - bmin[0]) + sqr(bmax[1] - bmin[1]) + sqr(bmax[2] - bmin[2]))
                                / 2);

src/DotRecast.Recast.Demo/Tools/ConvexVolumeTool.cs

@@ -47,7 +47,7 @@ public class ConvexVolumeTool : Tool
         sample = m_sample;
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
         DemoInputGeomProvider geom = sample.getInputGeom();
         if (geom == null)

src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs

@@ -362,7 +362,7 @@ public class CrowdProfilingTool
             foreach (CrowdAgent ag in crowd.getActiveAgents())
             {
                 float radius = ag.option.radius;
-                float[] pos = ag.npos;
+                Vector3f pos = ag.npos;
                 dd.debugDrawCircle(pos[0], pos[1], pos[2], radius, duRGBA(0, 0, 0, 32), 2.0f);
             }
 
@@ -372,7 +372,7 @@ public class CrowdProfilingTool
 
                 float height = ag.option.height;
                 float radius = ag.option.radius;
-                float[] pos = ag.npos;
+                Vector3f pos = ag.npos;
 
                 int col = duRGBA(220, 220, 220, 128);
                 if (agentData.type == AgentType.TRAVELLER)

src/DotRecast.Recast.Demo/Tools/CrowdTool.cs

@@ -22,7 +22,6 @@ using System.Collections.Generic;
 using System.Collections.Immutable;
 using System.Diagnostics;
 using DotRecast.Core;
-using Silk.NET.Windowing;
 using DotRecast.Detour;
 using DotRecast.Detour.Crowd;
 using DotRecast.Detour.Crowd.Tracking;
@@ -147,7 +146,7 @@ public class CrowdTool : Tool
         }
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
         if (m_mode == CrowdToolMode.PROFILING)
         {
@@ -259,7 +258,7 @@ public class CrowdTool : Tool
         return ap;
     }
 
-    private CrowdAgent hitTestAgents(float[] s, Vector3f p)
+    private CrowdAgent hitTestAgents(Vector3f s, Vector3f p)
     {
         CrowdAgent isel = null;
         float tsel = float.MaxValue;
@@ -312,14 +311,14 @@ public class CrowdTool : Tool
             // Request velocity
             if (m_agentDebug.agent != null)
             {
-                float[] vel = calcVel(ref m_agentDebug.agent.npos, p, m_agentDebug.agent.option.maxSpeed);
+                Vector3f vel = calcVel(m_agentDebug.agent.npos, p, m_agentDebug.agent.option.maxSpeed);
                 crowd.requestMoveVelocity(m_agentDebug.agent, vel);
             }
             else
             {
                 foreach (CrowdAgent ag in crowd.getActiveAgents())
                 {
-                    float[] vel = calcVel(ag.npos, p, ag.option.maxSpeed);
+                    Vector3f vel = calcVel(ag.npos, p, ag.option.maxSpeed);
                     crowd.requestMoveVelocity(ag, vel);
                 }
             }

src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs

@@ -134,7 +134,7 @@ public class DynamicUpdateTool : Tool
         this.sample = sample;
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
         if (mode == DynamicUpdateToolMode.COLLIDERS)
         {
@@ -215,7 +215,7 @@ public class DynamicUpdateTool : Tool
         }
     }
 
-    private Tuple<Collider, ColliderGizmo> sphereCollider(float[] p)
+    private Tuple<Collider, ColliderGizmo> sphereCollider(Vector3f p)
     {
         float radius = 1 + (float)random.NextDouble() * 10;
         return Tuple.Create<Collider, ColliderGizmo>(
@@ -223,22 +223,26 @@ public class DynamicUpdateTool : Tool
             GizmoFactory.sphere(p, radius));
     }
 
-    private Tuple<Collider, ColliderGizmo> capsuleCollider(float[] p)
+    private Tuple<Collider, ColliderGizmo> capsuleCollider(Vector3f p)
     {
         float radius = 0.4f + (float)random.NextDouble() * 4f;
-        float[] a = new float[] { (1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) };
+        Vector3f a = Vector3f.Of(
-        vNormalize(a);
+            (1f - 2 * (float)random.NextDouble()),
+            0.01f + (float)random.NextDouble(),
+            (1f - 2 * (float)random.NextDouble())
+        );
+        vNormalize(ref a);
         float len = 1f + (float)random.NextDouble() * 20f;
         a[0] *= len;
         a[1] *= len;
         a[2] *= len;
-        float[] start = new float[] { p[0], p[1], p[2] };
+        Vector3f start = Vector3f.Of(p[0], p[1], p[2]);
-        float[] end = new float[] { p[0] + a[0], p[1] + a[1], p[2] + a[2] };
+        Vector3f end = Vector3f.Of(p[0] + a[0], p[1] + a[1], p[2] + a[2]);
-        return Tuple.Create<Collider, ColliderGizmo>(new CapsuleCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER,
+        return Tuple.Create<Collider, ColliderGizmo>(new CapsuleCollider(
-            dynaMesh.config.walkableClimb), GizmoFactory.capsule(start, end, radius));
+            start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb), GizmoFactory.capsule(start, end, radius));
     }
 
-    private Tuple<Collider, ColliderGizmo> boxCollider(float[] p)
+    private Tuple<Collider, ColliderGizmo> boxCollider(Vector3f p)
     {
         float[] extent = new float[]
         {
@@ -253,7 +257,7 @@ public class DynamicUpdateTool : Tool
             GizmoFactory.box(p, halfEdges));
     }
 
-    private Tuple<Collider, ColliderGizmo> cylinderCollider(float[] p)
+    private Tuple<Collider, ColliderGizmo> cylinderCollider(Vector3f p)
     {
         float radius = 0.7f + (float)random.NextDouble() * 4f;
         float[] a = new float[] { (1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) };
@@ -262,20 +266,20 @@ public class DynamicUpdateTool : Tool
         a[0] *= len;
         a[1] *= len;
         a[2] *= len;
-        float[] start = new float[] { p[0], p[1], p[2] };
+        Vector3f start = Vector3f.Of(p[0], p[1], p[2]);
-        float[] end = new float[] { p[0] + a[0], p[1] + a[1], p[2] + a[2] };
+        Vector3f end = Vector3f.Of(p[0] + a[0], p[1] + a[1], p[2] + a[2]);
         return Tuple.Create<Collider, ColliderGizmo>(new CylinderCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER,
             dynaMesh.config.walkableClimb), GizmoFactory.cylinder(start, end, radius));
     }
 
-    private Tuple<Collider, ColliderGizmo> compositeCollider(float[] p)
+    private Tuple<Collider, ColliderGizmo> compositeCollider(Vector3f p)
     {
-        float[] baseExtent = new float[] { 5, 3, 8 };
+        Vector3f baseExtent = Vector3f.Of(5, 3, 8);
-        float[] baseCenter = new float[] { p[0], p[1] + 3, p[2] };
+        Vector3f baseCenter = Vector3f.Of(p[0], p[1] + 3, p[2]);
-        float[] baseUp = new float[] { 0, 1, 0 };
+        Vector3f baseUp = Vector3f.Of(0, 1, 0);
-        float[] forward = new float[] { (1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()) };
+        Vector3f forward = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
-        vNormalize(forward);
+        vNormalize(ref forward);
-        float[] side = RecastMath.vCross(forward, baseUp);
+        Vector3f side = vCross(forward, baseUp);
         BoxCollider @base = new BoxCollider(baseCenter, BoxCollider.getHalfEdges(baseUp, forward, baseExtent),
             SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb);
         float[] roofExtent = new float[] { 4.5f, 4.5f, 8f };
@@ -308,7 +312,7 @@ public class DynamicUpdateTool : Tool
         return Tuple.Create<Collider, ColliderGizmo>(collider, gizmo);
     }
 
-    private Tuple<Collider, ColliderGizmo> trimeshBridge(float[] p)
+    private Tuple<Collider, ColliderGizmo> trimeshBridge(Vector3f p)
     {
         return trimeshCollider(p, bridgeGeom);
     }
@@ -326,7 +330,7 @@ public class DynamicUpdateTool : Tool
         return Tuple.Create<Collider, ColliderGizmo>(collider, GizmoFactory.trimesh(verts, convexGeom.faces));
     }
 
-    private Tuple<Collider, ColliderGizmo> trimeshCollider(float[] p, DemoInputGeomProvider geom)
+    private Tuple<Collider, ColliderGizmo> trimeshCollider(Vector3f p, DemoInputGeomProvider geom)
     {
         float[] verts = transformVertices(p, geom, 0);
         TrimeshCollider collider = new TrimeshCollider(verts, geom.faces, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD,
@@ -334,7 +338,7 @@ public class DynamicUpdateTool : Tool
         return Tuple.Create<Collider, ColliderGizmo>(collider, GizmoFactory.trimesh(verts, geom.faces));
     }
 
-    private float[] transformVertices(float[] p, DemoInputGeomProvider geom, float ax)
+    private float[] transformVertices(Vector3f p, DemoInputGeomProvider geom, float ax)
     {
         float[] rx = GLU.build_4x4_rotation_matrix((float)random.NextDouble() * ax, 1, 0, 0);
         float[] ry = GLU.build_4x4_rotation_matrix((float)random.NextDouble() * 360, 0, 1, 0);
@@ -366,8 +370,17 @@ public class DynamicUpdateTool : Tool
         resultvector[2] = matrix[2] * pvector[0] + matrix[6] * pvector[1] + matrix[10] * pvector[2];
         return resultvector;
     }
+    
+    private float[] mulMatrixVector(ref Vector3f resultvector, float[] matrix, float[] pvector)
+    {
+        resultvector[0] = matrix[0] * pvector[0] + matrix[4] * pvector[1] + matrix[8] * pvector[2];
+        resultvector[1] = matrix[1] * pvector[0] + matrix[5] * pvector[1] + matrix[9] * pvector[2];
+        resultvector[2] = matrix[2] * pvector[0] + matrix[6] * pvector[1] + matrix[10] * pvector[2];
+        return resultvector;
+    }
 
-    public override void handleClickRay(float[] start, float[] dir, bool shift)
+
+    public override void handleClickRay(Vector3f start, float[] dir, bool shift)
     {
         if (mode == DynamicUpdateToolMode.COLLIDERS)
         {
@@ -387,7 +400,7 @@ public class DynamicUpdateTool : Tool
         }
     }
 
-    private bool hit(float[] point, float[] dir, float[] bounds)
+    private bool hit(Vector3f point, float[] dir, float[] bounds)
     {
         float cx = 0.5f * (bounds[0] + bounds[3]);
         float cy = 0.5f * (bounds[1] + bounds[4]);
@@ -522,14 +535,14 @@ public class DynamicUpdateTool : Tool
         ImGui.Separator();
 
         if (mode == DynamicUpdateToolMode.BUILD)
-        { 
+        {
             var loadVoxelPopupStrId = "Load Voxels Popup";
             bool isLoadVoxelPopup = true;
             if (ImGui.Button("Load Voxels..."))
             {
                 ImGui.OpenPopup(loadVoxelPopupStrId);
             }
-            
+
             if (ImGui.BeginPopupModal(loadVoxelPopupStrId, ref isLoadVoxelPopup, ImGuiWindowFlags.NoTitleBar))
             {
                 var picker = ImFilePicker.GetFilePicker(loadVoxelPopupStrId, Path.Combine(Environment.CurrentDirectory), ".voxels");
@@ -538,6 +551,7 @@ public class DynamicUpdateTool : Tool
                     load(picker.SelectedFile);
                     ImFilePicker.RemoveFilePicker(loadVoxelPopupStrId);
                 }
+
                 ImGui.EndPopup();
             }
 
@@ -550,7 +564,7 @@ public class DynamicUpdateTool : Tool
                 {
                     ImGui.BeginPopup(saveVoxelPopupStrId);
                 }
-                
+
                 if (ImGui.BeginPopupModal(saveVoxelPopupStrId, ref isSaveVoxelPopup, ImGuiWindowFlags.NoTitleBar))
                 {
                     var picker = ImFilePicker.GetFilePicker(saveVoxelPopupStrId, Path.Combine(Environment.CurrentDirectory), ".voxels");
@@ -558,12 +572,12 @@ public class DynamicUpdateTool : Tool
                     {
                         if (string.IsNullOrEmpty(picker.SelectedFile))
                             save(picker.SelectedFile);
-                        
+
                         ImFilePicker.RemoveFilePicker(saveVoxelPopupStrId);
                     }
+
                     ImGui.EndPopup();
                 }
-
             }
 
             ImGui.NewLine();
@@ -690,7 +704,7 @@ public class DynamicUpdateTool : Tool
             dynaMesh.config.keepIntermediateResults = true;
             updateUI();
             buildDynaMesh();
-            
+
             colliders.Clear();
         }
         catch (Exception e)

src/DotRecast.Recast.Demo/Tools/Gizmos/BoxGizmo.cs

@@ -1,4 +1,5 @@
-using DotRecast.Detour.Dynamic.Colliders;
+using DotRecast.Core;
+using DotRecast.Detour.Dynamic.Colliders;
 using DotRecast.Recast.Demo.Draw;
 
 namespace DotRecast.Recast.Demo.Tools.Gizmos;
@@ -25,14 +26,14 @@ public class BoxGizmo : ColliderGizmo
 
     private readonly float[] vertices = new float[8 * 3];
     private readonly float[] center;
-    private readonly float[][] halfEdges;
+    private readonly Vector3f[] halfEdges;
 
-    public BoxGizmo(float[] center, float[] extent, float[] forward, float[] up) :
+    public BoxGizmo(float[] center, float[] extent, Vector3f forward, Vector3f up) :
         this(center, BoxCollider.getHalfEdges(up, forward, extent))
     {
     }
 
-    public BoxGizmo(float[] center, float[][] halfEdges)
+    public BoxGizmo(float[] center, Vector3f[] halfEdges)
     {
         this.center = center;
         this.halfEdges = halfEdges;

src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs

@@ -1,4 +1,5 @@
-using DotRecast.Recast.Demo.Draw;
+using DotRecast.Core;
+using DotRecast.Recast.Demo.Draw;
 using static DotRecast.Recast.RecastVectors;
 using static DotRecast.Core.RecastMath;
 using static DotRecast.Recast.Demo.Tools.Gizmos.GizmoHelper;
@@ -12,21 +13,21 @@ public class CapsuleGizmo : ColliderGizmo
     private readonly float[] center;
     private readonly float[] gradient;
 
-    public CapsuleGizmo(float[] start, float[] end, float radius)
+    public CapsuleGizmo(Vector3f start, Vector3f end, float radius)
     {
         center = new float[]
         {
             0.5f * (start[0] + end[0]), 0.5f * (start[1] + end[1]),
             0.5f * (start[2] + end[2])
         };
-        float[] axis = new float[] { end[0] - start[0], end[1] - start[1], end[2] - start[2] };
+        Vector3f axis = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]);
-        float[][] normals = new float[3][];
+        Vector3f[] normals = new Vector3f[3];
-        normals[1] = new float[] { end[0] - start[0], end[1] - start[1], end[2] - start[2] };
+        normals[1] = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]);
-        normalize(normals[1]);
+        normalize(ref normals[1]);
         normals[0] = getSideVector(axis);
-        normals[2] = new float[3];
+        normals[2] = Vector3f.Zero;
-        cross(normals[2], normals[0], normals[1]);
+        cross(ref normals[2], normals[0], normals[1]);
-        normalize(normals[2]);
+        normalize(ref normals[2]);
         triangles = generateSphericalTriangles();
         float[] trX = new float[] { normals[0][0], normals[1][0], normals[2][0] };
         float[] trY = new float[] { normals[0][1], normals[1][1], normals[2][1] };
@@ -48,23 +49,23 @@ public class CapsuleGizmo : ColliderGizmo
             v[0] = vertices[i] - center[0];
             v[1] = vertices[i + 1] - center[1];
             v[2] = vertices[i + 2] - center[2];
-            normalize(v);
+            normalize(ref v);
             gradient[i / 3] = clamp(0.57735026f * (v[0] + v[1] + v[2]), -1, 1);
         }
     }
 
-    private float[] getSideVector(float[] axis)
+    private Vector3f getSideVector(Vector3f axis)
     {
-        float[] side = { 1, 0, 0 };
+        Vector3f side = Vector3f.Of(1, 0, 0);
         if (axis[0] > 0.8)
         {
-            side = new float[] { 0, 0, 1 };
+            side = Vector3f.Of(0, 0, 1);
         }
 
         Vector3f forward = new Vector3f();
-        cross(forward, side, axis);
+        cross(ref forward, side, axis);
-        cross(side, axis, forward);
+        cross(ref side, axis, forward);
-        normalize(side);
+        normalize(ref side);
         return side;
     }
 

src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoFactory.cs

@@ -1,4 +1,6 @@
-namespace DotRecast.Recast.Demo.Tools.Gizmos;
+using DotRecast.Core;
+
+namespace DotRecast.Recast.Demo.Tools.Gizmos;
 
 public static class GizmoFactory
 {
@@ -7,17 +9,17 @@ public static class GizmoFactory
         return new BoxGizmo(center, halfEdges);
     }
 
-    public static ColliderGizmo sphere(float[] center, float radius)
+    public static ColliderGizmo sphere(Vector3f center, float radius)
     {
         return new SphereGizmo(center, radius);
     }
 
-    public static ColliderGizmo capsule(float[] start, float[] end, float radius)
+    public static ColliderGizmo capsule(Vector3f start, Vector3f end, float radius)
     {
         return new CapsuleGizmo(start, end, radius);
     }
 
-    public static ColliderGizmo cylinder(float[] start, float[] end, float radius)
+    public static ColliderGizmo cylinder(Vector3f start, Vector3f end, float radius)
     {
         return new CylinderGizmo(start, end, radius);
     }

src/DotRecast.Recast.Demo/Tools/Gizmos/SphereGizmo.cs

@@ -1,4 +1,5 @@
-using DotRecast.Recast.Demo.Draw;
+using DotRecast.Core;
+using DotRecast.Recast.Demo.Draw;
 using static DotRecast.Core.RecastMath;
 using static DotRecast.Recast.Demo.Tools.Gizmos.GizmoHelper;
 
@@ -10,9 +11,9 @@ public class SphereGizmo : ColliderGizmo
     private readonly float[] vertices;
     private readonly int[] triangles;
     private readonly float radius;
-    private readonly float[] center;
+    private readonly Vector3f center;
 
-    public SphereGizmo(float[] center, float radius)
+    public SphereGizmo(Vector3f center, float radius)
     {
         this.center = center;
         this.radius = radius;

src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs

@@ -45,7 +45,7 @@ public class JumpLinkBuilderTool : Tool
         annotationBuilder = null;
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
     }
 

src/DotRecast.Recast.Demo/Tools/OffMeshConnectionTool.cs

@@ -33,7 +33,7 @@ public class OffMeshConnectionTool : Tool
 {
     private Sample sample;
     private bool hitPosSet;
-    private float[] hitPos;
+    private Vector3f hitPos;
     private int bidir;
 
     public override void setSample(Sample m_sample)
@@ -41,7 +41,7 @@ public class OffMeshConnectionTool : Tool
         sample = m_sample;
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
         DemoInputGeomProvider geom = sample.getInputGeom();
         if (geom == null)
@@ -75,7 +75,7 @@ public class OffMeshConnectionTool : Tool
             // Create
             if (!hitPosSet)
             {
-                hitPos = ArrayUtils.CopyOf(p, p.Length);
+                hitPos = p;
                 hitPosSet = true;
             }
             else

src/DotRecast.Recast.Demo/Tools/PathUtils.cs

@@ -64,11 +64,11 @@ public static class PathUtils
         if (ns >= straightPath.Count)
             return null;
 
-        float[] steerPos = new float[]
+        Vector3f steerPos = Vector3f.Of(
-        {
+            straightPath[ns].getPos()[0], 
-            straightPath[ns].getPos()[0], startPos[1],
+            startPos[1],
             straightPath[ns].getPos()[2]
-        };
+        );
         int steerPosFlag = straightPath[ns].getFlags();
         long steerPosRef = straightPath[ns].getRef();
 

src/DotRecast.Recast.Demo/Tools/SteerTarget.cs

@@ -1,13 +1,15 @@
-namespace DotRecast.Recast.Demo.Tools;
+using DotRecast.Core;
+
+namespace DotRecast.Recast.Demo.Tools;
 
 public class SteerTarget
 {
-    public readonly float[] steerPos;
+    public readonly Vector3f steerPos;
     public readonly int steerPosFlag;
     public readonly long steerPosRef;
     public readonly float[] steerPoints;
 
-    public SteerTarget(float[] steerPos, int steerPosFlag, long steerPosRef, float[] steerPoints)
+    public SteerTarget(Vector3f steerPos, int steerPosFlag, long steerPosRef, float[] steerPoints)
     {
         this.steerPos = steerPos;
         this.steerPosFlag = steerPosFlag;

src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs

@@ -28,9 +28,9 @@ public class TestNavmeshTool : Tool
     private readonly Vector3f m_polyPickExt = Vector3f.Of(2, 4, 2);
     private long m_startRef;
     private long m_endRef;
-    private float[] m_hitPos;
+    private Vector3f m_hitPos;
     private float m_distanceToWall;
-    private float[] m_hitNormal;
+    private Vector3f m_hitNormal;
     private List<StraightPathItem> m_straightPath;
     private int m_straightPathOptions;
     private List<long> m_polys;
@@ -41,7 +41,7 @@ public class TestNavmeshTool : Tool
     private List<Vector3f> m_smoothPath;
     private Status m_pathFindStatus = Status.FAILURE;
     private bool enableRaycast = true;
-    private readonly List<float[]> randomPoints = new();
+    private readonly List<Vector3f> randomPoints = new();
     private bool constrainByCircle;
 
     private int includeFlags = SampleAreaModifications.SAMPLE_POLYFLAGS_ALL;
@@ -58,7 +58,7 @@ public class TestNavmeshTool : Tool
         this.m_sample = m_sample;
     }
 
-    public override void handleClick(float[] s, Vector3f p, bool shift)
+    public override void handleClick(Vector3f s, Vector3f p, bool shift)
     {
         if (shift)
         {
@@ -224,8 +224,8 @@ public class TestNavmeshTool : Tool
                             : false;
 
                         // Find movement delta.
-                        float[] delta = vSub(steerTarget.steerPos, iterPos);
+                        Vector3f delta = vSub(steerTarget.steerPos, iterPos);
-                        float len = (float)Math.Sqrt(RecastMath.vDot(delta, delta));
+                        float len = (float)Math.Sqrt(vDot(delta, delta));
                         // If the steer target is end of path or off-mesh link, do not move past the location.
                         if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                         {
@@ -236,13 +236,12 @@ public class TestNavmeshTool : Tool
                             len = STEP_SIZE / len;
                         }
 
-                        float[] moveTgt = vMad(iterPos, delta, len);
+                        Vector3f moveTgt = vMad(iterPos, delta, len);
                         // Move
                         Result<MoveAlongSurfaceResult> result = m_navQuery.moveAlongSurface(polys[0], iterPos,
                             moveTgt, m_filter);
                         MoveAlongSurfaceResult moveAlongSurface = result.result;
 
-                        iterPos = new float[3];
                         iterPos[0] = moveAlongSurface.getResultPos()[0];
                         iterPos[1] = moveAlongSurface.getResultPos()[1];
                         iterPos[2] = moveAlongSurface.getResultPos()[2];
@@ -261,7 +260,7 @@ public class TestNavmeshTool : Tool
                         if (endOfPath && PathUtils.inRange(iterPos, steerTarget.steerPos, SLOP, 1.0f))
                         {
                             // Reached end of path.
-                            vCopy(iterPos, targetPos);
+                            vCopy(ref iterPos, targetPos);
                             if (m_smoothPath.Count < MAX_SMOOTH)
                             {
                                 m_smoothPath.Add(iterPos);
@@ -287,12 +286,11 @@ public class TestNavmeshTool : Tool
                             polys = polys.GetRange(npos, polys.Count - npos);
 
                             // Handle the connection.
-                            Result<Tuple<float[], float[]>> offMeshCon = m_navMesh
+                            var offMeshCon = m_navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef);
-                                .getOffMeshConnectionPolyEndPoints(prevRef, polyRef);
                             if (offMeshCon.succeeded())
                             {
-                                float[] startPos = offMeshCon.result.Item1;
+                                var startPos = offMeshCon.result.Item1;
-                                float[] endPos = offMeshCon.result.Item2;
+                                var endPos = offMeshCon.result.Item2;
                                 if (m_smoothPath.Count < MAX_SMOOTH)
                                 {
                                     m_smoothPath.Add(startPos);
@@ -304,7 +302,7 @@ public class TestNavmeshTool : Tool
                                 }
 
                                 // Move position at the other side of the off-mesh link.
-                                vCopy(iterPos, endPos);
+                                vCopy(ref iterPos, endPos);
                                 iterPos[1] = m_navQuery.getPolyHeight(polys[0], iterPos).result;
                             }
                         }
@@ -332,7 +330,7 @@ public class TestNavmeshTool : Tool
                 if (0 < m_polys.Count)
                 {
                     // In case of partial path, make sure the end point is clamped to the last polygon.
-                    float[] epos = new float[] { m_epos[0], m_epos[1], m_epos[2] };
+                    var epos = Vector3f.Of(m_epos[0], m_epos[1], m_epos[2]);
                     if (m_polys[m_polys.Count - 1] != m_endRef)
                     {
                         Result<ClosestPointOnPolyResult> result = m_navQuery
@@ -375,14 +373,14 @@ public class TestNavmeshTool : Tool
                         if (hit.result.t > 1)
                         {
                             // No hit
-                            m_hitPos = ArrayUtils.CopyOf(m_epos, m_epos.Length);
+                            m_hitPos = m_epos;
                             m_hitResult = false;
                         }
                         else
                         {
                             // Hit
                             m_hitPos = vLerp(m_spos, m_epos, hit.result.t);
-                            m_hitNormal = ArrayUtils.CopyOf(hit.result.hitNormal, hit.result.hitNormal.Length);
+                            m_hitNormal = hit.result.hitNormal;
                             m_hitResult = true;
                         }
 
@@ -926,7 +924,7 @@ public class TestNavmeshTool : Tool
         }
     }
 
-    private void drawAgent(RecastDebugDraw dd, float[] pos, int col)
+    private void drawAgent(RecastDebugDraw dd, Vector3f pos, int col)
     {
         float r = m_sample.getSettingsUI().getAgentRadius();
         float h = m_sample.getSettingsUI().getAgentHeight();

src/DotRecast.Recast.Demo/Tools/Tool.cs

@@ -28,7 +28,7 @@ public abstract class Tool
 {
     public abstract void setSample(Sample m_sample);
 
-    public abstract void handleClick(float[] s, Vector3f p, bool shift);
+    public abstract void handleClick(Vector3f s, Vector3f p, bool shift);
 
     public abstract void handleRender(NavMeshRenderer renderer);
 
@@ -38,7 +38,7 @@ public abstract class Tool
 
     public abstract string getName();
 
-    public virtual void handleClickRay(float[] start, float[] direction, bool shift)
+    public virtual void handleClickRay(Vector3f start, float[] direction, bool shift)
     {
         // ...
     }

src/DotRecast.Recast/Geom/InputGeomProvider.cs

@@ -19,14 +19,15 @@ freely, subject to the following restrictions:
 */
 
 using System.Collections.Generic;
+using DotRecast.Core;
 
 namespace DotRecast.Recast.Geom
 {
     public interface InputGeomProvider : ConvexVolumeProvider
     {
-        float[] getMeshBoundsMin();
+        Vector3f getMeshBoundsMin();
 
-        float[] getMeshBoundsMax();
+        Vector3f getMeshBoundsMax();
 
         IEnumerable<TriMesh> meshes();
     }

src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs

@@ -30,8 +30,8 @@ namespace DotRecast.Recast.Geom
         public readonly float[] vertices;
         public readonly int[] faces;
         public readonly float[] normals;
-        readonly float[] bmin;
+        private Vector3f bmin;
-        readonly float[] bmax;
+        private Vector3f bmax;
         readonly List<ConvexVolume> volumes = new List<ConvexVolume>();
 
         public SimpleInputGeomProvider(List<float> vertexPositions, List<int> meshFaces)
@@ -67,23 +67,23 @@ namespace DotRecast.Recast.Geom
             this.faces = faces;
             normals = new float[faces.Length];
             calculateNormals();
-            bmin = new float[3];
+            bmin = Vector3f.Zero;
-            bmax = new float[3];
+            bmax = Vector3f.Zero;
-            RecastVectors.copy(bmin, vertices, 0);
+            RecastVectors.copy(ref bmin, vertices, 0);
-            RecastVectors.copy(bmax, vertices, 0);
+            RecastVectors.copy(ref bmax, vertices, 0);
             for (int i = 1; i < vertices.Length / 3; i++)
             {
-                RecastVectors.min(bmin, vertices, i * 3);
+                RecastVectors.min(ref bmin, vertices, i * 3);
-                RecastVectors.max(bmax, vertices, i * 3);
+                RecastVectors.max(ref bmax, vertices, i * 3);
             }
         }
 
-        public float[] getMeshBoundsMin()
+        public Vector3f getMeshBoundsMin()
         {
             return bmin;
         }
 
-        public float[] getMeshBoundsMax()
+        public Vector3f getMeshBoundsMax()
         {
             return bmax;
         }

src/DotRecast.Recast/Geom/SingleTrimeshInputGeomProvider.cs

@@ -19,36 +19,37 @@ freely, subject to the following restrictions:
 using System;
 using System.Collections.Generic;
 using System.Collections.Immutable;
+using DotRecast.Core;
 
 namespace DotRecast.Recast.Geom
 {
     public class SingleTrimeshInputGeomProvider : InputGeomProvider
     {
-        private readonly float[] bmin;
+        private readonly Vector3f bmin;
-        private readonly float[] bmax;
+        private readonly Vector3f bmax;
         private readonly TriMesh[] _meshes;
 
         public SingleTrimeshInputGeomProvider(float[] vertices, int[] faces)
         {
-            bmin = new float[3];
+            bmin = Vector3f.Zero;
-            bmax = new float[3];
+            bmax = Vector3f.Zero;
-            RecastVectors.copy(bmin, vertices, 0);
+            RecastVectors.copy(ref bmin, vertices, 0);
-            RecastVectors.copy(bmax, vertices, 0);
+            RecastVectors.copy(ref bmax, vertices, 0);
             for (int i = 1; i < vertices.Length / 3; i++)
             {
-                RecastVectors.min(bmin, vertices, i * 3);
+                RecastVectors.min(ref bmin, vertices, i * 3);
-                RecastVectors.max(bmax, vertices, i * 3);
+                RecastVectors.max(ref bmax, vertices, i * 3);
             }
 
             _meshes = new[] { new TriMesh(vertices, faces) };
         }
 
-        public float[] getMeshBoundsMin()
+        public Vector3f getMeshBoundsMin()
         {
             return bmin;
         }
 
-        public float[] getMeshBoundsMax()
+        public Vector3f getMeshBoundsMax()
         {
             return bmax;
         }

src/DotRecast.Recast/Recast.cs

@@ -62,9 +62,9 @@ namespace DotRecast.Recast
         }
 
 
-        public static int[] calcTileCount(float[] bmin, float[] bmax, float cs, int tileSizeX, int tileSizeZ)
+        public static int[] calcTileCount(Vector3f bmin, Vector3f bmax, float cs, int tileSizeX, int tileSizeZ)
         {
-            int[] gwd = Recast.calcGridSize(bmin, bmax, cs);
+            int[] gwd = calcGridSize(bmin, bmax, cs);
             int gw = gwd[0];
             int gd = gwd[1];
             int tw = (gw + tileSizeX - 1) / tileSizeX;

src/DotRecast.Recast/RecastBuilder.cs

@@ -48,8 +48,8 @@ namespace DotRecast.Recast
 
         public List<RecastBuilderResult> buildTiles(InputGeomProvider geom, RecastConfig cfg, TaskFactory taskFactory)
         {
-            float[] bmin = geom.getMeshBoundsMin();
+            Vector3f bmin = geom.getMeshBoundsMin();
-            float[] bmax = geom.getMeshBoundsMax();
+            Vector3f bmax = geom.getMeshBoundsMax();
             int[] twh = Recast.calcTileCount(bmin, bmax, cfg.cs, cfg.tileSizeX, cfg.tileSizeZ);
             int tw = twh[0];
             int th = twh[1];
@@ -69,8 +69,8 @@ namespace DotRecast.Recast
 
         public Task buildTilesAsync(InputGeomProvider geom, RecastConfig cfg, int threads, List<RecastBuilderResult> results, TaskFactory taskFactory, CancellationToken cancellationToken)
         {
-            float[] bmin = geom.getMeshBoundsMin();
+            Vector3f bmin = geom.getMeshBoundsMin();
-            float[] bmax = geom.getMeshBoundsMax();
+            Vector3f bmax = geom.getMeshBoundsMax();
             int[] twh = Recast.calcTileCount(bmin, bmax, cfg.cs, cfg.tileSizeX, cfg.tileSizeZ);
             int tw = twh[0];
             int th = twh[1];
@@ -87,7 +87,7 @@ namespace DotRecast.Recast
             return task;
         }
 
-        private Task buildSingleThreadAsync(InputGeomProvider geom, RecastConfig cfg, float[] bmin, float[] bmax,
+        private Task buildSingleThreadAsync(InputGeomProvider geom, RecastConfig cfg, Vector3f bmin, Vector3f bmax,
             int tw, int th, List<RecastBuilderResult> results)
         {
             AtomicInteger counter = new AtomicInteger(0);
@@ -102,7 +102,7 @@ namespace DotRecast.Recast
             return Task.CompletedTask;
         }
 
-        private Task buildMultiThreadAsync(InputGeomProvider geom, RecastConfig cfg, float[] bmin, float[] bmax,
+        private Task buildMultiThreadAsync(InputGeomProvider geom, RecastConfig cfg, Vector3f bmin, Vector3f bmax,
             int tw, int th, List<RecastBuilderResult> results, TaskFactory taskFactory, CancellationToken cancellationToken)
         {
             AtomicInteger counter = new AtomicInteger(0);
@@ -152,7 +152,7 @@ namespace DotRecast.Recast
             return Task.WhenAll(tasks.ToArray());
         }
 
-        private RecastBuilderResult buildTile(InputGeomProvider geom, RecastConfig cfg, float[] bmin, float[] bmax, int tx,
+        private RecastBuilderResult buildTile(InputGeomProvider geom, RecastConfig cfg, Vector3f bmin, Vector3f bmax, int tx,
             int ty, AtomicInteger counter, int total)
         {
             RecastBuilderResult result = build(geom, new RecastBuilderConfig(cfg, bmin, bmax, tx, ty));

src/DotRecast.Recast/RecastBuilderConfig.cs

@@ -43,11 +43,11 @@ namespace DotRecast.Recast
         /** The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu] **/
         public readonly Vector3f bmax = new Vector3f();
 
-        public RecastBuilderConfig(RecastConfig cfg, float[] bmin, float[] bmax) : this(cfg, bmin, bmax, 0, 0)
+        public RecastBuilderConfig(RecastConfig cfg, Vector3f bmin, Vector3f bmax) : this(cfg, bmin, bmax, 0, 0)
         {
         }
 
-        public RecastBuilderConfig(RecastConfig cfg, float[] bmin, float[] bmax, int tileX, int tileZ)
+        public RecastBuilderConfig(RecastConfig cfg, Vector3f bmin, Vector3f bmax, int tileX, int tileZ)
         {
             this.tileX = tileX;
             this.tileZ = tileZ;

src/DotRecast.Recast/RecastFilledVolumeRasterization.cs

@@ -75,7 +75,7 @@ namespace DotRecast.Recast
             ctx.stopTimer("RASTERIZE_CYLINDER");
         }
 
-        public static void rasterizeBox(Heightfield hf, float[] center, float[][] halfEdges, int area, int flagMergeThr,
+        public static void rasterizeBox(Heightfield hf, Vector3f center, float[][] halfEdges, int area, int flagMergeThr,
             Telemetry ctx)
         {
             ctx.startTimer("RASTERIZE_BOX");

test/DotRecast.Detour.Test/Io/MeshSetReaderWriterTest.cs

@@ -66,8 +66,8 @@ public class MeshSetReaderWriterTest
         header.numTiles = 0;
         NavMesh mesh = new NavMesh(header.option, 6);
 
-        float[] bmin = geom.getMeshBoundsMin();
+        Vector3f bmin = geom.getMeshBoundsMin();
-        float[] bmax = geom.getMeshBoundsMax();
+        Vector3f bmax = geom.getMeshBoundsMax();
         int[] twh = DotRecast.Recast.Recast.calcTileCount(bmin, bmax, m_cellSize, m_tileSize, m_tileSize);
         int tw = twh[0];
         int th = twh[1];

test/DotRecast.Detour.TileCache.Test/TestTileLayerBuilder.cs

@@ -56,8 +56,8 @@ public class TestTileLayerBuilder : AbstractTileLayersBuilder
             PartitionType.WATERSHED, m_cellSize, m_cellHeight, m_agentMaxSlope, true, true, true, m_agentHeight,
             m_agentRadius, m_agentMaxClimb, m_regionMinArea, m_regionMergeArea, m_edgeMaxLen, m_edgeMaxError, m_vertsPerPoly,
             true, m_detailSampleDist, m_detailSampleMaxError, SampleAreaModifications.SAMPLE_AREAMOD_GROUND);
-        float[] bmin = geom.getMeshBoundsMin();
+        Vector3f bmin = geom.getMeshBoundsMin();
-        float[] bmax = geom.getMeshBoundsMax();
+        Vector3f bmax = geom.getMeshBoundsMax();
         int[] twh = Recast.Recast.calcTileCount(bmin, bmax, m_cellSize, m_tileSize, m_tileSize);
         tw = twh[0];
         th = twh[1];
@@ -120,8 +120,8 @@ public class TestTileLayerBuilder : AbstractTileLayersBuilder
     protected HeightfieldLayerSet getHeightfieldSet(int tx, int ty)
     {
         RecastBuilder rcBuilder = new RecastBuilder();
-        float[] bmin = geom.getMeshBoundsMin();
+        Vector3f bmin = geom.getMeshBoundsMin();
-        float[] bmax = geom.getMeshBoundsMax();
+        Vector3f bmax = geom.getMeshBoundsMax();
         RecastBuilderConfig cfg = new RecastBuilderConfig(rcConfig, bmin, bmax, tx, ty);
         HeightfieldLayerSet lset = rcBuilder.buildLayers(geom, cfg);
         return lset;

test/DotRecast.Recast.Test/RecastSoloMeshTest.cs

@@ -97,8 +97,8 @@ public class RecastSoloMeshTest
         m_partitionType = partitionType;
         InputGeomProvider geomProvider = ObjImporter.load(Loader.ToBytes(filename));
         long time = Stopwatch.GetTimestamp();
-        float[] bmin = geomProvider.getMeshBoundsMin();
+        Vector3f bmin = geomProvider.getMeshBoundsMin();
-        float[] bmax = geomProvider.getMeshBoundsMax();
+        Vector3f bmax = geomProvider.getMeshBoundsMax();
         Telemetry m_ctx = new Telemetry();
         //
         // Step 1. Initialize build config.