From 04fa38bb96f1dc1fe3f58aae0eaaf280b66a5583 Mon Sep 17 00:00:00 2001 From: ikpil Date: Tue, 24 Oct 2023 23:46:45 +0900 Subject: [PATCH] refactor: RcVecUtils.SafeNormalize --- src/DotRecast.Core/Numerics/RcVec3f.cs | 41 ++++--------------- src/DotRecast.Core/Numerics/RcVecUtils.cs | 22 ++++++++++ src/DotRecast.Detour.Crowd/DtCrowd.cs | 4 +- .../DtObstacleAvoidanceQuery.cs | 4 +- .../Jumplink/AbstractGroundSampler.cs | 2 +- src/DotRecast.Detour/DtNavMeshQuery.cs | 2 +- src/DotRecast.Detour/DtUtils.cs | 2 +- src/DotRecast.Recast.Demo/Draw/DebugDraw.cs | 4 +- .../Tools/TestNavmeshSampleTool.cs | 4 +- .../Geom/DemoInputGeomProvider.cs | 2 +- .../Tools/RcTestNavMeshTool.cs | 6 +-- .../Geom/SimpleInputGeomProvider.cs | 2 +- src/DotRecast.Recast/RcAreas.cs | 6 +-- .../RcFilledVolumeRasterization.cs | 6 +-- src/DotRecast.Recast/RcMeshDetails.cs | 16 ++++---- 15 files changed, 60 insertions(+), 63 deletions(-) diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 2c6d5c9..3c4f06d 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -23,7 +23,6 @@ namespace DotRecast.Core.Numerics { public struct RcVec3f { - public const float EPSILON = 1e-6f; public float X; public float Y; @@ -144,29 +143,10 @@ namespace DotRecast.Core.Numerics [MethodImpl(MethodImplOptions.AggressiveInlining)] public override int GetHashCode() { - int hash = X.GetHashCode(); - hash = RcHashCodes.CombineHashCodes(hash, Y.GetHashCode()); - hash = RcHashCodes.CombineHashCodes(hash, Z.GetHashCode()); - return hash; + return HashCode.Combine(X, Y, Z); } - /// Normalizes the vector if the length is greater than zero. - /// If the magnitude is zero, the vector is unchanged. - /// @param[in,out] v The vector to normalize. [(x, y, z)] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void SafeNormalize() - { - float sqMag = RcMath.Sqr(X) + RcMath.Sqr(Y) + RcMath.Sqr(Z); - if (sqMag > EPSILON) - { - float inverseMag = 1.0f / (float)Math.Sqrt(sqMag); - X *= inverseMag; - Y *= inverseMag; - Z *= inverseMag; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Min(float[] @in, int i) { @@ -368,7 +348,7 @@ namespace DotRecast.Core.Numerics { float dx = v2.X - v1.X; float dz = v2.Z - v1.Z; - return (float)Math.Sqrt(dx * dx + dz * dz); + return (float)MathF.Sqrt(dx * dx + dz * dz); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -457,18 +437,13 @@ namespace DotRecast.Core.Numerics [MethodImpl(MethodImplOptions.AggressiveInlining)] public static RcVec3f Normalize(RcVec3f v) { - float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z))); + float d = 1.0f / MathF.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z)); - if (d != 0) - { - return new RcVec3f( - v.X *= d, - v.Y *= d, - v.Z *= d - ); - } - - return v; + return new RcVec3f( + v.X *= d, + v.Y *= d, + v.Z *= d + ); } } } \ No newline at end of file diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs index 58ecd57..075e479 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/src/DotRecast.Core/Numerics/RcVecUtils.cs @@ -5,6 +5,8 @@ namespace DotRecast.Core.Numerics { public static class RcVecUtils { + public const float EPSILON = 1e-6f; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Get(this RcVec2f v, int i) { @@ -120,5 +122,25 @@ namespace DotRecast.Core.Numerics v1[1] * vector2.Y + v1[2] * vector2.Z; } + + /// Normalizes the vector if the length is greater than zero. + /// If the magnitude is zero, the vector is unchanged. + /// @param[in,out] v The vector to normalize. [(x, y, z)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static RcVec3f SafeNormalize(RcVec3f v) + { + float sqMag = RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z); + if (sqMag > EPSILON) + { + float inverseMag = 1.0f / MathF.Sqrt(sqMag); + return new RcVec3f( + v.X *= inverseMag, + v.Y *= inverseMag, + v.Z *= inverseMag + ); + } + + return v; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index fe59f36..39043a7 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -1091,7 +1091,7 @@ namespace DotRecast.Detour.Crowd continue; } - float dist = (float)Math.Sqrt(distSqr); + float dist = MathF.Sqrt(distSqr); float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist)); disp = RcVec3f.Mad(disp, diff, weight / dist); @@ -1233,7 +1233,7 @@ namespace DotRecast.Detour.Crowd continue; } - dist = (float)Math.Sqrt(dist); + dist = MathF.Sqrt(dist); float pen = (ag.option.radius + nei.option.radius) - dist; if (dist < 0.0001f) { diff --git a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs index 140789d..39299fc 100644 --- a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs @@ -175,7 +175,7 @@ namespace DotRecast.Detour.Crowd return false; // no intersection. a = 1.0f / a; - float rd = (float)Math.Sqrt(d); + float rd = MathF.Sqrt(d); tmin = (b - rd) * a; tmax = (b + rd) * a; @@ -364,7 +364,7 @@ namespace DotRecast.Detour.Crowd // vector normalization that ignores the y-component. void DtNormalize2D(float[] v) { - float d = (float)Math.Sqrt(v[0] * v[0] + v[2] * v[2]); + float d = MathF.Sqrt(v[0] * v[0] + v[2] * v[2]); if (d == 0) return; d = 1.0f / d; diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 0f7614d..4eb0887 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 = (float)Math.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q)); + float dist = MathF.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q)); int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs)); SampleGroundSegment(heightFunc, es.start, ngsamples); diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index a2c998a..5430041 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -3302,7 +3302,7 @@ namespace DotRecast.Detour hitNormal = RcVec3f.Normalize(new RcVec3f(tangent.Z, 0, -tangent.X)); } - hitDist = (float)Math.Sqrt(radiusSqr); + hitDist = MathF.Sqrt(radiusSqr); return status; } diff --git a/src/DotRecast.Detour/DtUtils.cs b/src/DotRecast.Detour/DtUtils.cs index 8580a84..2637c19 100644 --- a/src/DotRecast.Detour/DtUtils.cs +++ b/src/DotRecast.Detour/DtUtils.cs @@ -193,7 +193,7 @@ namespace DotRecast.Detour acc += dacc; } - float v = (float)Math.Sqrt(t); + float v = MathF.Sqrt(t); float a = 1 - v; float b = (1 - u) * v; diff --git a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs index 3e9bd83..0b4e057 100644 --- a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs +++ b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs @@ -335,7 +335,7 @@ public class DebugDraw float dx = x1 - x0; float dy = y1 - y0; float dz = z1 - z0; - float len = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); + float len = MathF.Sqrt(dx * dx + dy * dy + dz * dz); RcVec3f prev = new RcVec3f(); EvalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, ref prev); for (int i = 1; i <= NUM_ARC_PTS; ++i) @@ -677,7 +677,7 @@ public class DebugDraw private void NormalizePlane(float px, float py, float pz, float pw, ref float[] plane) { - float length = (float)Math.Sqrt(px * px + py * py + pz * pz); + float length = MathF.Sqrt(px * px + py * py + pz * pz); if (length != 0) { length = 1f / length; diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index a239f96..3be0a7b 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -424,7 +424,7 @@ public class TestNavmeshSampleTool : ISampleTool dd.DepthMask(false); float dx = m_epos.X - m_spos.X; float dz = m_epos.Z - m_spos.Z; - float dist = (float)Math.Sqrt(dx * dx + dz * dz); + float dist = MathF.Sqrt(dx * dx + dz * dz); dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f); dd.DepthMask(true); } @@ -566,7 +566,7 @@ public class TestNavmeshSampleTool : ISampleTool dd.DepthMask(false); float dx = m_epos.X - m_spos.X; float dz = m_epos.Z - m_spos.Z; - float dist = (float)Math.Sqrt(dx * dx + dz * dz); + float dist = MathF.Sqrt(dx * dx + dz * dz); dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f); dd.DepthMask(true); } diff --git a/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs b/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs index 12b7754..76ad3aa 100644 --- a/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs +++ b/src/DotRecast.Recast.Toolset/Geom/DemoInputGeomProvider.cs @@ -101,7 +101,7 @@ namespace DotRecast.Recast.Toolset.Geom normals[i] = e0.Y * e1.Z - e0.Z * e1.Y; normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z; normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X; - float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]); + float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]); if (d > 0) { d = 1.0f / d; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 88e94a4..d688ef4 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -74,7 +74,7 @@ namespace DotRecast.Recast.Toolset.Tools // Find movement delta. RcVec3f delta = RcVec3f.Subtract(steerPos, iterPos); - float len = (float)Math.Sqrt(RcVec3f.Dot(delta, delta)); + float len = MathF.Sqrt(RcVec3f.Dot(delta, delta)); // If the steer target is end of path or off-mesh link, do not move past the location. if ((endOfPath || offMeshConnection) && len < STEP_SIZE) { @@ -329,7 +329,7 @@ namespace DotRecast.Recast.Toolset.Tools float dx = epos.X - spos.X; float dz = epos.Z - spos.Z; - float dist = (float)Math.Sqrt(dx * dx + dz * dz); + float dist = MathF.Sqrt(dx * dx + dz * dz); List tempResultRefs = new List(); List tempParentRefs = new List(); @@ -417,7 +417,7 @@ namespace DotRecast.Recast.Toolset.Tools float dx = epos.X - spos.X; float dz = epos.Z - spos.Z; - float dist = (float)Math.Sqrt(dx * dx + dz * dz); + float dist = MathF.Sqrt(dx * dx + dz * dz); IDtPolygonByCircleConstraint constraint = constrainByCircle ? DtStrictDtPolygonByCircleConstraint.Shared diff --git a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs index 495445a..b252552 100644 --- a/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs +++ b/src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs @@ -163,7 +163,7 @@ namespace DotRecast.Recast.Geom normals[i] = e0.Y * e1.Z - e0.Z * e1.Y; normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z; normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X; - float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]); + float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]); if (d > 0) { d = 1.0f / d; diff --git a/src/DotRecast.Recast/RcAreas.cs b/src/DotRecast.Recast/RcAreas.cs index be1c3e2..49ae660 100644 --- a/src/DotRecast.Recast/RcAreas.cs +++ b/src/DotRecast.Recast/RcAreas.cs @@ -760,12 +760,12 @@ namespace DotRecast.Recast // From A to B on the x/z plane RcVec3f prevSegmentDir = RcVec3f.Subtract(vertB, vertA); prevSegmentDir.Y = 0; // Squash onto x/z plane - prevSegmentDir.SafeNormalize(); + prevSegmentDir = RcVecUtils.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.SafeNormalize(); + currSegmentDir = RcVecUtils.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 @@ -792,7 +792,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 > RcVec3f.EPSILON) + if (cornerMiterSqMag > RcVecUtils.EPSILON) { float scale = 1.0f / cornerMiterSqMag; cornerMiterX *= scale; diff --git a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs index 995fe05..20450fa 100644 --- a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs @@ -253,7 +253,7 @@ namespace DotRecast.Recast return null; } - float discrSqrt = (float)Math.Sqrt(discr); + float discrSqrt = MathF.Sqrt(discr); float tmin = -b - discrSqrt; float tmax = -b + discrSqrt; @@ -346,7 +346,7 @@ namespace DotRecast.Recast float discr = b * b - c; if (discr > EPSILON) { - float discrSqrt = (float)Math.Sqrt(discr); + float discrSqrt = MathF.Sqrt(discr); float t1 = -b - discrSqrt; float t2 = -b + discrSqrt; if (t1 <= 1 && t2 >= 0) @@ -454,7 +454,7 @@ namespace DotRecast.Recast return null; // No real roots; no intersection } - float discSqrt = (float)Math.Sqrt(discr); + float discSqrt = MathF.Sqrt(discr); float t1 = (-b - discSqrt) / a; float t2 = (-b + discSqrt) / a; diff --git a/src/DotRecast.Recast/RcMeshDetails.cs b/src/DotRecast.Recast/RcMeshDetails.cs index 12bfa81..7fc2121 100644 --- a/src/DotRecast.Recast/RcMeshDetails.cs +++ b/src/DotRecast.Recast/RcMeshDetails.cs @@ -60,7 +60,7 @@ namespace DotRecast.Recast private static float Vdist2(float[] verts, int p, int q) { - return (float)Math.Sqrt(VdistSq2(verts, p, q)); + return MathF.Sqrt(VdistSq2(verts, p, q)); } private static float VdistSq2(float[] p, float[] q) @@ -88,17 +88,17 @@ namespace DotRecast.Recast private static float Vdist2(float[] p, float[] q) { - return (float)Math.Sqrt(VdistSq2(p, q)); + return MathF.Sqrt(VdistSq2(p, q)); } private static float Vdist2(RcVec3f p, RcVec3f q) { - return (float)Math.Sqrt(VdistSq2(p, q)); + return MathF.Sqrt(VdistSq2(p, q)); } private static float Vdist2(float[] p, RcVec3f q) { - return (float)Math.Sqrt(VdistSq2(p, q)); + return MathF.Sqrt(VdistSq2(p, q)); } @@ -119,12 +119,12 @@ namespace DotRecast.Recast private static float Vdist2(float[] p, float[] verts, int q) { - return (float)Math.Sqrt(VdistSq2(p, verts, q)); + return MathF.Sqrt(VdistSq2(p, verts, q)); } private static float Vdist2(RcVec3f p, float[] verts, int q) { - return (float)Math.Sqrt(VdistSq2(p, verts, q)); + return MathF.Sqrt(VdistSq2(p, verts, q)); } @@ -743,7 +743,7 @@ namespace DotRecast.Recast minDist = Math.Min(minDist, maxEdgeDist); } - return (float)Math.Sqrt(minDist); + return MathF.Sqrt(minDist); } private static void TriangulateHull(int nverts, float[] verts, int nhull, int[] hull, int nin, List tris) @@ -890,7 +890,7 @@ namespace DotRecast.Recast float dx = @in[vi + 0] - @in[vj + 0]; float dy = @in[vi + 1] - @in[vj + 1]; float dz = @in[vi + 2] - @in[vj + 2]; - float d = (float)Math.Sqrt(dx * dx + dz * dz); + float d = MathF.Sqrt(dx * dx + dz * dz); int nn = 1 + (int)Math.Floor(d / sampleDist); if (nn >= MAX_VERTS_PER_EDGE) {