From 5f6a1ac0702c648adda0bb73110b47d8f74c9bb0 Mon Sep 17 00:00:00 2001 From: ikpil Date: Wed, 29 Mar 2023 00:03:33 +0900 Subject: [PATCH] change float[3] -> struct vector3f --- src/DotRecast.Core/RecastMath.cs | 154 ++++++++++++++++-- src/DotRecast.Core/Vector3f.cs | 10 ++ src/DotRecast.Detour.Crowd/Crowd.cs | 12 +- src/DotRecast.Detour.Crowd/CrowdAgent.cs | 16 +- .../CrowdAgentAnimation.cs | 6 +- src/DotRecast.Detour.Crowd/LocalBoundary.cs | 2 +- .../ObstacleAvoidanceQuery.cs | 32 ++-- src/DotRecast.Detour.Crowd/PathCorridor.cs | 4 +- src/DotRecast.Detour.Crowd/PathQuery.cs | 4 +- .../Tracking/CrowdAgentDebugInfo.cs | 4 +- .../Tracking/ObstacleAvoidanceDebugData.cs | 2 +- src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs | 2 +- .../Io/VoxelFileReader.cs | 4 +- src/DotRecast.Detour.Extras/BVTreeBuilder.cs | 4 +- src/DotRecast.Detour.Extras/Jumplink/Edge.cs | 4 +- .../Jumplink/EdgeSampler.cs | 6 +- .../Jumplink/EdgeSamplerFactory.cs | 4 +- .../Jumplink/GroundSample.cs | 2 +- .../Jumplink/GroundSegment.cs | 4 +- src/DotRecast.Detour.TileCache/TileCache.cs | 8 +- .../TileCacheBuilder.cs | 4 +- .../TileCacheLayerHeader.cs | 4 +- .../TileCacheObstacle.cs | 10 +- .../TileCacheParams.cs | 2 +- .../ConvexConvexIntersection.cs | 75 ++++++--- src/DotRecast.Detour/LegacyNavMeshQuery.cs | 16 +- src/DotRecast.Detour/MeshHeader.cs | 6 +- src/DotRecast.Detour/NavMesh.cs | 28 ++-- src/DotRecast.Detour/NavMeshBuilder.cs | 16 +- src/DotRecast.Detour/NavMeshParams.cs | 4 +- src/DotRecast.Detour/NavMeshQuery.cs | 24 +-- src/DotRecast.Detour/Node.cs | 3 +- src/DotRecast.Detour/QueryData.cs | 6 +- src/DotRecast.Detour/RaycastHit.cs | 3 +- src/DotRecast.Recast.Demo/Draw/DebugDraw.cs | 10 +- .../Draw/RecastDebugDraw.cs | 2 +- .../Geom/DemoInputGeomProvider.cs | 2 +- .../Geom/Intersections.cs | 2 +- src/DotRecast.Recast.Demo/RecastDemo.cs | 10 +- .../Tools/CrowdProfilingTool.cs | 2 +- src/DotRecast.Recast.Demo/Tools/CrowdTool.cs | 4 +- .../Tools/DynamicUpdateTool.cs | 4 +- .../Tools/Gizmos/CapsuleGizmo.cs | 4 +- .../Tools/Gizmos/CylinderGizmo.cs | 4 +- .../Tools/Gizmos/GizmoHelper.cs | 4 +- .../Tools/JumpLinkBuilderTool.cs | 2 +- .../Tools/TestNavmeshTool.cs | 4 +- src/DotRecast.Recast/CompactHeightfield.cs | 6 +- src/DotRecast.Recast/ContourSet.cs | 5 +- .../Geom/SimpleInputGeomProvider.cs | 3 +- src/DotRecast.Recast/HeightfieldLayerSet.cs | 6 +- src/DotRecast.Recast/PolyMesh.cs | 6 +- src/DotRecast.Recast/Recast.cs | 39 ++++- src/DotRecast.Recast/RecastArea.cs | 21 +-- src/DotRecast.Recast/RecastBuilderConfig.cs | 10 +- src/DotRecast.Recast/RecastCompact.cs | 4 +- src/DotRecast.Recast/RecastContour.cs | 4 +- src/DotRecast.Recast/RecastLayers.cs | 13 +- src/DotRecast.Recast/RecastMesh.cs | 16 +- src/DotRecast.Recast/RecastMeshDetail.cs | 120 ++++++++++---- src/DotRecast.Recast/RecastRasterization.cs | 27 ++- src/DotRecast.Recast/RecastVectors.cs | 139 ++++++++++++++++ src/DotRecast.Recast/RecastVoxelization.cs | 2 +- .../AbstractCrowdTest.cs | 2 +- 64 files changed, 683 insertions(+), 279 deletions(-) diff --git a/src/DotRecast.Core/RecastMath.cs b/src/DotRecast.Core/RecastMath.cs index 7e4f6d1..bd30b71 100644 --- a/src/DotRecast.Core/RecastMath.cs +++ b/src/DotRecast.Core/RecastMath.cs @@ -37,13 +37,13 @@ namespace DotRecast.Core public static float[] vCross(float[] v1, float[] v2) { - float[] dest = new float[3]; + 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; + return dest.ToArray(); } - + public static float vDot(float[] v1, float[] v2) { return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; @@ -101,11 +101,11 @@ namespace DotRecast.Core /// @param[in] s The amount to scale @p v2 by before adding to @p v1. public static float[] vMad(float[] v1, float[] v2, float s) { - float[] dest = new float[3]; + 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; + return dest.ToArray(); } /// Performs a linear interpolation between two vectors. (@p v1 toward @p @@ -116,47 +116,57 @@ namespace DotRecast.Core /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] public static float[] vLerp(float[] verts, int v1, int v2, float t) { - float[] dest = new float[3]; + Vector3f dest = new Vector3f(); dest[0] = verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t; dest[1] = verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t; dest[2] = verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t; - return dest; + return dest.ToArray(); } public static float[] vLerp(float[] v1, float[] v2, float t) { - float[] dest = new float[3]; + 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; + return dest.ToArray(); } - + public static float[] vSub(VectorPtr v1, VectorPtr v2) { - float[] dest = new float[3]; + 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; + return dest.ToArray(); } public static float[] vSub(float[] v1, float[] v2) { - float[] dest = new float[3]; + Vector3f dest = new Vector3f(); dest[0] = v1[0] - v2[0]; dest[1] = v1[1] - v2[1]; dest[2] = v1[2] - v2[2]; - return dest; + return dest.ToArray(); } + + public static float[] 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(); + } + public static float[] vAdd(float[] v1, float[] v2) { - float[] dest = new float[3]; + Vector3f dest = new Vector3f(); dest[0] = v1[0] + v2[0]; dest[1] = v1[1] + v2[1]; dest[2] = v1[2] + v2[2]; - return dest; + return dest.ToArray(); } public static float[] vCopy(float[] @in) @@ -174,6 +184,14 @@ namespace DotRecast.Core @out[1] = b; @out[2] = c; } + + public static void vSet(ref Vector3f @out, float a, float b, float c) + { + @out.x = a; + @out.y = b; + @out.z = c; + } + public static void vCopy(float[] @out, float[] @in) { @@ -181,6 +199,20 @@ namespace DotRecast.Core @out[1] = @in[1]; @out[2] = @in[2]; } + + public static void vCopy(ref Vector3f @out, float[] @in) + { + @out.x = @in[0]; + @out.y = @in[1]; + @out.z = @in[2]; + } + + public static void vCopy(ref Vector3f @out, Vector3f @in) + { + @out.x = @in[0]; + @out.y = @in[1]; + @out.z = @in[2]; + } public static void vCopy(float[] @out, float[] @in, int i) { @@ -188,6 +220,13 @@ namespace DotRecast.Core @out[1] = @in[i + 1]; @out[2] = @in[i + 2]; } + + public static void vCopy(ref Vector3f @out, float[] @in, int i) + { + @out.x = @in[i]; + @out.y = @in[i + 1]; + @out.z = @in[i + 2]; + } public static void vMin(float[] @out, float[] @in, int i) { @@ -195,6 +234,14 @@ namespace DotRecast.Core @out[1] = Math.Min(@out[1], @in[i + 1]); @out[2] = Math.Min(@out[2], @in[i + 2]); } + + public static void vMin(ref Vector3f @out, float[] @in, int i) + { + @out.x = Math.Min(@out.x, @in[i]); + @out.y = Math.Min(@out.y, @in[i + 1]); + @out.z = Math.Min(@out.z, @in[i + 2]); + } + public static void vMax(float[] @out, float[] @in, int i) { @@ -202,6 +249,14 @@ namespace DotRecast.Core @out[1] = Math.Max(@out[1], @in[i + 1]); @out[2] = Math.Max(@out[2], @in[i + 2]); } + + public static void vMax(ref Vector3f @out, float[] @in, int i) + { + @out.x = Math.Max(@out.x, @in[i]); + @out.y = Math.Max(@out.y, @in[i + 1]); + @out.z = Math.Max(@out.z, @in[i + 2]); + } + /// Returns the distance between two points. /// @param[in] v1 A point. [(x, y, z)] @@ -214,6 +269,24 @@ namespace DotRecast.Core float dz = v2[2] - v1[2]; return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); } + + public static float vDist(Vector3f v1, float[] v2) + { + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); + } + + public static float vDist(Vector3f v1, Vector3f v2) + { + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); + } + + /// Returns the distance between two points. /// @param[in] v1 A point. [(x, y, z)] @@ -226,6 +299,15 @@ namespace DotRecast.Core 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]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return dx * dx + dy * dy + dz * dz; + } + /// Derives the square of the scalar length of the vector. (len * len) /// @param[in] v The vector. [(x, y, z)] @@ -289,6 +371,17 @@ namespace DotRecast.Core v[2] *= d; } } + + public static void vNormalize(ref Vector3f v) + { + float d = (float)(1.0f / Math.Sqrt(sqr(v[0]) + sqr(v[1]) + sqr(v[2]))); + if (d != 0) + { + v.x *= d; + v.y *= d; + v.z *= d; + } + } /// Performs a 'sloppy' colocation check of the specified points. @@ -364,6 +457,16 @@ namespace DotRecast.Core float acz = c[2] - a[2]; return acx * abz - abx * acz; } + + public static float triArea2D(Vector3f a, Vector3f b, Vector3f c) + { + float abx = b[0] - a[0]; + float abz = b[2] - a[2]; + float acx = c[0] - a[0]; + float acz = c[2] - a[2]; + return acx * abz - abx * acz; + } + /// Determines if two axis-aligned bounding boxes overlap. /// @param[in] amin Minimum bounds of box A. [(x, y, z)] @@ -521,7 +624,7 @@ namespace DotRecast.Core return ((amin + eps) > bmax || (amax - eps) < bmin) ? false : true; } - static float eps = 1e-4f; + const float eps = 1e-4f; /// @par /// @@ -764,6 +867,23 @@ namespace DotRecast.Core float t = vperpXZ(u, w) / d; return Tuple.Create(s, t); } + + public static Tuple? intersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq) + { + float[] u = vSub(aq, ap); + float[] v = vSub(bq, bp); + float[] w = vSub(ap, bp); + float d = vperpXZ(u, v); + if (Math.Abs(d) < 1e-6f) + { + return null; + } + + float s = vperpXZ(v, w) / d; + float t = vperpXZ(u, w) / d; + return Tuple.Create(s, t); + } + public static float[] vScale(float[] @in, float scale) { diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index aaeecff..f0156b2 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -26,6 +26,16 @@ namespace DotRecast.Core public float y; public float z; + public static Vector3f Of(float[] f) + { + return new Vector3f() + { + x = f[0], + y = f[1], + z = f[2], + }; + } + public Vector3f(float x, float y, float z) { this.x = x; diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index a105f55..381d16a 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -147,7 +147,7 @@ namespace DotRecast.Detour.Crowd private readonly ObstacleAvoidanceQuery.ObstacleAvoidanceParams[] m_obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]; private readonly ObstacleAvoidanceQuery m_obstacleQuery; private ProximityGrid m_grid; - private readonly float[] m_ext = new float[3]; + private readonly Vector3f m_ext = new Vector3f(); private readonly QueryFilter[] m_filters = new QueryFilter[DT_CROWD_MAX_QUERY_FILTER_TYPE]; private NavMeshQuery navQuery; private NavMesh navMesh; @@ -453,7 +453,7 @@ namespace DotRecast.Detour.Crowd bool replan = false; // First check that the current location is valid. - float[] agentPos = new float[3]; + Vector3f agentPos = new Vector3f(); long agentRef = ag.corridor.getFirstPoly(); vCopy(agentPos, ag.npos); if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType])) @@ -605,7 +605,7 @@ namespace DotRecast.Detour.Crowd } List reqPath = pathFound.result; - float[] reqPos = new float[3]; + Vector3f reqPos = new Vector3f(); if (pathFound.succeeded() && reqPath.Count > 0) { // In progress or succeed. @@ -1047,7 +1047,7 @@ namespace DotRecast.Detour.Crowd continue; } - float[] dvel = new float[3]; + Vector3f dvel = new Vector3f(); if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { @@ -1082,7 +1082,7 @@ namespace DotRecast.Detour.Crowd float separationWeight = ag.option.separationWeight; float w = 0; - float[] disp = new float[3]; + Vector3f disp = new Vector3f(); for (int j = 0; j < ag.neis.Count; ++j) { @@ -1156,7 +1156,7 @@ namespace DotRecast.Detour.Crowd for (int j = 0; j < ag.boundary.getSegmentCount(); ++j) { float[] s = ag.boundary.getSegment(j); - float[] s3 = new float[3]; + Vector3f s3 = new Vector3f(); Array.Copy(s, 3, s3, 0, 3); if (triArea2D(ag.npos, s, s3) < 0.0f) { diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index df9cf36..5e240f1 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -77,23 +77,23 @@ namespace DotRecast.Detour.Crowd /// The desired speed. public float desiredSpeed; - public float[] npos = new float[3]; + public Vector3f npos = new Vector3f(); /// < The current agent position. [(x, y, z)] - public float[] disp = new float[3]; + public Vector3f disp = new Vector3f(); /// < A temporary value used to accumulate agent displacement during iterative /// collision resolution. [(x, y, z)] - public float[] dvel = new float[3]; + public Vector3f dvel = new Vector3f(); /// < The desired velocity of the agent. Based on the current path, calculated /// from /// scratch each frame. [(x, y, z)] - public float[] nvel = new float[3]; + public Vector3f nvel = new Vector3f(); /// < The desired velocity adjusted by obstacle avoidance, calculated from scratch each /// frame. [(x, y, z)] - public float[] vel = new float[3]; + public Vector3f vel = new Vector3f(); /// < The actual velocity of the agent. The change from nvel -> vel is /// constrained by max acceleration. [(x, y, z)] @@ -109,7 +109,7 @@ namespace DotRecast.Detour.Crowd public long targetRef; /// < Target polyref of the movement request. - public float[] targetPos = new float[3]; + public Vector3f targetPos = new Vector3f(); /// < Target position of the movement request (or velocity in case of /// DT_CROWDAGENT_TARGET_VELOCITY). @@ -184,7 +184,7 @@ namespace DotRecast.Detour.Crowd public float[] calcSmoothSteerDirection() { - float[] dir = new float[3]; + Vector3f dir = new Vector3f(); if (0 < corners.Count) { int ip0 = 0; @@ -214,7 +214,7 @@ namespace DotRecast.Detour.Crowd public float[] calcStraightSteerDirection() { - float[] dir = new float[3]; + Vector3f dir = new Vector3f(); if (0 < corners.Count) { dir = vSub(corners[0].getPos(), npos); diff --git a/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs b/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs index 45137bf..da1b66c 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs @@ -23,9 +23,9 @@ namespace DotRecast.Detour.Crowd public class CrowdAgentAnimation { public bool active; - public float[] initPos = new float[3]; - public float[] startPos = new float[3]; - public float[] endPos = new float[3]; + public Vector3f initPos = new Vector3f(); + public Vector3f startPos = new Vector3f(); + public Vector3f endPos = new Vector3f(); public long polyRef; public float t, tmax; } diff --git a/src/DotRecast.Detour.Crowd/LocalBoundary.cs b/src/DotRecast.Detour.Crowd/LocalBoundary.cs index 228ed35..20e6d95 100644 --- a/src/DotRecast.Detour.Crowd/LocalBoundary.cs +++ b/src/DotRecast.Detour.Crowd/LocalBoundary.cs @@ -38,7 +38,7 @@ namespace DotRecast.Detour.Crowd public float d; } - float[] m_center = new float[3]; + Vector3f m_center = new Vector3f(); List m_segs = new List(); List m_polys = new List(); diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index 14c111a..f1c2415 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -36,31 +36,31 @@ namespace DotRecast.Detour.Crowd public class ObstacleCircle { /** Position of the obstacle */ - public readonly float[] p = new float[3]; + public readonly Vector3f p = new Vector3f(); /** Velocity of the obstacle */ - public readonly float[] vel = new float[3]; + public readonly Vector3f vel = new Vector3f(); /** Velocity of the obstacle */ - public readonly float[] dvel = new float[3]; + public readonly Vector3f dvel = new Vector3f(); /** Radius of the obstacle */ public float rad; /** Use for side selection during sampling. */ - public readonly float[] dp = new float[3]; + public readonly Vector3f dp = new Vector3f(); /** Use for side selection during sampling. */ - public readonly float[] np = new float[3]; + public readonly Vector3f np = new Vector3f(); } public class ObstacleSegment { /** End points of the obstacle segment */ - public readonly float[] p = new float[3]; + public readonly Vector3f p = new Vector3f(); /** End points of the obstacle segment */ - public readonly float[] q = new float[3]; + public readonly Vector3f q = new Vector3f(); public bool touch; } @@ -205,7 +205,7 @@ namespace DotRecast.Detour.Crowd float[] pb = cir.p; float[] orig = { 0f, 0f, 0f }; - float[] dv = new float[3]; + Vector3f dv = new Vector3f(); vCopy(cir.dp, vSub(pb, pa)); vNormalize(cir.dp); dv = vSub(cir.dvel, dvel); @@ -346,7 +346,7 @@ namespace DotRecast.Detour.Crowd { // Special case when the agent is very close to the segment. float[] sdir = vSub(seg.q, seg.p); - float[] snorm = new float[3]; + Vector3f snorm = new Vector3f(); snorm[0] = -sdir[2]; snorm[2] = sdir[0]; // If the velocity is pointing towards the segment, no collision. @@ -399,7 +399,7 @@ namespace DotRecast.Detour.Crowd m_vmax = vmax; m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue; - float[] nvel = new float[3]; + Vector3f nvel = new Vector3f(); vSet(nvel, 0f, 0f, 0f); if (debug != null) @@ -417,7 +417,7 @@ namespace DotRecast.Detour.Crowd { for (int x = 0; x < m_params.gridSize; ++x) { - float[] vcand = new float[3]; + Vector3f vcand = new Vector3f(); vSet(vcand, cvx + x * cs - half, 0f, cvz + y * cs - half); if (sqr(vcand[0]) + sqr(vcand[2]) > sqr(vmax + cs / 2)) @@ -450,7 +450,7 @@ namespace DotRecast.Detour.Crowd // vector normalization that ignores the y-component. float[] dtRotate2D(float[] v, float ang) { - float[] dest = new float[3]; + Vector3f dest = new Vector3f(); float c = (float)Math.Cos(ang); float s = (float)Math.Sin(ang); dest[0] = v[0] * c - v[2] * s; @@ -470,7 +470,7 @@ namespace DotRecast.Detour.Crowd m_vmax = vmax; m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue; - float[] nvel = new float[3]; + Vector3f nvel = new Vector3f(); vSet(nvel, 0f, 0f, 0f); if (debug != null) @@ -537,18 +537,18 @@ namespace DotRecast.Detour.Crowd // Start sampling. float cr = vmax * (1.0f - m_params.velBias); - float[] res = new float[3]; + Vector3f res = new Vector3f(); vSet(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias); int ns = 0; for (int k = 0; k < depth; ++k) { float minPenalty = float.MaxValue; - float[] bvel = new float[3]; + Vector3f bvel = new Vector3f(); vSet(bvel, 0, 0, 0); for (int i = 0; i < npat; ++i) { - float[] vcand = new float[3]; + Vector3f vcand = new Vector3f(); vSet(vcand, res[0] + pat[i * 2 + 0] * cr, 0f, res[2] + pat[i * 2 + 1] * cr); if (sqr(vcand[0]) + sqr(vcand[2]) > sqr(vmax + 0.001f)) continue; diff --git a/src/DotRecast.Detour.Crowd/PathCorridor.cs b/src/DotRecast.Detour.Crowd/PathCorridor.cs index ff8fc35..e52810c 100644 --- a/src/DotRecast.Detour.Crowd/PathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/PathCorridor.cs @@ -64,8 +64,8 @@ namespace DotRecast.Detour.Crowd */ public class PathCorridor { - private readonly float[] m_pos = new float[3]; - private readonly float[] m_target = new float[3]; + private readonly Vector3f m_pos = new Vector3f(); + private readonly Vector3f m_target = new Vector3f(); private List m_path; protected List mergeCorridorStartMoved(List path, List visited) diff --git a/src/DotRecast.Detour.Crowd/PathQuery.cs b/src/DotRecast.Detour.Crowd/PathQuery.cs index 3cf6872..f106012 100644 --- a/src/DotRecast.Detour.Crowd/PathQuery.cs +++ b/src/DotRecast.Detour.Crowd/PathQuery.cs @@ -23,9 +23,9 @@ namespace DotRecast.Detour.Crowd public class PathQuery { /// Path find start and end location. - public float[] startPos = new float[3]; + public Vector3f startPos = new Vector3f(); - public float[] endPos = new float[3]; + public Vector3f endPos = new Vector3f(); public long startRef; public long endRef; public QueryFilter filter; diff --git a/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs b/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs index 0880662..cbe9c41 100644 --- a/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs +++ b/src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs @@ -23,8 +23,8 @@ namespace DotRecast.Detour.Crowd.Tracking public class CrowdAgentDebugInfo { public CrowdAgent agent; - public float[] optStart = new float[3]; - public float[] optEnd = new float[3]; + public Vector3f optStart = new Vector3f(); + public Vector3f optEnd = new Vector3f(); public ObstacleAvoidanceDebugData vod; } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs b/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs index a6790c8..6ff1b10 100644 --- a/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs +++ b/src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs @@ -102,7 +102,7 @@ namespace DotRecast.Detour.Crowd.Tracking public float[] getSampleVelocity(int i) { - float[] vel = new float[3]; + Vector3f vel = new Vector3f(); vel[0] = m_vel[i * 3]; vel[1] = m_vel[i * 3 + 1]; vel[2] = m_vel[i * 3 + 2]; diff --git a/src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs b/src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs index 31d2e6c..9289f17 100644 --- a/src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs +++ b/src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs @@ -52,7 +52,7 @@ namespace DotRecast.Detour.Dynamic.Io public bool useTiles; public int tileSizeX; public int tileSizeZ; - public float[] rotation = new float[3]; + public Vector3f rotation = new Vector3f(); public float[] bounds = new float[6]; public readonly List tiles = new List(); diff --git a/src/DotRecast.Detour.Dynamic/Io/VoxelFileReader.cs b/src/DotRecast.Detour.Dynamic/Io/VoxelFileReader.cs index 3c8112d..c863fc5 100644 --- a/src/DotRecast.Detour.Dynamic/Io/VoxelFileReader.cs +++ b/src/DotRecast.Detour.Dynamic/Io/VoxelFileReader.cs @@ -101,11 +101,11 @@ namespace DotRecast.Detour.Dynamic.Io int width = buf.getInt(); int depth = buf.getInt(); int borderSize = buf.getInt(); - float[] boundsMin = new float[3]; + Vector3f boundsMin = new Vector3f(); boundsMin[0] = buf.getFloat(); boundsMin[1] = buf.getFloat(); boundsMin[2] = buf.getFloat(); - float[] boundsMax = new float[3]; + Vector3f boundsMax = new Vector3f(); boundsMax[0] = buf.getFloat(); boundsMax[1] = buf.getFloat(); boundsMax[2] = buf.getFloat(); diff --git a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs index 62defa2..65dd210 100644 --- a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs +++ b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs @@ -38,8 +38,8 @@ namespace DotRecast.Detour.Extras NavMeshBuilder.BVItem it = new NavMeshBuilder.BVItem(); items[i] = it; it.i = i; - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); vCopy(bmin, data.verts, data.polys[i].verts[0] * 3); vCopy(bmax, data.verts, data.polys[i].verts[0] * 3); for (int j = 1; j < data.polys[i].vertCount; j++) diff --git a/src/DotRecast.Detour.Extras/Jumplink/Edge.cs b/src/DotRecast.Detour.Extras/Jumplink/Edge.cs index 52a3e12..c5e95bf 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/Edge.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/Edge.cs @@ -2,7 +2,7 @@ namespace DotRecast.Detour.Extras.Jumplink { public class Edge { - public readonly float[] sp = new float[3]; - public readonly float[] sq = new float[3]; + public readonly Vector3f sp = new Vector3f(); + public readonly Vector3f sq = new Vector3f(); } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs index 260f619..13d7d85 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs @@ -9,9 +9,9 @@ namespace DotRecast.Detour.Extras.Jumplink public readonly List end = new List(); public readonly Trajectory trajectory; - public readonly float[] ax = new float[3]; - public readonly float[] ay = new float[3]; - public readonly float[] az = new float[3]; + public readonly Vector3f ax = new Vector3f(); + public readonly Vector3f ay = new Vector3f(); + public readonly Vector3f az = new Vector3f(); public EdgeSampler(Edge edge, Trajectory trajectory) { diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs index f6f856d..e8238d6 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs @@ -28,7 +28,7 @@ namespace DotRecast.Detour.Extras.Jumplink { EdgeSampler es = new EdgeSampler(edge, new JumpTrajectory(acfg.jumpHeight)); es.start.height = acfg.agentClimb * 2; - float[] offset = new float[3]; + Vector3f offset = new Vector3f(); trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb }); vadd(es.start.p, edge.sp, offset); vadd(es.start.q, edge.sq, offset); @@ -56,7 +56,7 @@ namespace DotRecast.Detour.Extras.Jumplink { EdgeSampler es = new EdgeSampler(edge, new ClimbTrajectory()); es.start.height = acfg.agentClimb * 2; - float[] offset = new float[3]; + Vector3f offset = new Vector3f(); trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb }); vadd(es.start.p, edge.sp, offset); vadd(es.start.q, edge.sq, offset); diff --git a/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs b/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs index 8ad5259..e308e78 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs @@ -2,7 +2,7 @@ namespace DotRecast.Detour.Extras.Jumplink { public class GroundSample { - public readonly float[] p = new float[3]; + public readonly Vector3f p = new Vector3f(); public bool validTrajectory; public bool validHeight; } diff --git a/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs b/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs index fb5387f..2e505df 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs @@ -2,8 +2,8 @@ namespace DotRecast.Detour.Extras.Jumplink { public class GroundSegment { - public readonly float[] p = new float[3]; - public readonly float[] q = new float[3]; + public readonly Vector3f p = new Vector3f(); + public readonly Vector3f q = new Vector3f(); public GroundSample[] gsamples; public float height; } diff --git a/src/DotRecast.Detour.TileCache/TileCache.cs b/src/DotRecast.Detour.TileCache/TileCache.cs index 6af7550..bdd5f46 100644 --- a/src/DotRecast.Detour.TileCache/TileCache.cs +++ b/src/DotRecast.Detour.TileCache/TileCache.cs @@ -455,8 +455,8 @@ namespace DotRecast.Detour.TileCache foreach (long i in tiles) { CompressedTile tile = m_tiles[decodeTileIdTile(i)]; - float[] tbmin = new float[3]; - float[] tbmax = new float[3]; + Vector3f tbmin = new Vector3f(); + Vector3f tbmax = new Vector3f(); calcTightTileBounds(tile.header, tbmin, tbmax); if (overlapBounds(bmin, bmax, tbmin, tbmax)) { @@ -499,8 +499,8 @@ namespace DotRecast.Detour.TileCache if (req.action == ObstacleRequestAction.REQUEST_ADD) { // Find touched tiles. - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); getObstacleBounds(ob, bmin, bmax); ob.touched = queryTiles(bmin, bmax); // Add tiles to update list. diff --git a/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs b/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs index 6f0a1f4..7429c59 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheBuilder.cs @@ -1859,8 +1859,8 @@ namespace DotRecast.Detour.TileCache public void markCylinderArea(TileCacheLayer layer, float[] orig, float cs, float ch, float[] pos, float radius, float height, int areaId) { - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); bmin[0] = pos[0] - radius; bmin[1] = pos[1]; bmin[2] = pos[2] - radius; diff --git a/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs b/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs index 1be20d0..50267ed 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs @@ -35,8 +35,8 @@ namespace DotRecast.Detour.TileCache /// < Data version public int tx, ty, tlayer; - public float[] bmin = new float[3]; - public float[] bmax = new float[3]; + public Vector3f bmin = new Vector3f(); + public Vector3f bmax = new Vector3f(); public int hmin, hmax; /// < Height min/max range diff --git a/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs b/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs index 86b5625..7909bb9 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheObstacle.cs @@ -33,12 +33,12 @@ namespace DotRecast.Detour.TileCache public readonly int index; public TileCacheObstacleType type; - public readonly float[] pos = new float[3]; - public readonly float[] bmin = new float[3]; - public readonly float[] bmax = new float[3]; + public readonly Vector3f pos = new Vector3f(); + public readonly Vector3f bmin = new Vector3f(); + public readonly Vector3f bmax = new Vector3f(); public float radius, height; - public readonly float[] center = new float[3]; - public readonly float[] extents = new float[3]; + public readonly Vector3f center = new Vector3f(); + public readonly Vector3f extents = new Vector3f(); public readonly float[] rotAux = new float[2]; // { cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 } public List touched = new List(); public readonly List pending = new List(); diff --git a/src/DotRecast.Detour.TileCache/TileCacheParams.cs b/src/DotRecast.Detour.TileCache/TileCacheParams.cs index 78b59b7..229a27a 100644 --- a/src/DotRecast.Detour.TileCache/TileCacheParams.cs +++ b/src/DotRecast.Detour.TileCache/TileCacheParams.cs @@ -22,7 +22,7 @@ namespace DotRecast.Detour.TileCache { public class TileCacheParams { - public readonly float[] orig = new float[3]; + public readonly Vector3f orig = new Vector3f(); public float cs, ch; public int width, height; public float walkableHeight; diff --git a/src/DotRecast.Detour/ConvexConvexIntersection.cs b/src/DotRecast.Detour/ConvexConvexIntersection.cs index f6a4bee..450b69c 100644 --- a/src/DotRecast.Detour/ConvexConvexIntersection.cs +++ b/src/DotRecast.Detour/ConvexConvexIntersection.cs @@ -17,6 +17,7 @@ freely, subject to the following restrictions: */ using System; +using DotRecast.Core; namespace DotRecast.Detour { @@ -50,10 +51,10 @@ namespace DotRecast.Detour float[] inters = new float[Math.Max(m, n) * 3 * 3]; int ii = 0; /* Initialize variables. */ - float[] a = new float[3]; - float[] b = new float[3]; - float[] a1 = new float[3]; - float[] b1 = new float[3]; + Vector3f a = new Vector3f(); + Vector3f b = new Vector3f(); + Vector3f a1 = new Vector3f(); + Vector3f b1 = new Vector3f(); int aa = 0; int ba = 0; @@ -62,15 +63,15 @@ namespace DotRecast.Detour InFlag f = InFlag.Unknown; bool FirstPoint = true; - float[] ip = new float[3]; - float[] iq = new float[3]; + Vector3f ip = new Vector3f(); + Vector3f iq = new Vector3f(); do { - vCopy(a, p, 3 * (ai % n)); - vCopy(b, q, 3 * (bi % m)); - vCopy(a1, p, 3 * ((ai + n - 1) % n)); // prev a - vCopy(b1, q, 3 * ((bi + m - 1) % m)); // prev b + vCopy(ref a, p, 3 * (ai % n)); + vCopy(ref b, q, 3 * (bi % m)); + vCopy(ref a1, p, 3 * ((ai + n - 1) % n)); // prev a + vCopy(ref b1, q, 3 * ((bi + m - 1) % m)); // prev b float[] A = vSub(a, a1); float[] B = vSub(b, b1); @@ -84,7 +85,7 @@ namespace DotRecast.Detour } bool parallel = cross == 0f; - Intersection code = parallel ? parallelInt(a1, a, b1, b, ip, iq) : segSegInt(a1, a, b1, b, ip, iq); + Intersection code = parallel ? parallelInt(a1, a, b1, b, ref ip, ref iq) : segSegInt(a1, a, b1, b, ref ip, ref iq); if (code == Intersection.Single) { @@ -209,6 +210,28 @@ namespace DotRecast.Detour inters[ii + 2] = p[2]; return ii + 3; } + + private static int addVertex(float[] inters, int ii, Vector3f p) + { + if (ii > 0) + { + if (inters[ii - 3] == p[0] && inters[ii - 2] == p[1] && inters[ii - 1] == p[2]) + { + return ii; + } + + if (inters[0] == p[0] && inters[1] == p[1] && inters[2] == p[2]) + { + return ii; + } + } + + inters[ii] = p[0]; + inters[ii + 1] = p[1]; + inters[ii + 2] = p[2]; + return ii + 3; + } + private static InFlag inOut(InFlag inflag, float aHB, float bHA) { @@ -224,7 +247,7 @@ namespace DotRecast.Detour return inflag; } - private static Intersection segSegInt(float[] a, float[] b, float[] c, float[] d, float[] p, float[] q) + private static Intersection segSegInt(Vector3f a, Vector3f b, Vector3f c, Vector3f d, ref Vector3f p, ref Vector3f q) { var isec = intersectSegSeg2D(a, b, c, d); if (null != isec) @@ -243,54 +266,54 @@ namespace DotRecast.Detour return Intersection.None; } - private static Intersection parallelInt(float[] a, float[] b, float[] c, float[] d, float[] p, float[] q) + private static Intersection parallelInt(Vector3f a, Vector3f b, Vector3f c, Vector3f d, ref Vector3f p, ref Vector3f q) { if (between(a, b, c) && between(a, b, d)) { - vCopy(p, c); - vCopy(q, d); + vCopy(ref p, c); + vCopy(ref q, d); return Intersection.Overlap; } if (between(c, d, a) && between(c, d, b)) { - vCopy(p, a); - vCopy(q, b); + vCopy(ref p, a); + vCopy(ref q, b); return Intersection.Overlap; } if (between(a, b, c) && between(c, d, b)) { - vCopy(p, c); - vCopy(q, b); + vCopy(ref p, c); + vCopy(ref q, b); return Intersection.Overlap; } if (between(a, b, c) && between(c, d, a)) { - vCopy(p, c); - vCopy(q, a); + vCopy(ref p, c); + vCopy(ref q, a); return Intersection.Overlap; } if (between(a, b, d) && between(c, d, b)) { - vCopy(p, d); - vCopy(q, b); + vCopy(ref p, d); + vCopy(ref q, b); return Intersection.Overlap; } if (between(a, b, d) && between(c, d, a)) { - vCopy(p, d); - vCopy(q, a); + vCopy(ref p, d); + vCopy(ref q, a); return Intersection.Overlap; } return Intersection.None; } - private static bool between(float[] a, float[] b, float[] c) + private static bool between(Vector3f a, Vector3f b, Vector3f c) { if (Math.Abs(a[0] - b[0]) > Math.Abs(a[2] - b[2])) { diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index 8c65049..ca3c075 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -60,7 +60,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 = vDist(startPos, endPos) * H_SCALE; @@ -148,7 +148,7 @@ namespace DotRecast.Detour neighbourTile); if (!midpod.failed()) { - neighbourNode.pos = midpod.result; + neighbourNode.pos = Vector3f.Of(midpod.result); } } @@ -160,9 +160,9 @@ namespace DotRecast.Detour if (neighbourRef == endRef) { // Cost - float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly, + float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); - float endCost = filter.getCost(neighbourNode.pos, endPos, bestRef, bestTile, bestPoly, neighbourRef, + float endCost = filter.getCost(neighbourNode.pos.ToArray(), endPos, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0L, null, null); cost = bestNode.cost + curCost + endCost; @@ -171,7 +171,7 @@ namespace DotRecast.Detour else { // Cost - float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly, + float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); cost = bestNode.cost + curCost; heuristic = vDist(neighbourNode.pos, endPos) * H_SCALE; @@ -365,7 +365,7 @@ namespace DotRecast.Detour neighbourTile); if (!midpod.failed()) { - neighbourNode.pos = midpod.result; + neighbourNode.pos = Vector3f.Of(midpod.result); } } @@ -669,7 +669,7 @@ namespace DotRecast.Detour m_openList.push(startNode); float radiusSqr = sqr(maxRadius); - float[] hitPos = new float[3]; + Vector3f hitPos = new Vector3f(); VectorPtr bestvj = null; VectorPtr bestvi = null; while (!m_openList.isEmpty()) @@ -842,7 +842,7 @@ namespace DotRecast.Detour } // Calc hit normal. - float[] hitNormal = new float[3]; + Vector3f hitNormal = new Vector3f(); if (bestvi != null && bestvj != null) { float[] tangent = vSub(bestvi, bestvj); diff --git a/src/DotRecast.Detour/MeshHeader.cs b/src/DotRecast.Detour/MeshHeader.cs index 582e377..858fbd9 100644 --- a/src/DotRecast.Detour/MeshHeader.cs +++ b/src/DotRecast.Detour/MeshHeader.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 { /** Provides high level information related to a dtMeshTile object. */ @@ -95,10 +97,10 @@ namespace DotRecast.Detour public float walkableClimb; /** The minimum bounds of the tile's AABB. [(x, y, z)] */ - public readonly float[] bmin = new float[3]; + public Vector3f bmin = new Vector3f(); /** The maximum bounds of the tile's AABB. [(x, y, z)] */ - public readonly float[] bmax = new float[3]; + public Vector3f bmax = new Vector3f(); /** The bounding volume quantization factor. */ public float bvQuantFactor; diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index 45a080f..ae15590 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -55,7 +55,7 @@ namespace DotRecast.Detour private readonly NavMeshParams m_params; /// < Current initialization params. TODO: do not store this info twice. - private readonly float[] m_orig; + private readonly Vector3f m_orig; /// < Origin of the tile (0,0) // float m_orig[3]; ///< Origin of the tile (0,0) @@ -324,7 +324,7 @@ namespace DotRecast.Detour private static NavMeshParams getNavMeshParams(MeshData data) { NavMeshParams option = new NavMeshParams(); - vCopy(option.orig, data.header.bmin); + vCopy(ref option.orig, data.header.bmin); option.tileWidth = data.header.bmax[0] - data.header.bmin[0]; option.tileHeight = data.header.bmax[2] - data.header.bmin[2]; option.maxTiles = 1; @@ -341,8 +341,8 @@ namespace DotRecast.Detour if (tile.data.bvTree != null) { int nodeIndex = 0; - float[] tbmin = tile.data.header.bmin; - float[] tbmax = tile.data.header.bmax; + var tbmin = tile.data.header.bmin; + var tbmax = tile.data.header.bmax; float qfac = tile.data.header.bvQuantFactor; // Calculate quantized box int[] bmin = new int[3]; @@ -391,8 +391,8 @@ namespace DotRecast.Detour } else { - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); long @base = getPolyRefBase(tile); for (int i = 0; i < tile.data.header.polyCount; ++i) { @@ -405,13 +405,13 @@ namespace DotRecast.Detour // Calc polygon bounds. int v = p.verts[0] * 3; - vCopy(bmin, tile.data.verts, v); - vCopy(bmax, tile.data.verts, v); + vCopy(ref bmin, tile.data.verts, v); + vCopy(ref bmax, tile.data.verts, v); for (int j = 1; j < p.vertCount; ++j) { v = p.verts[j] * 3; - vMin(bmin, tile.data.verts, v); - vMax(bmax, tile.data.verts, v); + vMin(ref bmin, tile.data.verts, v); + vMax(ref bmax, tile.data.verts, v); } if (overlapBounds(qmin, qmax, bmin, bmax)) @@ -836,7 +836,7 @@ namespace DotRecast.Detour float[] ext = new float[] { targetCon.rad, target.data.header.walkableClimb, targetCon.rad }; // Find polygon to connect to. - float[] p = new float[3]; + Vector3f p = new Vector3f(); p[0] = targetCon.pos[3]; p[1] = targetCon.pos[4]; p[2] = targetCon.pos[5]; @@ -1309,7 +1309,7 @@ namespace DotRecast.Detour Tuple tileAndPoly = getTileAndPolyByRefUnsafe(refs); MeshTile tile = tileAndPoly.Item1; Poly poly = tileAndPoly.Item2; - float[] closest = new float[3]; + Vector3f closest = new Vector3f(); vCopy(closest, pos); float? h = getPolyHeight(tile, poly, pos); if (null != h) @@ -1555,8 +1555,8 @@ namespace DotRecast.Detour } } - float[] startPos = new float[3]; - float[] endPos = new float[3]; + Vector3f startPos = new Vector3f(); + Vector3f endPos = new Vector3f(); vCopy(startPos, tile.data.verts, poly.verts[idx0] * 3); vCopy(endPos, tile.data.verts, poly.verts[idx1] * 3); return Results.success(Tuple.Create(startPos, endPos)); diff --git a/src/DotRecast.Detour/NavMeshBuilder.cs b/src/DotRecast.Detour/NavMeshBuilder.cs index f25bf04..1390216 100644 --- a/src/DotRecast.Detour/NavMeshBuilder.cs +++ b/src/DotRecast.Detour/NavMeshBuilder.cs @@ -190,15 +190,15 @@ namespace DotRecast.Detour { int vb = option.detailMeshes[i * 4 + 0]; int ndv = option.detailMeshes[i * 4 + 1]; - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); int dv = vb * 3; - vCopy(bmin, option.detailVerts, dv); - vCopy(bmax, option.detailVerts, dv); + vCopy(ref bmin, option.detailVerts, dv); + vCopy(ref bmax, option.detailVerts, dv); for (int j = 1; j < ndv; j++) { - vMin(bmin, option.detailVerts, dv + j * 3); - vMax(bmax, option.detailVerts, dv + j * 3); + vMin(ref bmin, option.detailVerts, dv + j * 3); + vMax(ref bmax, option.detailVerts, dv + j * 3); } // BV-tree uses cs for all dimensions @@ -341,8 +341,8 @@ namespace DotRecast.Detour hmin -= option.walkableClimb; hmax += option.walkableClimb; - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); vCopy(bmin, option.bmin); vCopy(bmax, option.bmax); bmin[1] = hmin; diff --git a/src/DotRecast.Detour/NavMeshParams.cs b/src/DotRecast.Detour/NavMeshParams.cs index 43e22e2..67dee5c 100644 --- a/src/DotRecast.Detour/NavMeshParams.cs +++ b/src/DotRecast.Detour/NavMeshParams.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 { /** @@ -29,7 +31,7 @@ namespace DotRecast.Detour public class NavMeshParams { /** The world space origin of the navigation mesh's tile space. [(x, y, z)] */ - public readonly float[] orig = new float[3]; + public Vector3f orig = new Vector3f(); /** The width of each tile. (Along the x-axis.) */ public float tileWidth; diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 3e935ce..edd5e2c 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -263,7 +263,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; @@ -382,7 +382,7 @@ namespace DotRecast.Detour // Cost if (neighbourNode.flags == 0) { - neighbourNode.pos = vLerp(va, vb, 0.5f); + neighbourNode.pos = Vector3f.Of(vLerp(va, vb, 0.5f)); } float total = bestNode.total + vDist(bestNode.pos, neighbourNode.pos); @@ -650,8 +650,8 @@ namespace DotRecast.Detour } else { - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); long @base = m_nav.getPolyRefBase(tile); for (int i = 0; i < tile.data.header.polyCount; ++i) { @@ -1843,7 +1843,7 @@ namespace DotRecast.Detour LinkedList stack = new LinkedList(); stack.AddLast(startNode); - float[] bestPos = new float[3]; + Vector3f bestPos = new Vector3f(); float bestDist = float.MaxValue; Node bestNode = null; vCopy(bestPos, startPos); @@ -2045,8 +2045,8 @@ namespace DotRecast.Detour protected Result getPortalPoints(long from, Poly fromPoly, MeshTile fromTile, long to, Poly toPoly, MeshTile toTile, int fromType, int toType) { - float[] left = new float[3]; - float[] right = new float[3]; + Vector3f left = new Vector3f(); + Vector3f right = new Vector3f(); // Find the link that points to the 'to' polygon. Link link = null; for (int i = fromTile.polyLinks[fromPoly.index]; i != NavMesh.DT_NULL_LINK; i = fromTile.links[i].next) @@ -2132,7 +2132,7 @@ namespace DotRecast.Detour float[] left = ppoints.result.left; float[] right = ppoints.result.right; - float[] mid = new float[3]; + Vector3f mid = new Vector3f(); mid[0] = (left[0] + right[0]) * 0.5f; mid[1] = (left[1] + right[1]) * 0.5f; mid[2] = (left[2] + right[2]) * 0.5f; @@ -2230,9 +2230,9 @@ namespace DotRecast.Detour float[] verts = new float[m_nav.getMaxVertsPerPoly() * 3 + 3]; - float[] curPos = new float[3], lastPos = new float[3]; + Vector3f curPos = new Vector3f(), lastPos = new Vector3f(); - vCopy(curPos, startPos); + vCopy(ref curPos, startPos); float[] dir = vSub(endPos, startPos); MeshTile prevTile, tile, nextTile; @@ -3230,7 +3230,7 @@ namespace DotRecast.Detour m_openList.push(startNode); float radiusSqr = sqr(maxRadius); - float[] hitPos = new float[3]; + Vector3f hitPos = new Vector3f(); VectorPtr bestvj = null; VectorPtr bestvi = null; while (!m_openList.isEmpty()) @@ -3403,7 +3403,7 @@ namespace DotRecast.Detour } // Calc hit normal. - float[] hitNormal = new float[3]; + Vector3f hitNormal = new Vector3f(); if (bestvi != null && bestvj != null) { float[] tangent = vSub(bestvi, bestvj); diff --git a/src/DotRecast.Detour/Node.cs b/src/DotRecast.Detour/Node.cs index bb0645f..f60e174 100644 --- a/src/DotRecast.Detour/Node.cs +++ b/src/DotRecast.Detour/Node.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System.Collections.Generic; +using DotRecast.Core; namespace DotRecast.Detour { @@ -33,7 +34,7 @@ namespace DotRecast.Detour public readonly int index; /** Position of the node. */ - public float[] pos = new float[3]; + public Vector3f pos = new Vector3f(); /** Cost of reaching the given node. */ public float cost; diff --git a/src/DotRecast.Detour/QueryData.cs b/src/DotRecast.Detour/QueryData.cs index 011622d..ae6b05e 100644 --- a/src/DotRecast.Detour/QueryData.cs +++ b/src/DotRecast.Detour/QueryData.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 { public class QueryData @@ -26,8 +28,8 @@ namespace DotRecast.Detour public Node lastBestNode; public float lastBestNodeCost; public long startRef, endRef; - public float[] startPos = new float[3]; - public float[] endPos = new float[3]; + public Vector3f startPos = new Vector3f(); + public Vector3f endPos = new Vector3f(); public QueryFilter filter; public int options; public float raycastLimitSqr; diff --git a/src/DotRecast.Detour/RaycastHit.cs b/src/DotRecast.Detour/RaycastHit.cs index ff00d75..f56b3b0 100644 --- a/src/DotRecast.Detour/RaycastHit.cs +++ b/src/DotRecast.Detour/RaycastHit.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System.Collections.Generic; +using DotRecast.Core; namespace DotRecast.Detour { @@ -31,7 +32,7 @@ namespace DotRecast.Detour public float t; /** hitNormal The normal of the nearest wall hit. [(x, y, z)] */ - public readonly float[] hitNormal = new float[3]; + public readonly Vector3f hitNormal = new Vector3f(); /** Visited polygons. */ public readonly List path = new List(); diff --git a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs index c4d4dda..d57ff39 100644 --- a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs +++ b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs @@ -253,12 +253,12 @@ public class DebugDraw float dy = y1 - y0; float dz = z1 - z0; float len = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); - float[] prev = new float[3]; + Vector3f prev = new Vector3f(); evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, prev); for (int i = 1; i <= NUM_ARC_PTS; ++i) { float u = PAD + i * ARC_PTS_SCALE; - float[] pt = new float[3]; + Vector3f pt = new Vector3f(); evalArc(x0, y0, z0, dx, dy, dz, len * h, u, pt); vertex(prev[0], prev[1], prev[2], col); vertex(pt[0], pt[1], pt[2], col); @@ -270,7 +270,7 @@ public class DebugDraw // End arrows if (as0 > 0.001f) { - float[] p = new float[3], q = new float[3]; + Vector3f p = new Vector3f(), q = new float[3]; evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, p); evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD + 0.05f, q); appendArrowHead(p, q, as0, col); @@ -278,7 +278,7 @@ public class DebugDraw if (as1 > 0.001f) { - float[] p = new float[3], q = new float[3]; + Vector3f p = new Vector3f(), q = new float[3]; evalArc(x0, y0, z0, dx, dy, dz, len * h, 1 - PAD, p); evalArc(x0, y0, z0, dx, dy, dz, len * h, 1 - (PAD + 0.05f), q); appendArrowHead(p, q, as1, col); @@ -392,7 +392,7 @@ public class DebugDraw return; } - float[] ax = new float[3], ay = { 0, 1, 0 }, az = new float[3]; + Vector3f ax = new Vector3f(), ay = { 0, 1, 0 }, az = new float[3]; vsub(az, q, p); vnormalize(az); vcross(ax, ay, az); diff --git a/src/DotRecast.Recast.Demo/Draw/RecastDebugDraw.cs b/src/DotRecast.Recast.Demo/Draw/RecastDebugDraw.cs index 6200817..4b12c56 100644 --- a/src/DotRecast.Recast.Demo/Draw/RecastDebugDraw.cs +++ b/src/DotRecast.Recast.Demo/Draw/RecastDebugDraw.cs @@ -577,7 +577,7 @@ public class RecastDebugDraw : DebugDraw private float[] getContourCenter(Contour cont, float[] orig, float cs, float ch) { - float[] center = new float[3]; + Vector3f center = new Vector3f(); center[0] = 0; center[1] = 0; center[2] = 0; diff --git a/src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs b/src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs index ccd228c..1606f37 100644 --- a/src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs +++ b/src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs @@ -100,7 +100,7 @@ public class DemoInputGeomProvider : InputGeomProvider int v0 = faces[i] * 3; int v1 = faces[i + 1] * 3; int v2 = faces[i + 2] * 3; - float[] e0 = new float[3], e1 = new float[3]; + Vector3f e0 = new Vector3f(), e1 = new float[3]; for (int j = 0; j < 3; ++j) { e0[j] = vertices[v1 + j] - vertices[v0 + j]; diff --git a/src/DotRecast.Recast.Demo/Geom/Intersections.cs b/src/DotRecast.Recast.Demo/Geom/Intersections.cs index 34adb1c..0f3ef86 100644 --- a/src/DotRecast.Recast.Demo/Geom/Intersections.cs +++ b/src/DotRecast.Recast.Demo/Geom/Intersections.cs @@ -82,7 +82,7 @@ public class Intersections { float EPS = 1e-6f; - float[] d = new float[3]; + Vector3f d = new Vector3f(); d[0] = sq[0] - sp[0]; d[1] = sq[1] - sp[1]; d[2] = sq[2] - sp[2]; diff --git a/src/DotRecast.Recast.Demo/RecastDemo.cs b/src/DotRecast.Recast.Demo/RecastDemo.cs index 4d37d15..b499c2c 100644 --- a/src/DotRecast.Recast.Demo/RecastDemo.cs +++ b/src/DotRecast.Recast.Demo/RecastDemo.cs @@ -91,13 +91,13 @@ public class RecastDemo private float scrollZoom; private readonly float[] origMousePos = new float[2]; private readonly float[] origCameraEulers = new float[2]; - private readonly float[] origCameraPos = new float[3]; + private readonly Vector3f origCameraPos = new Vector3f(); private readonly float[] cameraEulers = { 45, -45 }; private readonly float[] cameraPos = { 0, 0, 0 }; - private readonly float[] rayStart = new float[3]; - private readonly float[] rayEnd = new float[3]; + private readonly Vector3f rayStart = new Vector3f(); + private readonly 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 float[] markerPosition = new float[3]; + private readonly Vector3f markerPosition = new Vector3f(); private ToolsView toolsUI; private RcSettingsView settingsUI; private long prevFrameTime; @@ -656,7 +656,7 @@ public class RecastDemo } else { - float[] pos = new float[3]; + Vector3f pos = new Vector3f(); pos[0] = rayStart[0] + (rayEnd[0] - rayStart[0]) * hitTime; pos[1] = rayStart[1] + (rayEnd[1] - rayStart[1]) * hitTime; pos[2] = rayStart[2] + (rayEnd[2] - rayStart[2]) * hitTime; diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs index b804b53..e4ea4a3 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs @@ -420,7 +420,7 @@ public class CrowdProfilingTool private class AgentData { public readonly AgentType type; - public readonly float[] home = new float[3]; + public readonly Vector3f home = new Vector3f(); public AgentData(AgentType type, float[] home) { diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs index b658c43..8dce152 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs @@ -265,7 +265,7 @@ public class CrowdTool : Tool foreach (CrowdAgent ag in crowd.getActiveAgents()) { - float[] bmin = new float[3], bmax = new float[3]; + Vector3f bmin = new Vector3f(), bmax = new float[3]; getAgentBounds(ag, bmin, bmax); float[] isect = Intersections.intersectSegmentAABB(s, p, bmin, bmax); if (null != isect) @@ -432,7 +432,7 @@ public class CrowdTool : Tool float[] pos = ag.npos; dd.begin(LINES, 3.0f); - float[] prev = new float[3]; + Vector3f prev = new Vector3f(); float preva = 1; vCopy(prev, pos); for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j) diff --git a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs index 3887ba9..0706a61 100644 --- a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs @@ -340,8 +340,8 @@ public class DynamicUpdateTool : Tool float[] ry = GLU.build_4x4_rotation_matrix((float)random.NextDouble() * 360, 0, 1, 0); float[] m = GLU.mul(rx, ry); float[] verts = new float[geom.vertices.Length]; - float[] v = new float[3]; - float[] vr = new float[3]; + Vector3f v = new Vector3f(); + Vector3f vr = new Vector3f(); for (int i = 0; i < geom.vertices.Length; i += 3) { v[0] = geom.vertices[i]; diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs index 0e81ab2..3ee4789 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs @@ -35,7 +35,7 @@ public class CapsuleGizmo : ColliderGizmo float halfLength = 0.5f * vLen(axis); vertices = new float[spVertices.Length]; gradient = new float[spVertices.Length / 3]; - float[] v = new float[3]; + Vector3f v = new Vector3f(); for (int i = 0; i < spVertices.Length; i += 3) { float offset = (i >= spVertices.Length / 2) ? -halfLength : halfLength; @@ -61,7 +61,7 @@ public class CapsuleGizmo : ColliderGizmo side = new float[] { 0, 0, 1 }; } - float[] forward = new float[3]; + Vector3f forward = new Vector3f(); cross(forward, side, axis); cross(side, axis, forward); normalize(side); diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs index 0c38615..1fcbd01 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs @@ -35,7 +35,7 @@ public class CylinderGizmo : ColliderGizmo vertices = generateCylindricalVertices(); float halfLength = 0.5f * vLen(axis); gradient = new float[vertices.Length / 3]; - float[] v = new float[3]; + Vector3f v = new Vector3f(); for (int i = 0; i < vertices.Length; i += 3) { float offset = (i >= vertices.Length / 2) ? -halfLength : halfLength; @@ -68,7 +68,7 @@ public class CylinderGizmo : ColliderGizmo side = new float[] { 0, 0, 1 }; } - float[] forward = new float[3]; + Vector3f forward = new Vector3f(); cross(forward, side, axis); cross(side, axis, forward); normalize(side); diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs index 2c041f0..dc2a962 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs @@ -175,8 +175,8 @@ public class GizmoHelper public static int getColorByNormal(float[] vertices, int v0, int v1, int v2) { - float[] e0 = new float[3], e1 = new float[3]; - float[] normal = new float[3]; + Vector3f e0 = new Vector3f(), e1 = new float[3]; + Vector3f normal = new Vector3f(); for (int j = 0; j < 3; ++j) { e0[j] = vertices[v1 + j] - vertices[v0 + j]; diff --git a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs b/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs index 506443a..acbe337 100644 --- a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs @@ -431,7 +431,7 @@ public class JumpLinkBuilderTool : Tool { int area = SampleAreaModifications.SAMPLE_POLYAREA_TYPE_JUMP_AUTO; int flags = SampleAreaModifications.SAMPLE_POLYFLAGS_JUMP; - float[] prev = new float[3]; + Vector3f prev = new Vector3f(); for (int i = 0; i < link.startSamples.Length; i++) { float[] p = link.startSamples[i].p; diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 207ca23..850f3c7 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -949,7 +949,7 @@ public class TestNavmeshTool : Tool private float[] getPolyCenter(NavMesh navMesh, long refs) { - float[] center = new float[3]; + Vector3f center = new Vector3f(); center[0] = 0; center[1] = 0; center[2] = 0; @@ -993,7 +993,7 @@ public class TestNavmeshTool : Tool if (m_polys != null) { // In case of partial path, make sure the end point is clamped to the last polygon. - float[] epos = new float[3]; + Vector3f epos = new Vector3f(); vCopy(epos, m_epos); if (m_polys[m_polys.Count - 1] != m_endRef) { diff --git a/src/DotRecast.Recast/CompactHeightfield.cs b/src/DotRecast.Recast/CompactHeightfield.cs index 900e6fe..929559c 100644 --- a/src/DotRecast.Recast/CompactHeightfield.cs +++ b/src/DotRecast.Recast/CompactHeightfield.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 { /** A compact, static heightfield representing unobstructed space. */ @@ -48,10 +50,10 @@ namespace DotRecast.Recast public int maxRegions; /** The minimum bounds in world space. [(x, y, z)] */ - public float[] bmin = new float[3]; + public Vector3f bmin = new Vector3f(); /** The maximum bounds in world space. [(x, y, z)] */ - public float[] bmax = new float[3]; + public Vector3f bmax = new Vector3f(); /** The size of each cell. (On the xz-plane.) */ public float cs; diff --git a/src/DotRecast.Recast/ContourSet.cs b/src/DotRecast.Recast/ContourSet.cs index 29fc19e..2c5f0c1 100644 --- a/src/DotRecast.Recast/ContourSet.cs +++ b/src/DotRecast.Recast/ContourSet.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System.Collections.Generic; +using DotRecast.Core; namespace DotRecast.Recast { @@ -29,10 +30,10 @@ namespace DotRecast.Recast public List conts = new List(); /** The minimum bounds in world space. [(x, y, z)] */ - public float[] bmin = new float[3]; + public Vector3f bmin = new Vector3f(); /** The maximum bounds in world space. [(x, y, z)] */ - public float[] bmax = new float[3]; + public Vector3f bmax = new Vector3f(); /** The size of each cell. (On the xz-plane.) */ public float cs; diff --git a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs index 1c34f96..c5aa81b 100644 --- a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs +++ b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs @@ -21,6 +21,7 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; using System.Collections.Immutable; +using DotRecast.Core; namespace DotRecast.Recast.Geom { @@ -114,7 +115,7 @@ namespace DotRecast.Recast.Geom int v0 = faces[i] * 3; int v1 = faces[i + 1] * 3; int v2 = faces[i + 2] * 3; - float[] e0 = new float[3], e1 = new float[3]; + Vector3f e0 = new Vector3f(), e1 = new Vector3f(); for (int j = 0; j < 3; ++j) { e0[j] = vertices[v1 + j] - vertices[v0 + j]; diff --git a/src/DotRecast.Recast/HeightfieldLayerSet.cs b/src/DotRecast.Recast/HeightfieldLayerSet.cs index b9a04a9..d1d58ae 100644 --- a/src/DotRecast.Recast/HeightfieldLayerSet.cs +++ b/src/DotRecast.Recast/HeightfieldLayerSet.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 set of heightfield layers. @@ -29,10 +31,10 @@ namespace DotRecast.Recast /// @see rcHeightfieldLayerSet public class HeightfieldLayer { - public readonly float[] bmin = new float[3]; + public Vector3f bmin = new Vector3f(); /// < The minimum bounds in world space. [(x, y, z)] - public readonly float[] bmax = new float[3]; + public Vector3f bmax = new Vector3f(); /// < The maximum bounds in world space. [(x, y, z)] public float cs; diff --git a/src/DotRecast.Recast/PolyMesh.cs b/src/DotRecast.Recast/PolyMesh.cs index 2d91864..3879b5d 100644 --- a/src/DotRecast.Recast/PolyMesh.cs +++ b/src/DotRecast.Recast/PolyMesh.cs @@ -17,6 +17,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 polygon mesh suitable for use in building a navigation mesh. */ @@ -50,10 +52,10 @@ namespace DotRecast.Recast public int[] flags; /** The minimum bounds in world space. [(x, y, z)] */ - public readonly float[] bmin = new float[3]; + public Vector3f bmin = new Vector3f(); /** The maximum bounds in world space. [(x, y, z)] */ - public readonly float[] bmax = new float[3]; + public Vector3f bmax = new Vector3f(); /** The size of each cell. (On the xz-plane.) */ public float cs; diff --git a/src/DotRecast.Recast/Recast.cs b/src/DotRecast.Recast/Recast.cs index aafccdc..6dc89bf 100644 --- a/src/DotRecast.Recast/Recast.cs +++ b/src/DotRecast.Recast/Recast.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using DotRecast.Core; namespace DotRecast.Recast { @@ -49,6 +50,17 @@ namespace DotRecast.Recast { return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) }; } + + public static int[] calcGridSize(Vector3f bmin, float[] bmax, float cs) + { + return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) }; + } + + public static int[] calcGridSize(Vector3f bmin, Vector3f bmax, float cs) + { + return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) }; + } + public static int[] calcTileCount(float[] bmin, float[] bmax, float cs, int tileSizeX, int tileSizeZ) { @@ -72,11 +84,11 @@ namespace DotRecast.Recast { int[] areas = new int[nt]; float walkableThr = (float)Math.Cos(walkableSlopeAngle / 180.0f * Math.PI); - float[] norm = new float[3]; + Vector3f norm = new Vector3f(); for (int i = 0; i < nt; ++i) { int tri = i * 3; - calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], norm); + calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], ref norm); // Check if the face is walkable. if (norm[1] > walkableThr) areas[i] = areaMod.apply(areas[i]); @@ -87,13 +99,24 @@ namespace DotRecast.Recast static void calcTriNormal(float[] verts, int v0, int v1, int v2, float[] norm) { - float[] e0 = new float[3]; - float[] e1 = new float[3]; - RecastVectors.sub(e0, verts, v1 * 3, v0 * 3); - RecastVectors.sub(e1, verts, v2 * 3, v0 * 3); + Vector3f e0 = new Vector3f(); + Vector3f e1 = new Vector3f(); + RecastVectors.sub(ref e0, verts, v1 * 3, v0 * 3); + RecastVectors.sub(ref e1, verts, v2 * 3, v0 * 3); RecastVectors.cross(norm, e0, e1); RecastVectors.normalize(norm); } + + static void calcTriNormal(float[] verts, int v0, int v1, int v2, ref Vector3f norm) + { + Vector3f e0 = new Vector3f(); + Vector3f e1 = new Vector3f(); + RecastVectors.sub(ref e0, verts, v1 * 3, v0 * 3); + RecastVectors.sub(ref e1, verts, v2 * 3, v0 * 3); + RecastVectors.cross(ref norm, e0, e1); + RecastVectors.normalize(ref norm); + } + /// @par /// @@ -108,12 +131,12 @@ namespace DotRecast.Recast { float walkableThr = (float)Math.Cos(walkableSlopeAngle / 180.0f * Math.PI); - float[] norm = new float[3]; + Vector3f norm = new Vector3f(); for (int i = 0; i < nt; ++i) { int tri = i * 3; - calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], norm); + calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], ref norm); // Check if the face is walkable. if (norm[1] <= walkableThr) areas[i] = RC_NULL_AREA; diff --git a/src/DotRecast.Recast/RecastArea.cs b/src/DotRecast.Recast/RecastArea.cs index 423e6d6..14fae9a 100644 --- a/src/DotRecast.Recast/RecastArea.cs +++ b/src/DotRecast.Recast/RecastArea.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using DotRecast.Core; namespace DotRecast.Recast { @@ -331,7 +332,7 @@ namespace DotRecast.Recast ctx.stopTimer("MARK_BOX_AREA"); } - static bool pointInPoly(float[] verts, float[] p) + static bool pointInPoly(float[] verts, Vector3f p) { bool c = false; int i, j; @@ -361,14 +362,14 @@ namespace DotRecast.Recast { ctx.startTimer("MARK_CONVEXPOLY_AREA"); - float[] bmin = new float[3]; - float[] bmax = new float[3]; - RecastVectors.copy(bmin, verts, 0); - RecastVectors.copy(bmax, verts, 0); + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); + RecastVectors.copy(ref bmin, verts, 0); + RecastVectors.copy(ref bmax, verts, 0); for (int i = 3; i < verts.Length; i += 3) { - RecastVectors.min(bmin, verts, i); - RecastVectors.max(bmax, verts, i); + RecastVectors.min(ref bmin, verts, i); + RecastVectors.max(ref bmax, verts, i); } bmin[1] = hmin; @@ -412,7 +413,7 @@ namespace DotRecast.Recast continue; if (s.y >= miny && s.y <= maxy) { - float[] p = new float[3]; + Vector3f p = new Vector3f(); p[0] = chf.bmin[0] + (x + 0.5f) * chf.cs; p[1] = 0; p[2] = chf.bmin[2] + (z + 0.5f) * chf.cs; @@ -517,8 +518,8 @@ namespace DotRecast.Recast { ctx.startTimer("MARK_CYLINDER_AREA"); - float[] bmin = new float[3]; - float[] bmax = new float[3]; + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); bmin[0] = pos[0] - r; bmin[1] = pos[1]; bmin[2] = pos[2] - r; diff --git a/src/DotRecast.Recast/RecastBuilderConfig.cs b/src/DotRecast.Recast/RecastBuilderConfig.cs index 5e9ba0b..4aa1e0a 100644 --- a/src/DotRecast.Recast/RecastBuilderConfig.cs +++ b/src/DotRecast.Recast/RecastBuilderConfig.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 { using static RecastVectors; @@ -36,10 +38,10 @@ namespace DotRecast.Recast public readonly int height; /** The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu] **/ - public readonly float[] bmin = new float[3]; + public readonly Vector3f bmin = new Vector3f(); /** The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu] **/ - public readonly float[] bmax = new float[3]; + public readonly Vector3f bmax = new Vector3f(); public RecastBuilderConfig(RecastConfig cfg, float[] bmin, float[] bmax) : this(cfg, bmin, bmax, 0, 0) { @@ -50,8 +52,8 @@ namespace DotRecast.Recast this.tileX = tileX; this.tileZ = tileZ; this.cfg = cfg; - copy(this.bmin, bmin); - copy(this.bmax, bmax); + copy(ref this.bmin, bmin); + copy(ref this.bmax, bmax); if (cfg.useTiles) { float tsx = cfg.tileSizeX * cfg.cs; diff --git a/src/DotRecast.Recast/RecastCompact.cs b/src/DotRecast.Recast/RecastCompact.cs index 275041d..fd0344c 100644 --- a/src/DotRecast.Recast/RecastCompact.cs +++ b/src/DotRecast.Recast/RecastCompact.cs @@ -56,8 +56,8 @@ namespace DotRecast.Recast chf.walkableHeight = walkableHeight; chf.walkableClimb = walkableClimb; chf.maxRegions = 0; - copy(chf.bmin, hf.bmin); - copy(chf.bmax, hf.bmax); + copy(ref chf.bmin, hf.bmin); + copy(ref chf.bmax, hf.bmax); chf.bmax[1] += walkableHeight * hf.ch; chf.cs = hf.cs; chf.ch = hf.ch; diff --git a/src/DotRecast.Recast/RecastContour.cs b/src/DotRecast.Recast/RecastContour.cs index 2b6617d..731fa01 100644 --- a/src/DotRecast.Recast/RecastContour.cs +++ b/src/DotRecast.Recast/RecastContour.cs @@ -781,8 +781,8 @@ namespace DotRecast.Recast ContourSet cset = new ContourSet(); ctx.startTimer("CONTOURS"); - RecastVectors.copy(cset.bmin, chf.bmin, 0); - RecastVectors.copy(cset.bmax, chf.bmax, 0); + RecastVectors.copy(ref cset.bmin, chf.bmin, 0); + RecastVectors.copy(ref cset.bmax, chf.bmax, 0); if (borderSize > 0) { // If the heightfield was build with bordersize, remove the offset. diff --git a/src/DotRecast.Recast/RecastLayers.cs b/src/DotRecast.Recast/RecastLayers.cs index 403c619..756562a 100644 --- a/src/DotRecast.Recast/RecastLayers.cs +++ b/src/DotRecast.Recast/RecastLayers.cs @@ -20,6 +20,7 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; +using DotRecast.Core; namespace DotRecast.Recast { @@ -427,10 +428,10 @@ namespace DotRecast.Recast int lh = h - borderSize * 2; // Build contracted bbox for layers. - float[] bmin = new float[3]; - float[] bmax = new float[3]; - copy(bmin, chf.bmin); - copy(bmax, chf.bmax); + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); + copy(ref bmin, chf.bmin); + copy(ref bmax, chf.bmax); bmin[0] += borderSize * chf.cs; bmin[2] += borderSize * chf.cs; bmax[0] -= borderSize * chf.cs; @@ -474,8 +475,8 @@ namespace DotRecast.Recast layer.ch = chf.ch; // Adjust the bbox to fit the heightfield. - copy(layer.bmin, bmin); - copy(layer.bmax, bmax); + copy(ref layer.bmin, bmin); + copy(ref layer.bmax, bmax); layer.bmin[1] = bmin[1] + hmin * chf.ch; layer.bmax[1] = bmin[1] + hmax * chf.ch; layer.hmin = hmin; diff --git a/src/DotRecast.Recast/RecastMesh.cs b/src/DotRecast.Recast/RecastMesh.cs index a8feaca..675775a 100644 --- a/src/DotRecast.Recast/RecastMesh.cs +++ b/src/DotRecast.Recast/RecastMesh.cs @@ -976,8 +976,8 @@ namespace DotRecast.Recast { ctx.startTimer("POLYMESH"); PolyMesh mesh = new PolyMesh(); - RecastVectors.copy(mesh.bmin, cset.bmin, 0); - RecastVectors.copy(mesh.bmax, cset.bmax, 0); + RecastVectors.copy(ref mesh.bmin, cset.bmin, 0); + RecastVectors.copy(ref mesh.bmax, cset.bmax, 0); mesh.cs = cset.cs; mesh.ch = cset.ch; mesh.borderSize = cset.borderSize; @@ -1231,16 +1231,16 @@ namespace DotRecast.Recast mesh.nvp = meshes[0].nvp; mesh.cs = meshes[0].cs; mesh.ch = meshes[0].ch; - RecastVectors.copy(mesh.bmin, meshes[0].bmin, 0); - RecastVectors.copy(mesh.bmax, meshes[0].bmax, 0); + RecastVectors.copy(ref mesh.bmin, meshes[0].bmin, 0); + RecastVectors.copy(ref mesh.bmax, meshes[0].bmax, 0); int maxVerts = 0; int maxPolys = 0; int maxVertsPerMesh = 0; for (int i = 0; i < nmeshes; ++i) { - RecastVectors.min(mesh.bmin, meshes[i].bmin, 0); - RecastVectors.max(mesh.bmax, meshes[i].bmax, 0); + RecastVectors.min(ref mesh.bmin, meshes[i].bmin, 0); + RecastVectors.max(ref mesh.bmax, meshes[i].bmax, 0); maxVertsPerMesh = Math.Max(maxVertsPerMesh, meshes[i].nverts); maxVerts += meshes[i].nverts; maxPolys += meshes[i].npolys; @@ -1361,8 +1361,8 @@ namespace DotRecast.Recast dst.npolys = src.npolys; dst.maxpolys = src.npolys; dst.nvp = src.nvp; - RecastVectors.copy(dst.bmin, src.bmin, 0); - RecastVectors.copy(dst.bmax, src.bmax, 0); + RecastVectors.copy(ref dst.bmin, src.bmin, 0); + RecastVectors.copy(ref dst.bmax, src.bmax, 0); dst.cs = src.cs; dst.ch = src.ch; dst.borderSize = src.borderSize; diff --git a/src/DotRecast.Recast/RecastMeshDetail.cs b/src/DotRecast.Recast/RecastMeshDetail.cs index a0d8098..9d7e813 100644 --- a/src/DotRecast.Recast/RecastMeshDetail.cs +++ b/src/DotRecast.Recast/RecastMeshDetail.cs @@ -51,6 +51,12 @@ namespace DotRecast.Recast { return a[0] * b[0] + a[2] * b[2]; } + + private static float vdot2(Vector3f a, Vector3f b) + { + return a[0] * b[0] + a[2] * b[2]; + } + private static float vdistSq2(float[] verts, int p, int q) { @@ -58,7 +64,7 @@ namespace DotRecast.Recast float dy = verts[q + 2] - verts[p + 2]; return dx * dx + dy * dy; } - + private static float vdist2(float[] verts, int p, int q) { return (float)Math.Sqrt(vdistSq2(verts, p, q)); @@ -70,11 +76,39 @@ namespace DotRecast.Recast float dy = q[2] - p[2]; return dx * dx + dy * dy; } + + private static float vdistSq2(float[] p, Vector3f q) + { + float dx = q[0] - p[0]; + float dy = q[2] - p[2]; + return dx * dx + dy * dy; + } + + + private static float vdistSq2(Vector3f p, Vector3f q) + { + float dx = q[0] - p[0]; + float dy = q[2] - p[2]; + return dx * dx + dy * dy; + } + private static float vdist2(float[] p, float[] q) { return (float)Math.Sqrt(vdistSq2(p, q)); } + + private static float vdist2(Vector3f p, Vector3f q) + { + return (float)Math.Sqrt(vdistSq2(p, q)); + } + + private static float vdist2(float[] p, Vector3f q) + { + return (float)Math.Sqrt(vdistSq2(p, q)); + } + + private static float vdistSq2(float[] p, float[] verts, int q) { @@ -82,11 +116,25 @@ namespace DotRecast.Recast float dy = verts[q + 2] - p[2]; return dx * dx + dy * dy; } + + private static float vdistSq2(Vector3f p, float[] verts, int q) + { + float dx = verts[q + 0] - p[0]; + float dy = verts[q + 2] - p[2]; + return dx * dx + dy * dy; + } + private static float vdist2(float[] p, float[] verts, int q) { return (float)Math.Sqrt(vdistSq2(p, verts, q)); } + + private static float vdist2(Vector3f p, float[] verts, int q) + { + return (float)Math.Sqrt(vdistSq2(p, verts, q)); + } + private static float vcross2(float[] verts, int p1, int p2, int p3) { @@ -105,16 +153,26 @@ namespace DotRecast.Recast float v2 = p3[2] - p1[2]; return u1 * v2 - v1 * u2; } + + private static float vcross2(Vector3f p1, Vector3f p2, Vector3f p3) + { + float u1 = p2[0] - p1[0]; + float v1 = p2[2] - p1[2]; + float u2 = p3[0] - p1[0]; + float v2 = p3[2] - p1[2]; + return u1 * v2 - v1 * u2; + } - private static bool circumCircle(float[] verts, int p1, int p2, int p3, float[] c, AtomicFloat r) + + private static bool circumCircle(float[] verts, int p1, int p2, int p3, ref Vector3f c, AtomicFloat r) { float EPS = 1e-6f; // Calculate the circle relative to p1, to avoid some precision issues. - float[] v1 = new float[3]; - float[] v2 = new float[3]; - float[] v3 = new float[3]; - RecastVectors.sub(v2, verts, p2, p1); - RecastVectors.sub(v3, verts, p3, p1); + Vector3f v1 = new Vector3f(); + Vector3f v2 = new Vector3f(); + Vector3f v3 = new Vector3f(); + RecastVectors.sub(ref v2, verts, p2, p1); + RecastVectors.sub(ref v3, verts, p3, p1); float cp = vcross2(v1, v2, v3); if (Math.Abs(cp) > EPS) @@ -126,23 +184,23 @@ namespace DotRecast.Recast c[1] = 0; c[2] = (v1Sq * (v3[0] - v2[0]) + v2Sq * (v1[0] - v3[0]) + v3Sq * (v2[0] - v1[0])) / (2 * cp); r.Exchange(vdist2(c, v1)); - RecastVectors.add(c, c, verts, p1); + RecastVectors.add(ref c, c, verts, p1); return true; } - RecastVectors.copy(c, verts, p1); + RecastVectors.copy(ref c, verts, p1); r.Exchange(0f); return false; } private static float distPtTri(float[] p, float[] verts, int a, int b, int c) { - float[] v0 = new float[3]; - float[] v1 = new float[3]; - float[] v2 = new float[3]; - RecastVectors.sub(v0, verts, c, a); - RecastVectors.sub(v1, verts, b, a); - RecastVectors.sub(v2, p, verts, a); + Vector3f v0 = new Vector3f(); + Vector3f v1 = new Vector3f(); + Vector3f v2 = new Vector3f(); + RecastVectors.sub(ref v0, verts, c, a); + RecastVectors.sub(ref v1, verts, b, a); + RecastVectors.sub(ref v2, p, verts, a); float dot00 = vdot2(v0, v0); float dot01 = vdot2(v0, v1); @@ -460,7 +518,7 @@ namespace DotRecast.Recast // Find best point on left of edge. int pt = npts; - float[] c = new float[3]; + Vector3f c = new Vector3f(); AtomicFloat r = new AtomicFloat(-1f); for (int u = 0; u < npts; ++u) { @@ -475,7 +533,7 @@ namespace DotRecast.Recast { // The circle is not updated yet, do it now. pt = u; - circumCircle(pts, s * 3, t * 3, u * 3, c, r); + circumCircle(pts, s * 3, t * 3, u * 3, ref c, r); continue; } @@ -490,7 +548,7 @@ namespace DotRecast.Recast { // Inside safe circumcircle, update circle. pt = u; - circumCircle(pts, s * 3, t * 3, u * 3, c, r); + circumCircle(pts, s * 3, t * 3, u * 3, ref c, r); } else { @@ -508,7 +566,7 @@ namespace DotRecast.Recast // Edge is valid. pt = u; - circumCircle(pts, s * 3, t * 3, u * 3, c, r); + circumCircle(pts, s * 3, t * 3, u * 3, ref c, r); } } } @@ -927,14 +985,14 @@ namespace DotRecast.Recast if (sampleDist > 0) { // Create sample locations in a grid. - float[] bmin = new float[3]; - float[] bmax = new float[3]; - RecastVectors.copy(bmin, @in, 0); - RecastVectors.copy(bmax, @in, 0); + Vector3f bmin = new Vector3f(); + Vector3f bmax = new Vector3f(); + RecastVectors.copy(ref bmin, @in, 0); + RecastVectors.copy(ref bmax, @in, 0); for (int i = 1; i < nin; ++i) { - RecastVectors.min(bmin, @in, i * 3); - RecastVectors.max(bmax, @in, i * 3); + RecastVectors.min(ref bmin, @in, i * 3); + RecastVectors.max(ref bmax, @in, i * 3); } int x0 = (int)Math.Floor(bmin[0] / sampleDist); @@ -946,12 +1004,12 @@ namespace DotRecast.Recast { for (int x = x0; x < x1; ++x) { - float[] pt = new float[3]; + Vector3f pt = new Vector3f(); pt[0] = x * sampleDist; pt[1] = (bmax[1] + bmin[1]) * 0.5f; pt[2] = z * sampleDist; // Make sure the samples are not too close to the edges. - if (distToPoly(nin, @in, pt) > -sampleDist / 2) + if (distToPoly(nin, @in, pt.ToArray()) > -sampleDist / 2) { continue; } @@ -975,7 +1033,7 @@ namespace DotRecast.Recast } // Find sample with most error. - float[] bestpt = new float[3]; + Vector3f bestpt = new Vector3f(); float bestd = 0; int besti = -1; for (int i = 0; i < nsamples; ++i) @@ -986,13 +1044,13 @@ namespace DotRecast.Recast continue; // skip added. } - float[] pt = new float[3]; + Vector3f pt = new Vector3f(); // The sample location is jittered to get rid of some bad triangulations // which are cause by symmetrical data from the grid structure. pt[0] = samples[s + 0] * sampleDist + getJitterX(i) * cs * 0.1f; pt[1] = samples[s + 1] * chf.ch; pt[2] = samples[s + 2] * sampleDist + getJitterY(i) * cs * 0.1f; - float d = distToTriMesh(pt, verts, nverts, tris, tris.Count / 4); + float d = distToTriMesh(pt.ToArray(), verts, nverts, tris, tris.Count / 4); if (d < 0) { continue; // did not hit the mesh. @@ -1360,7 +1418,7 @@ namespace DotRecast.Recast int nvp = mesh.nvp; float cs = mesh.cs; float ch = mesh.ch; - float[] orig = mesh.bmin; + Vector3f orig = mesh.bmin; int borderSize = mesh.borderSize; int heightSearchRadius = (int)Math.Max(1, Math.Ceiling(mesh.maxEdgeError)); diff --git a/src/DotRecast.Recast/RecastRasterization.cs b/src/DotRecast.Recast/RecastRasterization.cs index 3ba9fbe..fe6283d 100644 --- a/src/DotRecast.Recast/RecastRasterization.cs +++ b/src/DotRecast.Recast/RecastRasterization.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using DotRecast.Core; using static DotRecast.Core.RecastMath; using static DotRecast.Recast.RecastConstants; @@ -48,6 +49,16 @@ namespace DotRecast.Recast overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; return overlap; } + + private static bool overlapBounds(float[] amin, float[] 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; + } + /** * Adds a span to the heightfield. If the new span overlaps existing spans, it will merge the new span with the @@ -247,17 +258,17 @@ namespace DotRecast.Recast private static void rasterizeTri(float[] verts, int v0, int v1, int v2, int area, Heightfield hf, float[] hfBBMin, float[] hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold) { - float[] tmin = new float[3]; - float[] tmax = new float[3]; + Vector3f tmin = new Vector3f(); + Vector3f tmax = new Vector3f(); float by = hfBBMax[1] - hfBBMin[1]; // Calculate the bounding box of the triangle. - RecastVectors.copy(tmin, verts, v0 * 3); - RecastVectors.copy(tmax, verts, v0 * 3); - RecastVectors.min(tmin, verts, v1 * 3); - RecastVectors.min(tmin, verts, v2 * 3); - RecastVectors.max(tmax, verts, v1 * 3); - RecastVectors.max(tmax, verts, v2 * 3); + RecastVectors.copy(ref tmin, verts, v0 * 3); + RecastVectors.copy(ref tmax, verts, v0 * 3); + RecastVectors.min(ref tmin, verts, v1 * 3); + RecastVectors.min(ref tmin, verts, v2 * 3); + RecastVectors.max(ref tmax, verts, v1 * 3); + RecastVectors.max(ref tmax, verts, v2 * 3); // If the triangle does not touch the bbox of the heightfield, skip the triagle. if (!overlapBounds(hfBBMin, hfBBMax, tmin, tmax)) diff --git a/src/DotRecast.Recast/RecastVectors.cs b/src/DotRecast.Recast/RecastVectors.cs index fff997a..2da95c5 100644 --- a/src/DotRecast.Recast/RecastVectors.cs +++ b/src/DotRecast.Recast/RecastVectors.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using DotRecast.Core; namespace DotRecast.Recast { @@ -30,6 +31,22 @@ namespace DotRecast.Recast a[1] = Math.Min(a[1], b[i + 1]); a[2] = Math.Min(a[2], b[i + 2]); } + + public static void min(ref Vector3f a, float[] b, int i) + { + a[0] = Math.Min(a[0], b[i + 0]); + a[1] = Math.Min(a[1], b[i + 1]); + a[2] = Math.Min(a[2], b[i + 2]); + } + + public static void min(ref Vector3f a, Vector3f b, int i) + { + a[0] = Math.Min(a[0], b[i + 0]); + a[1] = Math.Min(a[1], b[i + 1]); + a[2] = Math.Min(a[2], b[i + 2]); + } + + public static void max(float[] a, float[] b, int i) { @@ -37,16 +54,55 @@ namespace DotRecast.Recast a[1] = Math.Max(a[1], b[i + 1]); a[2] = Math.Max(a[2], b[i + 2]); } + + public static void max(ref Vector3f a, float[] b, int i) + { + a[0] = Math.Max(a[0], b[i + 0]); + a[1] = Math.Max(a[1], b[i + 1]); + a[2] = Math.Max(a[2], b[i + 2]); + } + + public static void max(ref Vector3f a, Vector3f b, int i) + { + a[0] = Math.Max(a[0], b[i + 0]); + a[1] = Math.Max(a[1], b[i + 1]); + a[2] = Math.Max(a[2], b[i + 2]); + } + + public static void copy(float[] @out, float[] @in, int i) { copy(@out, 0, @in, i); } + + public static void copy(ref Vector3f @out, float[] @in, int i) + { + copy(ref @out, 0, @in, i); + } + + public static void copy(ref Vector3f @out, Vector3f @in, int i) + { + copy(ref @out, 0, @in, i); + } + + public static void copy(float[] @out, float[] @in) { copy(@out, 0, @in, 0); } + + public static void copy(ref Vector3f @out, float[] @in) + { + copy(ref @out, 0, @in, 0); + } + + public static void copy(ref Vector3f @out, Vector3f @in) + { + @out = @in; + } + public static void copy(float[] @out, int n, float[] @in, int m) { @@ -54,6 +110,29 @@ namespace DotRecast.Recast @out[n + 1] = @in[m + 1]; @out[n + 2] = @in[m + 2]; } + + public static void copy(float[] @out, int n, Vector3f @in, int m) + { + @out[n] = @in[m]; + @out[n + 1] = @in[m + 1]; + @out[n + 2] = @in[m + 2]; + } + + + + public static void copy(ref Vector3f @out, int n, float[] @in, int m) + { + @out[n] = @in[m]; + @out[n + 1] = @in[m + 1]; + @out[n + 2] = @in[m + 2]; + } + + public static void copy(ref Vector3f @out, int n, Vector3f @in, int m) + { + @out[n] = @in[m]; + @out[n + 1] = @in[m + 1]; + @out[n + 2] = @in[m + 2]; + } public static void add(float[] e0, float[] a, float[] verts, int i) { @@ -61,6 +140,20 @@ namespace DotRecast.Recast e0[1] = a[1] + verts[i + 1]; e0[2] = a[2] + verts[i + 2]; } + + public static void add(ref Vector3f e0, float[] a, float[] verts, int i) + { + e0[0] = a[0] + verts[i]; + e0[1] = a[1] + verts[i + 1]; + e0[2] = a[2] + verts[i + 2]; + } + + public static void add(ref Vector3f e0, Vector3f a, float[] verts, int i) + { + e0[0] = a[0] + verts[i]; + e0[1] = a[1] + verts[i + 1]; + e0[2] = a[2] + verts[i + 2]; + } public static void sub(float[] e0, float[] verts, int i, int j) { @@ -68,6 +161,14 @@ namespace DotRecast.Recast e0[1] = verts[i + 1] - verts[j + 1]; e0[2] = verts[i + 2] - verts[j + 2]; } + + public static void sub(ref Vector3f e0, float[] verts, int i, int j) + { + e0[0] = verts[i] - verts[j]; + e0[1] = verts[i + 1] - verts[j + 1]; + e0[2] = verts[i + 2] - verts[j + 2]; + } + public static void sub(float[] e0, float[] i, float[] verts, int j) { @@ -75,6 +176,14 @@ namespace DotRecast.Recast e0[1] = i[1] - verts[j + 1]; e0[2] = i[2] - verts[j + 2]; } + + public static void sub(ref Vector3f e0, float[] i, float[] verts, int j) + { + e0[0] = i[0] - verts[j]; + e0[1] = i[1] - verts[j + 1]; + e0[2] = i[2] - verts[j + 2]; + } + public static void cross(float[] dest, float[] v1, float[] v2) { @@ -82,6 +191,28 @@ namespace DotRecast.Recast dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; dest[2] = v1[0] * v2[1] - v1[1] * v2[0]; } + + public static void cross(float[] dest, Vector3f v1, Vector3f v2) + { + 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]; + } + + public static void cross(ref Vector3f dest, float[] v1, float[] v2) + { + 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]; + } + + public static void cross(ref Vector3f dest, Vector3f v1, Vector3f v2) + { + 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]; + } + public static void normalize(float[] v) { @@ -90,6 +221,14 @@ namespace DotRecast.Recast v[1] *= d; v[2] *= d; } + + public static void normalize(ref Vector3f v) + { + float d = (float)(1.0f / Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; + } public static float dot(float[] v1, float[] v2) { diff --git a/src/DotRecast.Recast/RecastVoxelization.cs b/src/DotRecast.Recast/RecastVoxelization.cs index 1eb869f..a863920 100644 --- a/src/DotRecast.Recast/RecastVoxelization.cs +++ b/src/DotRecast.Recast/RecastVoxelization.cs @@ -29,7 +29,7 @@ namespace DotRecast.Recast RecastConfig cfg = builderCfg.cfg; // Allocate voxel heightfield where we rasterize our input data to. - Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin, builderCfg.bmax, cfg.cs, cfg.ch, cfg.borderSize); + Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin.ToArray(), builderCfg.bmax.ToArray(), cfg.cs, cfg.ch, cfg.borderSize); // Allocate array that can hold triangle area types. // If you have multiple meshes you need to process, allocate diff --git a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs index e3261be..190239e 100644 --- a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs @@ -117,7 +117,7 @@ public class AbstractCrowdTest { for (int j = 0; j < size; j++) { - float[] pos = new float[3]; + Vector3f pos = new Vector3f(); pos[0] = startPos[0] + i * distance; pos[1] = startPos[1]; pos[2] = startPos[2] + j * distance;