Add a Voxel Demo

src/Jitter2/Collision/DynamicTree.cs

@@ -324,22 +324,24 @@ public void Query(List<T> hits, in JBBox aabb)
         stack.Clear();
     }
 
+    private Random? optimizeRandom = null;
 
     /// <summary>
     /// Randomly removes and adds entities to the tree to facilitate optimization.
     /// </summary>
     /// <param name="sweeps">The number of optimization iterations to perform. The default value is 100.</param>
     public void Optimize(int sweeps = 100)
     {
-        Random random = new(0);
+        optimizeRandom ??= new Random(0);
+
         Stack<T> temp = new();
         for (int e = 0; e < sweeps; e++)
         {
             for (int i = 0; i < activeList.Count; i++)
             {
                 T proxy = activeList[i];
 
-                if (random.NextDouble() > 0.05d) continue;
+                if (optimizeRandom.NextDouble() > 0.05d) continue;
 
                 temp.Push(proxy);
                 InternalRemoveProxy(proxy);

src/JitterDemo/Demos/Demo21.cs

@@ -0,0 +1,97 @@
+using System;
+using Jitter2;
+using Jitter2.LinearMath;
+using JitterDemo.Renderer;
+using JitterDemo.Renderer.OpenGL;
+
+namespace JitterDemo;
+
+public class Demo21 : IDemo
+{
+    public string Name => "Voxel Demo";
+    private VoxelGrid voxelGrid = null!;
+    private Playground pg = null!;
+    private World world = null!;
+    private Player player = null!;
+
+    public void Build()
+    {
+        pg = (Playground)RenderWindow.Instance;
+        world = pg.World;
+
+        pg.ResetScene(false);
+
+        voxelGrid = new VoxelGrid(world);
+
+        // create a plane
+        for (int i = 0; i < 100; i++)
+        {
+            for (int e = 0; e < 100; e++)
+            {
+                voxelGrid.AddVoxel(e, 0, i);
+            }
+        }
+
+        // create a sphere
+        for (int i = 0; i < 40; i++)
+        {
+            for (int e = 30; e < 70; e++)
+            {
+                for (int k = 30; k < 70; k++)
+                {
+                    if ((i - 20) * (i - 20) + (e - 50) * (e - 50) + (k - 50) * (k - 50) > 300) continue;
+                    voxelGrid.AddVoxel(e, i, k);
+                }
+            }
+        }
+
+        var body = world.CreateRigidBody();
+        body.IsStatic = true;
+
+        foreach(var voxel in voxelGrid.Voxels)
+        {
+            body.AddShape(new VoxelShape(voxelGrid, voxel), false);
+        }
+
+        body.SetMassInertia(JMatrix.Identity, 1.0f);
+        body.SetActivationState(false);
+
+        world.NarrowPhaseFilter = new VoxelEdgeCollisionFilter();
+
+        Console.WriteLine("Optimizing tree..");
+
+        for (int i = 0; i < 3; i++)
+        {
+            Console.WriteLine($"({i+1}/3) Current cost: {(long)world.DynamicTree.CalculateCost()}");
+            world.DynamicTree.Optimize(40);
+        }
+
+        Console.WriteLine("Done.");
+
+        player = new Player(world, new JVector(50, 40, 50));
+    }
+
+    public void Draw()
+    {
+        var cd = RenderWindow.Instance.CSMRenderer.GetInstance<Cube>();
+
+        foreach(var voxel in voxelGrid.Voxels)
+        {
+            var pos = voxelGrid.PositionFromIndex(voxel);
+            cd.PushMatrix(MatrixHelper.CreateTranslation(pos.X, pos.Y, pos.Z), 
+                          ColorGenerator.GetColor(Math.Abs(voxel * voxel + voxel)));
+        }
+
+        Keyboard kb = Keyboard.Instance;
+
+        if (kb.IsKeyDown(Keyboard.Key.Left)) player.SetAngularInput(-1.0f);
+        else if (kb.IsKeyDown(Keyboard.Key.Right)) player.SetAngularInput(1.0f);
+        else player.SetAngularInput(0.0f);
+
+        if (kb.IsKeyDown(Keyboard.Key.Up)) player.SetLinearInput(-JVector.UnitZ);
+        else if (kb.IsKeyDown(Keyboard.Key.Down)) player.SetLinearInput(JVector.UnitZ);
+        else player.SetLinearInput(JVector.Zero);
+
+        if (kb.IsKeyDown(Keyboard.Key.LeftControl)) player.Jump();
+    }
+}

src/JitterDemo/Demos/Voxels/VoxelEdgeCollisionFilter.cs

@@ -0,0 +1,69 @@
+using Jitter2.Collision;
+using Jitter2.Collision.Shapes;
+using Jitter2.LinearMath;
+
+namespace JitterDemo;
+
+public class VoxelEdgeCollisionFilter : INarrowPhaseFilter
+{
+    public float Threshold { get; set; } = 0.1f;
+
+    public bool Filter(Shape shapeA, Shape shapeB, ref JVector pAA, ref JVector pBB, ref JVector normal,
+        ref float penetration)
+    {
+        VoxelShape? vs1 = shapeA as VoxelShape;
+        VoxelShape? vs2 = shapeB as VoxelShape;
+
+        bool c1 = vs1 != null;
+        bool c2 = vs2 != null;
+
+        // both shapes are voxels or both of them are not -> return
+        if (c1 == c2) return true;
+
+        VoxelShape vshape = c1 ? vs1! : vs2!;
+
+        if (shapeA.RigidBody == null || shapeB.RigidBody == null)
+        {
+            return true;
+        }
+
+        float trsh = Threshold;
+        uint nb = vshape.Neighbours;
+
+        JVector cnormal = normal;
+        if (c2) cnormal.Negate();
+
+        // Check if collision normal points into a specific direction.
+        // If yes, check if there is a neighbouring voxel. If yes,
+        // discard the collision.
+
+        if(cnormal.X > trsh)
+        {
+            if((nb & 1) != 0) return false;
+        }
+        else if(cnormal.X < -trsh)
+        {
+            if((nb & 2) != 0) return false;
+        }
+
+        if(cnormal.Y > trsh)
+        {
+            if((nb & 4) != 0) return false;
+        }
+        else if(cnormal.Y < -trsh)
+        {
+            if((nb & 8) != 0) return false;
+        }
+
+        if(cnormal.Z > trsh)
+        {
+            if((nb & 16) != 0) return false;
+        }
+        else if(cnormal.Z < -trsh)
+        {
+            if((nb & 32) != 0) return false;
+        }
+
+        return true;
+    }
+}

src/JitterDemo/Demos/Voxels/VoxelGrid.cs

@@ -0,0 +1,58 @@
+using System;
+using System.Collections.Generic;
+using Jitter2;
+using Jitter2.Dynamics;
+using Jitter2.LinearMath;
+
+namespace JitterDemo;
+
+public class VoxelGrid
+{
+    public const int Size = 100;
+    private readonly World world;
+    public HashSet<int> Voxels = new HashSet<int>();
+
+    public VoxelGrid(World world)
+    {
+        this.world = world;
+    }
+
+    public uint GetNeighbours(int index)
+    {
+        uint result = 0;
+
+        if(Voxels.Contains(index + 1)) result |= 1;
+        if(Voxels.Contains(index - 1)) result |= 2;
+        if(Voxels.Contains(index + Size)) result |= 4;
+        if(Voxels.Contains(index - Size)) result |= 8;
+        if(Voxels.Contains(index + Size * Size)) result |= 16;
+        if(Voxels.Contains(index - Size * Size)) result |= 32; 
+
+        return result;
+    }
+
+    public JVector PositionFromIndex(int index)
+    {
+        if(index < 0 || index >= Size * Size * Size) 
+        {
+            throw new ArgumentOutOfRangeException();
+        }
+
+        int z = index / (Size * Size);
+        int y = (index - z * (Size * Size)) / Size;
+        int x = index - z * (Size * Size) - y * Size;
+        return new JVector(x, y, z);
+    }
+
+    public RigidBody? Body { get; private set; }
+
+    public bool AddVoxel(int x, int y, int z)
+    {
+        if (x < 0 || x >= Size || y < 0 || y >= Size || z < 0 || z >= Size) 
+        {
+            throw new ArgumentOutOfRangeException();
+        }
+
+        return Voxels.Add(x + y * Size + z * Size * Size);
+    }
+}

src/JitterDemo/Demos/Voxels/VoxelShape.cs

@@ -0,0 +1,53 @@
+using System;
+using Jitter2.Collision.Shapes;
+using Jitter2.LinearMath;
+
+namespace JitterDemo;
+
+public class VoxelShape : Shape
+{
+    public JVector Position { private set; get; }
+    public int VoxelIndex { private set; get; }
+    public VoxelGrid VoxelGrid { private set; get; }
+
+    public uint Neighbours { private set; get; } = 0;
+
+    public VoxelShape(VoxelGrid grid, int index)
+    {
+        this.Position = grid.PositionFromIndex(index);
+        this.Neighbours = grid.GetNeighbours(index);
+        this.VoxelIndex = index;
+        this.VoxelGrid = grid;
+        UpdateShape();
+    }
+
+    public override void SupportMap(in JVector direction, out JVector result)
+    {
+        // this is the support function of a box with size 1.
+        result.X = Math.Sign(direction.X) * 0.5f;
+        result.Y = Math.Sign(direction.Y) * 0.5f;
+        result.Z = Math.Sign(direction.Z) * 0.5f;
+
+        result += Position;
+    }
+
+    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    {
+        // NOTE: We do not support any transformation of the body here.
+        System.Diagnostics.Debug.Assert(MathHelper.CloseToZero(orientation.GetColumn(0) - JVector.UnitX));
+        System.Diagnostics.Debug.Assert(MathHelper.CloseToZero(orientation.GetColumn(1) - JVector.UnitY));
+        System.Diagnostics.Debug.Assert(MathHelper.CloseToZero(orientation.GetColumn(2) - JVector.UnitZ));
+        System.Diagnostics.Debug.Assert(MathHelper.CloseToZero(position));
+
+        box.Min = Position - JVector.One * 0.5f;
+        box.Max = Position + JVector.One * 0.5f;
+    }
+
+    public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass)
+    {
+        // Do not try to calculate mass properties here.
+        mass = 1;
+        inertia = JMatrix.Identity;
+        com = Position;
+    }
+}

src/JitterDemo/Playground.cs

@@ -49,7 +49,8 @@ public partial class Playground : RenderWindow
         new Demo17(),
         // new Demo18(),  // point test
         // new Demo19(),  // ray cast test
-        new Demo20()
+        new Demo20(),
+        new Demo21()
     };
 
     private IDemo? currentDemo;