Impement analytical box and sphere ray casting (#168)

notgiven688 · Aug 19, 2024 · 791e5c0 · 791e5c0
1 parent 043a573
commit 791e5c0
Show file tree

Hide file tree

Showing 5 changed files with 194 additions and 3 deletions.
diff --git 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
@@ -47,7 +47,7 @@ public float Radius
     }
 
     /// <summary>
-    /// Initializes a new instance of the <see cref="SphereShape"/> class with an optional radius parameter. 
+    /// Initializes a new instance of the <see cref="SphereShape"/> class with an optional radius parameter.
     /// The default radius is 1.0 units.
     /// </summary>
     /// <param name="radius">The radius of the sphere. Defaults to 1.0f.</param>
@@ -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
@@ -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
@@ -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;
     }
 
+    /// <inheritdoc cref="RayCast(JVector, JVector, RayCastFilterPre?, RayCastFilterPost?, out IDynamicTreeProxy?, out JVector, out float)"/>
+    /// <param name="maxFraction">Maximum fraction of the ray's length to consider for intersections.</param>
+    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
@@ -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()
     {