diff --git a/src/DotRecast.Core/RecastMath.cs b/src/DotRecast.Core/RecastMath.cs index 2ca8aef..6be148a 100644 --- a/src/DotRecast.Core/RecastMath.cs +++ b/src/DotRecast.Core/RecastMath.cs @@ -19,7 +19,6 @@ freely, subject to the following restrictions: */ using System; -using System.Numerics; namespace DotRecast.Core { @@ -36,6 +35,14 @@ namespace DotRecast.Core return dx * dx + dy * dy + dz * dz; } + public static float vDistSqr(Vector3f v1, float[] v2, int i) + { + float dx = v2[i] - v1[0]; + float dy = v2[i + 1] - v1[1]; + float dz = v2[i + 2] - v1[2]; + return dx * dx + dy * dy + dz * dz; + } + public static float vDistSqr(Vector3f v1, Vector3f v2, int i) { float dx = v2[i] - v1[0]; @@ -126,6 +133,16 @@ namespace DotRecast.Core dest[2] = v1[2] + v2[2] * s; return dest; } + + public static Vector3f vMad(float[] v1, Vector3f v2, float s) + { + Vector3f dest = new Vector3f(); + dest[0] = v1[0] + v2[0] * s; + dest[1] = v1[1] + v2[1] * s; + dest[2] = v1[2] + v2[2] * s; + return dest; + } + /// Performs a linear interpolation between two vectors. (@p v1 toward @p @@ -595,6 +612,16 @@ namespace DotRecast.Core float acz = c[2] - a[2]; return acx * abz - abx * acz; } + + public static float triArea2D(Vector3f a, float[] 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. diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index a9c95c3..5b69308 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -85,5 +85,17 @@ namespace DotRecast.Core { return new float[] { x, y, z }; } + + public static bool operator ==(Vector3f left, Vector3f right) + { + return (left.x == right.x) + && (left.y == right.y) + && (left.z == right.z); + } + + public static bool operator !=(Vector3f left, Vector3f right) + { + return !(left == right); + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Dynamic/Colliders/BoxCollider.cs b/src/DotRecast.Detour.Dynamic/Colliders/BoxCollider.cs index 3bba3fd..8080c6d 100644 --- a/src/DotRecast.Detour.Dynamic/Colliders/BoxCollider.cs +++ b/src/DotRecast.Detour.Dynamic/Colliders/BoxCollider.cs @@ -25,16 +25,16 @@ namespace DotRecast.Detour.Dynamic.Colliders public class BoxCollider : AbstractCollider { private readonly Vector3f center; - private readonly float[][] halfEdges; + private readonly Vector3f[] halfEdges; - public BoxCollider(Vector3f center, float[][] halfEdges, int area, float flagMergeThreshold) : + public BoxCollider(Vector3f center, Vector3f[] halfEdges, int area, float flagMergeThreshold) : base(area, flagMergeThreshold, bounds(center, halfEdges)) { this.center = center; this.halfEdges = halfEdges; } - private static float[] bounds(Vector3f center, float[][] halfEdges) + private static float[] bounds(Vector3f center, Vector3f[] halfEdges) { float[] bounds = new float[] { @@ -62,11 +62,11 @@ namespace DotRecast.Detour.Dynamic.Colliders public void rasterize(Heightfield hf, Telemetry telemetry) { - RecastFilledVolumeRasterization.rasterizeBox(hf, center, halfEdges, area, (int)Math.Floor(flagMergeThreshold / hf.ch), - telemetry); + RecastFilledVolumeRasterization.rasterizeBox( + hf, center, halfEdges, area, (int)Math.Floor(flagMergeThreshold / hf.ch), telemetry); } - public static Vector3f[] getHalfEdges(Vector3f up, Vector3f forward, float[] extent) + public static Vector3f[] getHalfEdges(Vector3f up, Vector3f forward, Vector3f extent) { Vector3f[] halfEdges = { diff --git a/src/DotRecast.Detour.Dynamic/VoxelQuery.cs b/src/DotRecast.Detour.Dynamic/VoxelQuery.cs index 39bf31f..6779e3e 100644 --- a/src/DotRecast.Detour.Dynamic/VoxelQuery.cs +++ b/src/DotRecast.Detour.Dynamic/VoxelQuery.cs @@ -47,12 +47,12 @@ namespace DotRecast.Detour.Dynamic * * @return Optional with hit parameter (t) or empty if no hit found */ - public float? raycast(float[] start, float[] end) + public float? raycast(Vector3f start, Vector3f end) { return traverseTiles(start, end); } - private float? traverseTiles(float[] start, float[] end) + private float? traverseTiles(Vector3f start, Vector3f end) { float relStartX = start[0] - origin[0]; float relStartZ = start[2] - origin[2]; @@ -107,7 +107,7 @@ namespace DotRecast.Detour.Dynamic return null; } - private float? traversHeightfield(int x, int z, float[] start, float[] end, float tMin, float tMax) + private float? traversHeightfield(int x, int z, Vector3f start, Vector3f end, float tMin, float tMax) { Heightfield hf = heightfieldProvider.Invoke(x, z); if (null != hf) diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs index a07b814..210e907 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs @@ -399,7 +399,7 @@ public class CrowdTool : Tool float gridy = -float.MaxValue; foreach (CrowdAgent ag in crowd.getActiveAgents()) { - float[] pos = ag.corridor.getPos(); + Vector3f pos = ag.corridor.getPos(); gridy = Math.Max(gridy, pos[1]); } @@ -430,12 +430,12 @@ public class CrowdTool : Tool foreach (CrowdAgent ag in crowd.getActiveAgents()) { AgentTrail trail = m_trails[ag.idx]; - float[] pos = ag.npos; + Vector3f pos = ag.npos; dd.begin(LINES, 3.0f); Vector3f prev = new Vector3f(); float preva = 1; - vCopy(prev, pos); + vCopy(ref prev, pos); for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j) { int idx = (trail.htrail + AGENT_MAX_TRAIL - j) % AGENT_MAX_TRAIL; @@ -444,7 +444,7 @@ public class CrowdTool : Tool dd.vertex(prev[0], prev[1] + 0.1f, prev[2], duRGBA(0, 0, 0, (int)(128 * preva))); dd.vertex(trail.trail[v], trail.trail[v + 1] + 0.1f, trail.trail[v + 2], duRGBA(0, 0, 0, (int)(128 * a))); preva = a; - vCopy(prev, trail.trail, v); + vCopy(ref prev, trail.trail, v); } dd.end(); @@ -457,7 +457,7 @@ public class CrowdTool : Tool continue; float radius = ag.option.radius; - float[] pos = ag.npos; + Vector3f pos = ag.npos; if (toolParams.m_showCorners) { @@ -466,8 +466,8 @@ public class CrowdTool : Tool dd.begin(LINES, 2.0f); for (int j = 0; j < ag.corners.Count; ++j) { - float[] va = j == 0 ? pos : ag.corners[j - 1].getPos(); - float[] vb = ag.corners[j].getPos(); + Vector3f va = j == 0 ? pos : ag.corners[j - 1].getPos(); + Vector3f vb = ag.corners[j].getPos(); dd.vertex(va[0], va[1] + radius, va[2], duRGBA(128, 0, 0, 192)); dd.vertex(vb[0], vb[1] + radius, vb[2], duRGBA(128, 0, 0, 192)); } @@ -475,7 +475,7 @@ public class CrowdTool : Tool if ((ag.corners[ag.corners.Count - 1].getFlags() & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0) { - float[] v = ag.corners[ag.corners.Count - 1].getPos(); + Vector3f v = ag.corners[ag.corners.Count - 1].getPos(); dd.vertex(v[0], v[1], v[2], duRGBA(192, 0, 0, 192)); dd.vertex(v[0], v[1] + radius * 2, v[2], duRGBA(192, 0, 0, 192)); } @@ -509,10 +509,9 @@ public class CrowdTool : Tool if (toolParams.m_showCollisionSegments) { - float[] center = ag.boundary.getCenter(); + Vector3f center = ag.boundary.getCenter(); dd.debugDrawCross(center[0], center[1] + radius, center[2], 0.2f, duRGBA(192, 0, 128, 255), 2.0f); - dd.debugDrawCircle(center[0], center[1] + radius, center[2], ag.option.collisionQueryRange, - duRGBA(192, 0, 128, 128), 2.0f); + dd.debugDrawCircle(center[0], center[1] + radius, center[2], ag.option.collisionQueryRange, duRGBA(192, 0, 128, 128), 2.0f); dd.begin(LINES, 3.0f); for (int j = 0; j < ag.boundary.getSegmentCount(); ++j) @@ -524,7 +523,7 @@ public class CrowdTool : Tool if (triArea2D(pos, s0, s3) < 0.0f) col = duDarkenCol(col); - dd.appendArrow(s[0], s[1] + 0.2f, s[2], s[3], s[4] + 0.2f, s[5], 0.0f, 0.3f, col); + dd.appendArrow(s[0][0], s[0][1] + 0.2f, s[0][2], s[1][0], s[1][2] + 0.2f, s[1][2], 0.0f, 0.3f, col); } dd.end(); diff --git a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs index d942b67..e654d65 100644 --- a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs @@ -22,6 +22,7 @@ using System.Collections.Generic; using System.Collections.Immutable; using System.Diagnostics; using System.IO; +using System.Numerics; using System.Runtime.Serialization; using System.Threading.Tasks; using DotRecast.Core; @@ -116,10 +117,10 @@ public class DynamicUpdateTool : Tool private readonly DemoInputGeomProvider convexGeom; private bool sposSet; private bool eposSet; - private float[] spos; - private float[] epos; + private Vector3f spos; + private Vector3f epos; private bool raycastHit; - private float[] raycastHitPos; + private Vector3f raycastHitPos; public DynamicUpdateTool() { @@ -191,25 +192,25 @@ public class DynamicUpdateTool : Tool if (shift) { sposSet = true; - spos = ArrayUtils.CopyOf(p, p.Length); + spos = p; } else { eposSet = true; - epos = ArrayUtils.CopyOf(p, p.Length); + epos = p; } if (sposSet && eposSet && dynaMesh != null) { - float[] sp = { spos[0], spos[1] + 1.3f, spos[2] }; - float[] ep = { epos[0], epos[1] + 1.3f, epos[2] }; + Vector3f sp = Vector3f.Of(spos[0], spos[1] + 1.3f, spos[2]); + Vector3f ep = Vector3f.Of(epos[0], epos[1] + 1.3f, epos[2]); long t1 = Stopwatch.GetTimestamp(); float? hitPos = dynaMesh.voxelQuery().raycast(sp, ep); long t2 = Stopwatch.GetTimestamp(); raycastTime = (t2 - t1) / TimeSpan.TicksPerMillisecond; raycastHit = hitPos.HasValue; raycastHitPos = hitPos.HasValue - ? new float[] { sp[0] + hitPos.Value * (ep[0] - sp[0]), sp[1] + hitPos.Value * (ep[1] - sp[1]), sp[2] + hitPos.Value * (ep[2] - sp[2]) } + ? Vector3f.Of(sp[0] + hitPos.Value * (ep[0] - sp[0]), sp[1] + hitPos.Value * (ep[1] - sp[1]), sp[2] + hitPos.Value * (ep[2] - sp[2])) : ep; } } @@ -244,17 +245,16 @@ public class DynamicUpdateTool : Tool private Tuple boxCollider(Vector3f p) { - float[] extent = new float[] - { - 0.5f + (float)random.NextDouble() * 6f, 0.5f + (float)random.NextDouble() * 6f, + Vector3f extent = Vector3f.Of( + 0.5f + (float)random.NextDouble() * 6f, + 0.5f + (float)random.NextDouble() * 6f, 0.5f + (float)random.NextDouble() * 6f - }; - float[] forward = new float[] { (1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()) }; - float[] up = new float[] { (1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) }; - float[][] halfEdges = BoxCollider.getHalfEdges(up, forward, extent); + ); + Vector3f forward = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); + Vector3f up = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble())); + Vector3f[] halfEdges = BoxCollider.getHalfEdges(up, forward, extent); return Tuple.Create( - new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb), - GizmoFactory.box(p, halfEdges)); + new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb), GizmoFactory.box(p, halfEdges)); } private Tuple cylinderCollider(Vector3f p) @@ -282,25 +282,25 @@ public class DynamicUpdateTool : Tool 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 }; + var roofUp = Vector3f.Zero; + Vector3f roofExtent = Vector3f.Of(4.5f, 4.5f, 8f); float[] rx = GLU.build_4x4_rotation_matrix(45, forward[0], forward[1], forward[2]); - float[] roofUp = mulMatrixVector(new float[3], rx, baseUp); - float[] roofCenter = new float[] { p[0], p[1] + 6, p[2] }; + roofUp = mulMatrixVector(ref roofUp, rx, baseUp); + Vector3f roofCenter = Vector3f.Of(p[0], p[1] + 6, p[2]); BoxCollider roof = new BoxCollider(roofCenter, BoxCollider.getHalfEdges(roofUp, forward, roofExtent), SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb); - float[] trunkStart = new float[] - { - baseCenter[0] - forward[0] * 15 + side[0] * 6, p[1], + Vector3f trunkStart = Vector3f.Of( + baseCenter[0] - forward[0] * 15 + side[0] * 6, + p[1], baseCenter[2] - forward[2] * 15 + side[2] * 6 - }; - float[] trunkEnd = new float[] { trunkStart[0], trunkStart[1] + 10, trunkStart[2] }; + ); + Vector3f trunkEnd = Vector3f.Of(trunkStart[0], trunkStart[1] + 10, trunkStart[2]); CapsuleCollider trunk = new CapsuleCollider(trunkStart, trunkEnd, 0.5f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb); - float[] crownCenter = new float[] - { + Vector3f crownCenter = Vector3f.Of( baseCenter[0] - forward[0] * 15 + side[0] * 6, p[1] + 10, baseCenter[2] - forward[2] * 15 + side[2] * 6 - }; + ); SphereCollider crown = new SphereCollider(crownCenter, 4f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GRASS, dynaMesh.config.walkableClimb); CompositeCollider collider = new CompositeCollider(@base, roof, trunk, crown); @@ -317,12 +317,12 @@ public class DynamicUpdateTool : Tool return trimeshCollider(p, bridgeGeom); } - private Tuple trimeshHouse(float[] p) + private Tuple trimeshHouse(Vector3f p) { return trimeshCollider(p, houseGeom); } - private Tuple convexTrimesh(float[] p) + private Tuple convexTrimesh(Vector3f p) { float[] verts = transformVertices(p, convexGeom, 360); ConvexTrimeshCollider collider = new ConvexTrimeshCollider(verts, convexGeom.faces, @@ -351,7 +351,7 @@ public class DynamicUpdateTool : Tool v[0] = geom.vertices[i]; v[1] = geom.vertices[i + 1]; v[2] = geom.vertices[i + 2]; - mulMatrixVector(vr, m, v); + mulMatrixVector(ref vr, m, v); vr[0] += p[0]; vr[1] += p[1] - 0.1f; vr[2] += p[2]; @@ -370,8 +370,8 @@ 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) + + private Vector3f mulMatrixVector(ref Vector3f resultvector, float[] matrix, Vector3f 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]; @@ -454,7 +454,7 @@ public class DynamicUpdateTool : Tool } dd.depthMask(false); - if (raycastHitPos != null) + if (raycastHitPos != Vector3f.Zero) { int spathCol = raycastHit ? duRGBA(128, 32, 16, 220) : duRGBA(64, 128, 240, 220); dd.begin(LINES, 2.0f); @@ -467,7 +467,7 @@ public class DynamicUpdateTool : Tool } } - private void drawAgent(RecastDebugDraw dd, float[] pos, int col) + private void drawAgent(RecastDebugDraw dd, Vector3f pos, int col) { float r = sample.getSettingsUI().getAgentRadius(); float h = sample.getSettingsUI().getAgentHeight(); diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/BoxGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/BoxGizmo.cs index 04b5e9a..ce41d9d 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/BoxGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/BoxGizmo.cs @@ -25,15 +25,15 @@ public class BoxGizmo : ColliderGizmo }; private readonly float[] vertices = new float[8 * 3]; - private readonly float[] center; + private readonly Vector3f center; private readonly Vector3f[] halfEdges; - public BoxGizmo(float[] center, float[] extent, Vector3f forward, Vector3f up) : + public BoxGizmo(Vector3f center, Vector3f extent, Vector3f forward, Vector3f up) : this(center, BoxCollider.getHalfEdges(up, forward, extent)) { } - public BoxGizmo(float[] center, Vector3f[] halfEdges) + public BoxGizmo(Vector3f center, Vector3f[] halfEdges) { this.center = center; this.halfEdges = halfEdges; diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs index 1fcbd01..4bb2624 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.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; @@ -13,25 +14,25 @@ public class CylinderGizmo : ColliderGizmo private readonly float[] center; private readonly float[] gradient; - public CylinderGizmo(float[] start, float[] end, float radius) + public CylinderGizmo(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] }; - float[][] normals = new float[3][]; - normals[1] = new float[] { end[0] - start[0], end[1] - start[1], end[2] - start[2] }; - normalize(normals[1]); + Vector3f axis = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]); + Vector3f[] normals = new Vector3f[3]; + normals[1] = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]); + normalize(ref normals[1]); normals[0] = getSideVector(axis); - normals[2] = new float[3]; - cross(normals[2], normals[0], normals[1]); - normalize(normals[2]); + normals[2] = Vector3f.Zero; + cross(ref normals[2], normals[0], normals[1]); + normalize(ref normals[2]); triangles = generateCylindricalTriangles(); - 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] }; - float[] trZ = new float[] { normals[0][2], normals[1][2], normals[2][2] }; + Vector3f trX = Vector3f.Of(normals[0][0], normals[1][0], normals[2][0]); + Vector3f trY = Vector3f.Of(normals[0][1], normals[1][1], normals[2][1]); + Vector3f trZ = Vector3f.Of(normals[0][2], normals[1][2], normals[2][2]); vertices = generateCylindricalVertices(); float halfLength = 0.5f * vLen(axis); gradient = new float[vertices.Length / 3]; @@ -54,24 +55,24 @@ public class CylinderGizmo : 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(side, axis, forward); - normalize(side); + cross(ref forward, side, axis); + cross(ref side, axis, forward); + normalize(ref side); return side; } diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoFactory.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoFactory.cs index 5d56aff..5d8f0d8 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoFactory.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoFactory.cs @@ -4,7 +4,7 @@ namespace DotRecast.Recast.Demo.Tools.Gizmos; public static class GizmoFactory { - public static ColliderGizmo box(float[] center, float[][] halfEdges) + public static ColliderGizmo box(Vector3f center, Vector3f[] halfEdges) { return new BoxGizmo(center, halfEdges); } diff --git a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs index dc2a962..1130aaa 100644 --- a/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs +++ b/src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs @@ -1,4 +1,5 @@ using System; +using DotRecast.Core; using DotRecast.Recast.Demo.Draw; using static DotRecast.Core.RecastMath; @@ -175,7 +176,8 @@ public class GizmoHelper public static int getColorByNormal(float[] vertices, int v0, int v1, int v2) { - Vector3f e0 = new Vector3f(), e1 = new float[3]; + Vector3f e0 = new Vector3f(); + Vector3f e1 = new Vector3f(); Vector3f normal = new Vector3f(); for (int j = 0; j < 3; ++j) { @@ -186,7 +188,7 @@ public class GizmoHelper normal[0] = e0[1] * e1[2] - e0[2] * e1[1]; normal[1] = e0[2] * e1[0] - e0[0] * e1[2]; normal[2] = e0[0] * e1[1] - e0[1] * e1[0]; - RecastVectors.normalize(normal); + RecastVectors.normalize(ref normal); float c = clamp(0.57735026f * (normal[0] + normal[1] + normal[2]), -1, 1); int col = DebugDraw.duLerpCol(DebugDraw.duRGBA(32, 32, 0, 160), DebugDraw.duRGBA(220, 220, 0, 160), (int)(127 * (1 + c))); diff --git a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs b/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs index 437d9de..5d0a90a 100644 --- a/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs @@ -236,7 +236,7 @@ public class JumpLinkBuilderTool : Tool { GroundSample s = link.start.gsamples[i]; float u = i / (float)(link.start.gsamples.Length - 1); - float[] spt = vLerp(link.start.p, link.start.q, u); + Vector3f spt = vLerp(link.start.p, link.start.q, u); int col = duRGBA(48, 16, 16, 255); // duRGBA(255,(s->flags & 4)?255:0,0,255); float off = 0.1f; if (!s.validHeight) @@ -256,7 +256,7 @@ public class JumpLinkBuilderTool : Tool { GroundSample s = link.start.gsamples[i]; float u = i / (float)(link.start.gsamples.Length - 1); - float[] spt = vLerp(link.start.p, link.start.q, u); + Vector3f spt = vLerp(link.start.p, link.start.q, u); int col = duRGBA(255, 255, 255, 255); float off = 0; if (s.validHeight) @@ -276,7 +276,7 @@ public class JumpLinkBuilderTool : Tool { GroundSample s = end.gsamples[i]; float u = i / (float)(end.gsamples.Length - 1); - float[] spt = vLerp(end.p, end.q, u); + Vector3f spt = vLerp(end.p, end.q, u); int col = duRGBA(48, 16, 16, 255); // duRGBA(255,(s->flags & 4)?255:0,0,255); float off = 0.1f; if (!s.validHeight) @@ -295,7 +295,7 @@ public class JumpLinkBuilderTool : Tool { GroundSample s = end.gsamples[i]; float u = i / (float)(end.gsamples.Length - 1); - float[] spt = vLerp(end.p, end.q, u); + Vector3f spt = vLerp(end.p, end.q, u); int col = duRGBA(255, 255, 255, 255); float off = 0; if (s.validHeight) @@ -316,7 +316,7 @@ public class JumpLinkBuilderTool : Tool dd.depthMask(true); } - private void drawTrajectory(RecastDebugDraw dd, JumpLink link, float[] pa, float[] pb, Trajectory tra, int cola) + private void drawTrajectory(RecastDebugDraw dd, JumpLink link, Vector3f pa, Vector3f pb, Trajectory tra, int cola) { } @@ -434,8 +434,8 @@ public class JumpLinkBuilderTool : Tool Vector3f prev = new Vector3f(); for (int i = 0; i < link.startSamples.Length; i++) { - float[] p = link.startSamples[i].p; - float[] q = link.endSamples[i].p; + Vector3f p = link.startSamples[i].p; + Vector3f q = link.endSamples[i].p; if (i == 0 || vDist2D(prev, p) > agentRadius) { geom.addOffMeshConnection(p, q, agentRadius, false, area, flags); diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index fc7e1fb..9c8442d 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -730,13 +730,13 @@ public class TestNavmeshTool : Tool { dd.debugDrawNavMeshPoly(m_navMesh, m_startRef, startCol); dd.depthMask(false); - if (m_spos != null) + if (m_spos != Vector3f.Zero) { dd.debugDrawCircle(m_spos[0], m_spos[1] + agentHeight / 2, m_spos[2], m_distanceToWall, duRGBA(64, 16, 0, 220), 2.0f); } - if (m_hitPos != null) + if (m_hitPos != Vector3f.Zero) { dd.begin(LINES, 3.0f); dd.vertex(m_hitPos[0], m_hitPos[1] + 0.02f, m_hitPos[2], duRGBA(0, 0, 0, 192)); @@ -757,8 +757,8 @@ public class TestNavmeshTool : Tool if (m_parent[i] != 0) { dd.depthMask(false); - float[] p0 = getPolyCenter(m_navMesh, m_parent[i]); - float[] p1 = getPolyCenter(m_navMesh, m_polys[i]); + Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]); + Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]); dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0, 0, 0, 128), 2.0f); dd.depthMask(true); @@ -790,8 +790,8 @@ public class TestNavmeshTool : Tool if (m_parent[i] != 0) { dd.depthMask(false); - float[] p0 = getPolyCenter(m_navMesh, m_parent[i]); - float[] p1 = getPolyCenter(m_navMesh, m_polys[i]); + Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]); + Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]); dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0, 0, 0, 128), 2.0f); dd.depthMask(true); @@ -827,8 +827,8 @@ public class TestNavmeshTool : Tool if (m_parent[i] != 0) { dd.depthMask(false); - float[] p0 = getPolyCenter(m_navMesh, m_parent[i]); - float[] p1 = getPolyCenter(m_navMesh, m_polys[i]); + Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]); + Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]); dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0, 0, 0, 128), 2.0f); dd.depthMask(true); @@ -846,7 +846,7 @@ public class TestNavmeshTool : Tool for (int j = 0; j < wallSegments.getSegmentVerts().Count; ++j) { float[] s = wallSegments.getSegmentVerts()[j]; - float[] s3 = new float[] { s[3], s[4], s[5] }; + Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]); // Skip too distant segments. Tuple distSqr = distancePtSegSqr2D(m_spos, s, 0, 3); if (distSqr.Item1 > RecastMath.sqr(m_neighbourhoodRadius)) @@ -854,11 +854,11 @@ public class TestNavmeshTool : Tool continue; } - float[] delta = vSub(s3, s); - float[] p0 = vMad(s, delta, 0.5f); - float[] norm = new float[] { delta[2], 0, -delta[0] }; - vNormalize(norm); - float[] p1 = vMad(p0, norm, agentRadius * 0.5f); + Vector3f delta = vSub(s3, s); + Vector3f p0 = vMad(s, delta, 0.5f); + Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]); + vNormalize(ref norm); + Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f); // Skip backfacing segments. if (wallSegments.getSegmentRefs()[j] != 0) { @@ -903,7 +903,7 @@ public class TestNavmeshTool : Tool dd.depthMask(false); dd.begin(POINTS, 4.0f); int col = duRGBA(64, 16, 0, 220); - foreach (float[] point in randomPoints) + foreach (Vector3f point in randomPoints) { dd.vertex(point[0], point[1] + 0.1f, point[2], col); } @@ -945,12 +945,10 @@ public class TestNavmeshTool : Tool dd.depthMask(true); } - private float[] getPolyCenter(NavMesh navMesh, long refs) + private Vector3f getPolyCenter(NavMesh navMesh, long refs) { - Vector3f center = new Vector3f(); - center[0] = 0; - center[1] = 0; - center[2] = 0; + Vector3f center = Vector3f.Zero; + Result> tileAndPoly = navMesh.getTileAndPolyByRef(refs); if (tileAndPoly.succeeded()) { @@ -992,7 +990,7 @@ public class TestNavmeshTool : Tool { // In case of partial path, make sure the end point is clamped to the last polygon. Vector3f epos = new Vector3f(); - vCopy(epos, m_epos); + vCopy(ref epos, m_epos); if (m_polys[m_polys.Count - 1] != m_endRef) { Result result = m_navQuery diff --git a/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs b/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs index 8ac0b0d..e893eef 100644 --- a/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RecastFilledVolumeRasterization.cs @@ -18,7 +18,6 @@ freely, subject to the following restrictions: using System; using DotRecast.Core; - using static DotRecast.Core.RecastMath; using static DotRecast.Recast.RecastConstants; using static DotRecast.Recast.RecastVectors; @@ -75,19 +74,19 @@ namespace DotRecast.Recast ctx.stopTimer("RASTERIZE_CYLINDER"); } - public static void rasterizeBox(Heightfield hf, Vector3f center, float[][] halfEdges, int area, int flagMergeThr, + public static void rasterizeBox(Heightfield hf, Vector3f center, Vector3f[] halfEdges, int area, int flagMergeThr, Telemetry ctx) { ctx.startTimer("RASTERIZE_BOX"); - float[][] normals = + Vector3f[] normals = { - new[] { halfEdges[0][0], halfEdges[0][1], halfEdges[0][2] }, - new[] { halfEdges[1][0], halfEdges[1][1], halfEdges[1][2] }, - new[] { halfEdges[2][0], halfEdges[2][1], halfEdges[2][2] } + Vector3f.Of(halfEdges[0][0], halfEdges[0][1], halfEdges[0][2]), + Vector3f.Of(halfEdges[1][0], halfEdges[1][1], halfEdges[1][2]), + Vector3f.Of(halfEdges[2][0], halfEdges[2][1], halfEdges[2][2]), }; - normalize(normals[0]); - normalize(normals[1]); - normalize(normals[2]); + normalize(ref normals[0]); + normalize(ref normals[1]); + normalize(ref normals[2]); float[] vertices = new float[8 * 3]; float[] bounds = new float[] diff --git a/test/DotRecast.Detour.Dynamic.Test/VoxelQueryTest.cs b/test/DotRecast.Detour.Dynamic.Test/VoxelQueryTest.cs index f04a1c4..d538843 100644 --- a/test/DotRecast.Detour.Dynamic.Test/VoxelQueryTest.cs +++ b/test/DotRecast.Detour.Dynamic.Test/VoxelQueryTest.cs @@ -54,8 +54,8 @@ public class VoxelQueryTest }); VoxelQuery query = new VoxelQuery(ORIGIN, TILE_WIDTH, TILE_DEPTH, hfProvider.Object); - float[] start = { 120, 10, 365 }; - float[] end = { 320, 10, 57 }; + Vector3f start = Vector3f.Of(120, 10, 365); + Vector3f end = Vector3f.Of(320, 10, 57); // When query.raycast(start, end); @@ -70,8 +70,8 @@ public class VoxelQueryTest { DynamicNavMesh mesh = createDynaMesh(); VoxelQuery query = mesh.voxelQuery(); - float[] start = { 7.4f, 0.5f, -64.8f }; - float[] end = { 31.2f, 0.5f, -75.3f }; + Vector3f start = Vector3f.Of(7.4f, 0.5f, -64.8f); + Vector3f end = Vector3f.Of(31.2f, 0.5f, -75.3f); float? hit = query.raycast(start, end); Assert.That(hit, Is.Null); } @@ -81,8 +81,8 @@ public class VoxelQueryTest { DynamicNavMesh mesh = createDynaMesh(); VoxelQuery query = mesh.voxelQuery(); - float[] start = { 32.3f, 0.5f, 47.9f }; - float[] end = { -31.2f, 0.5f, -29.8f }; + Vector3f start = Vector3f.Of(32.3f, 0.5f, 47.9f); + Vector3f end = Vector3f.Of(-31.2f, 0.5f, -29.8f); float? hit = query.raycast(start, end); Assert.That(hit, Is.Not.Null); Assert.That(hit.Value, Is.EqualTo(0.5263836f).Within(1e-7f));