diff --git a/src/Jitter2/Collision/Shapes/BoxShape.cs b/src/Jitter2/Collision/Shapes/BoxShape.cs index 4801764b..f5f0f848 100644 --- a/src/Jitter2/Collision/Shapes/BoxShape.cs +++ b/src/Jitter2/Collision/Shapes/BoxShape.cs @@ -85,6 +85,89 @@ public override void SupportMap(in JVector direction, out JVector result) result.Z = Math.Sign(direction.Z) * halfSize.Z; } + public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + { + float epsilon = 1e-22f; + + JVector min = -halfSize; + JVector max = halfSize; + + normal = JVector.Zero; + lambda = 0.0f; + + float exit = float.PositiveInfinity; + + if (MathF.Abs(direction.X) > epsilon) + { + float ix = 1.0f / direction.X; + float t0 = (min.X - origin.X) * ix; + float t1 = (max.X - origin.X) * ix; + + if (t0 > t1) (t0, t1) = (t1, t0); + + if (t0 > exit || t1 < lambda) return false; + + if (t0 > lambda) + { + lambda = t0; + normal = direction.X < 0.0f ? JVector.UnitX : -JVector.UnitX; + } + + if (t1 < exit) exit = t1; + } + else if (origin.X < min.X || origin.X > max.X) + { + return false; + } + + if (MathF.Abs(direction.Y) > epsilon) + { + float iy = 1.0f / direction.Y; + float t0 = (min.Y - origin.Y) * iy; + float t1 = (max.Y - origin.Y) * iy; + + if (t0 > t1) (t0, t1) = (t1, t0); + + if (t0 > exit || t1 < lambda) return false; + + if (t0 > lambda) + { + lambda = t0; + normal = direction.Y < 0.0f ? JVector.UnitY : -JVector.UnitY; + } + + if (t1 < exit) exit = t1; + } + else if (origin.Y < min.Y || origin.Y > max.Y) + { + return false; + } + + if (MathF.Abs(direction.Z) > epsilon) + { + float iz = 1.0f / direction.Z; + float t0 = (min.Z - origin.Z) * iz; + float t1 = (max.Z - origin.Z) * iz; + + if (t0 > t1) (t0, t1) = (t1, t0); + + if (t0 > exit || t1 < lambda) return false; + + if (t0 > lambda) + { + lambda = t0; + normal = direction.Z < 0.0f ? JVector.UnitZ : -JVector.UnitZ; + } + //if (t1 < exit) exit = t1; + } + else if (origin.Z < min.Z || origin.Z > max.Z) + { + return false; + } + + return true; + } + public override void GetCenter(out JVector point) { point = JVector.Zero; diff --git a/src/Jitter2/Collision/Shapes/SphereShape.cs b/src/Jitter2/Collision/Shapes/SphereShape.cs index b098a095..5cb17ace 100644 --- a/src/Jitter2/Collision/Shapes/SphereShape.cs +++ b/src/Jitter2/Collision/Shapes/SphereShape.cs @@ -47,7 +47,7 @@ public float Radius } /// - /// Initializes a new instance of the class with an optional radius parameter. + /// Initializes a new instance of the class with an optional radius parameter. /// The default radius is 1.0 units. /// /// The radius of the sphere. Defaults to 1.0f. @@ -82,6 +82,32 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector JVector.Add(box.Max, position, out box.Max); } + public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + { + normal = JVector.Zero; + lambda = 0.0f; + + float disq = 1.0f / direction.LengthSquared(); + float p = JVector.Dot(direction, origin) * disq; + float d = p * p - (origin.LengthSquared() - radius * radius) * disq; + + if (d < 0.0f) return false; + + float sqrtd = MathF.Sqrt(d); + + float t0 = -p - sqrtd; + float t1 = -p + sqrtd; + + if (t0 >= 0.0f) + { + lambda = t0; + JVector.Normalize(origin + t0 * direction, out normal); + return true; + } + + return MathF.Max(t0, t1) > 0.0f; + } + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) { mass = 4.0f / 3.0f * MathF.PI * radius * radius * radius; diff --git a/src/Jitter2/LinearMath/JVector.cs b/src/Jitter2/LinearMath/JVector.cs index a370415e..2eb96e6c 100644 --- a/src/Jitter2/LinearMath/JVector.cs +++ b/src/Jitter2/LinearMath/JVector.cs @@ -517,11 +517,17 @@ public static void Multiply(in JVector value1, float scaleFactor, out JVector re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JVector operator -(JVector left) + public static JVector operator -(in JVector left) { return Multiply(left, -1.0f); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static JVector operator +(in JVector left) + { + return left; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static JVector operator +(in JVector value1, in JVector value2) { diff --git a/src/Jitter2/World.RayCast.cs b/src/Jitter2/World.RayCast.cs index ac88c108..c2e79776 100644 --- a/src/Jitter2/World.RayCast.cs +++ b/src/Jitter2/World.RayCast.cs @@ -63,7 +63,7 @@ private struct Ray public RayCastFilterPost? FilterPost; public RayCastFilterPre? FilterPre; - public readonly float Lambda; + public float Lambda; public Ray(in JVector origin, in JVector direction) { @@ -103,6 +103,24 @@ public bool RayCast(JVector origin, JVector direction, RayCastFilterPre? pre, Ra return result.Hit; } + /// + /// Maximum fraction of the ray's length to consider for intersections. + public bool RayCast(JVector origin, JVector direction, float maxFraction, RayCastFilterPre? pre, RayCastFilterPost? post, + out IDynamicTreeProxy? shape, out JVector normal, out float fraction) + { + Ray ray = new(origin, direction) + { + FilterPre = pre, + FilterPost = post, + Lambda = maxFraction + }; + var result = QueryRay(ray); + shape = result.Entity; + normal = result.Normal; + fraction = result.Fraction; + return result.Hit; + } + private RayCastResult QueryRay(in Ray ray) { if (DynamicTree.Root == -1) return new RayCastResult(); diff --git a/src/JitterTests/CollisionTests.cs b/src/JitterTests/CollisionTests.cs index bf9fd158..8e1cdc17 100644 --- a/src/JitterTests/CollisionTests.cs +++ b/src/JitterTests/CollisionTests.cs @@ -1,3 +1,5 @@ +using System.Diagnostics; + namespace JitterTests; public class CollisionTests @@ -16,6 +18,62 @@ public void NoBodyWorldBoundingBox() Assert.That(MathHelper.CloseToZero(shape.WorldBoundingBox.Min + shape.Size * 0.5f)); } + [TestCase] + public void SphereRayCast() + { + SphereShape ss = new SphereShape(1.2f); + + const float epsilon = 1e-12f; + + bool hit = ss.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -JVector.UnitY, out JVector normal, out float lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda - 0.25f), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); + + hit = ss.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -2.0f * JVector.UnitY, out normal, out lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda - 0.125f), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); + + hit = ss.LocalRayCast(new JVector(0, 1.2f - 0.25f, 0), -JVector.UnitY, out normal, out lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal)); + + hit = ss.LocalRayCast(new JVector(0, -1.2f - 0.25f, 0), -JVector.UnitY * 1.1f, out normal, out lambda); + Assert.That(!hit); + Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal)); + } + + [TestCase] + public void BoxRayCast() + { + BoxShape bs = new BoxShape(1.2f * 2.0f); + + const float epsilon = 1e-12f; + + bool hit = bs.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -JVector.UnitY, out JVector normal, out float lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda - 0.25f), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); + + hit = bs.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -2.0f * JVector.UnitY, out normal, out lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda - 0.125f), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); + + hit = bs.LocalRayCast(new JVector(0, 1.2f - 0.25f, 0), -JVector.UnitY, out normal, out lambda); + Assert.That(hit); + Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal)); + + hit = bs.LocalRayCast(new JVector(0, -1.2f - 0.25f, 0), -JVector.UnitY * 1.1f, out normal, out lambda); + Assert.That(!hit); + Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathHelper.CloseToZero(normal)); + } + [TestCase] public void RayCast() {