Improve raycasting and add NarrowPhase.PointTest (#57)

* Improve raycasting and add NarrowPhase.PointTest

* Initialize to Zero/default.

src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs

@@ -63,8 +63,8 @@ public struct Vertex
         public Vertex(JVector v)
         {
 #if NET6_0
-            System.Runtime.CompilerServices.Unsafe.SkipInit(out A);
-            System.Runtime.CompilerServices.Unsafe.SkipInit(out B);
+            A = new JVector();
+            B = new JVector();
 #endif
             V = v;
         }

src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs

@@ -40,6 +40,47 @@ private unsafe struct Solver
     {
         public MinkowskiDifference MKD;
         public ConvexPolytope ConvexPolytope;
+
+        public bool PointTest(in JVector origin)
+        {
+            const float CollideEpsilon = 1e-4f;
+            const int MaxIter = 34;
+
+            JVector x = origin;
+
+            var center = MKD.SupportA.GeometricCenter;
+            JVector v = x - center;
+
+            ConvexPolytope.InitHeap();
+            ConvexPolytope.InitTetrahedron(v);
+
+            int maxIter = MaxIter;
+
+            float distSq = v.LengthSquared();
+
+            while (distSq > CollideEpsilon * CollideEpsilon && maxIter-- != 0)
+            {
+                MKD.SupportA.SupportMap(v, out JVector p);
+                JVector.Subtract(x, p, out JVector w);
+
+                float vw = JVector.Dot(v, w);
+
+                if (vw >= 0.0f)
+                {
+                    return false;
+                }
+
+                if (!ConvexPolytope.AddPoint(w)) return false;
+
+                v = ConvexPolytope.GetClosestTriangle().ClosestToOrigin;
+
+                if (ConvexPolytope.OriginEnclosed) return true;
+
+                distSq = v.LengthSquared();
+            }
+
+            return true;
+        }
 
         public bool Raycast(in JVector origin, in JVector direction, out float fraction, out JVector normal)
         {
@@ -53,10 +94,9 @@ public bool Raycast(in JVector origin, in JVector direction, out float fraction,
 
             JVector r = direction;
             JVector x = origin;
-
-            MKD.SupportA.SupportMap(r, out JVector arbitraryPoint);
-
-            JVector.Subtract(x, arbitraryPoint, out JVector v);
+
+            var center = MKD.SupportA.GeometricCenter;
+            JVector v = x - center;
 
             ConvexPolytope.InitHeap();
             ConvexPolytope.InitTetrahedron(v);
@@ -99,8 +139,12 @@ public bool Raycast(in JVector origin, in JVector direction, out float fraction,
 
             fraction = lambda;
 
-            if (normal.LengthSquared() > NumericEpsilon)
-                normal.Normalize();
+            float nlen2 = normal.LengthSquared();
+
+            if (nlen2 > NumericEpsilon)
+            {
+                normal *= 1.0f / MathF.Sqrt(nlen2);
+            }
 
             return true;
         }
@@ -476,6 +520,39 @@ public bool SolveGJKEPA(out JVector point1, out JVector point2, out JVector norm
 
     // ------------------------------------------------------------------------------------------------------------
     [ThreadStatic] private static Solver solver;
+
+    /// <summary>
+    /// Check if a point is inside a shape.
+    /// </summary>
+    /// <param name="support">Support map representing the shape.</param>
+    /// <param name="point">Point to check.</param>
+    /// <returns>Returns true if the point is contained within the shape, false otherwise.</returns>
+    public static bool PointTest(ISupportMap support, in JVector point)
+    {
+        solver.MKD.SupportA = support;
+        solver.MKD.SupportB = null!;
+
+        return solver.PointTest(point);
+    }
+
+    /// <summary>
+    /// Check if a point is inside a shape.
+    /// </summary>
+    /// <param name="support">Support map representing the shape.</param>
+    /// <param name="orientation">Orientation of the shape.</param>
+    /// <param name="position">Position of the shape.</param>
+    /// <param name="point">Point to check.</param>
+    /// <returns>Returns true if the point is contained within the shape, false otherwise.</returns>
+    public static bool PointTest(ISupportMap support, in JMatrix orientation,
+        in JVector position, in JVector point)
+    {
+        JVector transformedOrigin = JVector.TransposedTransform(point - position, orientation);
+
+        solver.MKD.SupportA = support;
+        solver.MKD.SupportB = null!;
+
+        return solver.PointTest(transformedOrigin);
+    }
 
     /// <summary>
     /// Performs a raycast against a shape.

src/Jitter2/Collision/TriangleMesh.cs

@@ -83,7 +83,7 @@ public struct Triangle
         public Triangle(int a, int b, int c)
         {
 #if NET6_0
-            System.Runtime.CompilerServices.Unsafe.SkipInit(out Normal);
+            Normal = new JVector();
 #endif
 
             IndexA = a;

src/JitterDemo/Demos/Demo18.cs

@@ -0,0 +1,54 @@
+using Jitter2;
+using Jitter2.Collision;
+using Jitter2.Collision.Shapes;
+using Jitter2.LinearMath;
+using JitterDemo.Renderer;
+
+namespace JitterDemo;
+
+public class Demo18 : IDemo
+{
+    public void Draw()
+    {
+        float time = (float)pg.Time;
+
+        JMatrix ori = JMatrix.CreateRotationX(1.1f * time) *
+                      JMatrix.CreateRotationY(2.1f * time) *
+                      JMatrix.CreateRotationZ(1.7f * time);
+
+        for (int i = -15; i < 16; i++)
+        {
+            for (int e = -15; e < 16; e++)
+            {
+                for (int k = -15; k < 16; k++)
+                {
+                    JVector point = new JVector(i, e, k) * 0.1f;
+
+                    bool result = NarrowPhase.PointTest(testShape, ori, JVector.Zero, point);
+
+                    if (result)
+                    {
+                        pg.DebugRenderer.PushPoint(DebugRenderer.Color.White,
+                            Conversion.FromJitter(point), 0.01f);
+                    }
+                }
+            }
+        }
+    }
+
+    public string Name => "PointTest";
+
+    private Playground pg = null!;
+    private World world = null!;
+
+    private Shape testShape;
+
+    public void Build()
+    {
+        pg = (Playground)RenderWindow.Instance;
+        world = pg.World;
+
+        testShape = new ConeShape(1f);
+        pg.ResetScene(false);
+    }
+}

src/JitterDemo/Demos/Demo19.cs

@@ -0,0 +1,78 @@
+using Jitter2;
+using Jitter2.Collision.Shapes;
+using Jitter2.Dynamics;
+using Jitter2.LinearMath;
+using JitterDemo.Renderer;
+
+namespace JitterDemo;
+
+public class Demo19 : IDemo
+{
+    public void Draw()
+    {
+        var dr = pg.DebugRenderer;
+
+        var origin = new JVector(0, 20, 0);
+
+        JVector rayVector = new JVector(0, -1, 0);
+
+        for (int i = 0; i < 10000; i++)
+        {
+            JVector dir = JVector.Transform(rayVector, JMatrix.CreateRotationX(0.1f + 0.0001f * i));
+            dir = 60.0f * JVector.Transform(dir, JMatrix.CreateRotationY(0.004f * i));
+
+            dr.PushLine(DebugRenderer.Color.Green, Conversion.FromJitter(origin),
+                Conversion.FromJitter(origin + dir));
+
+            bool hit = world.Raycast(origin, dir, null, null, out Shape? shape,
+                out JVector normal, out float frac);
+
+            if (hit)
+            {
+                dr.PushPoint(DebugRenderer.Color.White, Conversion.FromJitter(origin + frac * dir), 0.2f);
+            }
+        }
+
+        JVector offset = new JVector(0.1f, 0.2f, 0.3f);
+
+        Raycast(new JVector(0, 0.5f, 0) + offset, box.Position - offset);
+    }
+
+    private void Raycast(JVector from, JVector to)
+    {
+        var dr = pg.DebugRenderer;
+        bool hit = world.Raycast(from, to - from, null, null, out Shape? shape,
+            out JVector normal, out float frac);
+
+        dr.PushLine(DebugRenderer.Color.Green, Conversion.FromJitter(from),
+            Conversion.FromJitter(to));
+
+        if (hit)
+        {
+            dr.PushPoint(DebugRenderer.Color.White, Conversion.FromJitter(from + frac * (to - from)), 0.2f);
+        }
+    }
+
+    public string Name => "RaycastTest";
+
+    private Playground pg = null!;
+    private World world = null!;
+
+    private Shape testShape;
+
+    private RigidBody box = null!;
+
+    public void Build()
+    {
+        pg = (Playground)RenderWindow.Instance;
+        world = pg.World;
+
+        testShape = new ConeShape(1f);
+        pg.ResetScene();
+
+        box = world.CreateRigidBody();
+        box.AddShape(new BoxShape(1));
+        box.Position = new JVector(0, 0.5f, -6);
+        box.IsStatic = true;
+    }
+}

src/JitterDemo/Playground.cs

@@ -46,7 +46,9 @@ public partial class Playground : RenderWindow
         new Demo14(),
         new Demo15(),
         new Demo16(),
-        new Demo17()
+        new Demo17(),
+        // new Demo18(),
+        // new Demo19()
     };
 
     private IDemo? currentDemo;