v0.11 剛体力学の実装

docs/roadmap.md

@@ -163,10 +163,10 @@
 
 ゴール: オブジェクトが物理法則に従って動く
 
-- [ ] PGS拘束ソルバー
-- [ ] リジッドボディ (質量、減衰、重力スケール、キネマティック)
-- [ ] ConvexHullコリジョン
-- [ ] CCD (高速移動体のトンネリング防止)
+- [x] PGS拘束ソルバー
+- [x] リジッドボディ (質量、減衰、重力スケール、キネマティック)
+- [x] ConvexHullコリジョン
+- [x] CCD (高速移動体のトンネリング防止)
 
 完動品としての価値: オブジェクトが衝突・反発・落下・積み重なる。高速移動体もすり抜けない。
 

src/Seed.Engine.App/Program.cs

@@ -10,6 +10,8 @@
 using Seed.Engine.GameLogic;
 using Seed.Engine.Graphics;
 using Seed.Engine.Graphics.Vulkan;
+using Seed.Engine.Physics;
+using Seed.Engine.Physics.Collision;
 using Seed.Engine.Platform.Input;
 using Seed.Engine.Platform.Window;
 using Seed.Engine.Platform.Window.Glfw;
@@ -552,6 +554,11 @@ private static void Main()
 
         scheduler.AddSystem(new MovementSystem());
         scheduler.AddSystem(new CharacterPhysicsSystem());
+        scheduler.AddSystem(new ForceIntegrationSystem(1f / 60f));
+        scheduler.AddSystem(new BroadPhaseSystem());
+        scheduler.AddSystem(new NarrowPhaseSystem());
+        scheduler.AddSystem(new ConstraintSolverSystem(1f / 60f));
+        scheduler.AddSystem(new IntegrationSystem(1f / 60f));
         scheduler.AddSystem(new TransformSystem());
         scheduler.AddSystem(new CameraViewSystem(
             (float)window.Width / window.Height));
@@ -1187,14 +1194,59 @@ private static void Main()
     }
 
     private static void CreateCubeEntity(
+        World world, int meshId, int materialId, Vector3 position,
+        Vector3? halfExtents = null)
+    {
+        Vector3 he = halfExtents ?? new Vector3(0.5f, 0.5f, 0.5f);
+
+        var ltType = ComponentRegistry.Get<LocalTransform>();
+        var ltwType = ComponentRegistry.Get<LocalToWorld>();
+        var meshType = ComponentRegistry.Get<MeshReference>();
+        var matType = ComponentRegistry.Get<MaterialReference>();
+        var boundsType = ComponentRegistry.Get<WorldBounds>();
+        var ccType = ComponentRegistry.Get<ColliderComponent>();
+        ReadOnlySpan<ComponentType> types =
+            [ltType, ltwType, meshType, matType, boundsType, ccType];
+
+        Entity entity = world.CreateEntity(types);
+        world.GetComponent<LocalTransform>(entity) =
+            LocalTransform.FromPositionRotationScale(
+                position, Quaternion.Identity, he * 2f);
+        world.GetComponent<LocalToWorld>(entity) = new LocalToWorld
+        {
+            Value = Matrix4x4.Identity,
+        };
+        world.GetComponent<MeshReference>(entity) = new MeshReference
+        {
+            MeshId = meshId,
+        };
+        world.GetComponent<MaterialReference>(entity) = new MaterialReference
+        {
+            MaterialId = materialId,
+        };
+        world.GetComponent<WorldBounds>(entity) = new WorldBounds
+        {
+            Value = new AABB(position - he, position + he),
+        };
+        world.GetComponent<ColliderComponent>(entity) = new ColliderComponent
+        {
+            Value = Collider.CreateBox(he),
+        };
+    }
+
+    private static void CreateDynamicCubeEntity(
         World world, int meshId, int materialId, Vector3 position)
     {
         var ltType = ComponentRegistry.Get<LocalTransform>();
         var ltwType = ComponentRegistry.Get<LocalToWorld>();
         var meshType = ComponentRegistry.Get<MeshReference>();
         var matType = ComponentRegistry.Get<MaterialReference>();
         var boundsType = ComponentRegistry.Get<WorldBounds>();
-        ReadOnlySpan<ComponentType> types = [ltType, ltwType, meshType, matType, boundsType];
+        var ccType = ComponentRegistry.Get<ColliderComponent>();
+        var rbType = ComponentRegistry.Get<RigidBody>();
+        var velType = ComponentRegistry.Get<Velocity>();
+        ReadOnlySpan<ComponentType> types =
+            [ltType, ltwType, meshType, matType, boundsType, ccType, rbType, velType];
 
         Entity entity = world.CreateEntity(types);
         world.GetComponent<LocalTransform>(entity) =
@@ -1216,6 +1268,21 @@ private static void CreateCubeEntity(
             Value = new AABB(position - new Vector3(0.5f, 0.5f, 0.5f),
                              position + new Vector3(0.5f, 0.5f, 0.5f)),
         };
+        world.GetComponent<ColliderComponent>(entity) = new ColliderComponent
+        {
+            Value = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)),
+        };
+        world.GetComponent<RigidBody>(entity) = new RigidBody
+        {
+            Mass = 1.0f,
+            InverseMass = 1.0f,
+            LinearDamping = 0.01f,
+            AngularDamping = 0.05f,
+            GravityScale = 1.0f,
+            IsKinematic = false,
+            FreezeRotation = new Vector3(0f, 0f, 0f),
+        };
+        world.GetComponent<Velocity>(entity) = new Velocity();
     }
 
     private static void RunMainLoop(
@@ -1232,13 +1299,14 @@ private static void RunMainLoop(
         var frameLoop = new FrameLoop(world, scheduler);
 
         CreateSingletonEntity(world, renderMode);
+        CreatePhysicsSingletonEntity(world);
         CreatePlatformTagEntity(world, renderMode);
 
         CreateCubeEntity(world, cubeMeshId, materialId,
-            new Vector3(0f, 0f, -3f));
+            new Vector3(-2f, -2.5f, -5f), new Vector3(2f, 0.5f, 2f));
         CreateCubeEntity(world, cubeMeshId, materialId,
             new Vector3(2f, 0f, -4f));
-        CreateCubeEntity(world, cubeMeshId, materialId,
+        CreateDynamicCubeEntity(world, cubeMeshId, materialId,
             new Vector3(-2f, 1f, -5f));
 
         CreatePlayerEntity(world);
@@ -1285,6 +1353,15 @@ private static void CreateSingletonEntity(
         }
     }
 
+    private static void CreatePhysicsSingletonEntity(World world)
+    {
+        var bpType = ComponentRegistry.Get<BroadPhaseResult>();
+        var npType = ComponentRegistry.Get<NarrowPhaseResult>();
+        var csType = ComponentRegistry.Get<ConstraintSolverResult>();
+        ReadOnlySpan<ComponentType> types = [bpType, npType, csType];
+        world.CreateEntity(types);
+    }
+
     private static void CreatePlatformTagEntity(
         World world, RenderMode renderMode)
     {

src/Seed.Engine.Tests/Physics/Ccd/CcdSweepTests.cs

@@ -0,0 +1,125 @@
+using Xunit;
+
+using Seed.Engine.Foundation.Error;
+using Seed.Engine.Foundation.Mathematics;
+using Seed.Engine.Physics.Ccd;
+using Seed.Engine.Physics.Collision;
+using Seed.Engine.Tests.Foundation.Mathematics;
+
+namespace Seed.Engine.Tests.Physics.Ccd;
+
+public class CcdSweepTests
+{
+    [Fact]
+    public void NeedsCcd_SmallDisplacement_ReturnsFalse()
+    {
+        // Given
+        var collider = Collider.CreateSphere(1.0f);
+        var displacement = new Vector3(0.1f, 0f, 0f);
+
+        // When
+        bool result = CcdSweep.NeedsCcd(
+            collider, Vector3.Zero, Quaternion.Identity, displacement);
+
+        // Then
+        Assert.False(result);
+    }
+
+    [Fact]
+    public void NeedsCcd_LargeDisplacement_ReturnsTrue()
+    {
+        // Given
+        var collider = Collider.CreateSphere(0.5f);
+        var displacement = new Vector3(10f, 0f, 0f);
+
+        // When
+        bool result = CcdSweep.NeedsCcd(
+            collider, Vector3.Zero, Quaternion.Identity, displacement);
+
+        // Then
+        Assert.True(result);
+    }
+
+    [Fact]
+    public void NeedsCcd_ZeroDisplacement_ReturnsFalse()
+    {
+        // Given
+        var collider = Collider.CreateSphere(1.0f);
+
+        // When
+        bool result = CcdSweep.NeedsCcd(
+            collider, Vector3.Zero, Quaternion.Identity, Vector3.Zero);
+
+        // Then
+        Assert.False(result);
+    }
+
+    [Fact]
+    public void ComputeToi_HeadOnCollision_ReturnsToi()
+    {
+        // Given: two spheres approaching each other
+        var colliderA = Collider.CreateSphere(1.0f);
+        var colliderB = Collider.CreateSphere(1.0f);
+
+        var startA = new Vector3(-5f, 0f, 0f);
+        var startB = new Vector3(5f, 0f, 0f);
+
+        var dispA = new Vector3(10f, 0f, 0f);
+        var dispB = Vector3.Zero;
+
+        // When
+        Result<float> result = CcdSweep.ComputeToi(
+            colliderA, startA, Quaternion.Identity, dispA,
+            colliderB, startB, Quaternion.Identity, dispB);
+
+        // Then: TOI should be around 0.3 (when A reaches x=-5+3=(-2), B at x=5, distance=7, but radius sum=2)
+        Assert.True(result.IsSuccess);
+        Assert.True(result.Value >= 0f);
+        Assert.True(result.Value <= 1f);
+    }
+
+    [Fact]
+    public void ComputeToi_NoCollision_Fails()
+    {
+        // Given: two spheres moving in parallel, never touching
+        var colliderA = Collider.CreateSphere(0.5f);
+        var colliderB = Collider.CreateSphere(0.5f);
+
+        var startA = new Vector3(0f, 0f, 0f);
+        var startB = new Vector3(0f, 10f, 0f);
+
+        var dispA = new Vector3(10f, 0f, 0f);
+        var dispB = new Vector3(10f, 0f, 0f);
+
+        // When
+        Result<float> result = CcdSweep.ComputeToi(
+            colliderA, startA, Quaternion.Identity, dispA,
+            colliderB, startB, Quaternion.Identity, dispB);
+
+        // Then
+        Assert.True(result.IsFailure);
+    }
+
+    [Fact]
+    public void ComputeToi_BoxCollision_ReturnsToi()
+    {
+        // Given
+        var colliderA = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f));
+        var colliderB = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f));
+
+        var startA = new Vector3(-5f, 0f, 0f);
+        var startB = new Vector3(0f, 0f, 0f);
+
+        var dispA = new Vector3(10f, 0f, 0f);
+
+        // When
+        Result<float> result = CcdSweep.ComputeToi(
+            colliderA, startA, Quaternion.Identity, dispA,
+            colliderB, startB, Quaternion.Identity, Vector3.Zero);
+
+        // Then
+        Assert.True(result.IsSuccess);
+        Assert.True(result.Value >= 0f);
+        Assert.True(result.Value <= 1f);
+    }
+}

src/Seed.Engine.Tests/Physics/CcdSystemTests.cs

@@ -0,0 +1,117 @@
+using System;
+using Xunit;
+
+using Seed.Engine.Ecs;
+using Seed.Engine.Ecs.Components;
+using Seed.Engine.Foundation.Mathematics;
+using Seed.Engine.Physics;
+using Seed.Engine.Physics.Collision;
+using Seed.Engine.Tests.Foundation.Mathematics;
+
+namespace Seed.Engine.Tests.Physics;
+
+public class CcdSystemTests
+{
+    [Fact]
+    public void Phase_IsPhysics()
+    {
+        // Given / When
+        var system = new CcdSystem(1f / 60f);
+
+        // Then
+        Assert.Equal(FramePhase.Physics, system.Phase);
+    }
+
+    [Fact]
+    public void Execute_SlowBody_NoCorrection()
+    {
+        // Given
+        var world = new World();
+        using var _ = world;
+
+        var rbType = ComponentRegistry.Get<RigidBody>();
+        var velType = ComponentRegistry.Get<Velocity>();
+        var ltType = ComponentRegistry.Get<LocalTransform>();
+        var ccType = ComponentRegistry.Get<ColliderComponent>();
+        ReadOnlySpan<ComponentType> entityTypes = [rbType, velType, ltType, ccType];
+
+        Entity entity = world.CreateEntity(entityTypes);
+        var rb = RigidBody.CreateDynamic(1.0f);
+        rb.GravityScale = 0f;
+        world.GetComponent<RigidBody>(entity) = rb;
+        world.GetComponent<Velocity>(entity) = new Velocity
+        {
+            Linear = new Vector3(0.1f, 0f, 0f),
+        };
+        world.GetComponent<LocalTransform>(entity) =
+            LocalTransform.FromPosition(Vector3.Zero);
+        world.GetComponent<ColliderComponent>(entity) = new ColliderComponent
+        {
+            Value = Collider.CreateSphere(1.0f),
+        };
+
+        var system = new CcdSystem(1f / 60f);
+
+        // When
+        system.Execute(world);
+
+        // Then: position unchanged (body too slow for CCD)
+        ref LocalTransform lt = ref world.GetComponent<LocalTransform>(entity);
+        AssertHelper.ApproximatelyEqual(Vector3.Zero, lt.Position, 0.01f);
+    }
+
+    [Fact]
+    public void Execute_FastBody_WithTarget_CorrectsTunneling()
+    {
+        // Given
+        var world = new World();
+        using var _ = world;
+
+        float dt = 1f / 60f;
+
+        var rbType = ComponentRegistry.Get<RigidBody>();
+        var velType = ComponentRegistry.Get<Velocity>();
+        var ltType = ComponentRegistry.Get<LocalTransform>();
+        var ccType = ComponentRegistry.Get<ColliderComponent>();
+        ReadOnlySpan<ComponentType> entityTypes = [rbType, velType, ltType, ccType];
+
+        // Fast moving bullet
+        Entity bullet = world.CreateEntity(entityTypes);
+        var bulletRb = RigidBody.CreateDynamic(1.0f);
+        bulletRb.GravityScale = 0f;
+        world.GetComponent<RigidBody>(bullet) = bulletRb;
+        float speed = 1000f;
+        world.GetComponent<Velocity>(bullet) = new Velocity
+        {
+            Linear = new Vector3(speed, 0f, 0f),
+        };
+        // Position after integration (already moved past the wall)
+        float displacement = speed * dt;
+        world.GetComponent<LocalTransform>(bullet) =
+            LocalTransform.FromPosition(new Vector3(displacement, 0f, 0f));
+        world.GetComponent<ColliderComponent>(bullet) = new ColliderComponent
+        {
+            Value = Collider.CreateSphere(0.1f),
+        };
+
+        // Static wall
+        Entity wall = world.CreateEntity(entityTypes);
+        world.GetComponent<RigidBody>(wall) = RigidBody.CreateKinematic();
+        world.GetComponent<Velocity>(wall) = new Velocity();
+        world.GetComponent<LocalTransform>(wall) =
+            LocalTransform.FromPosition(new Vector3(5f, 0f, 0f));
+        world.GetComponent<ColliderComponent>(wall) = new ColliderComponent
+        {
+            Value = Collider.CreateBox(new Vector3(0.5f, 5f, 5f)),
+        };
+
+        var system = new CcdSystem(dt);
+
+        // When
+        system.Execute(world);
+
+        // Then: bullet should have been stopped before passing through the wall
+        ref LocalTransform bulletLt = ref world.GetComponent<LocalTransform>(bullet);
+        Assert.True(bulletLt.Position.X < displacement);
+    }
+}