diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVec.cs similarity index 86% rename from src/DotRecast.Core/Numerics/RcVecUtils.cs rename to src/DotRecast.Core/Numerics/RcVec.cs index 9cdb5ed..6753661 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/src/DotRecast.Core/Numerics/RcVec.cs @@ -3,9 +3,10 @@ using System.Runtime.CompilerServices; namespace DotRecast.Core.Numerics { - public static class RcVecUtils + public static class RcVec { public const float EPSILON = 1e-6f; + private static readonly float EQUAL_THRESHOLD = RcMath.Sqr(1.0f / 16384.0f); [MethodImpl(MethodImplOptions.AggressiveInlining)] public static RcVec3f Create(Span values, int n) @@ -36,12 +37,28 @@ namespace DotRecast.Core.Numerics } } + /// Performs a 'sloppy' colocation check of the specified points. + /// @param[in] p0 A point. [(x, y, z)] + /// @param[in] p1 A point. [(x, y, z)] + /// @return True if the points are considered to be at the same location. + /// + /// Basically, this function will return true if the specified points are + /// close enough to eachother to be considered colocated. + public static bool Equal(RcVec3f p0, RcVec3f p1) + { + float d = RcVec3f.DistanceSquared(p0, p1); + return d < EQUAL_THRESHOLD; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Dot2(RcVec3f a, RcVec3f b) { return a.X * b.X + a.Z * b.Z; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float DistSq2(float[] verts, int p, int q) { float dx = verts[q + 0] - verts[p + 0]; @@ -49,11 +66,13 @@ namespace DotRecast.Core.Numerics return dx * dx + dy * dy; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Dist2(float[] verts, int p, int q) { return MathF.Sqrt(DistSq2(verts, p, q)); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float DistSq2(RcVec3f p, RcVec3f q) { float dx = q.X - p.X; @@ -61,11 +80,13 @@ namespace DotRecast.Core.Numerics return dx * dx + dy * dy; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Dist2(RcVec3f p, RcVec3f q) { return MathF.Sqrt(DistSq2(p, q)); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Cross2(float[] verts, int p1, int p2, int p3) { float u1 = verts[p2 + 0] - verts[p1 + 0]; @@ -75,6 +96,7 @@ namespace DotRecast.Core.Numerics return u1 * v2 - v1 * u2; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Cross2(RcVec3f p1, RcVec3f p2, RcVec3f p3) { float u1 = p2.X - p1.X; @@ -83,7 +105,7 @@ namespace DotRecast.Core.Numerics float v2 = p3.Z - p1.Z; return u1 * v2 - v1 * u2; } - + /// Derives the dot product of two vectors on the xz-plane. (@p u . @p v) /// @param[in] u A vector [(x, y, z)] /// @param[in] v A vector [(x, y, z)] @@ -162,6 +184,10 @@ namespace DotRecast.Core.Numerics return (float)MathF.Sqrt(dx * dx + dz * dz); } + /// Derives the square of the distance between the specified points on the xz-plane. + /// @param[in] v1 A point. [(x, y, z)] + /// @param[in] v2 A point. [(x, y, z)] + /// @return The square of the distance between the point on the xz-plane. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Dist2DSqr(RcVec3f v1, RcVec3f v2) { diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 94395ac..e8d83c1 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -875,7 +875,7 @@ namespace DotRecast.Detour.Crowd // Update the collision boundary after certain distance has been passed or // if it has become invalid. float updateThr = ag.option.collisionQueryRange * 0.25f; - if (RcVecUtils.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr) + if (RcVec.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr) || !ag.boundary.IsValid(_navQuery, _filters[ag.option.queryFilterType])) { ag.boundary.Update(ag.corridor.GetFirstPoly(), ag.npos, ag.option.collisionQueryRange, _navQuery, @@ -1007,7 +1007,7 @@ namespace DotRecast.Detour.Crowd anim.polyRef = refs[1]; anim.active = true; anim.t = 0.0f; - anim.tmax = (RcVecUtils.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f; + anim.tmax = (RcVec.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f; ag.state = DtCrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH; ag.ncorners = 0; @@ -1097,14 +1097,14 @@ namespace DotRecast.Detour.Crowd float dist = MathF.Sqrt(distSqr); float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist)); - disp = RcVecUtils.Mad(disp, diff, weight / dist); + disp = RcVec.Mad(disp, diff, weight / dist); w += 1.0f; } if (w > 0.0001f) { // Adjust desired velocity. - dvel = RcVecUtils.Mad(dvel, disp, 1.0f / w); + dvel = RcVec.Mad(dvel, disp, 1.0f / w); // Clamp desired velocity to desired speed. float speedSqr = dvel.LengthSquared(); float desiredSqr = RcMath.Sqr(ag.desiredSpeed); @@ -1260,7 +1260,7 @@ namespace DotRecast.Detour.Crowd pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor; } - ag.disp = RcVecUtils.Mad(ag.disp, diff, pen); + ag.disp = RcVec.Mad(ag.disp, diff, pen); w += 1.0f; } diff --git a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs index c186fed..efb465b 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs @@ -96,7 +96,7 @@ namespace DotRecast.Detour.Crowd // Integrate if (vel.Length() > 0.0001f) - npos = RcVecUtils.Mad(npos, vel, dt); + npos = RcVec.Mad(npos, vel, dt); else vel = RcVec3f.Zero; } @@ -112,7 +112,7 @@ namespace DotRecast.Detour.Crowd : false; if (offMeshConnection) { - float distSq = RcVecUtils.Dist2DSqr(npos, corners[ncorners - 1].pos); + float distSq = RcVec.Dist2DSqr(npos, corners[ncorners - 1].pos); if (distSq < radius * radius) return true; } @@ -127,7 +127,7 @@ namespace DotRecast.Detour.Crowd bool endOfPath = ((corners[ncorners - 1].flags & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0) ? true : false; if (endOfPath) - return Math.Min(RcVecUtils.Dist2D(npos, corners[ncorners - 1].pos), range); + return Math.Min(RcVec.Dist2D(npos, corners[ncorners - 1].pos), range); return range; } diff --git a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs index 2aabd8d..2203118 100644 --- a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs @@ -186,16 +186,16 @@ namespace DotRecast.Detour.Crowd { RcVec3f v = RcVec3f.Subtract(bq, bp); RcVec3f w = RcVec3f.Subtract(ap, bp); - float d = RcVecUtils.Perp2D(u, v); + float d = RcVec.Perp2D(u, v); if (MathF.Abs(d) < 1e-6f) return false; d = 1.0f / d; - t = RcVecUtils.Perp2D(v, w) * d; + t = RcVec.Perp2D(v, w) * d; if (t < 0 || t > 1) return false; - float s = RcVecUtils.Perp2D(u, w) * d; + float s = RcVec.Perp2D(u, w) * d; if (s < 0 || s > 1) return false; @@ -216,8 +216,8 @@ namespace DotRecast.Detour.Crowd float minPenalty, DtObstacleAvoidanceDebugData debug) { // penalty for straying away from the desired and current velocities - float vpen = m_params.weightDesVel * (RcVecUtils.Dist2D(vcand, dvel) * m_invVmax); - float vcpen = m_params.weightCurVel * (RcVecUtils.Dist2D(vcand, vel) * m_invVmax); + float vpen = m_params.weightDesVel * (RcVec.Dist2D(vcand, dvel) * m_invVmax); + float vcpen = m_params.weightCurVel * (RcVec.Dist2D(vcand, vel) * m_invVmax); // find the threshold hit time to bail out based on the early out penalty // (see how the penalty is calculated below to understand) diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 5a01d3c..37787ae 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -144,7 +144,7 @@ namespace DotRecast.Detour.Crowd while (0 < ncorners) { if ((corners[0].flags & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 || - RcVecUtils.Dist2DSqr(corners[0].pos, m_pos) > RcMath.Sqr(MIN_TARGET_DIST)) + RcVec.Dist2DSqr(corners[0].pos, m_pos) > RcMath.Sqr(MIN_TARGET_DIST)) { break; } @@ -197,7 +197,7 @@ namespace DotRecast.Detour.Crowd public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, DtNavMeshQuery navquery, IDtQueryFilter filter) { // Clamp the ray to max distance. - float dist = RcVecUtils.Dist2D(m_pos, next); + float dist = RcVec.Dist2D(m_pos, next); // If too close to the goal, do not try to optimize. if (dist < 0.01f) @@ -210,7 +210,7 @@ namespace DotRecast.Detour.Crowd // Adjust ray length. var delta = RcVec3f.Subtract(next, m_pos); - RcVec3f goal = RcVecUtils.Mad(m_pos, delta, pathOptimizationRange / dist); + RcVec3f goal = RcVec.Mad(m_pos, delta, pathOptimizationRange / dist); var res = new List(); var status = navquery.Raycast(m_path[0], m_pos, goal, filter, out var t, out var norm, ref res); diff --git a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs index e31200f..577effd 100644 --- a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs +++ b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs @@ -40,12 +40,12 @@ namespace DotRecast.Detour.Extras BVItem it = new BVItem(); items[i] = it; it.i = i; - RcVec3f bmin = RcVecUtils.Create(data.verts, data.polys[i].verts[0] * 3); - RcVec3f bmax = RcVecUtils.Create(data.verts, data.polys[i].verts[0] * 3); + RcVec3f bmin = RcVec.Create(data.verts, data.polys[i].verts[0] * 3); + RcVec3f bmax = RcVec.Create(data.verts, data.polys[i].verts[0] * 3); for (int j = 1; j < data.polys[i].vertCount; j++) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(data.verts, data.polys[i].verts[j] * 3)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(data.verts, data.polys[i].verts[j] * 3)); + bmin = RcVec3f.Min(bmin, RcVec.Create(data.verts, data.polys[i].verts[j] * 3)); + bmax = RcVec3f.Max(bmax, RcVec.Create(data.verts, data.polys[i].verts[j] * 3)); } it.bmin[0] = Math.Clamp((int)((bmin.X - data.header.bmin.X) * quantFactor), 0, 0x7fffffff); diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 8efafd2..375fa75 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs @@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc) { float cs = acfg.cellSize; - float dist = MathF.Sqrt(RcVecUtils.Dist2DSqr(es.start.p, es.start.q)); + float dist = MathF.Sqrt(RcVec.Dist2DSqr(es.start.p, es.start.q)); int ngsamples = Math.Max(2, (int)MathF.Ceiling(dist / cs)); SampleGroundSegment(heightFunc, es.start, ngsamples); diff --git a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs index 8550fa6..8382908 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs @@ -59,7 +59,7 @@ namespace DotRecast.Detour.Extras.Jumplink GroundSegment end = es.end[js.groundSegment]; RcVec3f ep = end.gsamples[js.startSample].p; RcVec3f eq = end.gsamples[js.startSample + js.samples - 1].p; - float d = Math.Min(RcVecUtils.Dist2DSqr(sp, sq), RcVecUtils.Dist2DSqr(ep, eq)); + float d = Math.Min(RcVec.Dist2DSqr(sp, sq), RcVec.Dist2DSqr(ep, eq)); if (d >= 4 * acfg.agentRadius * acfg.agentRadius) { JumpLink link = new JumpLink(); diff --git a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs index 7d3dcad..9ddd7ee 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs @@ -35,7 +35,7 @@ namespace DotRecast.Detour.Extras.Jumplink private bool SampleTrajectory(JumpLinkBuilderConfig acfg, RcHeightfield solid, RcVec3f pa, RcVec3f pb, ITrajectory tra) { float cs = Math.Min(acfg.cellSize, acfg.cellHeight); - float d = RcVecUtils.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y); + float d = RcVec.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y); int nsamples = Math.Max(2, (int)MathF.Ceiling(d / cs)); for (int i = 0; i < nsamples; ++i) { diff --git a/src/DotRecast.Detour/DtConvexConvexIntersections.cs b/src/DotRecast.Detour/DtConvexConvexIntersections.cs index 28aa04e..f66fa61 100644 --- a/src/DotRecast.Detour/DtConvexConvexIntersections.cs +++ b/src/DotRecast.Detour/DtConvexConvexIntersections.cs @@ -54,10 +54,10 @@ namespace DotRecast.Detour do { - a = RcVecUtils.Create(p, 3 * (ai % n)); - b = RcVecUtils.Create(q, 3 * (bi % m)); - a1 = RcVecUtils.Create(p, 3 * ((ai + n - 1) % n)); // prev a - b1 = RcVecUtils.Create(q, 3 * ((bi + m - 1) % m)); // prev b + a = RcVec.Create(p, 3 * (ai % n)); + b = RcVec.Create(q, 3 * (bi % m)); + a1 = RcVec.Create(p, 3 * ((ai + n - 1) % n)); // prev a + b1 = RcVec.Create(q, 3 * ((bi + m - 1) % m)); // prev b RcVec3f A = RcVec3f.Subtract(a, a1); RcVec3f B = RcVec3f.Subtract(b, b1); diff --git a/src/DotRecast.Detour/DtNavMesh.cs b/src/DotRecast.Detour/DtNavMesh.cs index b6aac0c..0960267 100644 --- a/src/DotRecast.Detour/DtNavMesh.cs +++ b/src/DotRecast.Detour/DtNavMesh.cs @@ -319,13 +319,13 @@ namespace DotRecast.Detour // Calc polygon bounds. int v = p.verts[0] * 3; - bmin = RcVecUtils.Create(tile.data.verts, v); - bmax = RcVecUtils.Create(tile.data.verts, v); + bmin = RcVec.Create(tile.data.verts, v); + bmax = RcVec.Create(tile.data.verts, v); for (int j = 1; j < p.vertCount; ++j) { v = p.verts[j] * 3; - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(tile.data.verts, v)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(tile.data.verts, v)); + bmin = RcVec3f.Min(bmin, RcVec.Create(tile.data.verts, v)); + bmax = RcVec3f.Max(bmax, RcVec.Create(tile.data.verts, v)); } if (DtUtils.OverlapBounds(qmin, qmax, bmin, bmax)) @@ -1489,8 +1489,8 @@ namespace DotRecast.Detour } } - startPos = RcVecUtils.Create(tile.data.verts, poly.verts[idx0] * 3); - endPos = RcVecUtils.Create(tile.data.verts, poly.verts[idx1] * 3); + startPos = RcVec.Create(tile.data.verts, poly.verts[idx0] * 3); + endPos = RcVec.Create(tile.data.verts, poly.verts[idx1] * 3); return DtStatus.DT_SUCCESS; } diff --git a/src/DotRecast.Detour/DtNavMeshBuilder.cs b/src/DotRecast.Detour/DtNavMeshBuilder.cs index ba6b452..bb9a276 100644 --- a/src/DotRecast.Detour/DtNavMeshBuilder.cs +++ b/src/DotRecast.Detour/DtNavMeshBuilder.cs @@ -164,12 +164,12 @@ namespace DotRecast.Detour int vb = option.detailMeshes[i * 4 + 0]; int ndv = option.detailMeshes[i * 4 + 1]; int dv = vb * 3; - var bmin = RcVecUtils.Create(option.detailVerts, dv); - var bmax = RcVecUtils.Create(option.detailVerts, dv); + var bmin = RcVec.Create(option.detailVerts, dv); + var bmax = RcVec.Create(option.detailVerts, dv); for (int j = 1; j < ndv; j++) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(option.detailVerts, dv + j * 3)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(option.detailVerts, dv + j * 3)); + bmin = RcVec3f.Min(bmin, RcVec.Create(option.detailVerts, dv + j * 3)); + bmax = RcVec3f.Max(bmax, RcVec.Create(option.detailVerts, dv + j * 3)); } // BV-tree uses cs for all dimensions @@ -322,8 +322,8 @@ namespace DotRecast.Detour for (int i = 0; i < option.offMeshConCount; ++i) { - var p0 = RcVecUtils.Create(option.offMeshConVerts, (i * 2 + 0) * 3); - var p1 = RcVecUtils.Create(option.offMeshConVerts, (i * 2 + 1) * 3); + var p0 = RcVec.Create(option.offMeshConVerts, (i * 2 + 0) * 3); + var p1 = RcVec.Create(option.offMeshConVerts, (i * 2 + 1) * 3); offMeshConClass[i * 2 + 0] = ClassifyOffMeshPoint(p0, bmin, bmax); offMeshConClass[i * 2 + 1] = ClassifyOffMeshPoint(p1, bmin, bmax); @@ -624,7 +624,7 @@ namespace DotRecast.Detour int endPts = i * 2 * 3; for (int j = 0; j < 2; ++j) { - con.pos[j] = RcVecUtils.Create(option.offMeshConVerts, endPts + (j * 3)); + con.pos[j] = RcVec.Create(option.offMeshConVerts, endPts + (j * 3)); } con.rad = option.offMeshConRad[i]; diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 2978f9a..1742472 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -487,7 +487,7 @@ namespace DotRecast.Detour int va = imin * 3; int vb = ((imin + 1) % nv) * 3; - closest = RcVecUtils.Lerp(verts, va, vb, edget[imin]); + closest = RcVec.Lerp(verts, va, vb, edget[imin]); } return DtStatus.DT_SUCCESS; @@ -670,13 +670,13 @@ namespace DotRecast.Detour // Calc polygon bounds. int v = p.verts[0] * 3; - bmin = RcVecUtils.Create(tile.data.verts, v); - bmax = RcVecUtils.Create(tile.data.verts, v); + bmin = RcVec.Create(tile.data.verts, v); + bmax = RcVec.Create(tile.data.verts, v); for (int j = 1; j < p.vertCount; ++j) { v = p.verts[j] * 3; - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(tile.data.verts, v)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(tile.data.verts, v)); + bmin = RcVec3f.Min(bmin, RcVec.Create(tile.data.verts, v)); + bmax = RcVec3f.Max(bmax, RcVec.Create(tile.data.verts, v)); } if (DtUtils.OverlapBounds(qmin, qmax, bmin, bmax)) @@ -1479,7 +1479,7 @@ namespace DotRecast.Detour protected DtStatus AppendVertex(RcVec3f pos, int flags, long refs, Span straightPath, ref int straightPathCount, int maxStraightPath) { - if (straightPathCount > 0 && DtUtils.VEqual(straightPath[straightPathCount - 1].pos, pos)) + if (straightPathCount > 0 && RcVec.Equal(straightPath[straightPathCount - 1].pos, pos)) { // The vertices are equal, update flags and poly. straightPath[straightPathCount - 1] = new DtStraightPath(straightPath[straightPathCount - 1].pos, flags, refs); @@ -1693,7 +1693,7 @@ namespace DotRecast.Detour // Right vertex. if (DtUtils.TriArea2D(portalApex, portalRight, right) <= 0.0f) { - if (DtUtils.VEqual(portalApex, portalRight) || DtUtils.TriArea2D(portalApex, portalLeft, right) > 0.0f) + if (RcVec.Equal(portalApex, portalRight) || DtUtils.TriArea2D(portalApex, portalLeft, right) > 0.0f) { portalRight = right; rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0; @@ -1749,7 +1749,7 @@ namespace DotRecast.Detour // Left vertex. if (DtUtils.TriArea2D(portalApex, portalLeft, left) >= 0.0f) { - if (DtUtils.VEqual(portalApex, portalLeft) || DtUtils.TriArea2D(portalApex, portalRight, left) < 0.0f) + if (RcVec.Equal(portalApex, portalLeft) || DtUtils.TriArea2D(portalApex, portalRight, left) < 0.0f) { portalLeft = left; leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0; @@ -1965,7 +1965,7 @@ namespace DotRecast.Detour if (distSqr < bestDist) { // Update nearest distance. - bestPos = RcVecUtils.Lerp(verts, vj, vi, tseg); + bestPos = RcVec.Lerp(verts, vj, vi, tseg); bestDist = distSqr; bestNode = curNode; } @@ -2153,8 +2153,8 @@ namespace DotRecast.Detour float s = 1.0f / 255.0f; float tmin = link.bmin * s; float tmax = link.bmax * s; - left = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); - right = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); + left = RcVec.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); + right = RcVec.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); } } @@ -2345,7 +2345,7 @@ namespace DotRecast.Detour int nv = 0; for (int i = 0; i < poly.vertCount; ++i) { - verts[nv] = RcVecUtils.Create(tile.data.verts, poly.verts[i] * 3); + verts[nv] = RcVec.Create(tile.data.verts, poly.verts[i] * 3); nv++; } @@ -2483,7 +2483,7 @@ namespace DotRecast.Detour // compute the intersection point at the furthest end of the polygon // and correct the height (since the raycast moves in 2d) lastPos = curPos; - curPos = RcVecUtils.Mad(startPos, dir, hit.t); + curPos = RcVec.Mad(startPos, dir, hit.t); var e1 = verts[segMax]; var e2 = verts[(segMax + 1) % nv]; var eDir = RcVec3f.Subtract(e2, e1); @@ -3174,8 +3174,8 @@ namespace DotRecast.Detour int ivj = poly.verts[j] * 3; int ivi = poly.verts[i] * 3; var seg = new RcSegmentVert(); - seg.vmin = RcVecUtils.Create(tile.data.verts, ivj); - seg.vmax = RcVecUtils.Create(tile.data.verts, ivi); + seg.vmin = RcVec.Create(tile.data.verts, ivj); + seg.vmax = RcVec.Create(tile.data.verts, ivi); // RcArrays.Copy(tile.data.verts, ivj, seg, 0, 3); // RcArrays.Copy(tile.data.verts, ivi, seg, 3, 3); segmentVerts.Add(seg); @@ -3198,8 +3198,8 @@ namespace DotRecast.Detour float tmin = ints[k].tmin / 255.0f; float tmax = ints[k].tmax / 255.0f; var seg = new RcSegmentVert(); - seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin); - seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax); + seg.vmin = RcVec.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = RcVec.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(ints[k].refs); } @@ -3212,8 +3212,8 @@ namespace DotRecast.Detour float tmin = imin / 255.0f; float tmax = imax / 255.0f; var seg = new RcSegmentVert(); - seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin); - seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax); + seg.vmin = RcVec.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = RcVec.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(0L); } @@ -3355,8 +3355,8 @@ namespace DotRecast.Detour hitPos.Y = bestTile.data.verts[vj + 1] + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg; hitPos.Z = bestTile.data.verts[vj + 2] + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg; hasBestV = true; - bestvj = RcVecUtils.Create(bestTile.data.verts, vj); - bestvi = RcVecUtils.Create(bestTile.data.verts, vi); + bestvj = RcVec.Create(bestTile.data.verts, vj); + bestvi = RcVec.Create(bestTile.data.verts, vi); } for (int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next) diff --git a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs index bf5266a..1c78b6e 100644 --- a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs @@ -37,7 +37,7 @@ namespace DotRecast.Detour int outsideVertex = -1; for (int pv = 0; pv < verts.Length; pv += 3) { - if (RcVecUtils.Dist2DSqr(center, verts, pv) > radiusSqr) + if (RcVec.Dist2DSqr(center, verts, pv) > radiusSqr) { outsideVertex = pv; break; diff --git a/src/DotRecast.Detour/DtUtils.cs b/src/DotRecast.Detour/DtUtils.cs index 5f93ea5..1e74375 100644 --- a/src/DotRecast.Detour/DtUtils.cs +++ b/src/DotRecast.Detour/DtUtils.cs @@ -1,13 +1,10 @@ using System; -using DotRecast.Core; using DotRecast.Core.Numerics; namespace DotRecast.Detour { public static class DtUtils { - private static readonly float EQUAL_THRESHOLD = RcMath.Sqr(1.0f / 16384.0f); - public static int NextPow2(int v) { v--; @@ -39,24 +36,6 @@ namespace DotRecast.Detour return r; } - /// Performs a 'sloppy' colocation check of the specified points. - /// @param[in] p0 A point. [(x, y, z)] - /// @param[in] p1 A point. [(x, y, z)] - /// @return True if the points are considered to be at the same location. - /// - /// Basically, this function will return true if the specified points are - /// close enough to eachother to be considered colocated. - public static bool VEqual(RcVec3f p0, RcVec3f p1) - { - return VEqual(p0, p1, EQUAL_THRESHOLD); - } - - public static bool VEqual(RcVec3f p0, RcVec3f p1, float thresholdSqr) - { - float d = RcVec3f.DistanceSquared(p0, p1); - return d < thresholdSqr; - } - /// Determines if two axis-aligned bounding boxes overlap. /// @param[in] amin Minimum bounds of box A. [(x, y, z)] /// @param[in] amax Maximum bounds of box A. [(x, y, z)] @@ -252,7 +231,7 @@ namespace DotRecast.Detour rmin = rmax = axis.Dot2D(new RcVec3f(poly)); for (int i = 1; i < npoly; ++i) { - float d = axis.Dot2D(RcVecUtils.Create(poly, i * 3)); + float d = axis.Dot2D(RcVec.Create(poly, i * 3)); rmin = Math.Min(rmin, d); rmax = Math.Max(rmax, d); } @@ -309,8 +288,8 @@ namespace DotRecast.Detour public static float DistancePtSegSqr2D(RcVec3f pt, Span verts, int p, int q, out float t) { - var vp = RcVecUtils.Create(verts, p); - var vq = RcVecUtils.Create(verts, q); + var vp = RcVec.Create(verts, p); + var vq = RcVec.Create(verts, q); return DistancePtSegSqr2D(pt, vp, vq, out t); } @@ -362,8 +341,8 @@ namespace DotRecast.Detour RcVec3f vpi = verts[i]; var edge = RcVec3f.Subtract(vpi, vpj); var diff = RcVec3f.Subtract(p0v, vpj); - float n = RcVecUtils.Perp2D(edge, diff); - float d = RcVecUtils.Perp2D(dir, edge); + float n = RcVec.Perp2D(edge, diff); + float d = RcVec.Perp2D(dir, edge); if (MathF.Abs(d) < EPS) { // S is nearly parallel to this edge @@ -425,14 +404,14 @@ namespace DotRecast.Detour RcVec3f u = RcVec3f.Subtract(aq, ap); RcVec3f v = RcVec3f.Subtract(bq, bp); RcVec3f w = RcVec3f.Subtract(ap, bp); - float d = RcVecUtils.PerpXZ(u, v); + float d = RcVec.PerpXZ(u, v); if (MathF.Abs(d) < 1e-6f) { return false; } - s = RcVecUtils.PerpXZ(v, w) / d; - t = RcVecUtils.PerpXZ(u, w) / d; + s = RcVec.PerpXZ(v, w) / d; + t = RcVec.PerpXZ(u, w) / d; return true; } diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs index eb67223..917e95f 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs @@ -234,7 +234,7 @@ public class CrowdSampleTool : ISampleTool dd.Vertex(prev.X, prev.Y + 0.1f, prev.Z, 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; - prev = RcVecUtils.Create(trail.trail, v); + prev = RcVec.Create(trail.trail, v); } dd.End(); diff --git a/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs b/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs index 987a081..993b50b 100644 --- a/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs +++ b/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs @@ -155,9 +155,9 @@ public static class GizmoRenderer debugDraw.Begin(DebugDrawPrimitives.TRIS); for (int i = 0; i < trimesh.triangles.Length; i += 3) { - RcVec3f v0 = RcVecUtils.Create(trimesh.vertices, 3 * trimesh.triangles[i]); - RcVec3f v1 = RcVecUtils.Create(trimesh.vertices, 3 * trimesh.triangles[i + 1]); - RcVec3f v2 = RcVecUtils.Create(trimesh.vertices, 3 * trimesh.triangles[i + 2]); + RcVec3f v0 = RcVec.Create(trimesh.vertices, 3 * trimesh.triangles[i]); + RcVec3f v1 = RcVec.Create(trimesh.vertices, 3 * trimesh.triangles[i + 1]); + RcVec3f v2 = RcVec.Create(trimesh.vertices, 3 * trimesh.triangles[i + 2]); int col = GetColorByNormal(v0, v1, v2); debugDraw.Vertex(v0.X, v0.Y, v0.Z, col); debugDraw.Vertex(v1.X, v1.Y, v1.Z, col); diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index 917bdb7..5d78eda 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -511,10 +511,10 @@ public class TestNavmeshSampleTool : ISampleTool } RcVec3f delta = RcVec3f.Subtract(s3, s.vmin); - RcVec3f p0 = RcVecUtils.Mad(s.vmin, delta, 0.5f); + RcVec3f p0 = RcVec.Mad(s.vmin, delta, 0.5f); RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X); norm = RcVec3f.Normalize(norm); - RcVec3f p1 = RcVecUtils.Mad(p0, norm, agentRadius * 0.5f); + RcVec3f p1 = RcVec.Mad(p0, norm, agentRadius * 0.5f); // Skip backfacing segments. if (segmentRefs[j] != 0) { diff --git a/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs b/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs index 498f70b..9c48c7f 100644 --- a/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs +++ b/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs @@ -61,8 +61,8 @@ namespace DotRecast.Recast.Toolset.Geom bmax = new RcVec3f(vertices); for (int i = 1; i < vertices.Length / 3; i++) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(vertices, i * 3)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(vertices, i * 3)); + bmin = RcVec3f.Min(bmin, RcVec.Create(vertices, i * 3)); + bmax = RcVec3f.Max(bmax, RcVec.Create(vertices, i * 3)); } _mesh = new RcTriMesh(vertices, faces); @@ -87,9 +87,9 @@ namespace DotRecast.Recast.Toolset.Geom { for (int i = 0; i < faces.Length; i += 3) { - RcVec3f v0 = RcVecUtils.Create(vertices, faces[i] * 3); - RcVec3f v1 = RcVecUtils.Create(vertices, faces[i + 1] * 3); - RcVec3f v2 = RcVecUtils.Create(vertices, faces[i + 2] * 3); + RcVec3f v0 = RcVec.Create(vertices, faces[i] * 3); + RcVec3f v1 = RcVec.Create(vertices, faces[i + 1] * 3); + RcVec3f v2 = RcVec.Create(vertices, faces[i + 2] * 3); RcVec3f e0 = v1 - v0; RcVec3f e1 = v2 - v0; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs index 105ca24..017d180 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcJumpLinkBuilderTool.cs @@ -116,7 +116,7 @@ namespace DotRecast.Recast.Toolset.Tools { RcVec3f p = link.startSamples[i].p; RcVec3f q = link.endSamples[i].p; - if (i == 0 || RcVecUtils.Dist2D(prev, p) > agentRadius) + if (i == 0 || RcVec.Dist2D(prev, p) > agentRadius) { geom.AddOffMeshConnection(p, q, agentRadius, false, area, flags); prev = p; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs index 2d7a960..f7ae779 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcOffMeshConnectionTool.cs @@ -34,7 +34,7 @@ namespace DotRecast.Recast.Toolset.Tools RcOffMeshConnection nearestConnection = null; foreach (RcOffMeshConnection offMeshCon in geom.GetOffMeshConnections()) { - float d = Math.Min(RcVecUtils.DistanceSquared(p, offMeshCon.verts, 0), RcVecUtils.DistanceSquared(p, offMeshCon.verts, 3)); + float d = Math.Min(RcVec.DistanceSquared(p, offMeshCon.verts, 0), RcVec.DistanceSquared(p, offMeshCon.verts, 3)); if (d < nearestDist && Math.Sqrt(d) < settings.agentRadius) { nearestDist = d; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 37a7b56..37702d0 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -91,7 +91,7 @@ namespace DotRecast.Recast.Toolset.Tools len = STEP_SIZE / len; } - RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); + RcVec3f moveTgt = RcVec.Mad(iterPos, delta, len); // Move navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, visited, out nvisited, 16); diff --git a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs index 56f281f..9c5700d 100644 --- a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs +++ b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs @@ -81,8 +81,8 @@ namespace DotRecast.Recast.Geom bmax = new RcVec3f(vertices); for (int i = 1; i < vertices.Length / 3; i++) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(vertices, i * 3)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(vertices, i * 3)); + bmin = RcVec3f.Min(bmin, RcVec.Create(vertices, i * 3)); + bmax = RcVec3f.Max(bmax, RcVec.Create(vertices, i * 3)); } _mesh = new RcTriMesh(vertices, faces); diff --git a/src/DotRecast.Recast/RcAreas.cs b/src/DotRecast.Recast/RcAreas.cs index 011d45e..82524d1 100644 --- a/src/DotRecast.Recast/RcAreas.cs +++ b/src/DotRecast.Recast/RcAreas.cs @@ -460,8 +460,8 @@ namespace DotRecast.Recast RcVec3f bmax = new RcVec3f(verts); for (int i = 3; i < verts.Length; i += 3) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(verts, i)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(verts, i)); + bmin = RcVec3f.Min(bmin, RcVec.Create(verts, i)); + bmax = RcVec3f.Max(bmax, RcVec.Create(verts, i)); } bmin.Y = minY; @@ -752,19 +752,19 @@ namespace DotRecast.Recast int vertIndexB = vertIndex; int vertIndexC = (vertIndex + 1) % numVerts; - RcVec3f vertA = RcVecUtils.Create(verts, vertIndexA * 3); - RcVec3f vertB = RcVecUtils.Create(verts, vertIndexB * 3); - RcVec3f vertC = RcVecUtils.Create(verts, vertIndexC * 3); + RcVec3f vertA = RcVec.Create(verts, vertIndexA * 3); + RcVec3f vertB = RcVec.Create(verts, vertIndexB * 3); + RcVec3f vertC = RcVec.Create(verts, vertIndexC * 3); // From A to B on the x/z plane RcVec3f prevSegmentDir = RcVec3f.Subtract(vertB, vertA); prevSegmentDir.Y = 0; // Squash onto x/z plane - prevSegmentDir = RcVecUtils.SafeNormalize(prevSegmentDir); + prevSegmentDir = RcVec.SafeNormalize(prevSegmentDir); // From B to C on the x/z plane RcVec3f currSegmentDir = RcVec3f.Subtract(vertC, vertB); currSegmentDir.Y = 0; // Squash onto x/z plane - currSegmentDir = RcVecUtils.SafeNormalize(currSegmentDir); + currSegmentDir = RcVec.SafeNormalize(currSegmentDir); // The y component of the cross product of the two normalized segment directions. // The X and Z components of the cross product are both zero because the two @@ -791,7 +791,7 @@ namespace DotRecast.Recast bool bevel = cornerMiterSqMag * MITER_LIMIT * MITER_LIMIT < 1.0f; // Scale the corner miter so it's proportional to how much the corner should be offset compared to the edges. - if (cornerMiterSqMag > RcVecUtils.EPSILON) + if (cornerMiterSqMag > RcVec.EPSILON) { float scale = 1.0f / cornerMiterSqMag; cornerMiterX *= scale; diff --git a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs index e6a20e9..5d490b5 100644 --- a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs @@ -176,7 +176,7 @@ namespace DotRecast.Recast private static void Plane(float[][] planes, int p, float[] v1, float[] v2, float[] vertices, int vert) { - RcVecUtils.Cross(planes[p], v1, v2); + RcVec.Cross(planes[p], v1, v2); planes[p][3] = planes[p][0] * vertices[vert] + planes[p][1] * vertices[vert + 1] + planes[p][2] * vertices[vert + 2]; } diff --git a/src/DotRecast.Recast/RcMeshDetails.cs b/src/DotRecast.Recast/RcMeshDetails.cs index a12f1e4..aabb3d0 100644 --- a/src/DotRecast.Recast/RcMeshDetails.cs +++ b/src/DotRecast.Recast/RcMeshDetails.cs @@ -27,7 +27,7 @@ using DotRecast.Core.Numerics; namespace DotRecast.Recast { using static RcRecast; - using static RcVecUtils; + using static RcVec; using static EdgeValues; public static class RcMeshDetails @@ -182,9 +182,9 @@ namespace DotRecast.Recast float dmin = float.MaxValue; for (int i = 0; i < ntris; ++i) { - RcVec3f va = RcVecUtils.Create(verts, tris[i * 4 + 0] * 3); - RcVec3f vb = RcVecUtils.Create(verts, tris[i * 4 + 1] * 3); - RcVec3f vc = RcVecUtils.Create(verts, tris[i * 4 + 2] * 3); + RcVec3f va = RcVec.Create(verts, tris[i * 4 + 0] * 3); + RcVec3f vb = RcVec.Create(verts, tris[i * 4 + 1] * 3); + RcVec3f vc = RcVec.Create(verts, tris[i * 4 + 2] * 3); float d = DistPtTri(p, va, vb, vc); if (d < dmin) { @@ -421,9 +421,9 @@ namespace DotRecast.Recast continue; } - RcVec3f vs = RcVecUtils.Create(pts, s * 3); - RcVec3f vt = RcVecUtils.Create(pts, t * 3); - RcVec3f vu = RcVecUtils.Create(pts, u * 3); + RcVec3f vs = RcVec.Create(pts, s * 3); + RcVec3f vt = RcVec.Create(pts, t * 3); + RcVec3f vu = RcVec.Create(pts, u * 3); if (Cross2(vs, vt, vu) > EPS) { @@ -728,7 +728,7 @@ namespace DotRecast.Recast for (int i = 0; i < nin; ++i) { - RcVecUtils.Copy(verts, i * 3, @in, i * 3); + RcVec.Copy(verts, i * 3, @in, i * 3); } edges.Clear(); @@ -844,7 +844,7 @@ namespace DotRecast.Recast { for (int k = nidx - 2; k > 0; --k) { - RcVecUtils.Copy(verts, nverts * 3, edge, idx[k] * 3); + RcVec.Copy(verts, nverts * 3, edge, idx[k] * 3); hull[nhull++] = nverts; nverts++; } @@ -853,7 +853,7 @@ namespace DotRecast.Recast { for (int k = 1; k < nidx - 1; ++k) { - RcVecUtils.Copy(verts, nverts * 3, edge, idx[k] * 3); + RcVec.Copy(verts, nverts * 3, edge, idx[k] * 3); hull[nhull++] = nverts; nverts++; } @@ -888,8 +888,8 @@ namespace DotRecast.Recast RcVec3f bmax = new RcVec3f(@in); for (int i = 1; i < nin; ++i) { - bmin = RcVec3f.Min(bmin, RcVecUtils.Create(@in, i * 3)); - bmax = RcVec3f.Max(bmax, RcVecUtils.Create(@in, i * 3)); + bmin = RcVec3f.Min(bmin, RcVec.Create(@in, i * 3)); + bmax = RcVec3f.Max(bmax, RcVec.Create(@in, i * 3)); } int x0 = (int)MathF.Floor(bmin.X / sampleDist); @@ -1561,7 +1561,7 @@ namespace DotRecast.Recast for (int k = 0; k < dm.nverts; ++k) { - RcVecUtils.Copy(mesh.verts, mesh.nverts * 3, dm.verts, k * 3); + RcVec.Copy(mesh.verts, mesh.nverts * 3, dm.verts, k * 3); mesh.nverts++; } diff --git a/src/DotRecast.Recast/RcRasterizations.cs b/src/DotRecast.Recast/RcRasterizations.cs index dbe74e1..177d1e3 100644 --- a/src/DotRecast.Recast/RcRasterizations.cs +++ b/src/DotRecast.Recast/RcRasterizations.cs @@ -233,7 +233,7 @@ namespace DotRecast.Recast inVerts[outVerts1 + poly1Vert * 3 + 0] = inVerts[inVertsOffset + inVertB * 3 + 0] + (inVerts[inVertsOffset + inVertA * 3 + 0] - inVerts[inVertsOffset + inVertB * 3 + 0]) * s; inVerts[outVerts1 + poly1Vert * 3 + 1] = inVerts[inVertsOffset + inVertB * 3 + 1] + (inVerts[inVertsOffset + inVertA * 3 + 1] - inVerts[inVertsOffset + inVertB * 3 + 1]) * s; inVerts[outVerts1 + poly1Vert * 3 + 2] = inVerts[inVertsOffset + inVertB * 3 + 2] + (inVerts[inVertsOffset + inVertA * 3 + 2] - inVerts[inVertsOffset + inVertB * 3 + 2]) * s; - RcVecUtils.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, outVerts1 + poly1Vert * 3); + RcVec.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, outVerts1 + poly1Vert * 3); poly1Vert++; poly2Vert++; @@ -241,12 +241,12 @@ namespace DotRecast.Recast // since these were already added above if (inVertAxisDelta[inVertA] > 0) { - RcVecUtils.Copy(inVerts, outVerts1 + poly1Vert * 3, inVerts, inVertsOffset + inVertA * 3); + RcVec.Copy(inVerts, outVerts1 + poly1Vert * 3, inVerts, inVertsOffset + inVertA * 3); poly1Vert++; } else if (inVertAxisDelta[inVertA] < 0) { - RcVecUtils.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, inVertsOffset + inVertA * 3); + RcVec.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, inVertsOffset + inVertA * 3); poly2Vert++; } } @@ -255,7 +255,7 @@ namespace DotRecast.Recast // add the i'th point to the right polygon. Addition is done even for points on the dividing line if (inVertAxisDelta[inVertA] >= 0) { - RcVecUtils.Copy(inVerts, outVerts1 + poly1Vert * 3, inVerts, inVertsOffset + inVertA * 3); + RcVec.Copy(inVerts, outVerts1 + poly1Vert * 3, inVerts, inVertsOffset + inVertA * 3); poly1Vert++; if (inVertAxisDelta[inVertA] != 0) { @@ -263,7 +263,7 @@ namespace DotRecast.Recast } } - RcVecUtils.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, inVertsOffset + inVertA * 3); + RcVec.Copy(inVerts, outVerts2 + poly2Vert * 3, inVerts, inVertsOffset + inVertA * 3); poly2Vert++; } } @@ -295,13 +295,13 @@ namespace DotRecast.Recast int flagMergeThreshold) { // Calculate the bounding box of the triangle. - RcVec3f triBBMin = RcVecUtils.Create(verts, v0 * 3); - triBBMin = RcVec3f.Min(triBBMin, RcVecUtils.Create(verts, v1 * 3)); - triBBMin = RcVec3f.Min(triBBMin, RcVecUtils.Create(verts, v2 * 3)); + RcVec3f triBBMin = RcVec.Create(verts, v0 * 3); + triBBMin = RcVec3f.Min(triBBMin, RcVec.Create(verts, v1 * 3)); + triBBMin = RcVec3f.Min(triBBMin, RcVec.Create(verts, v2 * 3)); - RcVec3f triBBMax = RcVecUtils.Create(verts, v0 * 3); - triBBMax = RcVec3f.Max(triBBMax, RcVecUtils.Create(verts, v1 * 3)); - triBBMax = RcVec3f.Max(triBBMax, RcVecUtils.Create(verts, v2 * 3)); + RcVec3f triBBMax = RcVec.Create(verts, v0 * 3); + triBBMax = RcVec3f.Max(triBBMax, RcVec.Create(verts, v1 * 3)); + triBBMax = RcVec3f.Max(triBBMax, RcVec.Create(verts, v2 * 3)); // If the triangle does not touch the bounding box of the heightfield, skip the triangle. if (!OverlapBounds(triBBMin, triBBMax, heightfieldBBMin, heightfieldBBMax)) @@ -328,9 +328,9 @@ namespace DotRecast.Recast int p1 = inRow + 7 * 3; int p2 = p1 + 7 * 3; - RcVecUtils.Copy(buf, 0, verts, v0 * 3); - RcVecUtils.Copy(buf, 3, verts, v1 * 3); - RcVecUtils.Copy(buf, 6, verts, v2 * 3); + RcVec.Copy(buf, 0, verts, v0 * 3); + RcVec.Copy(buf, 3, verts, v1 * 3); + RcVec.Copy(buf, 6, verts, v2 * 3); int nvRow; int nvIn = 3; diff --git a/src/DotRecast.Recast/RcRecast.cs b/src/DotRecast.Recast/RcRecast.cs index 393f4e3..39bef31 100644 --- a/src/DotRecast.Recast/RcRecast.cs +++ b/src/DotRecast.Recast/RcRecast.cs @@ -193,9 +193,9 @@ namespace DotRecast.Recast for (int i = 0; i < nt; ++i) { int tri = i * 3; - RcVec3f v0 = RcVecUtils.Create(verts, tris[tri + 0] * 3); - RcVec3f v1 = RcVecUtils.Create(verts, tris[tri + 1] * 3); - RcVec3f v2 = RcVecUtils.Create(verts, tris[tri + 2] * 3); + RcVec3f v0 = RcVec.Create(verts, tris[tri + 0] * 3); + RcVec3f v1 = RcVec.Create(verts, tris[tri + 1] * 3); + RcVec3f v2 = RcVec.Create(verts, tris[tri + 2] * 3); CalcTriNormal(v0, v1, v2, ref norm); // Check if the face is walkable. if (norm.Y > walkableThr) @@ -231,9 +231,9 @@ namespace DotRecast.Recast for (int i = 0; i < nt; ++i) { int tri = i * 3; - RcVec3f v0 = RcVecUtils.Create(verts, tris[tri + 0] * 3); - RcVec3f v1 = RcVecUtils.Create(verts, tris[tri + 1] * 3); - RcVec3f v2 = RcVecUtils.Create(verts, tris[tri + 2] * 3); + RcVec3f v0 = RcVec.Create(verts, tris[tri + 0] * 3); + RcVec3f v1 = RcVec.Create(verts, tris[tri + 1] * 3); + RcVec3f v2 = RcVec.Create(verts, tris[tri + 2] * 3); CalcTriNormal(v0, v1, v2, ref norm); // Check if the face is walkable. if (norm.Y <= walkableThr) diff --git a/test/DotRecast.Detour.Test/NavMeshBuilderTest.cs b/test/DotRecast.Detour.Test/NavMeshBuilderTest.cs index c2900e5..dc441ec 100644 --- a/test/DotRecast.Detour.Test/NavMeshBuilderTest.cs +++ b/test/DotRecast.Detour.Test/NavMeshBuilderTest.cs @@ -54,7 +54,7 @@ public class NavMeshBuilderTest for (int i = 0; i < 2; i++) { - Assert.That(RcVecUtils.Create(nmd.verts, 223 * 3 + (i * 3)), Is.EqualTo(nmd.offMeshCons[0].pos[i])); + Assert.That(RcVec.Create(nmd.verts, 223 * 3 + (i * 3)), Is.EqualTo(nmd.offMeshCons[0].pos[i])); } Assert.That(nmd.offMeshCons[0].rad, Is.EqualTo(0.1f)); diff --git a/test/DotRecast.Detour.Test/RandomPointTest.cs b/test/DotRecast.Detour.Test/RandomPointTest.cs index 414f116..9495c5f 100644 --- a/test/DotRecast.Detour.Test/RandomPointTest.cs +++ b/test/DotRecast.Detour.Test/RandomPointTest.cs @@ -108,7 +108,7 @@ public class RandomPointTest : AbstractDetourTest var status = query.FindRandomPointWithinCircle(randomRef, randomPt, radius, filter, f, out var nextRandomRef, out var nextRandomPt); Assert.That(status.Failed(), Is.False); - float distance = RcVecUtils.Dist2D(randomPt, nextRandomPt); + float distance = RcVec.Dist2D(randomPt, nextRandomPt); Assert.That(distance <= radius, Is.True); randomRef = nextRandomRef;