Rewrite physics engine

This time it sucks slightly less

TrueCraft.API/Entities/IMobEntity.cs

@@ -1,4 +1,5 @@
 using System;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.API.Entities
 {

TrueCraft.API/Physics/IAABBEntity.cs

@@ -3,7 +3,7 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 
-namespace TrueCraft.API.Entities
+namespace TrueCraft.API.Physics
 {
     public interface IAABBEntity : IPhysicsEntity
     {

TrueCraft.API/Physics/IPhysicsEngine.cs

@@ -0,0 +1,14 @@
+using System;
+using TrueCraft.API.Entities;
+using TrueCraft.API.World;
+
+namespace TrueCraft.API.Physics
+{
+    public interface IPhysicsEngine
+    {
+        IWorld World { get; set; }
+        void AddEntity(IPhysicsEntity entity);
+        void RemoveEntity(IPhysicsEntity entity);
+        void Update(TimeSpan time);
+    }
+}

TrueCraft.API/Physics/IPhysicsEntity.cs

@@ -3,14 +3,23 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 
-namespace TrueCraft.API.Entities
+namespace TrueCraft.API.Physics
 {
     public interface IPhysicsEntity
     {
         Vector3 Position { get; set; }
         Vector3 Velocity { get; set; }
+        /// <summary>
+        /// Acceleration due to gravity in meters per second squared.
+        /// </summary>
         float AccelerationDueToGravity { get; }
+        /// <summary>
+        /// Velocity *= (1 - Drag) each second
+        /// </summary>
         float Drag { get; }
+        /// <summary>
+        /// Terminal velocity in meters per second.
+        /// </summary>
         float TerminalVelocity { get; }
 
         bool BeginUpdate();

TrueCraft.API/TrueCraft.API.csproj

@@ -99,9 +99,7 @@
     <Compile Include="Server\ChatMessageEventArgs.cs" />
     <Compile Include="BlockFace.cs" />
     <Compile Include="Logic\IBlockRepository.cs" />
-    <Compile Include="Entities\IAABBEntity.cs" />
     <Compile Include="Entities\IBlockPhysicsProvider.cs" />
-    <Compile Include="Entities\IPhysicsEntity.cs" />
     <Compile Include="Logic\IItemRepository.cs" />
     <Compile Include="Logic\BlockDescriptor.cs" />
     <Compile Include="ArmorMaterial.cs" />
@@ -113,6 +111,9 @@
     <Compile Include="AI\IMobState.cs" />
     <Compile Include="Entities\IMobEntity.cs" />
     <Compile Include="AI\ISpawnRule.cs" />
+    <Compile Include="Physics\IPhysicsEngine.cs" />
+    <Compile Include="Physics\IPhysicsEntity.cs" />
+    <Compile Include="Physics\IAABBEntity.cs" />
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup />
@@ -127,5 +128,6 @@
   </ItemGroup>
   <ItemGroup>
     <Folder Include="AI\" />
+    <Folder Include="Physics\" />
   </ItemGroup>
 </Project>

TrueCraft.Client/MultiplayerClient.cs

@@ -14,6 +14,7 @@ using TrueCraft.API;
 using System.ComponentModel;
 using System.IO;
 using TrueCraft.Core;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Client
 {

TrueCraft.Client/PhysicsEngine.cs

@@ -6,6 +6,7 @@ using TrueCraft.Core.World;
 using TrueCraft.API.Entities;
 using TrueCraft.API.World;
 using TrueCraft.API;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Client
 {

TrueCraft.Core.Test/Physics/PhysicsEngineTest.cs

@@ -0,0 +1,212 @@
+using System;
+using NUnit.Framework;
+using TrueCraft.Core.TerrainGen;
+using TrueCraft.Core.Physics;
+using Moq;
+using TrueCraft.API.Entities;
+using TrueCraft.API.Physics;
+using TrueCraft.API;
+using TrueCraft.Core.Logic;
+using TrueCraft.Core.Logic.Blocks;
+using TrueCraft.API.Logic;
+
+namespace TrueCraft.Core.Test.Physics
+{
+    [TestFixture]
+    public class PhysicsEngineTest
+    {
+        private class TestEntity : IAABBEntity
+        {
+            public TestEntity()
+            {
+                TerminalVelocity = 10;
+                Size = new Size(1);
+                CollisionOccured = false;
+            }
+
+            public bool BeginUpdate()
+            {
+                return true;
+            }
+
+            public void EndUpdate(Vector3 newPosition)
+            {
+                Position = newPosition;
+            }
+
+            public Vector3 Position { get; set; }
+            public Vector3 Velocity { get; set; }
+            public float AccelerationDueToGravity { get; set; }
+            public float Drag { get; set; }
+            public float TerminalVelocity { get; set; }
+            public Vector3 CollisionPoint { get; set; }
+            public bool CollisionOccured { get; set; }
+
+            public void TerrainCollision(Vector3 collisionPoint, Vector3 collisionDirection)
+            {
+                CollisionPoint = collisionPoint;
+                CollisionOccured = true;
+            }
+
+            public BoundingBox BoundingBox
+            {
+                get
+                {
+                    return new BoundingBox(Position, Position + Size);
+                }
+            }
+
+            public Size Size { get; set; }
+        }
+
+        private IBlockPhysicsProvider GetBlockRepository()
+        {
+            var repository = new BlockRepository();
+            repository.RegisterBlockProvider(new AirBlock());
+            repository.RegisterBlockProvider(new StoneBlock());
+            repository.RegisterBlockProvider(new GrassBlock());
+            repository.RegisterBlockProvider(new DirtBlock());
+            repository.RegisterBlockProvider(new BedrockBlock());
+            return repository;
+        }
+
+        [Test]
+        public void TestGravity()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 100, 0);
+            entity.AccelerationDueToGravity = 1;
+            entity.Drag = 0;
+            physics.AddEntity(entity);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(99, entity.Position.Y);
+
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(97, entity.Position.Y);
+        }
+
+        [Test]
+        public void TestDrag()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 100, 0);
+            entity.AccelerationDueToGravity = 0;
+            entity.Drag = 0.5f;
+            entity.Velocity = Vector3.Down * 2;
+            physics.AddEntity(entity);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(99, entity.Position.Y);
+        }
+
+        [Test]
+        public void TestTerrainCollision()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 4, 0);
+            entity.AccelerationDueToGravity = 1;
+            physics.AddEntity(entity);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(4, entity.Position.Y);
+        }
+
+        [Test]
+        public void TestExtremeTerrainCollision()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 4, 0);
+            entity.AccelerationDueToGravity = 10;
+            physics.AddEntity(entity);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(4, entity.Position.Y);
+        }
+
+        [Test]
+        public void TestAdjacentFall()
+        {
+            // Tests an entity that falls alongside a wall
+
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 10, 0);
+            entity.AccelerationDueToGravity = 1;
+            physics.AddEntity(entity);
+
+            // Create a wall
+            for (int y = 0; y < 12; y++)
+                world.SetBlockID(new Coordinates3D(1, y, 0), StoneBlock.BlockID);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(9, entity.Position.Y);
+            Assert.IsFalse(entity.CollisionOccured);
+        }
+
+        [Test]
+        public void TestCollisionPoint()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 5, 0);
+            entity.AccelerationDueToGravity = 1;
+            entity.Drag = 0;
+            physics.AddEntity(entity);
+
+            world.SetBlockID(new Coordinates3D(0, 4, 0), StoneBlock.BlockID);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(new Vector3(0, 4, 0), entity.CollisionPoint);
+        }
+
+        [Test]
+        public void TestHorizontalCollision()
+        {
+            var repository = GetBlockRepository();
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var physics = new PhysicsEngine(world, repository);
+            var entity = new TestEntity();
+            entity.Position = new Vector3(0, 5, 0);
+            entity.AccelerationDueToGravity = 0;
+            entity.Drag = 0;
+            entity.Velocity = new Vector3(1, 0, 0);
+            physics.AddEntity(entity);
+            world.SetBlockID(new Coordinates3D(1, 5, 0), StoneBlock.BlockID);
+
+            // Test
+            physics.Update(TimeSpan.FromSeconds(1));
+
+            Assert.AreEqual(0, entity.Position.X);
+        }
+    }
+}

TrueCraft.Core.Test/TrueCraft.Core.Test.csproj

@@ -84,6 +84,7 @@
     <Compile Include="Logic\BlockProviderTest.cs" />
     <Compile Include="Lighting\WorldLighterTest.cs" />
     <Compile Include="AI\PathFindingTest.cs" />
+    <Compile Include="Physics\PhysicsEngineTest.cs" />
   </ItemGroup>
   <ItemGroup>
     <Folder Include="World\" />
@@ -92,5 +93,6 @@
     <Folder Include="Logic\" />
     <Folder Include="Lighting\" />
     <Folder Include="AI\" />
+    <Folder Include="Physics\" />
   </ItemGroup>
 </Project>

TrueCraft.Core/Entities/FallingSandEntity.cs

@@ -6,6 +6,7 @@ using TrueCraft.API.Entities;
 using TrueCraft.API.Server;
 using TrueCraft.API.World;
 using TrueCraft.Core.Logic.Blocks;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Core.Entities
 {
@@ -22,7 +23,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return new Size(1);
+                return new Size(0.98);
             }
         }
 
@@ -48,7 +49,7 @@ namespace TrueCraft.Core.Entities
                 if (EntityType == 71)
                     id = GravelBlock.BlockID;
                 EntityManager.DespawnEntity(this);
-                World.SetBlockID((Coordinates3D)_Position, id);
+                World.SetBlockID((Coordinates3D)collisionPoint, id);
             }
         }
 
@@ -76,7 +77,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 0.04f;
+                return 16f;
             }
         }
 
@@ -84,7 +85,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 0.02f;
+                return 0.40f;
             }
         }
 
@@ -92,7 +93,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 1.96f;
+                return 39.2f;
             }
         }
     }

TrueCraft.Core/Entities/ItemEntity.cs

@@ -9,6 +9,7 @@ using TrueCraft.Core.Networking.Packets;
 using TrueCraft.Core;
 using TrueCraft.API.Server;
 using TrueCraft.Core.Logic.Blocks;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Core.Entities
 {
@@ -117,17 +118,17 @@ namespace TrueCraft.Core.Entities
 
         public float AccelerationDueToGravity
         {
-            get { return 0.04f; }
+            get { return 16f; }
         }
 
         public float Drag
         {
-            get { return 0.02f; }
+            get { return 0.40f; }
         }
 
         public float TerminalVelocity
         {
-            get { return 1.96f; }
+            get { return 39.2f; }
         }
     }
 }

TrueCraft.Core/Entities/MobEntity.cs

@@ -7,6 +7,7 @@ using TrueCraft.API.Server;
 using System.Linq;
 using TrueCraft.API.AI;
 using TrueCraft.Core.AI;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Core.Entities
 {
@@ -65,7 +66,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 0.08f;
+                return 32f;
             }
         }
 
@@ -73,7 +74,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 0.02f;
+                return 0.40f;
             }
         }
 
@@ -81,7 +82,7 @@ namespace TrueCraft.Core.Entities
         {
             get
             {
-                return 3.92f;
+                return 78.4f;
             }
         }
 

TrueCraft.Core/Logic/BlockProvider.cs

@@ -10,6 +10,7 @@ using TrueCraft.Core.Logic.Blocks;
 using System.Linq;
 using fNbt;
 using TrueCraft.Core.Logic.Items;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft.Core.Logic
 {

TrueCraft.Core/Physics/PhysicsEngine.cs

@@ -0,0 +1,342 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using TrueCraft.Core.World;
+using TrueCraft.API.Entities;
+using TrueCraft.API.World;
+using TrueCraft.API;
+using TrueCraft.API.Physics;
+
+namespace TrueCraft.Core.Physics
+{
+    public class PhysicsEngine : IPhysicsEngine
+    {
+        public PhysicsEngine(IWorld world, IBlockPhysicsProvider physicsProvider)
+        {
+            World = world;
+            Entities = new List<IPhysicsEntity>();
+            EntityLock = new object();
+            BlockPhysicsProvider = physicsProvider;
+        }
+
+        public IWorld World { get; set; }
+        public IBlockPhysicsProvider BlockPhysicsProvider { get; set; }
+        public List<IPhysicsEntity> Entities { get; set; }
+        private object EntityLock { get; set; }
+
+        public void AddEntity(IPhysicsEntity entity)
+        {
+            if (Entities.Contains(entity))
+                return;
+            lock (EntityLock)
+                Entities.Add(entity);
+        }
+
+        public void RemoveEntity(IPhysicsEntity entity)
+        {
+            if (!Entities.Contains(entity))
+                return;
+            lock (EntityLock)
+                Entities.Remove(entity);
+        }
+
+        public void Update(TimeSpan time)
+        {
+            double multiplier = time.TotalSeconds;
+            lock (EntityLock)
+            {
+                for (int i = 0; i < Entities.Count; i++)
+                {
+                    var entity = Entities[i];
+                    if (entity.BeginUpdate())
+                    {
+                        entity.Velocity -= new Vector3(0, entity.AccelerationDueToGravity * multiplier, 0);
+                        entity.Velocity *= 1 - entity.Drag * multiplier;
+                        if (entity.Velocity.Distance < 0.001)
+                            entity.Velocity = Vector3.Zero;
+                        entity.Velocity.Clamp(entity.TerminalVelocity);
+
+                        Vector3 collision, before = entity.Velocity;
+
+                        var aabbEntity = entity as IAABBEntity;
+                        if (aabbEntity != null)
+                        {
+                            if (TestTerrainCollisionY(aabbEntity, out collision))
+                                aabbEntity.TerrainCollision(collision, before.Y < 0 ? Vector3.Down : Vector3.Up);
+                            if (TestTerrainCollisionX(aabbEntity, out collision))
+                                aabbEntity.TerrainCollision(collision, before.X < 0 ? Vector3.Left : Vector3.Right);
+                        }
+
+                        entity.EndUpdate(entity.Position + entity.Velocity);
+                    }
+                }
+            }
+        }
+
+        public bool TestTerrainCollisionY(IAABBEntity entity, out Vector3 collisionPoint)
+        {
+            // Things we need to do:
+            // 1 - expand bounding box to include the destination and everything within
+            // 2 - collect all blocks within that area
+            // 3 - test bounding boxes in direction of motion
+
+            collisionPoint = Vector3.Zero;
+
+            if (entity.Velocity.Y == 0)
+                return false;
+
+            bool negative;
+
+            BoundingBox testBox;
+            if (entity.Velocity.Y < 0)
+            {
+                testBox = new BoundingBox(
+                    new Vector3(entity.BoundingBox.Min.X,
+                        entity.BoundingBox.Min.Y + entity.Velocity.Y - entity.Size.Height,
+                        entity.BoundingBox.Min.Z),
+                    entity.BoundingBox.Max);
+                negative = true;
+            }
+            else
+            {
+                testBox = new BoundingBox(
+                    entity.BoundingBox.Min,
+                    new Vector3(entity.BoundingBox.Max.X,
+                        entity.BoundingBox.Max.Y + entity.Velocity.Y,
+                        entity.BoundingBox.Max.Z));
+                negative = false;
+            }
+
+            double? collisionExtent = null;
+            for (int x = (int)(testBox.Min.X); x <= (int)(testBox.Max.X); x++)
+            {
+                for (int z = (int)(testBox.Min.Z); z <= (int)(testBox.Max.Z); z++)
+                {
+                    for (int y = (int)(testBox.Min.Y); y <= (int)(testBox.Max.Y); y++)
+                    {
+                        var coords = new Coordinates3D(x, y, z);
+                        if (!World.IsValidPosition(coords))
+                            continue;
+
+                        var _box = BlockPhysicsProvider.GetBoundingBox(World, coords);
+                        if (_box == null)
+                            continue;
+
+                        var box = _box.Value.OffsetBy(coords);
+                        if (testBox.Intersects(box))
+                        {
+                            if (negative)
+                            {
+                                if (collisionExtent == null || collisionExtent.Value < box.Max.Y)
+                                {
+                                    collisionExtent = box.Max.Y;
+                                    collisionPoint = coords;
+                                }
+                            }
+                            else
+                            {
+                                if (collisionExtent == null || collisionExtent.Value > box.Min.Y)
+                                {
+                                    collisionExtent = box.Min.Y;
+                                    collisionPoint = coords;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            if (collisionExtent != null) // Collision detected, adjust accordingly
+            {
+                var extent = collisionExtent.Value;
+                double diff;
+                if (negative)
+                    diff = entity.BoundingBox.Min.Y - extent;
+                else
+                    diff = extent - entity.BoundingBox.Max.Y;
+                entity.Velocity = new Vector3(entity.Velocity.X, diff, entity.Velocity.Z);
+                return true;
+            }
+            return false;
+        }
+
+        public bool TestTerrainCollisionX(IAABBEntity entity, out Vector3 collisionPoint)
+        {
+            // Things we need to do:
+            // 1 - expand bounding box to include the destination and everything within
+            // 2 - collect all blocks within that area
+            // 3 - test bounding boxes in direction of motion
+
+            collisionPoint = Vector3.Zero;
+
+            if (entity.Velocity.X == 0)
+                return false;
+
+            bool negative;
+
+            BoundingBox testBox;
+            if (entity.Velocity.X < 0)
+            {
+                testBox = new BoundingBox(
+                    new Vector3(
+                        entity.BoundingBox.Min.X + entity.Velocity.X - entity.Size.Width,
+                        entity.BoundingBox.Min.Y,
+                        entity.BoundingBox.Min.Z),
+                    entity.BoundingBox.Max);
+                negative = true;
+            }
+            else
+            {
+                testBox = new BoundingBox(
+                    entity.BoundingBox.Min,
+                    new Vector3(
+                        entity.BoundingBox.Max.X + entity.Velocity.X,
+                        entity.BoundingBox.Max.Y,
+                        entity.BoundingBox.Max.Z));
+                negative = false;
+            }
+
+            double? collisionExtent = null;
+            for (int x = (int)(testBox.Min.X); x <= (int)(testBox.Max.X); x++)
+            {
+                for (int z = (int)(testBox.Min.Z); z <= (int)(testBox.Max.Z); z++)
+                {
+                    for (int y = (int)(testBox.Min.Y); y <= (int)(testBox.Max.Y); y++)
+                    {
+                        var coords = new Coordinates3D(x, y, z);
+                        if (!World.IsValidPosition(coords))
+                            continue;
+
+                        var _box = BlockPhysicsProvider.GetBoundingBox(World, coords);
+                        if (_box == null)
+                            continue;
+
+                        var box = _box.Value.OffsetBy(coords);
+                        if (testBox.Intersects(box))
+                        {
+                            if (negative)
+                            {
+                                if (collisionExtent == null || collisionExtent.Value < box.Max.X)
+                                {
+                                    collisionExtent = box.Max.X;
+                                    collisionPoint = coords;
+                                }
+                            }
+                            else
+                            {
+                                if (collisionExtent == null || collisionExtent.Value > box.Min.X)
+                                {
+                                    collisionExtent = box.Min.X;
+                                    collisionPoint = coords;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            if (collisionExtent != null) // Collision detected, adjust accordingly
+            {
+                var extent = collisionExtent.Value;
+                double diff;
+                if (negative)
+                    diff = entity.BoundingBox.Min.X - extent;
+                else
+                    diff = extent - entity.BoundingBox.Max.X;
+                entity.Velocity = new Vector3(diff, entity.Velocity.Y, entity.Velocity.Z);
+                return true;
+            }
+            return false;
+        }
+
+        public bool TestTerrainCollisionZ(IAABBEntity entity, out Vector3 collisionPoint)
+        {
+            // Things we need to do:
+            // 1 - expand bounding box to include the destination and everything within
+            // 2 - collect all blocks within that area
+            // 3 - test bounding boxes in direction of motion
+
+            collisionPoint = Vector3.Zero;
+
+            if (entity.Velocity.Z == 0)
+                return false;
+
+            bool negative;
+
+            BoundingBox testBox;
+            if (entity.Velocity.Z < 0)
+            {
+                testBox = new BoundingBox(
+                    new Vector3(
+                        entity.BoundingBox.Min.X,
+                        entity.BoundingBox.Min.Y,
+                        entity.BoundingBox.Min.Z + entity.Velocity.Z - entity.Size.Depth),
+                    entity.BoundingBox.Max);
+                negative = true;
+            }
+            else
+            {
+                testBox = new BoundingBox(
+                    entity.BoundingBox.Min,
+                    new Vector3(
+                        entity.BoundingBox.Max.X,
+                        entity.BoundingBox.Max.Y,
+                        entity.BoundingBox.Max.Z + entity.Velocity.Z));
+                negative = false;
+            }
+
+            double? collisionExtent = null;
+            for (int x = (int)(testBox.Min.X); x <= (int)(testBox.Max.X); x++)
+            {
+                for (int z = (int)(testBox.Min.Z); z <= (int)(testBox.Max.Z); z++)
+                {
+                    for (int y = (int)(testBox.Min.Y); y <= (int)(testBox.Max.Y); y++)
+                    {
+                        var coords = new Coordinates3D(x, y, z);
+                        if (!World.IsValidPosition(coords))
+                            continue;
+
+                        var _box = BlockPhysicsProvider.GetBoundingBox(World, coords);
+                        if (_box == null)
+                            continue;
+
+                        var box = _box.Value.OffsetBy(coords);
+                        if (testBox.Intersects(box))
+                        {
+                            if (negative)
+                            {
+                                if (collisionExtent == null || collisionExtent.Value < box.Max.Z)
+                                {
+                                    collisionExtent = box.Max.Z;
+                                    collisionPoint = coords;
+                                }
+                            }
+                            else
+                            {
+                                if (collisionExtent == null || collisionExtent.Value > box.Min.Z)
+                                {
+                                    collisionExtent = box.Min.Z;
+                                    collisionPoint = coords;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            if (collisionExtent != null) // Collision detected, adjust accordingly
+            {
+                var extent = collisionExtent.Value;
+                double diff;
+                if (negative)
+                    diff = entity.BoundingBox.Min.Z - extent;
+                else
+                    diff = extent - entity.BoundingBox.Max.Z;
+                entity.Velocity = new Vector3(entity.Velocity.X, entity.Velocity.Y, diff);
+                return true;
+            }
+            return false;
+        }
+    }
+}

TrueCraft.Core/TrueCraft.Core.csproj

@@ -345,6 +345,7 @@
     <Compile Include="Entities\HenEntity.cs" />
     <Compile Include="Entities\SquidEntity.cs" />
     <Compile Include="AI\WanderState.cs" />
+    <Compile Include="Physics\PhysicsEngine.cs" />
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>
@@ -370,6 +371,7 @@
   <ItemGroup>
     <Folder Include="Lighting\" />
     <Folder Include="AI\" />
+    <Folder Include="Physics\" />
   </ItemGroup>
   <ItemGroup>
     <None Include="packages.config" />

TrueCraft/EntityManager.cs

@@ -15,6 +15,8 @@ using TrueCraft.API;
 using System.Collections.Concurrent;
 using TrueCraft.API.Logging;
 using TrueCraft.Core.Logic;
+using TrueCraft.Core.Physics;
+using TrueCraft.API.Physics;
 
 namespace TrueCraft
 {
@@ -296,7 +298,7 @@ namespace TrueCraft
         {
             TimeSinceLastUpdate = DateTime.UtcNow - LastUpdate;
             LastUpdate = DateTime.UtcNow;
-            PhysicsEngine.Update();
+            PhysicsEngine.Update(TimeSinceLastUpdate);
             try
             {
                 lock (Entities)

TrueCraft/PhysicsEngine.cs

@@ -1,402 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using TrueCraft.Core.World;
-using TrueCraft.API.Entities;
-using TrueCraft.API.World;
-using TrueCraft.API;
-
-namespace TrueCraft
-{
-    public class PhysicsEngine
-    {
-        public PhysicsEngine(IWorld world, IBlockPhysicsProvider physicsProvider)
-        {
-            World = world;
-            Entities = new List<IPhysicsEntity>();
-            EntityLock = new object();
-            LastUpdate = DateTime.MinValue;
-            BlockPhysicsProvider = physicsProvider;
-            MillisecondsBetweenUpdates = 1000 / 20;
-        }
-
-        public int MillisecondsBetweenUpdates { get; set; }
-        public IWorld World { get; set; }
-        public IBlockPhysicsProvider BlockPhysicsProvider { get; set; }
-        public List<IPhysicsEntity> Entities { get; set; }
-        private object EntityLock { get; set; }
-        private DateTime LastUpdate { get; set; }
-
-        public void AddEntity(IPhysicsEntity entity)
-        {
-            if (Entities.Contains(entity))
-                return;
-            lock (EntityLock)
-                Entities.Add(entity);
-        }
-
-        public void RemoveEntity(IPhysicsEntity entity)
-        {
-            if (!Entities.Contains(entity))
-                return;
-            lock (EntityLock)
-                Entities.Remove(entity);
-        }
-
-        private BoundingBox TempBoundingBox;
-        public void Update()
-        {
-            double multipler = (DateTime.UtcNow - LastUpdate).TotalMilliseconds / MillisecondsBetweenUpdates;
-            if (LastUpdate == DateTime.MinValue)
-                multipler = 1;
-            if (multipler > 5) multipler = 5;
-            if (multipler < 0.1) multipler = 0.1;
-            lock (EntityLock)
-            {
-                for (int i = 0; i < Entities.Count; i++)
-                {
-                    var entity = Entities[i];
-                    if (entity.BeginUpdate())
-                    {
-                        entity.Velocity -= new Vector3(0, entity.AccelerationDueToGravity * multipler, 0);
-                        entity.Velocity.Clamp(entity.TerminalVelocity);
-                        if (entity is IAABBEntity)
-                            CheckWithTerrain((IAABBEntity)entity, World);
-                        entity.EndUpdate(entity.Position + entity.Velocity);
-                    }
-                }
-            }
-            LastUpdate = DateTime.UtcNow;
-        }
-
-        private void CheckWithTerrain(IAABBEntity entity, IWorld world)
-        {
-            Vector3 collisionPoint, collisionDirection;
-            if (entity.Position.Y > 0 && entity.Position.Y <= 127) // Don't do checks outside the map
-            {
-                bool fireEvent = entity.Velocity != Vector3.Zero;
-                // Do terrain collisions
-                if (AdjustVelocityX(entity, world, out collisionPoint, out collisionDirection))
-                {
-                    if (fireEvent)
-                        entity.TerrainCollision(collisionPoint, collisionDirection);
-                }
-                if (AdjustVelocityY(entity, world, out collisionPoint, out collisionDirection))
-                {
-                    entity.Velocity *= new Vector3(0.1, 1, 0.1); // TODO: More sophisticated friction
-                    if (fireEvent)
-                        entity.TerrainCollision(collisionPoint, collisionDirection);
-                }
-                if (AdjustVelocityZ(entity, world, out collisionPoint, out collisionDirection))
-                {
-                    if (fireEvent)
-                        entity.TerrainCollision(collisionPoint, collisionDirection);
-                }
-            }
-        }
-
-        // TODO: There's a lot of code replication here, perhaps it can be consolidated
-        /// <summary>
-        /// Performs terrain collision tests and adjusts the X-axis velocity accordingly
-        /// </summary>
-        /// <returns>True if the entity collides with the terrain</returns>
-        private bool AdjustVelocityX(IAABBEntity entity, IWorld world, out Vector3 collision, out Vector3 collisionDirection)
-        {
-            collision = Vector3.Zero;
-            collisionDirection = Vector3.Zero;
-            if (entity.Velocity.X == 0)
-                return false;
-            // Do some enviornment guessing to improve speed
-            int minY = (int)entity.Position.Y - (entity.Position.Y < 0 ? 1 : 0);
-            int maxY = (int)(entity.Position.Y + entity.Size.Width) - (entity.Position.Y < 0 ? 1 : 0);
-            int minZ = (int)entity.Position.Z - (entity.Position.Z < 0 ? 1 : 0);
-            int maxZ = (int)(entity.Position.Z + entity.Size.Depth) - (entity.Position.Z < 0 ? 1 : 0);
-            int minX, maxX;
-
-            // Expand bounding box to include area to be tested
-            if (entity.Velocity.X < 0)
-            {
-                TempBoundingBox = new BoundingBox(
-                    new Vector3(entity.BoundingBox.Min.X + entity.Velocity.X, entity.BoundingBox.Min.Y, entity.BoundingBox.Min.Z),
-                    entity.BoundingBox.Max);
-
-                maxX = (int)(TempBoundingBox.Max.X);
-                minX = (int)(TempBoundingBox.Min.X + entity.Velocity.X) - 1;
-            }
-            else
-            {
-                TempBoundingBox = new BoundingBox(
-                    entity.BoundingBox.Min,
-                    new Vector3(entity.BoundingBox.Max.X + entity.Velocity.X, entity.BoundingBox.Max.Y, entity.BoundingBox.Max.Z));
-                minX = (int)(entity.BoundingBox.Min.X);
-                maxX = (int)(entity.BoundingBox.Max.X + entity.Velocity.X) + 1;
-            }
-
-            // Do terrain checks
-            double? collisionPoint = null;
-            BoundingBox blockBox;
-            for (int x = minX; x <= maxX; x++)
-            {
-                for (int y = minY; y <= maxY; y++)
-                {
-                    for (int z = minZ; z <= maxZ; z++)
-                    {
-                        var position = new Coordinates3D(x, y, z);
-                        var boundingBox = BlockPhysicsProvider.GetBoundingBox(world, position);
-                        if (boundingBox == null)
-                            continue;
-                        blockBox = boundingBox.Value.OffsetBy(position + new Vector3(0.5));
-                        if (TempBoundingBox.Intersects(blockBox))
-                        {
-                            if (entity.Velocity.X < 0)
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Max.X;
-                                else if (collisionPoint.Value < blockBox.Max.X)
-                                    collisionPoint = blockBox.Max.X;
-                            }
-                            else
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Min.X;
-                                else if (collisionPoint.Value > blockBox.Min.X)
-                                    collisionPoint = blockBox.Min.X;
-                            }
-                            collision = position;
-                        }
-                    }
-                }
-            }
-
-            if (collisionPoint != null)
-            {
-                if (entity.Velocity.X < 0)
-                {
-                    entity.Velocity = new Vector3(
-                        entity.Velocity.X - (TempBoundingBox.Min.X - collisionPoint.Value),
-                        entity.Velocity.Y,
-                        entity.Velocity.Z);
-                    collisionDirection = Vector3.Left;
-                }
-                else if (entity.Velocity.X > 0)
-                {
-                    entity.Velocity = new Vector3(
-                        entity.Velocity.X - (TempBoundingBox.Max.X - collisionPoint.Value),
-                        entity.Velocity.Y,
-                        entity.Velocity.Z);
-                    collisionDirection = Vector3.Right;
-                }
-                return true;
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// Performs terrain collision tests and adjusts the Y-axis velocity accordingly
-        /// </summary>
-        /// <returns>True if the entity collides with the terrain</returns>
-        private bool AdjustVelocityY(IAABBEntity entity, IWorld world, out Vector3 collision, out Vector3 collisionDirection)
-        {
-            collision = Vector3.Zero;
-            collisionDirection = Vector3.Zero;
-            if (entity.Velocity.Y == 0)
-                return false;
-            // Do some enviornment guessing to improve speed
-            int minX = (int)entity.Position.X - (entity.Position.X < 0 ? 1 : 0);
-            int maxX = (int)(entity.Position.X + entity.Size.Width) - (entity.Position.X < 0 ? 1 : 0);
-            int minZ = (int)entity.Position.Z - (entity.Position.Z < 0 ? 1 : 0);
-            int maxZ = (int)(entity.Position.Z + entity.Size.Depth) - (entity.Position.Z < 0 ? 1 : 0);
-            int minY, maxY;
-
-            // Expand bounding box to include area to be tested
-            if (entity.Velocity.Y < 0)
-            {
-                TempBoundingBox = new BoundingBox(
-                    new Vector3(entity.BoundingBox.Min.X, entity.BoundingBox.Min.Y + entity.Velocity.Y, entity.BoundingBox.Min.Z),
-                    entity.BoundingBox.Max);
-
-                maxY = (int)(TempBoundingBox.Max.Y);
-                minY = (int)(TempBoundingBox.Min.Y + entity.Velocity.Y) - 1;
-            }
-            else
-            {
-                TempBoundingBox = new BoundingBox(
-                    entity.BoundingBox.Min,
-                    new Vector3(entity.BoundingBox.Max.X, entity.BoundingBox.Max.Y + entity.Velocity.Y, entity.BoundingBox.Max.Z));
-                minY = (int)(entity.BoundingBox.Min.Y);
-                maxY = (int)(entity.BoundingBox.Max.Y + entity.Velocity.Y) + 1;
-            }
-
-            // Clamp Y into map boundaries
-            if (minY < 0)
-                minY = 0;
-            if (minY >= TrueCraft.Core.World.World.Height)
-                minY = TrueCraft.Core.World.World.Height - 1;
-            if (maxY < 0)
-                maxY = 0;
-            if (maxY >= TrueCraft.Core.World.World.Height)
-                maxY = TrueCraft.Core.World.World.Height - 1;
-
-            // Do terrain checks
-            double? collisionPoint = null;
-            BoundingBox blockBox;
-            for (int x = minX; x <= maxX; x++)
-            {
-                for (int y = minY; y <= maxY; y++)
-                {
-                    for (int z = minZ; z <= maxZ; z++)
-                    {
-                        var position = new Coordinates3D(x, y, z);
-                        if (!World.IsValidPosition(position))
-                            continue;
-                        var boundingBox = BlockPhysicsProvider.GetBoundingBox(world, position);
-                        if (boundingBox == null)
-                            continue;
-                        blockBox = boundingBox.Value.OffsetBy(position + new Vector3(0.5));
-                        if (TempBoundingBox.Intersects(blockBox))
-                        {
-                            if (entity.Velocity.Y < 0)
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Max.Y;
-                                else if (collisionPoint.Value < blockBox.Max.Y)
-                                    collisionPoint = blockBox.Max.Y;
-                            }
-                            else
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Min.Y;
-                                else if (collisionPoint.Value > blockBox.Min.Y)
-                                    collisionPoint = blockBox.Min.Y;
-                            }
-                            collision = position;
-                        }
-                    }
-                }
-            }
-
-            if (collisionPoint != null)
-            {
-                if (entity.Velocity.Y < 0)
-                {
-                    // TODO: Do block event
-                    //var block = world.GetBlock(collision);
-                    //block.OnBlockWalkedOn(world, collision, this);
-                    entity.Velocity = new Vector3(entity.Velocity.X,
-                        entity.Velocity.Y + (collisionPoint.Value - TempBoundingBox.Min.Y),
-                        entity.Velocity.Z);
-                    collisionDirection = Vector3.Down;
-                }
-                else if (entity.Velocity.Y > 0)
-                {
-                    entity.Velocity = new Vector3(entity.Velocity.X,
-                        entity.Velocity.Y - (TempBoundingBox.Max.Y - collisionPoint.Value),
-                        entity.Velocity.Z);
-                    collisionDirection = Vector3.Up;
-                }
-                return true;
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// Performs terrain collision tests and adjusts the Z-axis velocity accordingly
-        /// </summary>
-        /// <returns>True if the entity collides with the terrain</returns>
-        private bool AdjustVelocityZ(IAABBEntity entity, IWorld world, out Vector3 collision, out Vector3 collisionDirection)
-        {
-            collision = Vector3.Zero;
-            collisionDirection = Vector3.Zero;
-            if (entity.Velocity.Z == 0)
-                return false;
-            // Do some enviornment guessing to improve speed
-            int minX = (int)entity.Position.X - (entity.Position.X < 0 ? 1 : 0);
-            int maxX = (int)(entity.Position.X + entity.Size.Depth) - (entity.Position.X < 0 ? 1 : 0);
-            int minY = (int)entity.Position.Y - (entity.Position.Y < 0 ? 1 : 0);
-            int maxY = (int)(entity.Position.Y + entity.Size.Width) - (entity.Position.Y < 0 ? 1 : 0);
-            int minZ, maxZ;
-
-            // Expand bounding box to include area to be tested
-            if (entity.Velocity.Z < 0)
-            {
-                TempBoundingBox = new BoundingBox(
-                    new Vector3(entity.BoundingBox.Min.X, entity.BoundingBox.Min.Y, entity.BoundingBox.Min.Z + entity.Velocity.Z),
-                    entity.BoundingBox.Max);
-
-                maxZ = (int)(TempBoundingBox.Max.Z);
-                minZ = (int)(TempBoundingBox.Min.Z + entity.Velocity.Z) - 1;
-            }
-            else
-            {
-                TempBoundingBox = new BoundingBox(
-                    entity.BoundingBox.Min,
-                    new Vector3(entity.BoundingBox.Max.X, entity.BoundingBox.Max.Y, entity.BoundingBox.Max.Z + entity.Velocity.Z)
-                );
-                minZ = (int)(entity.BoundingBox.Min.Z);
-                maxZ = (int)(entity.BoundingBox.Max.Z + entity.Velocity.Z) + 1;
-            }
-
-            // Do terrain checks
-            double? collisionPoint = null;
-            BoundingBox blockBox;
-            for (int x = minX; x <= maxX; x++)
-            {
-                for (int y = minY; y <= maxY; y++)
-                {
-                    for (int z = minZ; z <= maxZ; z++)
-                    {
-                        var position = new Coordinates3D(x, y, z);
-                        var boundingBox = BlockPhysicsProvider.GetBoundingBox(world, position);
-                        if (boundingBox == null)
-                            continue;
-                        blockBox = boundingBox.Value.OffsetBy(position + new Vector3(0.5));
-                        if (TempBoundingBox.Intersects(blockBox))
-                        {
-                            if (entity.Velocity.Z < 0)
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Max.Z;
-                                else if (collisionPoint.Value < blockBox.Max.Z)
-                                    collisionPoint = blockBox.Max.Z;
-                            }
-                            else
-                            {
-                                if (!collisionPoint.HasValue)
-                                    collisionPoint = blockBox.Min.Z;
-                                else if (collisionPoint.Value > blockBox.Min.Z)
-                                    collisionPoint = blockBox.Min.Z;
-                            }
-                            collision = position;
-                        }
-                    }
-                }
-            }
-
-            if (collisionPoint != null)
-            {
-                if (entity.Velocity.Z < 0)
-                {
-                    entity.Velocity = new Vector3(
-                        entity.Velocity.X,
-                        entity.Velocity.Y,
-                        entity.Velocity.Z - (TempBoundingBox.Min.Z - collisionPoint.Value));
-                    collisionDirection = Vector3.Backwards;
-                }
-                else if (entity.Velocity.Z > 0)
-                {
-                    entity.Velocity = new Vector3(
-                        entity.Velocity.X,
-                        entity.Velocity.Y,
-                        entity.Velocity.Z - (TempBoundingBox.Max.Z - collisionPoint.Value));
-                    collisionDirection = Vector3.Forwards;
-                }
-                return true;
-            }
-
-            return false;
-        }
-    }
-}

TrueCraft/TrueCraft.csproj

@@ -57,7 +57,6 @@
     <Compile Include="Exceptions\PlayerDisconnectException.cs" />
     <Compile Include="CraftingRepository.cs" />
     <Compile Include="ServerConfiguration.cs" />
-    <Compile Include="PhysicsEngine.cs" />
     <Compile Include="MobManager.cs" />
     <Compile Include="Rules\OverworldSpawnRules.cs" />
   </ItemGroup>