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()
{