diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 683b69d..2335816 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -23,6 +23,8 @@ namespace DotRecast.Core.Numerics { public struct RcVec3f { + public const float EPSILON = 1e-6f; + public float X; public float Y; public float Z; @@ -148,21 +150,6 @@ namespace DotRecast.Core.Numerics return hash; } - /// Normalizes the vector. - /// @param[in,out] v The vector to normalize. [(x, y, z)] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Normalize() - { - float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(X) + RcMath.Sqr(Y) + RcMath.Sqr(Z))); - if (d != 0) - { - X *= d; - Y *= d; - Z *= d; - } - } - - public const float EPSILON = 1e-6f; /// Normalizes the vector if the length is greater than zero. /// If the magnitude is zero, the vector is unchanged. @@ -535,13 +522,23 @@ namespace DotRecast.Core.Numerics dest.Z = v1.X * v2.Y - v1.Y * v2.X; } + /// Normalizes the vector. + /// @param[in,out] v The vector to normalize. [(x, y, z)] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Normalize(ref RcVec3f v) + public static RcVec3f Normalize(RcVec3f v) { - float d = (float)(1.0f / Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z)); - v.X *= d; - v.Y *= d; - v.Z *= d; + float d = (float)(1.0f / Math.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; } } } \ No newline at end of file diff --git a/src/DotRecast.Core/Numerics/RcVecExtensions.cs b/src/DotRecast.Core/Numerics/RcVecExtensions.cs index b17786c..1cd2952 100644 --- a/src/DotRecast.Core/Numerics/RcVecExtensions.cs +++ b/src/DotRecast.Core/Numerics/RcVecExtensions.cs @@ -29,6 +29,7 @@ namespace DotRecast.Core.Numerics } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static RcVec3f Scale(this RcVec3f v, float scale) { diff --git a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs index 1e37f78..f0a93ce 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs @@ -180,7 +180,7 @@ namespace DotRecast.Detour.Crowd dir.X = dir0.X - dir1.X * len0 * 0.5f; dir.Y = 0; dir.Z = dir0.Z - dir1.Z * len0 * 0.5f; - dir.Normalize(); + dir = RcVec3f.Normalize(dir); } return dir; @@ -193,7 +193,7 @@ namespace DotRecast.Detour.Crowd { dir = RcVec3f.Subtract(corners[0].pos, npos); dir.Y = 0; - dir.Normalize(); + dir = RcVec3f.Normalize(dir); } return dir; diff --git a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs index e928fd4..140789d 100644 --- a/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs @@ -127,7 +127,7 @@ namespace DotRecast.Detour.Crowd RcVec3f orig = new RcVec3f(); RcVec3f dv = new RcVec3f(); cir.dp = RcVec3f.Subtract(pb, pa); - cir.dp.Normalize(); + cir.dp = RcVec3f.Normalize(cir.dp); dv = RcVec3f.Subtract(cir.dvel, dvel); float a = DtUtils.TriArea2D(orig, cir.dp, dv); diff --git a/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs b/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs index f58afbb..517c198 100644 --- a/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs +++ b/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs @@ -76,11 +76,12 @@ namespace DotRecast.Detour.Dynamic.Colliders new RcVec3f(up.X, up.Y, up.Z), RcVec3f.Zero }; - RcVec3f.Normalize(ref halfEdges[1]); + + halfEdges[1] = RcVec3f.Normalize(halfEdges[1]); RcVec3f.Cross(ref halfEdges[0], up, forward); - RcVec3f.Normalize(ref halfEdges[0]); + halfEdges[0] = RcVec3f.Normalize(halfEdges[0]); RcVec3f.Cross(ref halfEdges[2], halfEdges[0], up); - RcVec3f.Normalize(ref halfEdges[2]); + halfEdges[2] = RcVec3f.Normalize(halfEdges[2]); halfEdges[0].X *= extent.X; halfEdges[0].Y *= extent.X; halfEdges[0].Z *= extent.X; diff --git a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs index 69662d5..9a5a468 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs @@ -17,9 +17,11 @@ namespace DotRecast.Detour.Extras.Jumplink { this.trajectory = trajectory; ax = RcVec3f.Subtract(edge.sq, edge.sp); - ax.Normalize(); + ax = RcVec3f.Normalize(ax); + az = new RcVec3f(ax.Z, 0, -ax.X); - az.Normalize(); + az = RcVec3f.Normalize(az); + ay = new RcVec3f(0, 1, 0); } } diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 21c460f..a2c998a 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -2363,10 +2363,7 @@ namespace DotRecast.Detour // int vb = b * 3; float dx = verts[b].X - verts[a].X; float dz = verts[b].Z - verts[a].X; - hit.hitNormal.X = dz; - hit.hitNormal.Y = 0; - hit.hitNormal.Z = -dx; - hit.hitNormal.Normalize(); + hit.hitNormal = RcVec3f.Normalize(new RcVec3f(dz, 0, -dx)); return DtStatus.DT_SUCCSESS; } @@ -3302,10 +3299,7 @@ namespace DotRecast.Detour if (hasBestV) { var tangent = RcVec3f.Subtract(bestvi, bestvj); - hitNormal.X = tangent.Z; - hitNormal.Y = 0; - hitNormal.Z = -tangent.X; - hitNormal.Normalize(); + hitNormal = RcVec3f.Normalize(new RcVec3f(tangent.Z, 0, -tangent.X)); } hitDist = (float)Math.Sqrt(radiusSqr); diff --git a/src/DotRecast.Recast.Demo/RecastDemo.cs b/src/DotRecast.Recast.Demo/RecastDemo.cs index 5cbf46c..59ac45e 100644 --- a/src/DotRecast.Recast.Demo/RecastDemo.cs +++ b/src/DotRecast.Recast.Demo/RecastDemo.cs @@ -786,8 +786,10 @@ public class RecastDemo : IRecastDemoChannel } RcVec3f rayDir = new RcVec3f(rayEnd.X - rayStart.X, rayEnd.Y - rayStart.Y, rayEnd.Z - rayStart.Z); + rayDir = RcVec3f.Normalize(rayDir); + ISampleTool raySampleTool = toolset.GetTool(); - rayDir.Normalize(); + if (raySampleTool != null) { Logger.Information($"click ray - tool({raySampleTool.GetTool().GetName()}) rayStart({rayStart.X:0.#},{rayStart.Y:0.#},{rayStart.Z:0.#}) pos({rayDir.X:0.#},{rayDir.Y:0.#},{rayDir.Z:0.#}) shift({processHitTestShift})"); diff --git a/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs b/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs index 7048f5e..bcf02fc 100644 --- a/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs +++ b/src/DotRecast.Recast.Demo/Tools/GizmoRenderer.cs @@ -50,7 +50,7 @@ public static class GizmoRenderer normal.X = e0.Y * e1.Z - e0.Z * e1.Y; normal.Y = e0.Z * e1.X - e0.X * e1.Z; normal.Z = e0.X * e1.Y - e0.Y * e1.X; - RcVec3f.Normalize(ref normal); + normal = RcVec3f.Normalize(normal); float c = Math.Clamp(0.57735026f * (normal.X + normal.Y + normal.Z), -1, 1); int col = DebugDraw.DuLerpCol( DebugDraw.DuRGBA(32, 32, 0, 160), diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index 8e2b585..a239f96 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -510,7 +510,7 @@ public class TestNavmeshSampleTool : ISampleTool RcVec3f delta = RcVec3f.Subtract(s3, s.vmin); RcVec3f p0 = RcVec3f.Mad(s.vmin, delta, 0.5f); RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X); - norm.Normalize(); + norm = RcVec3f.Normalize(norm); RcVec3f p1 = RcVec3f.Mad(p0, norm, agentRadius * 0.5f); // Skip backfacing segments. if (segmentRefs[j] != 0) diff --git a/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs b/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs index f45651f..7736529 100644 --- a/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs +++ b/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs @@ -22,11 +22,11 @@ namespace DotRecast.Recast.Toolset.Gizmos RcVec3f axis = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z); RcVec3f[] normals = new RcVec3f[3]; normals[1] = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z); - RcVec3f.Normalize(ref normals[1]); + normals[1] = RcVec3f.Normalize(normals[1]); normals[0] = GetSideVector(axis); normals[2] = RcVec3f.Zero; RcVec3f.Cross(ref normals[2], normals[0], normals[1]); - RcVec3f.Normalize(ref normals[2]); + normals[2] = RcVec3f.Normalize(normals[2]); triangles = GenerateSphericalTriangles(); var trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X); var trY = new RcVec3f(normals[0].Y, normals[1].Y, normals[2].Y); @@ -48,7 +48,7 @@ namespace DotRecast.Recast.Toolset.Gizmos v.X = vertices[i] - center[0]; v.Y = vertices[i + 1] - center[1]; v.Z = vertices[i + 2] - center[2]; - RcVec3f.Normalize(ref v); + v = RcVec3f.Normalize(v); gradient[i / 3] = Math.Clamp(0.57735026f * (v.X + v.Y + v.Z), -1, 1); } } @@ -64,7 +64,7 @@ namespace DotRecast.Recast.Toolset.Gizmos RcVec3f forward = new RcVec3f(); RcVec3f.Cross(ref forward, side, axis); RcVec3f.Cross(ref side, axis, forward); - RcVec3f.Normalize(ref side); + side = RcVec3f.Normalize(side); return side; } } diff --git a/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs b/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs index 7a3389b..c77b46e 100644 --- a/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs +++ b/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs @@ -22,11 +22,11 @@ namespace DotRecast.Recast.Toolset.Gizmos RcVec3f axis = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z); RcVec3f[] normals = new RcVec3f[3]; normals[1] = new RcVec3f(end.X - start.X, end.Y - start.Y, end.Z - start.Z); - RcVec3f.Normalize(ref normals[1]); + normals[1] = RcVec3f.Normalize(normals[1]); normals[0] = GetSideVector(axis); normals[2] = RcVec3f.Zero; RcVec3f.Cross(ref normals[2], normals[0], normals[1]); - RcVec3f.Normalize(ref normals[2]); + normals[2] = RcVec3f.Normalize(normals[2]); triangles = GenerateCylindricalTriangles(); RcVec3f trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X); RcVec3f trY = new RcVec3f(normals[0].Y, normals[1].Y, normals[2].Y); @@ -53,7 +53,7 @@ namespace DotRecast.Recast.Toolset.Gizmos v.X = vertices[i] - center.X; v.Y = vertices[i + 1] - center.Y; v.Z = vertices[i + 2] - center.Z; - RcVec3f.Normalize(ref v); + v = RcVec3f.Normalize(v); gradient[i / 3] = Math.Clamp(0.57735026f * (v.X + v.Y + v.Z), -1, 1); } } @@ -70,7 +70,7 @@ namespace DotRecast.Recast.Toolset.Gizmos RcVec3f forward = new RcVec3f(); RcVec3f.Cross(ref forward, side, axis); RcVec3f.Cross(ref side, axis, forward); - RcVec3f.Normalize(ref side); + side = RcVec3f.Normalize(side); return side; } } diff --git a/src/DotRecast.Recast.Toolset/Tools/RcCrowdTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcCrowdTool.cs index eb9ef04..4bfbd5f 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcCrowdTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcCrowdTool.cs @@ -296,7 +296,7 @@ namespace DotRecast.Recast.Toolset.Tools { RcVec3f vel = RcVec3f.Subtract(tgt, pos); vel.Y = 0.0f; - vel.Normalize(); + vel = RcVec3f.Normalize(vel); return vel.Scale(speed); } diff --git a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs index ae8d608..0f5391d 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs @@ -183,7 +183,8 @@ namespace DotRecast.Recast.Toolset.Tools 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) ); - a.Normalize(); + a = RcVec3f.Normalize(a); + float len = 1f + (float)random.NextDouble() * 20f; a.X *= len; a.Y *= len; @@ -214,7 +215,7 @@ namespace DotRecast.Recast.Toolset.Tools { float radius = 0.7f + (float)random.NextDouble() * 4f; RcVec3f a = new RcVec3f(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble()); - a.Normalize(); + a = RcVec3f.Normalize(a); float len = 2f + (float)random.NextDouble() * 20f; a[0] *= len; a[1] *= len; @@ -233,7 +234,8 @@ namespace DotRecast.Recast.Toolset.Tools RcVec3f baseCenter = new RcVec3f(p.X, p.Y + 3, p.Z); RcVec3f baseUp = new RcVec3f(0, 1, 0); RcVec3f forward = new RcVec3f((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); - forward.Normalize(); + forward = RcVec3f.Normalize(forward); + RcVec3f side = RcVec3f.Cross(forward, baseUp); DtBoxCollider @base = new DtBoxCollider(baseCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(baseUp, forward, baseExtent), SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb); diff --git a/src/DotRecast.Recast/RcCommons.cs b/src/DotRecast.Recast/RcCommons.cs index 9e5e84d..ac48d4d 100644 --- a/src/DotRecast.Recast/RcCommons.cs +++ b/src/DotRecast.Recast/RcCommons.cs @@ -143,7 +143,7 @@ namespace DotRecast.Recast RcVec3f.Sub(ref e0, verts, v1 * 3, v0 * 3); RcVec3f.Sub(ref e1, verts, v2 * 3, v0 * 3); RcVec3f.Cross(ref norm, e0, e1); - RcVec3f.Normalize(ref norm); + norm = RcVec3f.Normalize(norm); } diff --git a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs index 9e16dc7..4282986 100644 --- a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs @@ -79,9 +79,9 @@ namespace DotRecast.Recast new RcVec3f(halfEdges[1].X, halfEdges[1].Y, halfEdges[1].Z), new RcVec3f(halfEdges[2].X, halfEdges[2].Y, halfEdges[2].Z), }; - RcVec3f.Normalize(ref normals[0]); - RcVec3f.Normalize(ref normals[1]); - RcVec3f.Normalize(ref normals[2]); + normals[0] = RcVec3f.Normalize(normals[0]); + normals[1] = RcVec3f.Normalize(normals[1]); + normals[2] = RcVec3f.Normalize(normals[2]); float[] vertices = new float[8 * 3]; float[] bounds = new float[] diff --git a/test/DotRecast.Core.Test/Vector3Tests.cs b/test/DotRecast.Core.Test/Vector3Tests.cs new file mode 100644 index 0000000..5c35582 --- /dev/null +++ b/test/DotRecast.Core.Test/Vector3Tests.cs @@ -0,0 +1,78 @@ +using System; +using System.Numerics; +using DotRecast.Core.Numerics; +using NUnit.Framework; + +namespace DotRecast.Core.Test; + +public class Vector3Tests +{ + [Test] + [Repeat(10000)] + public void TestVectorLength() + { + var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); + + Assert.That(v1.Length(), Is.EqualTo(v11.Length())); + Assert.That(v1.LengthSquared(), Is.EqualTo(v11.LengthSquared())); + } + + [Test] + [Repeat(10000)] + public void TestVectorSubtract() + { + var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v3 = Vector3.Subtract(v1, v2); + var v4 = v1 - v2; + Assert.That(v3, Is.EqualTo(v4)); + + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); + var v22 = new RcVec3f(v2.X, v2.Y, v2.Z); + var v33 = RcVec3f.Subtract(v11, v22); + var v44 = v11 - v22; + Assert.That(v33, Is.EqualTo(v44)); + + Assert.That(v3.X, Is.EqualTo(v33.X)); + Assert.That(v3.Y, Is.EqualTo(v33.Y)); + Assert.That(v3.Z, Is.EqualTo(v33.Z)); + } + + + [Test] + [Repeat(10000)] + public void TestVectorAdd() + { + var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v3 = Vector3.Add(v1, v2); + var v4 = v1 + v2; + Assert.That(v3, Is.EqualTo(v4)); + + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); + var v22 = new RcVec3f(v2.X, v2.Y, v2.Z); + var v33 = RcVec3f.Add(v11, v22); + var v44 = v11 + v22; + Assert.That(v33, Is.EqualTo(v44)); + + Assert.That(v3.X, Is.EqualTo(v33.X)); + Assert.That(v3.Y, Is.EqualTo(v33.Y)); + Assert.That(v3.Z, Is.EqualTo(v33.Z)); + } + + [Test] + [Repeat(10000)] + public void TestVectorNormalize() + { + var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v2 = Vector3.Normalize(v1); + + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); + var v22 = RcVec3f.Normalize(v11); + + Assert.That(v2.X, Is.EqualTo(v22.X).Within(0.000001d)); + Assert.That(v2.Y, Is.EqualTo(v22.Y).Within(0.000001d)); + Assert.That(v2.Z, Is.EqualTo(v22.Z).Within(0.000001d)); + } +} \ No newline at end of file diff --git a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs index 8fd4606..ec93b91 100644 --- a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs @@ -21,13 +21,10 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; using DotRecast.Core.Numerics; - using NUnit.Framework; namespace DotRecast.Detour.Crowd.Test; - - [Parallelizable] public class AbstractCrowdTest { @@ -155,7 +152,7 @@ public class AbstractCrowdTest { RcVec3f vel = RcVec3f.Subtract(tgt, pos); vel.Y = 0.0f; - vel.Normalize(); + vel = RcVec3f.Normalize(vel); vel = vel.Scale(speed); return vel; }