From 3f72b8e916fc48dd8291a6c8ac642b00ffe35bc1 Mon Sep 17 00:00:00 2001 From: ikpil Date: Thu, 25 May 2023 23:12:23 +0900 Subject: [PATCH] inline Vector3f --- src/DotRecast.Core/RcMath.cs | 47 +---------------- src/DotRecast.Core/Vector3f.cs | 26 ++++++++++ src/DotRecast.Detour.Crowd/Crowd.cs | 8 +-- .../ObstacleAvoidanceQuery.cs | 6 +-- src/DotRecast.Detour/FindNearestPolyQuery.cs | 2 +- src/DotRecast.Detour/NavMesh.cs | 2 +- src/DotRecast.Recast.Demo/RecastDemo.cs | 51 ++++++++++--------- .../Tools/DynamicUpdateTool.cs | 12 ++--- src/DotRecast.Recast.Demo/Tools/Tool.cs | 2 +- 9 files changed, 70 insertions(+), 86 deletions(-) diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index 61b349f..d36a92d 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -64,34 +64,6 @@ namespace DotRecast.Core return u * g + (1f - u) * f; } - - - - /// Derives the square of the scalar length of the vector. (len * len) - /// @param[in] v The vector. [(x, y, z)] - /// @return The square of the scalar length of the vector. - public static float VLenSqr(Vector3f v) - { - return v.x * v.x + v.y * v.y + v.z * v.z; - } - - - - - /// Normalizes the vector. - /// @param[in,out] v The vector to normalize. [(x, y, z)] - public static void VNormalize(float[] v) - { - float d = (float)(1.0f / Math.Sqrt(Sqr(v[0]) + Sqr(v[1]) + Sqr(v[2]))); - if (d != 0) - { - v[0] *= d; - v[1] *= d; - v[2] *= d; - } - } - - /// 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)] @@ -111,22 +83,7 @@ namespace DotRecast.Core } - /// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz) - /// @param[in] u The LHV vector [(x, y, z)] - /// @param[in] v The RHV vector [(x, y, z)] - /// @return The dot product on the xz-plane. - /// - /// The vectors are projected onto the xz-plane, so the y-values are - /// ignored. - public static float VPerp2D(float[] u, float[] v) - { - return u[2] * v[0] - u[0] * v[2]; - } - public static float VPerp2D(Vector3f u, Vector3f v) - { - return u.z * v.x - u.x * v.z; - } /// @} @@ -474,8 +431,8 @@ namespace DotRecast.Core Vector3f vpi = Vector3f.Of(verts, i * 3); var edge = vpi.Subtract(vpj); var diff = p0v.Subtract(vpj); - float n = VPerp2D(edge, diff); - float d = VPerp2D(dir, edge); + float n = Vector3f.Perp2D(edge, diff); + float d = Vector3f.Perp2D(dir, edge); if (Math.Abs(d) < EPS) { // S is nearly parallel to this edge diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 89690b2..b988e20 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -206,6 +206,8 @@ namespace DotRecast.Core return hash; } + /// Normalizes the vector. + /// @param[in,out] v The vector to normalize. [(x, y, z)] [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Normalize() { @@ -446,5 +448,29 @@ namespace DotRecast.Core float dz = verts[i + 2] - p.z; return dx * dx + dz * dz; } + + /// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz) + /// @param[in] u The LHV vector [(x, y, z)] + /// @param[in] v The RHV vector [(x, y, z)] + /// @return The dot product on the xz-plane. + /// + /// The vectors are projected onto the xz-plane, so the y-values are + /// ignored. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Perp2D(Vector3f u, Vector3f v) + { + return u.z * v.x - u.x * v.z; + } + + /// Derives the square of the scalar length of the vector. (len * len) + /// @param[in] v The vector. [(x, y, z)] + /// @return The square of the scalar length of the vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float LenSqr(Vector3f v) + { + return v.x * v.x + v.y * v.y + v.z * v.z; + } + } + } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index 899b836..88a2a11 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -924,7 +924,7 @@ namespace DotRecast.Detour.Crowd } diff.y = 0; - float distSqr = VLenSqr(diff); + float distSqr = Vector3f.LenSqr(diff); if (distSqr > Sqr(range)) { continue; @@ -1094,7 +1094,7 @@ namespace DotRecast.Detour.Crowd Vector3f diff = ag.npos.Subtract(nei.npos); diff.y = 0; - float distSqr = VLenSqr(diff); + float distSqr = Vector3f.LenSqr(diff); if (distSqr < 0.00001f) { continue; @@ -1117,7 +1117,7 @@ namespace DotRecast.Detour.Crowd // Adjust desired velocity. dvel = Vector3f.Mad(dvel, disp, 1.0f / w); // Clamp desired velocity to desired speed. - float speedSqr = VLenSqr(dvel); + float speedSqr = Vector3f.LenSqr(dvel); float desiredSqr = Sqr(ag.desiredSpeed); if (speedSqr > desiredSqr) { @@ -1248,7 +1248,7 @@ namespace DotRecast.Detour.Crowd Vector3f diff = ag.npos.Subtract(nei.npos); diff.y = 0; - float dist = VLenSqr(diff); + float dist = Vector3f.LenSqr(diff); if (dist > Sqr(ag.option.radius + nei.option.radius)) { continue; diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index 9e90130..017a4a3 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -179,16 +179,16 @@ namespace DotRecast.Detour.Crowd { Vector3f v = bq.Subtract(bp); Vector3f w = ap.Subtract(bp); - float d = VPerp2D(u, v); + float d = Vector3f.Perp2D(u, v); if (Math.Abs(d) < 1e-6f) return new IsectRaySegResult(false, 0f); d = 1.0f / d; - float t = VPerp2D(v, w) * d; + float t = Vector3f.Perp2D(v, w) * d; if (t < 0 || t > 1) return new IsectRaySegResult(false, 0f); - float s = VPerp2D(u, w) * d; + float s = Vector3f.Perp2D(u, w) * d; if (s < 0 || s > 1) return new IsectRaySegResult(false, 0f); diff --git a/src/DotRecast.Detour/FindNearestPolyQuery.cs b/src/DotRecast.Detour/FindNearestPolyQuery.cs index 27267c8..bc60f67 100644 --- a/src/DotRecast.Detour/FindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/FindNearestPolyQuery.cs @@ -41,7 +41,7 @@ namespace DotRecast.Detour } else { - d = VLenSqr(diff); + d = Vector3f.LenSqr(diff); } if (d < nearestDistanceSqr) diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index 1fef36d..0a258b6 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -1382,7 +1382,7 @@ namespace DotRecast.Detour } else { - d = VLenSqr(diff); + d = Vector3f.LenSqr(diff); } if (d < nearestDistanceSqr) diff --git a/src/DotRecast.Recast.Demo/RecastDemo.cs b/src/DotRecast.Recast.Demo/RecastDemo.cs index f94fa8f..11d772f 100644 --- a/src/DotRecast.Recast.Demo/RecastDemo.cs +++ b/src/DotRecast.Recast.Demo/RecastDemo.cs @@ -54,7 +54,7 @@ public class RecastDemo private IInputContext _input; private ImGuiController _imgui; private RecastDemoCanvas _canvas; - + private int width = 1000; private int height = 900; @@ -485,28 +485,29 @@ public class RecastDemo } else if (settingsUI.IsNavMeshInputTrigerred()) { - // try (MemoryStack stack = StackPush()) { - // PointerBuffer aFilterPatterns = stack.MallocPointer(4); - // aFilterPatterns.Put(stack.UTF8("*.bin")); - // aFilterPatterns.Put(stack.UTF8("*.zip")); - // aFilterPatterns.Put(stack.UTF8("*.bytes")); - // aFilterPatterns.Put(stack.UTF8("*.navmesh")); - // aFilterPatterns.Flip(); - // string filename = TinyFileDialogs.Tinyfd_openFileDialog("Open Nav Mesh File", "", aFilterPatterns, - // "Nav Mesh File", false); - // if (filename != null) { - // File file = new File(filename); - // if (file.Exists()) { - // try { - // LoadNavMesh(file, filename); - // geom = null; - // } catch (Exception e) { - // Console.WriteLine(e); - // } - // } - // } - // } + // try (MemoryStack stack = StackPush()) { + // PointerBuffer aFilterPatterns = stack.MallocPointer(4); + // aFilterPatterns.Put(stack.UTF8("*.bin")); + // aFilterPatterns.Put(stack.UTF8("*.zip")); + // aFilterPatterns.Put(stack.UTF8("*.bytes")); + // aFilterPatterns.Put(stack.UTF8("*.navmesh")); + // aFilterPatterns.Flip(); + // string filename = TinyFileDialogs.Tinyfd_openFileDialog("Open Nav Mesh File", "", aFilterPatterns, + // "Nav Mesh File", false); + // if (filename != null) { + // File file = new File(filename); + // if (file.Exists()) { + // try { + // LoadNavMesh(file, filename); + // geom = null; + // } catch (Exception e) { + // Console.WriteLine(e); + // } + // } + // } + // } } + if (settingsUI.IsBuildTriggered() && sample.GetInputGeom() != null) { if (!building) @@ -597,9 +598,9 @@ public class RecastDemo hit = PolyMeshRaycast.Raycast(sample.GetRecastResults(), rayStart, rayEnd); } - float[] rayDir = new float[] { rayEnd.x - rayStart.x, rayEnd.y - rayStart.y, rayEnd.z - rayStart.z }; + Vector3f rayDir = Vector3f.Of(rayEnd.x - rayStart.x, rayEnd.y - rayStart.y, rayEnd.z - rayStart.z); Tool rayTool = toolsUI.GetTool(); - VNormalize(rayDir); + rayDir.Normalize(); if (rayTool != null) { rayTool.HandleClickRay(rayStart, rayDir, processHitTestShift); @@ -737,4 +738,4 @@ public class RecastDemo window.SwapBuffers(); } -} +} \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs index dfe867a..06771d3 100644 --- a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs @@ -229,14 +229,14 @@ public class DynamicUpdateTool : Tool private Tuple CylinderCollider(Vector3f p) { float radius = 0.7f + (float)random.NextDouble() * 4f; - float[] a = new float[] { (1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) }; - VNormalize(a); + Vector3f a = Vector3f.Of(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble()); + a.Normalize(); float len = 2f + (float)random.NextDouble() * 20f; a[0] *= len; a[1] *= len; a[2] *= len; Vector3f start = Vector3f.Of(p.x, p.y, p.z); - Vector3f end = Vector3f.Of(p.x + a[0], p.y + a[1], p.z + a[2]); + Vector3f end = Vector3f.Of(p.x + a.x, p.y + a.y, p.z + a.z); return Tuple.Create(new CylinderCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb), GizmoFactory.Cylinder(start, end, radius)); } @@ -349,7 +349,7 @@ public class DynamicUpdateTool : Tool } - public override void HandleClickRay(Vector3f start, float[] dir, bool shift) + public override void HandleClickRay(Vector3f start, Vector3f dir, bool shift) { if (mode == DynamicUpdateToolMode.COLLIDERS) { @@ -369,7 +369,7 @@ public class DynamicUpdateTool : Tool } } - private bool Hit(Vector3f point, float[] dir, float[] bounds) + private bool Hit(Vector3f point, Vector3f dir, float[] bounds) { float cx = 0.5f * (bounds[0] + bounds[3]); float cy = 0.5f * (bounds[1] + bounds[4]); @@ -387,7 +387,7 @@ public class DynamicUpdateTool : Tool return true; } - float b = mx * dir[0] + my * dir[1] + mz * dir[2]; + float b = mx * dir.x + my * dir.y + mz * dir.z; if (b > 0.0f) { return false; diff --git a/src/DotRecast.Recast.Demo/Tools/Tool.cs b/src/DotRecast.Recast.Demo/Tools/Tool.cs index 1546de8..3991e1c 100644 --- a/src/DotRecast.Recast.Demo/Tools/Tool.cs +++ b/src/DotRecast.Recast.Demo/Tools/Tool.cs @@ -38,7 +38,7 @@ public abstract class Tool public abstract string GetName(); - public virtual void HandleClickRay(Vector3f start, float[] direction, bool shift) + public virtual void HandleClickRay(Vector3f start, Vector3f direction, bool shift) { // ... }