diff --git a/src/Jitter2/Collision/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree.cs index d4e21003..9c4ae50b 100644 --- a/src/Jitter2/Collision/DynamicTree.cs +++ b/src/Jitter2/Collision/DynamicTree.cs @@ -324,6 +324,7 @@ public void Query(List hits, in JBBox aabb) stack.Clear(); } + private Random? optimizeRandom = null; /// /// Randomly removes and adds entities to the tree to facilitate optimization. @@ -331,7 +332,8 @@ public void Query(List hits, in JBBox aabb) /// The number of optimization iterations to perform. The default value is 100. public void Optimize(int sweeps = 100) { - Random random = new(0); + optimizeRandom ??= new Random(0); + Stack temp = new(); for (int e = 0; e < sweeps; e++) { @@ -339,7 +341,7 @@ public void Optimize(int sweeps = 100) { T proxy = activeList[i]; - if (random.NextDouble() > 0.05d) continue; + if (optimizeRandom.NextDouble() > 0.05d) continue; temp.Push(proxy); InternalRemoveProxy(proxy); diff --git a/src/JitterDemo/Demos/Demo21.cs b/src/JitterDemo/Demos/Demo21.cs new file mode 100644 index 00000000..297729f6 --- /dev/null +++ b/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(); + + 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(); + } +} \ No newline at end of file diff --git a/src/JitterDemo/Demos/Voxels/VoxelEdgeCollisionFilter.cs b/src/JitterDemo/Demos/Voxels/VoxelEdgeCollisionFilter.cs new file mode 100644 index 00000000..88ee3856 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/JitterDemo/Demos/Voxels/VoxelGrid.cs b/src/JitterDemo/Demos/Voxels/VoxelGrid.cs new file mode 100644 index 00000000..15056611 --- /dev/null +++ b/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 Voxels = new HashSet(); + + 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); + } +} \ No newline at end of file diff --git a/src/JitterDemo/Demos/Voxels/VoxelShape.cs b/src/JitterDemo/Demos/Voxels/VoxelShape.cs new file mode 100644 index 00000000..26962f90 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/JitterDemo/Playground.cs b/src/JitterDemo/Playground.cs index 62a4423b..fd553ee0 100644 --- a/src/JitterDemo/Playground.cs +++ b/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;