diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index 33611a4..3636ae1 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -737,42 +737,23 @@ namespace DotRecast.Core return (side + 4) & 0x7; } - public static float VperpXZ(float[] a, float[] b) - { - return a[0] * b[2] - a[2] * b[0]; - } - - public static float VperpXZ(Vector3f a, Vector3f b) - { - return a.x * b.z - a.z * b.x; - } public static Tuple IntersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq) { Vector3f u = aq.Subtract(ap); Vector3f v = bq.Subtract(bp); Vector3f w = ap.Subtract(bp); - float d = VperpXZ(u, v); + float d = Vector3f.PerpXZ(u, v); if (Math.Abs(d) < 1e-6f) { return null; } - float s = VperpXZ(v, w) / d; - float t = VperpXZ(u, w) / d; + float s = Vector3f.PerpXZ(v, w) / d; + float t = Vector3f.PerpXZ(u, w) / d; return Tuple.Create(s, t); } - - public static Vector3f VScale(Vector3f @in, float scale) - { - var @out = new Vector3f(); - @out.x = @in.x * scale; - @out.y = @in.y * scale; - @out.z = @in.z * scale; - return @out; - } - - + /// Checks that the specified vector's components are all finite. /// @param[in] v A point. [(x, y, z)] /// @return True if all of the point's components are finite, i.e. not NaN diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 551ec99..30a7fd7 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -18,6 +18,7 @@ freely, subject to the following restrictions: using System; using System.Runtime.CompilerServices; +using System.Security.Permissions; namespace DotRecast.Core { @@ -52,6 +53,14 @@ namespace DotRecast.Core this.z = z; } + public Vector3f(float f) + { + x = f; + y = f; + z = f; + } + + public Vector3f(float[] f) { x = f[0]; @@ -120,6 +129,17 @@ namespace DotRecast.Core ); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly Vector3f Scale(float scale) + { + return new Vector3f( + x * scale, + y * scale, + z * scale + ); + } + + /// 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)] @@ -175,11 +195,13 @@ namespace DotRecast.Core } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator ==(Vector3f left, Vector3f right) { return left.Equals(right); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator !=(Vector3f left, Vector3f right) { return !left.Equals(right); @@ -197,6 +219,28 @@ namespace DotRecast.Core return left.Add(right); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3f operator *(Vector3f left, Vector3f right) + { + return new Vector3f( + left.x * right.x, + left.y * right.y, + left.z * right.z + ); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3f operator *(Vector3f left, float right) + { + return left * new Vector3f(right); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3f operator *(float left, Vector3f right) + { + return right * left; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3f Cross(Vector3f v1, Vector3f v2) { @@ -236,5 +280,11 @@ namespace DotRecast.Core return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float PerpXZ(Vector3f a, Vector3f b) + { + return (a.x * b.z) - (a.z * b.x); + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index f4e6700..23fa350 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -1074,7 +1074,7 @@ namespace DotRecast.Detour.Crowd float speedScale = ag.GetDistanceToGoal(slowDownRadius) / slowDownRadius; ag.desiredSpeed = ag.option.maxSpeed; - dvel = VScale(dvel, ag.desiredSpeed * speedScale); + dvel = dvel.Scale(ag.desiredSpeed * speedScale); } // Separation @@ -1121,7 +1121,7 @@ namespace DotRecast.Detour.Crowd float desiredSqr = Sqr(ag.desiredSpeed); if (speedSqr > desiredSqr) { - dvel = VScale(dvel, desiredSqr / speedSqr); + dvel = dvel.Scale(desiredSqr / speedSqr); } } } @@ -1283,7 +1283,7 @@ namespace DotRecast.Detour.Crowd if (w > 0.0001f) { float iw = 1.0f / w; - ag.disp = VScale(ag.disp, iw); + ag.disp = ag.disp.Scale(iw); } } diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index c09d826..3170387 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -118,7 +118,7 @@ namespace DotRecast.Detour.Crowd Vector3f dv = nvel.Subtract(vel); float ds = dv.Length(); if (ds > maxDelta) - dv = VScale(dv, maxDelta / ds); + dv = dv.Scale(maxDelta / ds); vel = vel.Add(dv); // Integrate @@ -177,7 +177,7 @@ namespace DotRecast.Detour.Crowd float len0 = dir0.Length(); float len1 = dir1.Length(); if (len1 > 0.001f) - dir1 = VScale(dir1, 1.0f / len1); + dir1 = dir1.Scale(1.0f / len1); dir.x = dir0.x - dir1.x * len0 * 0.5f; dir.y = 0; diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index c32bb57..bdd200e 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -229,7 +229,7 @@ namespace DotRecast.Detour.Crowd ObstacleCircle cir = m_circles[i]; // RVO - Vector3f vab = VScale(vcand, 2); + Vector3f vab = vcand.Scale(2); vab = vab.Subtract(vel); vab = vab.Subtract(cir.vel); diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs index e69e4a5..73ed834 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs @@ -315,7 +315,7 @@ public class CrowdTool : Tool Vector3f vel = tgt.Subtract(pos); vel.y = 0.0f; vel.Normalize(); - return VScale(vel, speed); + return vel.Scale(speed); } public override void HandleRender(NavMeshRenderer renderer) diff --git a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs index 8a4e5fa..c6bc511 100644 --- a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs @@ -156,7 +156,7 @@ public class AbstractCrowdTest Vector3f vel = tgt.Subtract(pos); vel.y = 0.0f; vel.Normalize(); - vel = VScale(vel, speed); + vel = vel.Scale(speed); return vel; }