From 3f961585b077db11eaaefacce01b02de96bebbe5 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 11:32:13 +0900 Subject: [PATCH 01/14] =?UTF-8?q?=E5=89=9B=E4=BD=93=E5=8A=9B=E5=AD=A6?= =?UTF-8?q?=E3=82=A8=E3=83=A9=E3=83=BC=E3=82=B3=E3=83=BC=E3=83=89=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0=20(606-609)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Foundation/Error/ErrorCode.cs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/Seed.Engine/Foundation/Error/ErrorCode.cs b/src/Seed.Engine/Foundation/Error/ErrorCode.cs index e8bbcef..8815b08 100644 --- a/src/Seed.Engine/Foundation/Error/ErrorCode.cs +++ b/src/Seed.Engine/Foundation/Error/ErrorCode.cs @@ -229,4 +229,17 @@ public static class ErrorCodes /// Spatial hash grid cell size is invalid. public static readonly ErrorCode InvalidCellSize = new(605, "Spatial hash grid cell size is invalid"); + + /// PGS solver did not converge. + public static readonly ErrorCode PgsSolverDidNotConverge = new(606, "PGS solver did not converge"); + + /// ConvexHull has no vertices. + public static readonly ErrorCode ConvexHullNoVertices = new(607, "ConvexHull has no vertices"); + + /// ConvexHull exceeds maximum vertex count. + public static readonly ErrorCode ConvexHullExceedsMaxVertices = + new(608, "ConvexHull exceeds maximum vertex count"); + + /// CCD sweep failed. + public static readonly ErrorCode CcdSweepFailed = new(609, "CCD sweep failed"); } From aadc86dce1f3aa3590976bda69735504dc5517e2 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 11:32:18 +0900 Subject: [PATCH 02/14] =?UTF-8?q?RigidBody=E3=83=BBVelocity=E3=82=B3?= =?UTF-8?q?=E3=83=B3=E3=83=9D=E3=83=BC=E3=83=8D=E3=83=B3=E3=83=88=E3=81=A8?= =?UTF-8?q?=E7=A9=8D=E5=88=86=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- .../Physics/Integration/IntegratorTests.cs | 172 ++++++++++++++++++ .../Physics/IntegrationSystemTests.cs | 119 ++++++++++++ .../Physics/RigidBodyComponentTests.cs | 101 ++++++++++ src/Seed.Engine/Ecs/Components/RigidBody.cs | 69 +++++++ src/Seed.Engine/Ecs/Components/Velocity.cs | 19 ++ .../Physics/Integration/Integrator.cs | 68 +++++++ src/Seed.Engine/Physics/IntegrationSystem.cs | 92 ++++++++++ 7 files changed, 640 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/Integration/IntegratorTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/RigidBodyComponentTests.cs create mode 100644 src/Seed.Engine/Ecs/Components/RigidBody.cs create mode 100644 src/Seed.Engine/Ecs/Components/Velocity.cs create mode 100644 src/Seed.Engine/Physics/Integration/Integrator.cs create mode 100644 src/Seed.Engine/Physics/IntegrationSystem.cs diff --git a/src/Seed.Engine.Tests/Physics/Integration/IntegratorTests.cs b/src/Seed.Engine.Tests/Physics/Integration/IntegratorTests.cs new file mode 100644 index 0000000..8c8f281 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Integration/IntegratorTests.cs @@ -0,0 +1,172 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Integration; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Integration; + +public class IntegratorTests +{ + [Fact] + public void IntegrateLinear_AppliesGravity() + { + // Given + var position = Vector3.Zero; + var velocity = Vector3.Zero; + float dt = 1f / 60f; + + // When + Integrator.IntegrateLinear( + ref position, ref velocity, + 1f, 1f, 0f, + Integrator.DefaultGravity, dt); + + // Then: velocity should have gravity applied + Assert.True(velocity.Y < 0f); + Assert.True(position.Y < 0f); + } + + [Fact] + public void IntegrateLinear_ZeroInverseMass_DoesNothing() + { + // Given + var position = Vector3.Zero; + var velocity = new Vector3(1f, 0f, 0f); + + // When + Integrator.IntegrateLinear( + ref position, ref velocity, + 0f, 1f, 0f, + Integrator.DefaultGravity, 1f / 60f); + + // Then + AssertHelper.ApproximatelyEqual(Vector3.Zero, position); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 0f, 0f), velocity); + } + + [Fact] + public void IntegrateLinear_AppliesDamping() + { + // Given + var position = Vector3.Zero; + var velocity = new Vector3(10f, 0f, 0f); + + // When + Integrator.IntegrateLinear( + ref position, ref velocity, + 1f, 0f, 2f, + Vector3.Zero, 1f / 60f); + + // Then: velocity should be reduced by damping + Assert.True(velocity.X < 10f); + Assert.True(velocity.X > 0f); + } + + [Fact] + public void IntegrateLinear_GravityScaleZero_NoGravity() + { + // Given + var position = Vector3.Zero; + var velocity = Vector3.Zero; + + // When + Integrator.IntegrateLinear( + ref position, ref velocity, + 1f, 0f, 0f, + Integrator.DefaultGravity, 1f / 60f); + + // Then + AssertHelper.ApproximatelyEqual(Vector3.Zero, velocity); + AssertHelper.ApproximatelyEqual(Vector3.Zero, position); + } + + [Fact] + public void IntegrateLinear_MovesPosition() + { + // Given + var position = Vector3.Zero; + var velocity = new Vector3(5f, 0f, 0f); + float dt = 0.1f; + + // When + Integrator.IntegrateLinear( + ref position, ref velocity, + 1f, 0f, 0f, + Vector3.Zero, dt); + + // Then + AssertHelper.ApproximatelyEqual(0.5f, position.X, 0.01f); + } + + [Fact] + public void IntegrateAngular_AppliesDamping() + { + // Given + var rotation = Quaternion.Identity; + var angularVelocity = new Vector3(0f, 1f, 0f); + + // When + Integrator.IntegrateAngular( + ref rotation, ref angularVelocity, + 2f, Vector3.Zero, 1f / 60f); + + // Then: angular velocity should be damped + Assert.True(angularVelocity.Y < 1f); + Assert.True(angularVelocity.Y > 0f); + } + + [Fact] + public void IntegrateAngular_FreezesAxis() + { + // Given + var rotation = Quaternion.Identity; + var angularVelocity = new Vector3(1f, 1f, 1f); + var freezeY = new Vector3(0f, 1f, 0f); + + // When + Integrator.IntegrateAngular( + ref rotation, ref angularVelocity, + 0f, freezeY, 1f / 60f); + + // Then: Y axis angular velocity should be zeroed + AssertHelper.ApproximatelyEqual(0f, angularVelocity.Y); + Assert.True(angularVelocity.X > 0f); + Assert.True(angularVelocity.Z > 0f); + } + + [Fact] + public void IntegrateAngular_RotatesQuaternion() + { + // Given + var rotation = Quaternion.Identity; + var angularVelocity = new Vector3(0f, MathHelper.Pi, 0f); + + // When + Integrator.IntegrateAngular( + ref rotation, ref angularVelocity, + 0f, Vector3.Zero, 1f / 60f); + + // Then: rotation should have changed from identity + Assert.False(rotation.Equals(Quaternion.Identity)); + // Should remain normalized + AssertHelper.ApproximatelyEqual(1f, rotation.Length, 0.001f); + } + + [Fact] + public void IntegrateAngular_ZeroAngularVelocity_KeepsRotation() + { + // Given + var rotation = Quaternion.Identity; + var angularVelocity = Vector3.Zero; + + // When + Integrator.IntegrateAngular( + ref rotation, ref angularVelocity, + 0f, Vector3.Zero, 1f / 60f); + + // Then + AssertHelper.ApproximatelyEqual( + Quaternion.Identity, rotation); + } +} diff --git a/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs b/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs new file mode 100644 index 0000000..f23fd17 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs @@ -0,0 +1,119 @@ +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.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics; + +public class IntegrationSystemTests +{ + [Fact] + public void Phase_IsPhysics() + { + // Given / When + var system = new IntegrationSystem(1f / 60f); + + // Then + Assert.Equal(FramePhase.Physics, system.Phase); + } + + [Fact] + public void Execute_DynamicBody_FallsUnderGravity() + { + // Given + var world = new World(); + using var _ = world; + + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType]; + + Entity entity = world.CreateEntity(entityTypes); + world.GetComponent(entity) = RigidBody.CreateDynamic(1.0f); + world.GetComponent(entity) = new Velocity(); + world.GetComponent(entity) = + LocalTransform.FromPosition(new Vector3(0f, 10f, 0f)); + + var system = new IntegrationSystem(1f / 60f); + + // When + system.Execute(world); + + // Then + ref LocalTransform lt = ref world.GetComponent(entity); + Assert.True(lt.Position.Y < 10f); + + ref Velocity vel = ref world.GetComponent(entity); + Assert.True(vel.Linear.Y < 0f); + } + + [Fact] + public void Execute_KinematicBody_DoesNotMove() + { + // Given + var world = new World(); + using var _ = world; + + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType]; + + Entity entity = world.CreateEntity(entityTypes); + world.GetComponent(entity) = RigidBody.CreateKinematic(); + world.GetComponent(entity) = new Velocity + { + Linear = new Vector3(1f, 0f, 0f), + }; + world.GetComponent(entity) = + LocalTransform.FromPosition(new Vector3(5f, 5f, 5f)); + + var system = new IntegrationSystem(1f / 60f); + + // When + system.Execute(world); + + // Then + ref LocalTransform lt = ref world.GetComponent(entity); + AssertHelper.ApproximatelyEqual(new Vector3(5f, 5f, 5f), lt.Position); + } + + [Fact] + public void Execute_WithLinearVelocity_MovesPosition() + { + // Given + var world = new World(); + using var _ = world; + + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType]; + + Entity entity = world.CreateEntity(entityTypes); + var rb = RigidBody.CreateDynamic(1.0f); + rb.GravityScale = 0f; + world.GetComponent(entity) = rb; + world.GetComponent(entity) = new Velocity + { + Linear = new Vector3(60f, 0f, 0f), + }; + world.GetComponent(entity) = + LocalTransform.FromPosition(Vector3.Zero); + + float dt = 1f / 60f; + var system = new IntegrationSystem(dt); + + // When + system.Execute(world); + + // Then + ref LocalTransform lt = ref world.GetComponent(entity); + AssertHelper.ApproximatelyEqual(1f, lt.Position.X, 0.01f); + } +} diff --git a/src/Seed.Engine.Tests/Physics/RigidBodyComponentTests.cs b/src/Seed.Engine.Tests/Physics/RigidBodyComponentTests.cs new file mode 100644 index 0000000..2260f5b --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/RigidBodyComponentTests.cs @@ -0,0 +1,101 @@ +using Xunit; + +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics; + +public class RigidBodyComponentTests +{ + [Fact] + public void CreateDynamic_SetsCorrectMassAndInverseMass() + { + // When + var rb = RigidBody.CreateDynamic(2.0f); + + // Then + AssertHelper.ApproximatelyEqual(2.0f, rb.Mass); + AssertHelper.ApproximatelyEqual(0.5f, rb.InverseMass); + Assert.False(rb.IsKinematic); + } + + [Fact] + public void CreateDynamic_DefaultsGravityScaleToOne() + { + // When + var rb = RigidBody.CreateDynamic(1.0f); + + // Then + AssertHelper.ApproximatelyEqual(1.0f, rb.GravityScale); + } + + [Fact] + public void CreateDynamic_DefaultsDampingToZero() + { + // When + var rb = RigidBody.CreateDynamic(1.0f); + + // Then + AssertHelper.ApproximatelyEqual(0f, rb.LinearDamping); + AssertHelper.ApproximatelyEqual(0f, rb.AngularDamping); + } + + [Fact] + public void CreateDynamic_DefaultsFreezeRotationToZero() + { + // When + var rb = RigidBody.CreateDynamic(1.0f); + + // Then + AssertHelper.ApproximatelyEqual(Vector3.Zero, rb.FreezeRotation); + } + + [Fact] + public void CreateKinematic_SetsZeroMassAndInverseMass() + { + // When + var rb = RigidBody.CreateKinematic(); + + // Then + AssertHelper.ApproximatelyEqual(0f, rb.Mass); + AssertHelper.ApproximatelyEqual(0f, rb.InverseMass); + Assert.True(rb.IsKinematic); + } + + [Fact] + public void CreateKinematic_SetsGravityScaleToZero() + { + // When + var rb = RigidBody.CreateKinematic(); + + // Then + AssertHelper.ApproximatelyEqual(0f, rb.GravityScale); + } + + [Fact] + public void Velocity_DefaultsToZero() + { + // When + var vel = new Velocity(); + + // Then + AssertHelper.ApproximatelyEqual(Vector3.Zero, vel.Linear); + AssertHelper.ApproximatelyEqual(Vector3.Zero, vel.Angular); + } + + [Fact] + public void Velocity_CanSetLinearAndAngular() + { + // When + var vel = new Velocity + { + Linear = new Vector3(1f, 2f, 3f), + Angular = new Vector3(0.1f, 0.2f, 0.3f), + }; + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 3f), vel.Linear); + AssertHelper.ApproximatelyEqual(new Vector3(0.1f, 0.2f, 0.3f), vel.Angular); + } +} diff --git a/src/Seed.Engine/Ecs/Components/RigidBody.cs b/src/Seed.Engine/Ecs/Components/RigidBody.cs new file mode 100644 index 0000000..258dae2 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/RigidBody.cs @@ -0,0 +1,69 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Ecs.Components; + +/// +/// ECS component representing a rigid body with mass, damping, and kinematic properties. +/// +[StructLayout(LayoutKind.Sequential)] +public struct RigidBody : IComponent +{ + /// The mass of the body in kilograms. + public float Mass; + + /// Precomputed inverse mass (1/Mass). Zero for kinematic bodies. + public float InverseMass; + + /// Linear velocity damping factor applied per frame. + public float LinearDamping; + + /// Angular velocity damping factor applied per frame. + public float AngularDamping; + + /// Multiplier for gravity applied to this body. + public float GravityScale; + + /// Whether this body is kinematic (infinite mass, unaffected by forces). + public bool IsKinematic; + + /// + /// Per-axis rotation freeze mask. 0.0 = free, 1.0 = frozen. + /// + public Vector3 FreezeRotation; + + /// + /// Creates a dynamic rigid body with the specified mass. + /// + public static RigidBody CreateDynamic(float mass) + { + return new RigidBody + { + Mass = mass, + InverseMass = 1f / mass, + LinearDamping = 0f, + AngularDamping = 0f, + GravityScale = 1f, + IsKinematic = false, + FreezeRotation = Vector3.Zero, + }; + } + + /// + /// Creates a kinematic rigid body (infinite mass, not affected by forces). + /// + public static RigidBody CreateKinematic() + { + return new RigidBody + { + Mass = 0f, + InverseMass = 0f, + LinearDamping = 0f, + AngularDamping = 0f, + GravityScale = 0f, + IsKinematic = true, + FreezeRotation = Vector3.Zero, + }; + } +} diff --git a/src/Seed.Engine/Ecs/Components/Velocity.cs b/src/Seed.Engine/Ecs/Components/Velocity.cs new file mode 100644 index 0000000..089e9a9 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/Velocity.cs @@ -0,0 +1,19 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Ecs.Components; + +/// +/// ECS component storing linear and angular velocity for a rigid body. +/// Position and rotation are stored in . +/// +[StructLayout(LayoutKind.Sequential)] +public struct Velocity : IComponent +{ + /// Linear velocity in world-space units per second. + public Vector3 Linear; + + /// Angular velocity in radians per second around each axis. + public Vector3 Angular; +} diff --git a/src/Seed.Engine/Physics/Integration/Integrator.cs b/src/Seed.Engine/Physics/Integration/Integrator.cs new file mode 100644 index 0000000..f2d9b89 --- /dev/null +++ b/src/Seed.Engine/Physics/Integration/Integrator.cs @@ -0,0 +1,68 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Integration; + +/// +/// Semi-implicit Euler integrator for rigid body dynamics. +/// +public static class Integrator +{ + /// Standard gravitational acceleration. + public static readonly Vector3 DefaultGravity = new(0f, -9.81f, 0f); + + /// + /// Integrates linear velocity: applies gravity, damping, then updates position. + /// + public static void IntegrateLinear( + ref Vector3 position, ref Vector3 linearVelocity, + float inverseMass, float gravityScale, float linearDamping, + Vector3 gravity, float dt) + { + if (inverseMass <= 0f) + { + return; + } + + linearVelocity = linearVelocity + gravity * gravityScale * dt; + + float dampingFactor = MathF.Max(0f, 1f - linearDamping * dt); + linearVelocity = linearVelocity * dampingFactor; + + position = position + linearVelocity * dt; + } + + /// + /// Integrates angular velocity: applies damping, freeze mask, then updates rotation. + /// + public static void IntegrateAngular( + ref Quaternion rotation, ref Vector3 angularVelocity, + float angularDamping, Vector3 freezeRotation, float dt) + { + float dampingFactor = MathF.Max(0f, 1f - angularDamping * dt); + angularVelocity = angularVelocity * dampingFactor; + + // Apply freeze mask: zero out frozen axes + angularVelocity = new Vector3( + angularVelocity.X * (1f - freezeRotation.X), + angularVelocity.Y * (1f - freezeRotation.Y), + angularVelocity.Z * (1f - freezeRotation.Z)); + + if (angularVelocity.LengthSquared < MathHelper.Epsilon * MathHelper.Epsilon) + { + return; + } + + // rotation += 0.5 * Quaternion(angularVelocity, 0) * rotation * dt + var omega = new Quaternion( + angularVelocity.X, angularVelocity.Y, angularVelocity.Z, 0f); + Quaternion spin = Quaternion.Multiply(omega, rotation); + rotation = new Quaternion( + rotation.X + spin.X * 0.5f * dt, + rotation.Y + spin.Y * 0.5f * dt, + rotation.Z + spin.Z * 0.5f * dt, + rotation.W + spin.W * 0.5f * dt); + rotation = rotation.Normalize(); + } +} diff --git a/src/Seed.Engine/Physics/IntegrationSystem.cs b/src/Seed.Engine/Physics/IntegrationSystem.cs new file mode 100644 index 0000000..a14ad3c --- /dev/null +++ b/src/Seed.Engine/Physics/IntegrationSystem.cs @@ -0,0 +1,92 @@ +using Seed.Engine.Ecs; +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Integration; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that integrates velocity into position and rotation for all rigid bodies. +/// Runs in the Physics frame phase, after ConstraintSolverSystem. +/// +public sealed class IntegrationSystem : ISystem +{ + private readonly QueryDescription _query; + private readonly float _fixedDt; + + /// + /// Initializes a new with the specified fixed time step. + /// + public IntegrationSystem(float fixedDt) + { + _fixedDt = fixedDt; + _query = new QueryBuilder() + .WithWrite() + .WithWrite() + .WithRead() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _query; + + /// + public void Execute(World world) + { + int rbTypeId = ComponentRegistry.Get().TypeId; + int velTypeId = ComponentRegistry.Get().TypeId; + int ltTypeId = ComponentRegistry.Get().TypeId; + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_query.Matches(storage.Archetype)) + { + continue; + } + + int rbIdx = storage.GetComponentIndex(rbTypeId); + int velIdx = storage.GetComponentIndex(velTypeId); + int ltIdx = storage.GetComponentIndex(ltTypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + int count = chunk.Count; + + for (int i = 0; i < count; i++) + { + ref RigidBody rb = ref chunk.GetComponent(rbIdx, i); + ref Velocity vel = ref chunk.GetComponent(velIdx, i); + ref LocalTransform lt = ref chunk.GetComponent(ltIdx, i); + + if (rb.IsKinematic) + { + continue; + } + + Vector3 position = lt.Position; + Vector3 linearVel = vel.Linear; + Integrator.IntegrateLinear( + ref position, ref linearVel, + rb.InverseMass, rb.GravityScale, rb.LinearDamping, + Integrator.DefaultGravity, _fixedDt); + + Quaternion rotation = lt.Rotation; + Vector3 angularVel = vel.Angular; + Integrator.IntegrateAngular( + ref rotation, ref angularVel, + rb.AngularDamping, rb.FreezeRotation, _fixedDt); + + vel.Linear = linearVel; + vel.Angular = angularVel; + lt.Position = position; + lt.Rotation = rotation; + } + } + } + } +} From cf8952b1d790a88b84e85d0b6bbf8b6b4bcc3974 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 11:32:24 +0900 Subject: [PATCH 03/14] =?UTF-8?q?PGS=E6=8B=98=E6=9D=9F=E3=82=BD=E3=83=AB?= =?UTF-8?q?=E3=83=90=E3=83=BC=E8=BF=BD=E5=8A=A0=20(Sequential=20Impulse,?= =?UTF-8?q?=20Baumgarte=E5=AE=89=E5=AE=9A=E5=8C=96)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- .../Physics/ConstraintSolverSystemTests.cs | 52 ++++ .../Physics/Solver/ContactConstraintTests.cs | 62 ++++ .../Physics/Solver/PgsSolverTests.cs | 199 +++++++++++++ .../Ecs/Components/ConstraintSolverResult.cs | 23 ++ .../Physics/ConstraintSolverSystem.cs | 274 ++++++++++++++++++ .../Physics/Solver/ContactConstraint.cs | 65 +++++ src/Seed.Engine/Physics/Solver/PgsSolver.cs | 245 ++++++++++++++++ 7 files changed, 920 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/ConstraintSolverSystemTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Solver/ContactConstraintTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Solver/PgsSolverTests.cs create mode 100644 src/Seed.Engine/Ecs/Components/ConstraintSolverResult.cs create mode 100644 src/Seed.Engine/Physics/ConstraintSolverSystem.cs create mode 100644 src/Seed.Engine/Physics/Solver/ContactConstraint.cs create mode 100644 src/Seed.Engine/Physics/Solver/PgsSolver.cs diff --git a/src/Seed.Engine.Tests/Physics/ConstraintSolverSystemTests.cs b/src/Seed.Engine.Tests/Physics/ConstraintSolverSystemTests.cs new file mode 100644 index 0000000..0a26860 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/ConstraintSolverSystemTests.cs @@ -0,0 +1,52 @@ +using System; +using Xunit; + +using Seed.Engine.Ecs; +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics; + +namespace Seed.Engine.Tests.Physics; + +public class ConstraintSolverSystemTests +{ + [Fact] + public void Phase_IsPhysics() + { + // Given / When + var system = new ConstraintSolverSystem(1f / 60f); + + // Then + Assert.Equal(FramePhase.Physics, system.Phase); + } + + [Fact] + public unsafe void Execute_NoContacts_ProducesZeroConstraints() + { + // Given + var world = new World(); + using var _ = world; + + var npType = ComponentRegistry.Get(); + var csType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes1 = [npType]; + ReadOnlySpan singletonTypes2 = [csType]; + + Entity npSingleton = world.CreateEntity(singletonTypes1); + Entity csSingleton = world.CreateEntity(singletonTypes2); + + ref NarrowPhaseResult np = ref world.GetComponent(npSingleton); + np.Contacts = null; + np.Pairs = null; + np.Count = 0; + + var system = new ConstraintSolverSystem(1f / 60f); + + // When + system.Execute(world); + + // Then + ref ConstraintSolverResult result = ref world.GetComponent(csSingleton); + Assert.Equal(0, result.Count); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Solver/ContactConstraintTests.cs b/src/Seed.Engine.Tests/Physics/Solver/ContactConstraintTests.cs new file mode 100644 index 0000000..9151e41 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Solver/ContactConstraintTests.cs @@ -0,0 +1,62 @@ +using Xunit; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Solver; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Solver; + +public class ContactConstraintTests +{ + [Fact] + public void DefaultConstraint_HasZeroAccumulatedImpulses() + { + // When + var constraint = new ContactConstraint(); + + // Then + AssertHelper.ApproximatelyEqual(0f, constraint.AccumulatedNormalImpulse); + AssertHelper.ApproximatelyEqual(0f, constraint.AccumulatedTangentImpulse1); + AssertHelper.ApproximatelyEqual(0f, constraint.AccumulatedTangentImpulse2); + } + + [Fact] + public void Constraint_StoresEntities() + { + // Given + var entityA = new Entity(1, 1); + var entityB = new Entity(2, 1); + + // When + var constraint = new ContactConstraint + { + EntityA = entityA, + EntityB = entityB, + }; + + // Then + Assert.Equal(entityA, constraint.EntityA); + Assert.Equal(entityB, constraint.EntityB); + } + + [Fact] + public void Constraint_StoresContactData() + { + // Given / When + var constraint = new ContactConstraint + { + Normal = Vector3.UnitY, + ContactPoint = new Vector3(1f, 0f, 0f), + PenetrationDepth = 0.01f, + Restitution = 0.5f, + Friction = 0.3f, + }; + + // Then + AssertHelper.ApproximatelyEqual(Vector3.UnitY, constraint.Normal); + AssertHelper.ApproximatelyEqual(0.01f, constraint.PenetrationDepth); + AssertHelper.ApproximatelyEqual(0.5f, constraint.Restitution); + AssertHelper.ApproximatelyEqual(0.3f, constraint.Friction); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Solver/PgsSolverTests.cs b/src/Seed.Engine.Tests/Physics/Solver/PgsSolverTests.cs new file mode 100644 index 0000000..a37ceed --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Solver/PgsSolverTests.cs @@ -0,0 +1,199 @@ +using System; +using Xunit; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Solver; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Solver; + +public class PgsSolverTests +{ + [Fact] + public void ComputeTangents_ProducesOrthogonalVectors() + { + // Given + var normal = Vector3.UnitY; + + // When + PgsSolver.ComputeTangents(normal, out Vector3 t1, out Vector3 t2); + + // Then + AssertHelper.ApproximatelyEqual(0f, Vector3.Dot(normal, t1), 0.001f); + AssertHelper.ApproximatelyEqual(0f, Vector3.Dot(normal, t2), 0.001f); + AssertHelper.ApproximatelyEqual(0f, Vector3.Dot(t1, t2), 0.001f); + } + + [Fact] + public void ComputeTangents_XNormal_ProducesOrthogonalVectors() + { + // Given + var normal = Vector3.UnitX; + + // When + PgsSolver.ComputeTangents(normal, out Vector3 t1, out Vector3 t2); + + // Then + AssertHelper.ApproximatelyEqual(0f, Vector3.Dot(normal, t1), 0.001f); + AssertHelper.ApproximatelyEqual(0f, Vector3.Dot(normal, t2), 0.001f); + } + + [Fact] + public void ComputeEffectiveMass_EqualMasses_ReturnsSymmetricResult() + { + // Given + float invMass = 1.0f; + var rA = new Vector3(0f, -0.5f, 0f); + var rB = new Vector3(0f, 0.5f, 0f); + var normal = Vector3.UnitY; + + // When + float mass = PgsSolver.ComputeEffectiveMass(invMass, invMass, rA, rB, normal); + + // Then + Assert.True(mass > 0f); + } + + [Fact] + public void ComputeEffectiveMass_ZeroInverseMass_ReturnsZero() + { + // Given + var rA = Vector3.Zero; + var rB = Vector3.Zero; + var normal = Vector3.UnitY; + + // When + float mass = PgsSolver.ComputeEffectiveMass(0f, 0f, rA, rB, normal); + + // Then + AssertHelper.ApproximatelyEqual(0f, mass); + } + + [Fact] + public void Solve_CollidingBodies_SeparatesVelocities() + { + // Given: two bodies approaching each other head-on + Span constraints = stackalloc ContactConstraint[1]; + constraints[0] = new ContactConstraint + { + EntityA = new Entity(1, 1), + EntityB = new Entity(2, 1), + Normal = Vector3.UnitY, + ContactPoint = new Vector3(0f, 0f, 0f), + PenetrationDepth = 0.01f, + RelativeContactA = new Vector3(0f, -0.5f, 0f), + RelativeContactB = new Vector3(0f, 0.5f, 0f), + Restitution = 0.5f, + Friction = 0.3f, + }; + + PgsSolver.ComputeTangents(constraints[0].Normal, out Vector3 t1, out Vector3 t2); + constraints[0].Tangent1 = t1; + constraints[0].Tangent2 = t2; + constraints[0].NormalMass = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, + constraints[0].Normal); + constraints[0].TangentMass1 = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, t1); + constraints[0].TangentMass2 = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, t2); + + Span linVels = stackalloc Vector3[2]; + linVels[0] = new Vector3(0f, -5f, 0f); + linVels[1] = new Vector3(0f, 5f, 0f); + + Span angVels = stackalloc Vector3[2]; + angVels[0] = Vector3.Zero; + angVels[1] = Vector3.Zero; + + Span invMasses = stackalloc float[2]; + invMasses[0] = 1f; + invMasses[1] = 1f; + + Span idxA = stackalloc int[1]; + idxA[0] = 0; + Span idxB = stackalloc int[1]; + idxB[0] = 1; + + // When + PgsSolver.Solve(constraints, linVels, angVels, invMasses, idxA, idxB, 1f / 60f); + + // Then: bodies should be pushed apart (A upward, B downward relative to original) + Assert.True(linVels[0].Y > -5f); + Assert.True(linVels[1].Y < 5f); + } + + [Fact] + public void Solve_NormalImpulse_NeverNegative() + { + // Given: bodies already separating + Span constraints = stackalloc ContactConstraint[1]; + constraints[0] = new ContactConstraint + { + Normal = Vector3.UnitY, + ContactPoint = Vector3.Zero, + PenetrationDepth = 0f, + RelativeContactA = new Vector3(0f, -0.5f, 0f), + RelativeContactB = new Vector3(0f, 0.5f, 0f), + Restitution = 0f, + Friction = 0f, + }; + + PgsSolver.ComputeTangents(Vector3.UnitY, out Vector3 t1, out Vector3 t2); + constraints[0].Tangent1 = t1; + constraints[0].Tangent2 = t2; + constraints[0].NormalMass = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, + Vector3.UnitY); + constraints[0].TangentMass1 = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, t1); + constraints[0].TangentMass2 = PgsSolver.ComputeEffectiveMass( + 1f, 1f, constraints[0].RelativeContactA, constraints[0].RelativeContactB, t2); + + Span linVels = stackalloc Vector3[2]; + linVels[0] = new Vector3(0f, 5f, 0f); + linVels[1] = new Vector3(0f, -5f, 0f); + + Span angVels = stackalloc Vector3[2]; + Span invMasses = stackalloc float[2]; + invMasses[0] = 1f; + invMasses[1] = 1f; + + Span idxA = stackalloc int[] { 0 }; + Span idxB = stackalloc int[] { 1 }; + + // When + PgsSolver.Solve(constraints, linVels, angVels, invMasses, idxA, idxB, 1f / 60f); + + // Then: accumulated normal impulse should be >= 0 + Assert.True(constraints[0].AccumulatedNormalImpulse >= 0f); + } + + [Fact] + public void PrepareConstraint_ComputesRelativeContacts() + { + // Given + var constraint = new ContactConstraint + { + Normal = Vector3.UnitY, + ContactPoint = new Vector3(1f, 0f, 0f), + PenetrationDepth = 0.01f, + }; + + var posA = new Vector3(1f, -1f, 0f); + var posB = new Vector3(1f, 1f, 0f); + + // When + PgsSolver.PrepareConstraint( + ref constraint, posA, posB, + 1f, 1f, + Vector3.Zero, Vector3.Zero, + Vector3.Zero, Vector3.Zero); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(0f, 1f, 0f), constraint.RelativeContactA); + AssertHelper.ApproximatelyEqual(new Vector3(0f, -1f, 0f), constraint.RelativeContactB); + Assert.True(constraint.NormalMass > 0f); + } +} diff --git a/src/Seed.Engine/Ecs/Components/ConstraintSolverResult.cs b/src/Seed.Engine/Ecs/Components/ConstraintSolverResult.cs new file mode 100644 index 0000000..aecf6e1 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/ConstraintSolverResult.cs @@ -0,0 +1,23 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Physics.Solver; + +namespace Seed.Engine.Ecs.Components; + +/// +/// Singleton ECS component holding a pointer to the constraint solver results. +/// Uses an unsafe pointer to satisfy unmanaged struct constraints. +/// +[StructLayout(LayoutKind.Sequential)] +public unsafe struct ConstraintSolverResult : IComponent +{ + /// + /// Pointer to the array of solved ContactConstraints. + /// + public ContactConstraint* Constraints; + + /// + /// Number of solved constraints. + /// + public int Count; +} diff --git a/src/Seed.Engine/Physics/ConstraintSolverSystem.cs b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs new file mode 100644 index 0000000..ee6db6c --- /dev/null +++ b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs @@ -0,0 +1,274 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Foundation.Memory; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Physics.Solver; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that builds contact constraints from narrow phase results +/// and solves them using the PGS solver. +/// Runs in the Physics frame phase, after NarrowPhaseSystem. +/// +public sealed class ConstraintSolverSystem : ISystem +{ + private const float DefaultRestitution = 0.3f; + private const float DefaultFriction = 0.5f; + + private readonly QueryDescription _narrowPhaseQuery; + private readonly QueryDescription _rigidBodyQuery; + private readonly QueryDescription _solverResultQuery; + private readonly float _fixedDt; + private NativeList _constraints; + private bool _initialized; + + /// + /// Initializes a new with the specified fixed time step. + /// + public ConstraintSolverSystem(float fixedDt) + { + _fixedDt = fixedDt; + + _narrowPhaseQuery = new QueryBuilder() + .WithRead() + .Build(); + + _rigidBodyQuery = new QueryBuilder() + .WithWrite() + .WithRead() + .WithRead() + .Build(); + + _solverResultQuery = new QueryBuilder() + .WithWrite() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _rigidBodyQuery; + + /// + public unsafe void Execute(World world) + { + if (!_initialized) + { + _constraints = new NativeList(256); + _initialized = true; + } + + _constraints.Clear(); + + ContactPoint* contacts = null; + CollisionPair* pairs = null; + int contactCount = 0; + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_narrowPhaseQuery.Matches(storage.Archetype)) + { + continue; + } + + int npIdx = storage.GetComponentIndex( + ComponentRegistry.Get().TypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + if (chunk.Count > 0) + { + ref NarrowPhaseResult np = ref chunk.GetComponent(npIdx, 0); + contacts = np.Contacts; + pairs = np.Pairs; + contactCount = np.Count; + break; + } + } + if (contactCount > 0) break; + } + + if (contactCount == 0 || contacts == null || pairs == null) + { + UpdateResult(world); + return; + } + + // Build constraints and collect entity velocity data + var linearVels = new NativeList(contactCount * 2); + var angularVels = new NativeList(contactCount * 2); + var invMasses = new NativeList(contactCount * 2); + var entityIndexA = new NativeList(contactCount); + var entityIndexB = new NativeList(contactCount); + + // Map entities to velocity array indices + var entityMap = new NativeList(contactCount * 2); + + for (int i = 0; i < contactCount; i++) + { + CollisionPair pair = pairs[i]; + ContactPoint cp = contacts[i]; + + if (!world.IsAlive(pair.EntityA) || !world.IsAlive(pair.EntityB)) + { + continue; + } + + int idxA = FindOrAddEntity(ref entityMap, ref linearVels, ref angularVels, ref invMasses, + pair.EntityA, world); + int idxB = FindOrAddEntity(ref entityMap, ref linearVels, ref angularVels, ref invMasses, + pair.EntityB, world); + + Vector3 midpoint = (cp.PointOnA + cp.PointOnB) * 0.5f; + Vector3 posA = world.HasComponent(pair.EntityA) + ? world.GetComponent(pair.EntityA).Position + : Vector3.Zero; + Vector3 posB = world.HasComponent(pair.EntityB) + ? world.GetComponent(pair.EntityB).Position + : Vector3.Zero; + + var constraint = new ContactConstraint + { + EntityA = pair.EntityA, + EntityB = pair.EntityB, + Normal = cp.Normal, + ContactPoint = midpoint, + PenetrationDepth = cp.PenetrationDepth, + Restitution = DefaultRestitution, + Friction = DefaultFriction, + }; + + PgsSolver.PrepareConstraint( + ref constraint, posA, posB, + invMasses[idxA], invMasses[idxB], + linearVels[idxA], angularVels[idxA], + linearVels[idxB], angularVels[idxB]); + + _constraints.Add(constraint); + entityIndexA.Add(idxA); + entityIndexB.Add(idxB); + } + + if (_constraints.Length > 0) + { + PgsSolver.Solve( + _constraints.AsSpan(), + linearVels.AsSpan(), + angularVels.AsSpan(), + invMasses.AsSpan(), + entityIndexA.AsSpan(), + entityIndexB.AsSpan(), + _fixedDt); + + // Write back solved velocities + for (int i = 0; i < entityMap.Length; i++) + { + Entity entity = entityMap[i]; + if (world.IsAlive(entity) && world.HasComponent(entity)) + { + ref Velocity vel = ref world.GetComponent(entity); + vel.Linear = linearVels[i]; + vel.Angular = angularVels[i]; + } + } + } + + linearVels.Dispose(); + angularVels.Dispose(); + invMasses.Dispose(); + entityIndexA.Dispose(); + entityIndexB.Dispose(); + entityMap.Dispose(); + + UpdateResult(world); + } + + private static int FindOrAddEntity( + ref NativeList entityMap, + ref NativeList linearVels, + ref NativeList angularVels, + ref NativeList invMasses, + Entity entity, World world) + { + for (int i = 0; i < entityMap.Length; i++) + { + if (entityMap[i].Equals(entity)) + { + return i; + } + } + + int idx = entityMap.Length; + entityMap.Add(entity); + + Vector3 linVel = Vector3.Zero; + Vector3 angVel = Vector3.Zero; + float invMass = 0f; + + if (world.HasComponent(entity)) + { + ref Velocity vel = ref world.GetComponent(entity); + linVel = vel.Linear; + angVel = vel.Angular; + } + + if (world.HasComponent(entity)) + { + ref RigidBody rb = ref world.GetComponent(entity); + invMass = rb.InverseMass; + } + + linearVels.Add(linVel); + angularVels.Add(angVel); + invMasses.Add(invMass); + + return idx; + } + + private unsafe void UpdateResult(World world) + { + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_solverResultQuery.Matches(storage.Archetype)) + { + continue; + } + + int csIdx = storage.GetComponentIndex( + ComponentRegistry.Get().TypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + if (chunk.Count > 0) + { + ref ConstraintSolverResult result = + ref chunk.GetComponent(csIdx, 0); + + if (_constraints.Length > 0) + { + fixed (ContactConstraint* ptr = &_constraints.AsSpan()[0]) + { + result.Constraints = ptr; + } + } + else + { + result.Constraints = null; + } + result.Count = _constraints.Length; + return; + } + } + } + } +} diff --git a/src/Seed.Engine/Physics/Solver/ContactConstraint.cs b/src/Seed.Engine/Physics/Solver/ContactConstraint.cs new file mode 100644 index 0000000..85f038a --- /dev/null +++ b/src/Seed.Engine/Physics/Solver/ContactConstraint.cs @@ -0,0 +1,65 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Solver; + +/// +/// Data structure describing a contact constraint between two rigid bodies. +/// Used by the PGS solver to resolve collisions. +/// +[StructLayout(LayoutKind.Sequential)] +public struct ContactConstraint +{ + /// First entity in the contact pair. + public Entity EntityA; + + /// Second entity in the contact pair. + public Entity EntityB; + + /// Contact normal pointing from B towards A. + public Vector3 Normal; + + /// Contact point in world space. + public Vector3 ContactPoint; + + /// Penetration depth (positive when overlapping). + public float PenetrationDepth; + + /// Contact point relative to body A center of mass. + public Vector3 RelativeContactA; + + /// Contact point relative to body B center of mass. + public Vector3 RelativeContactB; + + /// Inverse of effective mass in normal direction. + public float NormalMass; + + /// Inverse of effective mass in first tangent direction. + public float TangentMass1; + + /// Inverse of effective mass in second tangent direction. + public float TangentMass2; + + /// Accumulated normal impulse for warm starting. + public float AccumulatedNormalImpulse; + + /// Accumulated tangent impulse in first friction direction. + public float AccumulatedTangentImpulse1; + + /// Accumulated tangent impulse in second friction direction. + public float AccumulatedTangentImpulse2; + + /// Coefficient of restitution (bounciness). + public float Restitution; + + /// Coefficient of friction. + public float Friction; + + /// First tangent direction perpendicular to the normal. + public Vector3 Tangent1; + + /// Second tangent direction perpendicular to the normal. + public Vector3 Tangent2; +} diff --git a/src/Seed.Engine/Physics/Solver/PgsSolver.cs b/src/Seed.Engine/Physics/Solver/PgsSolver.cs new file mode 100644 index 0000000..74d78af --- /dev/null +++ b/src/Seed.Engine/Physics/Solver/PgsSolver.cs @@ -0,0 +1,245 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Solver; + +/// +/// Projected Gauss-Seidel (Sequential Impulse) constraint solver. +/// Resolves contact constraints by iteratively applying impulses. +/// +public static class PgsSolver +{ + /// Number of solver iterations. + public const int Iterations = 8; + + /// Baumgarte stabilization factor for position correction. + public const float BaumgarteBeta = 0.2f; + + /// Penetration slop below which no position correction is applied. + public const float PenetrationSlop = 0.005f; + + /// + /// Computes tangent vectors perpendicular to the contact normal. + /// + public static void ComputeTangents(Vector3 normal, out Vector3 tangent1, out Vector3 tangent2) + { + if (MathF.Abs(normal.X) < 0.9f) + { + tangent1 = Vector3.Cross(normal, Vector3.UnitX); + } + else + { + tangent1 = Vector3.Cross(normal, Vector3.UnitY); + } + + float len = tangent1.Length; + if (len > MathHelper.Epsilon) + { + tangent1 = tangent1 / len; + } + + tangent2 = Vector3.Cross(normal, tangent1); + float len2 = tangent2.Length; + if (len2 > MathHelper.Epsilon) + { + tangent2 = tangent2 / len2; + } + } + + /// + /// Computes the effective mass for a contact constraint direction. + /// + public static float ComputeEffectiveMass( + float inverseMassA, float inverseMassB, + Vector3 rA, Vector3 rB, Vector3 direction) + { + // For point masses (ignoring inertia tensor): + // effectiveMass = 1 / (invMassA + invMassB + cross(rA, n)^2 * invIA + cross(rB, n)^2 * invIB) + // Simplified: using unit inertia approximation + Vector3 crossA = Vector3.Cross(rA, direction); + Vector3 crossB = Vector3.Cross(rB, direction); + + float invMassSum = inverseMassA + inverseMassB + + Vector3.Dot(crossA, crossA) * inverseMassA + + Vector3.Dot(crossB, crossB) * inverseMassB; + + if (invMassSum < MathHelper.Epsilon) + { + return 0f; + } + + return 1f / invMassSum; + } + + /// + /// Prepares a contact constraint with precomputed data for the solver. + /// + public static void PrepareConstraint( + ref ContactConstraint constraint, + Vector3 posA, Vector3 posB, + float inverseMassA, float inverseMassB, + Vector3 linearVelA, Vector3 angularVelA, + Vector3 linearVelB, Vector3 angularVelB) + { + constraint.RelativeContactA = constraint.ContactPoint - posA; + constraint.RelativeContactB = constraint.ContactPoint - posB; + + ComputeTangents(constraint.Normal, out Vector3 t1, out Vector3 t2); + constraint.Tangent1 = t1; + constraint.Tangent2 = t2; + + constraint.NormalMass = ComputeEffectiveMass( + inverseMassA, inverseMassB, + constraint.RelativeContactA, constraint.RelativeContactB, + constraint.Normal); + + constraint.TangentMass1 = ComputeEffectiveMass( + inverseMassA, inverseMassB, + constraint.RelativeContactA, constraint.RelativeContactB, + t1); + + constraint.TangentMass2 = ComputeEffectiveMass( + inverseMassA, inverseMassB, + constraint.RelativeContactA, constraint.RelativeContactB, + t2); + } + + /// + /// Solves contact constraints iteratively, modifying velocities in place. + /// + public static void Solve( + Span constraints, + Span linearVelocities, + Span angularVelocities, + Span inverseMasses, + Span entityIndexA, + Span entityIndexB, + float dt) + { + for (int iter = 0; iter < Iterations; iter++) + { + for (int i = 0; i < constraints.Length; i++) + { + ref ContactConstraint c = ref constraints[i]; + int idxA = entityIndexA[i]; + int idxB = entityIndexB[i]; + + SolveNormal(ref c, + ref linearVelocities[idxA], ref angularVelocities[idxA], inverseMasses[idxA], + ref linearVelocities[idxB], ref angularVelocities[idxB], inverseMasses[idxB], + dt); + + SolveFriction(ref c, + ref linearVelocities[idxA], ref angularVelocities[idxA], inverseMasses[idxA], + ref linearVelocities[idxB], ref angularVelocities[idxB], inverseMasses[idxB]); + } + } + } + + private static void SolveNormal( + ref ContactConstraint c, + ref Vector3 linVelA, ref Vector3 angVelA, float invMassA, + ref Vector3 linVelB, ref Vector3 angVelB, float invMassB, + float dt) + { + Vector3 relVel = ComputeRelativeVelocity( + linVelA, angVelA, c.RelativeContactA, + linVelB, angVelB, c.RelativeContactB); + + float vn = Vector3.Dot(relVel, c.Normal); + + // Baumgarte position correction + float baumgarte = 0f; + if (c.PenetrationDepth > PenetrationSlop) + { + baumgarte = BaumgarteBeta / dt * (c.PenetrationDepth - PenetrationSlop); + } + + // Restitution bias + float restitutionBias = 0f; + if (vn < -1f) + { + restitutionBias = -c.Restitution * vn; + } + + float lambda = c.NormalMass * (-vn + baumgarte + restitutionBias); + + float oldAccum = c.AccumulatedNormalImpulse; + c.AccumulatedNormalImpulse = MathF.Max(0f, oldAccum + lambda); + lambda = c.AccumulatedNormalImpulse - oldAccum; + + ApplyImpulse(c.Normal, lambda, + ref linVelA, ref angVelA, invMassA, c.RelativeContactA, + ref linVelB, ref angVelB, invMassB, c.RelativeContactB); + } + + private static void SolveFriction( + ref ContactConstraint c, + ref Vector3 linVelA, ref Vector3 angVelA, float invMassA, + ref Vector3 linVelB, ref Vector3 angVelB, float invMassB) + { + float maxFriction = c.Friction * c.AccumulatedNormalImpulse; + + // Tangent 1 + { + Vector3 relVel = ComputeRelativeVelocity( + linVelA, angVelA, c.RelativeContactA, + linVelB, angVelB, c.RelativeContactB); + + float vt = Vector3.Dot(relVel, c.Tangent1); + float lambda = c.TangentMass1 * (-vt); + + float oldAccum = c.AccumulatedTangentImpulse1; + c.AccumulatedTangentImpulse1 = MathHelper.Clamp( + oldAccum + lambda, -maxFriction, maxFriction); + lambda = c.AccumulatedTangentImpulse1 - oldAccum; + + ApplyImpulse(c.Tangent1, lambda, + ref linVelA, ref angVelA, invMassA, c.RelativeContactA, + ref linVelB, ref angVelB, invMassB, c.RelativeContactB); + } + + // Tangent 2 + { + Vector3 relVel = ComputeRelativeVelocity( + linVelA, angVelA, c.RelativeContactA, + linVelB, angVelB, c.RelativeContactB); + + float vt = Vector3.Dot(relVel, c.Tangent2); + float lambda = c.TangentMass2 * (-vt); + + float oldAccum = c.AccumulatedTangentImpulse2; + c.AccumulatedTangentImpulse2 = MathHelper.Clamp( + oldAccum + lambda, -maxFriction, maxFriction); + lambda = c.AccumulatedTangentImpulse2 - oldAccum; + + ApplyImpulse(c.Tangent2, lambda, + ref linVelA, ref angVelA, invMassA, c.RelativeContactA, + ref linVelB, ref angVelB, invMassB, c.RelativeContactB); + } + } + + private static Vector3 ComputeRelativeVelocity( + Vector3 linVelA, Vector3 angVelA, Vector3 rA, + Vector3 linVelB, Vector3 angVelB, Vector3 rB) + { + Vector3 velA = linVelA + Vector3.Cross(angVelA, rA); + Vector3 velB = linVelB + Vector3.Cross(angVelB, rB); + return velA - velB; + } + + private static void ApplyImpulse( + Vector3 direction, float lambda, + ref Vector3 linVelA, ref Vector3 angVelA, float invMassA, Vector3 rA, + ref Vector3 linVelB, ref Vector3 angVelB, float invMassB, Vector3 rB) + { + Vector3 impulse = direction * lambda; + + linVelA = linVelA + impulse * invMassA; + angVelA = angVelA + Vector3.Cross(rA, impulse) * invMassA; + + linVelB = linVelB - impulse * invMassB; + angVelB = angVelB - Vector3.Cross(rB, impulse) * invMassB; + } +} From 962ec83ee3b89ef8adf502bf014d25776fe114c7 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 11:32:29 +0900 Subject: [PATCH 04/14] =?UTF-8?q?ConvexHull=E3=82=B3=E3=83=AA=E3=82=B8?= =?UTF-8?q?=E3=83=A7=E3=83=B3=E8=BF=BD=E5=8A=A0=20(64=E9=A0=82=E7=82=B9?= =?UTF-8?q?=E5=9B=BA=E5=AE=9A=E3=83=90=E3=83=83=E3=83=95=E3=82=A1,=20GJK/?= =?UTF-8?q?=E3=83=AC=E3=82=A4=E3=82=AD=E3=83=A3=E3=82=B9=E3=83=88=E5=AF=BE?= =?UTF-8?q?=E5=BF=9C)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- .../Physics/Collision/ConvexHullShapeTests.cs | 237 ++++++++++++++++++ src/Seed.Engine/Physics/Collision/Collider.cs | 19 ++ .../Physics/Collision/ColliderBounds.cs | 25 ++ .../Physics/Collision/ColliderData.cs | 4 + .../Physics/Collision/ColliderType.cs | 3 + .../Physics/Collision/ConvexHullShape.cs | 109 ++++++++ .../Physics/Collision/GjkSupport.cs | 15 ++ .../Physics/Query/RayShapeIntersection.cs | 132 ++++++++++ 8 files changed, 544 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/Collision/ConvexHullShapeTests.cs create mode 100644 src/Seed.Engine/Physics/Collision/ConvexHullShape.cs diff --git a/src/Seed.Engine.Tests/Physics/Collision/ConvexHullShapeTests.cs b/src/Seed.Engine.Tests/Physics/Collision/ConvexHullShapeTests.cs new file mode 100644 index 0000000..6f71e26 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/ConvexHullShapeTests.cs @@ -0,0 +1,237 @@ +using System; +using Xunit; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class ConvexHullShapeTests +{ + private static ReadOnlySpan CubeVertices => new Vector3[] + { + new(-1f, -1f, -1f), + new( 1f, -1f, -1f), + new( 1f, 1f, -1f), + new(-1f, 1f, -1f), + new(-1f, -1f, 1f), + new( 1f, -1f, 1f), + new( 1f, 1f, 1f), + new(-1f, 1f, 1f), + }; + + [Fact] + public void Create_WithValidVertices_Succeeds() + { + // When + Result result = ConvexHullShape.Create(CubeVertices); + + // Then + Assert.True(result.IsSuccess); + Assert.Equal(8, result.Value.VertexCount); + } + + [Fact] + public void Create_EmptyVertices_Fails() + { + // When + Result result = ConvexHullShape.Create(ReadOnlySpan.Empty); + + // Then + Assert.True(result.IsFailure); + Assert.Equal(ErrorCodes.ConvexHullNoVertices, result.Error); + } + + [Fact] + public void Create_ExceedMaxVertices_Fails() + { + // Given + Span tooMany = stackalloc Vector3[ConvexHullShape.MaxVertices + 1]; + + // When + Result result = ConvexHullShape.Create(tooMany); + + // Then + Assert.True(result.IsFailure); + Assert.Equal(ErrorCodes.ConvexHullExceedsMaxVertices, result.Error); + } + + [Fact] + public void GetVertex_ReturnsCorrectVertex() + { + // Given + Result result = ConvexHullShape.Create(CubeVertices); + ConvexHullShape shape = result.Value; + + // When / Then + AssertHelper.ApproximatelyEqual(new Vector3(-1f, -1f, -1f), shape.GetVertex(0)); + AssertHelper.ApproximatelyEqual(new Vector3(1f, -1f, -1f), shape.GetVertex(1)); + } + + [Fact] + public void Support_PositiveX_ReturnsFarthestInX() + { + // Given + Result result = ConvexHullShape.Create(CubeVertices); + ConvexHullShape shape = result.Value; + + // When + Vector3 support = shape.Support(Vector3.UnitX); + + // Then + AssertHelper.ApproximatelyEqual(1f, support.X); + } + + [Fact] + public void Support_NegativeY_ReturnsFarthestInNegY() + { + // Given + Result result = ConvexHullShape.Create(CubeVertices); + ConvexHullShape shape = result.Value; + + // When + Vector3 support = shape.Support(-Vector3.UnitY); + + // Then + AssertHelper.ApproximatelyEqual(-1f, support.Y); + } + + [Fact] + public void ComputeLocalAABB_Cube_ReturnsCorrectBounds() + { + // Given + Result result = ConvexHullShape.Create(CubeVertices); + ConvexHullShape shape = result.Value; + + // When + AABB aabb = shape.ComputeLocalAABB(); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(-1f, -1f, -1f), aabb.Min); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 1f, 1f), aabb.Max); + } + + [Fact] + public void ColliderType_ConvexHull_EqualsThree() + { + // Then + Assert.Equal((byte)3, (byte)ColliderType.ConvexHull); + } + + [Fact] + public void CreateConvexHullCollider_SetsCorrectType() + { + // When + Result result = Collider.CreateConvexHull(CubeVertices); + + // Then + Assert.True(result.IsSuccess); + Assert.Equal(ColliderType.ConvexHull, result.Value.Type); + } + + [Fact] + public void GjkSupport_ConvexHull_ReturnsWorldSpacePoint() + { + // Given + Result colliderResult = Collider.CreateConvexHull(CubeVertices); + Collider collider = colliderResult.Value; + var position = new Vector3(5f, 0f, 0f); + + // When + Result result = GjkSupport.Support( + collider, position, Quaternion.Identity, Vector3.UnitX); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(6f, result.Value.X, 0.01f); + } + + [Fact] + public void ColliderBounds_ConvexHull_ReturnsCorrectAABB() + { + // Given + Result colliderResult = Collider.CreateConvexHull(CubeVertices); + Collider collider = colliderResult.Value; + var position = new Vector3(10f, 0f, 0f); + + // When + Result result = ColliderBounds.ComputeAABB( + collider, position, Quaternion.Identity); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(9f, -1f, -1f), result.Value.Min, 0.01f); + AssertHelper.ApproximatelyEqual(new Vector3(11f, 1f, 1f), result.Value.Max, 0.01f); + } + + [Fact] + public void RayConvexHull_Hit_ReturnsTrue() + { + // Given + Result colliderResult = Collider.CreateConvexHull(CubeVertices); + Collider collider = colliderResult.Value; + var ray = new Ray(new Vector3(-5f, 0f, 0f), Vector3.UnitX); + + // When + Result result = Seed.Engine.Physics.Query.RayShapeIntersection.RayCollider( + ray, collider, Vector3.Zero, Quaternion.Identity, out float dist, out _); + + // Then + Assert.True(result.IsSuccess); + Assert.True(result.Value); + Assert.True(dist > 0f); + } + + [Fact] + public void RayConvexHull_Miss_ReturnsFalse() + { + // Given + Result colliderResult = Collider.CreateConvexHull(CubeVertices); + Collider collider = colliderResult.Value; + var ray = new Ray(new Vector3(-5f, 10f, 0f), Vector3.UnitX); + + // When + Result result = Seed.Engine.Physics.Query.RayShapeIntersection.RayCollider( + ray, collider, Vector3.Zero, Quaternion.Identity, out _, out _); + + // Then + Assert.True(result.IsSuccess); + Assert.False(result.Value); + } + + [Fact] + public void GjkIntersect_TwoConvexHulls_Overlapping_ReturnsTrue() + { + // Given + Result colliderA = Collider.CreateConvexHull(CubeVertices); + Result colliderB = Collider.CreateConvexHull(CubeVertices); + + // When: hulls at (0,0,0) and (1,0,0) should overlap + Result result = GjkEpa.Intersects( + colliderA.Value, Vector3.Zero, Quaternion.Identity, + colliderB.Value, new Vector3(1f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(result.IsSuccess); + Assert.True(result.Value); + } + + [Fact] + public void GjkIntersect_TwoConvexHulls_Separated_ReturnsFalse() + { + // Given + Result colliderA = Collider.CreateConvexHull(CubeVertices); + Result colliderB = Collider.CreateConvexHull(CubeVertices); + + // When: hulls at (0,0,0) and (10,0,0) should not overlap + Result result = GjkEpa.Intersects( + colliderA.Value, Vector3.Zero, Quaternion.Identity, + colliderB.Value, new Vector3(10f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(result.IsSuccess); + Assert.False(result.Value); + } +} diff --git a/src/Seed.Engine/Physics/Collision/Collider.cs b/src/Seed.Engine/Physics/Collision/Collider.cs index ff7d11a..a9ddd58 100644 --- a/src/Seed.Engine/Physics/Collision/Collider.cs +++ b/src/Seed.Engine/Physics/Collision/Collider.cs @@ -1,3 +1,6 @@ +using System; + +using Seed.Engine.Foundation.Error; using Seed.Engine.Foundation.Mathematics; namespace Seed.Engine.Physics.Collision; @@ -43,4 +46,20 @@ public static Collider CreateCapsule(float radius, float halfHeight) collider.Data.Capsule = new CapsuleShape(radius, halfHeight); return collider; } + + /// + /// Creates a convex hull collider from a span of vertices. + /// + public static Result CreateConvexHull(ReadOnlySpan vertices) + { + Result shapeResult = ConvexHullShape.Create(vertices); + if (shapeResult.IsFailure) + { + return Result.Fail(shapeResult.Error); + } + + var collider = new Collider { Type = ColliderType.ConvexHull }; + collider.Data.ConvexHull = shapeResult.Value; + return Result.Ok(collider); + } } diff --git a/src/Seed.Engine/Physics/Collision/ColliderBounds.cs b/src/Seed.Engine/Physics/Collision/ColliderBounds.cs index dd1513f..7a822ea 100644 --- a/src/Seed.Engine/Physics/Collision/ColliderBounds.cs +++ b/src/Seed.Engine/Physics/Collision/ColliderBounds.cs @@ -21,6 +21,8 @@ public static Result ComputeAABB(Collider collider, Vector3 position, Quat ColliderType.Box => Result.Ok(ComputeBoxAABB(collider.Data.Box, position, rotation)), ColliderType.Capsule => Result.Ok( ComputeCapsuleAABB(collider.Data.Capsule, position, rotation)), + ColliderType.ConvexHull => Result.Ok( + ComputeConvexHullAABB(collider.Data.ConvexHull, position, rotation)), _ => Result.Fail(ErrorCodes.InvalidColliderType), }; } @@ -62,4 +64,27 @@ private static AABB ComputeCapsuleAABB(CapsuleShape capsule, Vector3 position, Q return topAABB.Merge(bottomAABB); } + + private static AABB ComputeConvexHullAABB( + ConvexHullShape hull, Vector3 position, Quaternion rotation) + { + Vector3 firstWorld = position + rotation.RotateVector(hull.GetVertex(0)); + Vector3 min = firstWorld; + Vector3 max = firstWorld; + + for (int i = 1; i < hull.VertexCount; i++) + { + Vector3 worldVert = position + rotation.RotateVector(hull.GetVertex(i)); + min = new Vector3( + MathF.Min(min.X, worldVert.X), + MathF.Min(min.Y, worldVert.Y), + MathF.Min(min.Z, worldVert.Z)); + max = new Vector3( + MathF.Max(max.X, worldVert.X), + MathF.Max(max.Y, worldVert.Y), + MathF.Max(max.Z, worldVert.Z)); + } + + return new AABB(min, max); + } } diff --git a/src/Seed.Engine/Physics/Collision/ColliderData.cs b/src/Seed.Engine/Physics/Collision/ColliderData.cs index 7021e4c..ea08ed8 100644 --- a/src/Seed.Engine/Physics/Collision/ColliderData.cs +++ b/src/Seed.Engine/Physics/Collision/ColliderData.cs @@ -22,4 +22,8 @@ public struct ColliderData /// Capsule shape data. [FieldOffset(0)] public CapsuleShape Capsule; + + /// Convex hull shape data. + [FieldOffset(0)] + public ConvexHullShape ConvexHull; } diff --git a/src/Seed.Engine/Physics/Collision/ColliderType.cs b/src/Seed.Engine/Physics/Collision/ColliderType.cs index 2f121cb..33be5d4 100644 --- a/src/Seed.Engine/Physics/Collision/ColliderType.cs +++ b/src/Seed.Engine/Physics/Collision/ColliderType.cs @@ -13,4 +13,7 @@ public enum ColliderType : byte /// A capsule shape oriented along the local Y axis. Capsule = 2, + + /// A convex hull shape defined by a set of vertices. + ConvexHull = 3, } diff --git a/src/Seed.Engine/Physics/Collision/ConvexHullShape.cs b/src/Seed.Engine/Physics/Collision/ConvexHullShape.cs new file mode 100644 index 0000000..ebd5c85 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/ConvexHullShape.cs @@ -0,0 +1,109 @@ +using System; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A convex hull collision shape defined by a set of vertices. +/// Uses a fixed-size inline buffer to avoid heap allocation. +/// +public unsafe struct ConvexHullShape +{ + /// Maximum number of vertices supported. + public const int MaxVertices = 64; + + private fixed float _vertices[MaxVertices * 3]; + private int _vertexCount; + + /// + /// Gets the number of vertices in this convex hull. + /// + public readonly int VertexCount => _vertexCount; + + /// + /// Gets the vertex at the specified index. + /// + public readonly Vector3 GetVertex(int index) + { + int offset = index * 3; + return new Vector3(_vertices[offset], _vertices[offset + 1], _vertices[offset + 2]); + } + + /// + /// Creates a ConvexHullShape from a span of vertices. + /// + public static Result Create(ReadOnlySpan vertices) + { + if (vertices.Length == 0) + { + return Result.Fail(ErrorCodes.ConvexHullNoVertices); + } + + if (vertices.Length > MaxVertices) + { + return Result.Fail(ErrorCodes.ConvexHullExceedsMaxVertices); + } + + var shape = new ConvexHullShape(); + shape._vertexCount = vertices.Length; + + for (int i = 0; i < vertices.Length; i++) + { + int offset = i * 3; + shape._vertices[offset] = vertices[i].X; + shape._vertices[offset + 1] = vertices[i].Y; + shape._vertices[offset + 2] = vertices[i].Z; + } + + return Result.Ok(shape); + } + + /// + /// Returns the support point (farthest point in the given direction). + /// + public readonly Vector3 Support(Vector3 direction) + { + float bestDot = float.NegativeInfinity; + Vector3 bestVertex = GetVertex(0); + + for (int i = 0; i < _vertexCount; i++) + { + Vector3 v = GetVertex(i); + float dot = Vector3.Dot(v, direction); + if (dot > bestDot) + { + bestDot = dot; + bestVertex = v; + } + } + + return bestVertex; + } + + /// + /// Computes the AABB of this convex hull in local space. + /// + public readonly AABB ComputeLocalAABB() + { + Vector3 first = GetVertex(0); + Vector3 min = first; + Vector3 max = first; + + for (int i = 1; i < _vertexCount; i++) + { + Vector3 v = GetVertex(i); + min = new Vector3( + MathF.Min(min.X, v.X), + MathF.Min(min.Y, v.Y), + MathF.Min(min.Z, v.Z)); + max = new Vector3( + MathF.Max(max.X, v.X), + MathF.Max(max.Y, v.Y), + MathF.Max(max.Z, v.Z)); + } + + return new AABB(min, max); + } +} diff --git a/src/Seed.Engine/Physics/Collision/GjkSupport.cs b/src/Seed.Engine/Physics/Collision/GjkSupport.cs index c5a0c2b..7874db1 100644 --- a/src/Seed.Engine/Physics/Collision/GjkSupport.cs +++ b/src/Seed.Engine/Physics/Collision/GjkSupport.cs @@ -23,6 +23,8 @@ public static Result Support( BoxSupport(collider.Data.Box, position, rotation, direction)), ColliderType.Capsule => Result.Ok( CapsuleSupport(collider.Data.Capsule, position, rotation, direction)), + ColliderType.ConvexHull => Result.Ok( + ConvexHullSupport(collider.Data.ConvexHull, position, rotation, direction)), _ => Result.Fail(ErrorCodes.InvalidColliderType), }; } @@ -93,4 +95,17 @@ private static Vector3 CapsuleSupport( } return sphereCenter + direction.Normalize() * capsule.Radius; } + + private static Vector3 ConvexHullSupport( + ConvexHullShape hull, Vector3 position, Quaternion rotation, Vector3 direction) + { + // Transform direction to local space + Vector3 localDir = rotation.Conjugate().RotateVector(direction); + + // Find support in local space + Vector3 localSupport = hull.Support(localDir); + + // Transform back to world space + return position + rotation.RotateVector(localSupport); + } } diff --git a/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs b/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs index 89ff10f..b234b37 100644 --- a/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs +++ b/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs @@ -208,6 +208,10 @@ public static Result RayCollider( ray, position, rotation, collider.Data.Capsule.Radius, collider.Data.Capsule.HalfHeight, out distance, out normal)); + case ColliderType.ConvexHull: + return Result.Ok(RayConvexHull( + ray, position, rotation, collider.Data.ConvexHull, + out distance, out normal)); default: distance = 0f; normal = Vector3.Zero; @@ -267,6 +271,134 @@ private static bool RayCylinder( return true; } + /// + /// Tests a ray against a convex hull using support-function based slab test. + /// + public static bool RayConvexHull( + Ray ray, Vector3 hullCenter, Quaternion hullRotation, + Collision.ConvexHullShape hull, + out float distance, out Vector3 normal) + { + distance = 0f; + normal = Vector3.Zero; + + // Transform ray to hull local space + Quaternion invRot = hullRotation.Conjugate(); + Vector3 localOrigin = invRot.RotateVector(ray.Origin - hullCenter); + Vector3 localDir = invRot.RotateVector(ray.Direction); + + float tmin = 0f; + float tmax = float.MaxValue; + Vector3 hitNormal = Vector3.Zero; + + // Test against 3 principal axis slabs using support function + Vector3 axisX = Vector3.UnitX; + Vector3 axisY = Vector3.UnitY; + Vector3 axisZ = Vector3.UnitZ; + + if (!SlabTest(localOrigin, localDir, axisX, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, axisY, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, axisZ, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + + // Also test diagonal directions for better accuracy + Vector3 diag1 = new Vector3(1f, 1f, 0f).Normalize(); + Vector3 diag2 = new Vector3(1f, 0f, 1f).Normalize(); + Vector3 diag3 = new Vector3(0f, 1f, 1f).Normalize(); + Vector3 diag4 = new Vector3(1f, -1f, 0f).Normalize(); + Vector3 diag5 = new Vector3(1f, 0f, -1f).Normalize(); + Vector3 diag6 = new Vector3(0f, 1f, -1f).Normalize(); + + if (!SlabTest(localOrigin, localDir, diag1, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, diag2, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, diag3, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, diag4, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, diag5, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + if (!SlabTest(localOrigin, localDir, diag6, hull, ref tmin, ref tmax, ref hitNormal)) + { + return false; + } + + if (tmin < 0f) + { + return false; + } + + distance = tmin; + normal = hullRotation.RotateVector(hitNormal); + float normalLen = normal.Length; + if (normalLen > MathHelper.Epsilon) + { + normal = normal / normalLen; + } + + return true; + } + + private static bool SlabTest( + Vector3 localOrigin, Vector3 localDir, Vector3 axis, + Collision.ConvexHullShape hull, + ref float tmin, ref float tmax, ref Vector3 hitNormal) + { + // Project hull onto axis using support + float supportPos = Vector3.Dot(hull.Support(axis), axis); + float supportNeg = Vector3.Dot(hull.Support(-axis), -axis); + float slabMin = -supportNeg; + float slabMax = supportPos; + + float originProj = Vector3.Dot(localOrigin, axis); + float dirProj = Vector3.Dot(localDir, axis); + + if (MathF.Abs(dirProj) < MathHelper.Epsilon) + { + return originProj >= slabMin && originProj <= slabMax; + } + + float invD = 1f / dirProj; + float t1 = (slabMin - originProj) * invD; + float t2 = (slabMax - originProj) * invD; + + Vector3 candidateNormal = -axis; + if (t1 > t2) + { + (t1, t2) = (t2, t1); + candidateNormal = axis; + } + + if (t1 > tmin) + { + tmin = t1; + hitNormal = candidateNormal; + } + + tmax = MathF.Min(tmax, t2); + + return tmin <= tmax; + } + private static float GetComponent(Vector3 v, int axis) { return axis switch From 79e5cc108460f5f8d154b6361e16e3747b167102 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 11:32:35 +0900 Subject: [PATCH 05/14] =?UTF-8?q?CCD=E8=BF=BD=E5=8A=A0=20(=E4=BF=9D?= =?UTF-8?q?=E5=AE=88=E7=9A=84=E3=82=A2=E3=83=89=E3=83=90=E3=83=B3=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88,=20=E4=BA=8C=E5=88=86=E6=8E=A2?= =?UTF-8?q?=E7=B4=A2TOI)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- .../Physics/Ccd/CcdSweepTests.cs | 125 +++++++++++ .../Physics/CcdSystemTests.cs | 117 ++++++++++ src/Seed.Engine/Physics/Ccd/CcdSweep.cs | 127 +++++++++++ src/Seed.Engine/Physics/CcdSystem.cs | 208 ++++++++++++++++++ 4 files changed, 577 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/Ccd/CcdSweepTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/CcdSystemTests.cs create mode 100644 src/Seed.Engine/Physics/Ccd/CcdSweep.cs create mode 100644 src/Seed.Engine/Physics/CcdSystem.cs diff --git a/src/Seed.Engine.Tests/Physics/Ccd/CcdSweepTests.cs b/src/Seed.Engine.Tests/Physics/Ccd/CcdSweepTests.cs new file mode 100644 index 0000000..fcf6d50 --- /dev/null +++ b/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 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 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 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); + } +} diff --git a/src/Seed.Engine.Tests/Physics/CcdSystemTests.cs b/src/Seed.Engine.Tests/Physics/CcdSystemTests.cs new file mode 100644 index 0000000..7710558 --- /dev/null +++ b/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(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + var ccType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType, ccType]; + + Entity entity = world.CreateEntity(entityTypes); + var rb = RigidBody.CreateDynamic(1.0f); + rb.GravityScale = 0f; + world.GetComponent(entity) = rb; + world.GetComponent(entity) = new Velocity + { + Linear = new Vector3(0.1f, 0f, 0f), + }; + world.GetComponent(entity) = + LocalTransform.FromPosition(Vector3.Zero); + world.GetComponent(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(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(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + var ccType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType, ccType]; + + // Fast moving bullet + Entity bullet = world.CreateEntity(entityTypes); + var bulletRb = RigidBody.CreateDynamic(1.0f); + bulletRb.GravityScale = 0f; + world.GetComponent(bullet) = bulletRb; + float speed = 1000f; + world.GetComponent(bullet) = new Velocity + { + Linear = new Vector3(speed, 0f, 0f), + }; + // Position after integration (already moved past the wall) + float displacement = speed * dt; + world.GetComponent(bullet) = + LocalTransform.FromPosition(new Vector3(displacement, 0f, 0f)); + world.GetComponent(bullet) = new ColliderComponent + { + Value = Collider.CreateSphere(0.1f), + }; + + // Static wall + Entity wall = world.CreateEntity(entityTypes); + world.GetComponent(wall) = RigidBody.CreateKinematic(); + world.GetComponent(wall) = new Velocity(); + world.GetComponent(wall) = + LocalTransform.FromPosition(new Vector3(5f, 0f, 0f)); + world.GetComponent(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(bullet); + Assert.True(bulletLt.Position.X < displacement); + } +} diff --git a/src/Seed.Engine/Physics/Ccd/CcdSweep.cs b/src/Seed.Engine/Physics/Ccd/CcdSweep.cs new file mode 100644 index 0000000..b471780 --- /dev/null +++ b/src/Seed.Engine/Physics/Ccd/CcdSweep.cs @@ -0,0 +1,127 @@ +using System; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Physics.Ccd; + +/// +/// Computes the Time of Impact (TOI) for continuous collision detection +/// using conservative advancement with binary search. +/// +public static class CcdSweep +{ + /// Maximum number of binary search iterations for TOI. + public const int MaxIterations = 16; + + /// Number of linear samples to find initial overlap window. + private const int SampleCount = 32; + + /// + /// Determines whether an entity needs CCD based on its displacement + /// relative to its collider size. + /// + public static bool NeedsCcd( + Collider collider, Vector3 position, Quaternion rotation, + Vector3 displacement) + { + float displacementLength = displacement.Length; + if (displacementLength < MathHelper.Epsilon) + { + return false; + } + + Result aabbResult = ColliderBounds.ComputeAABB(collider, position, rotation); + if (aabbResult.IsFailure) + { + return false; + } + + AABB aabb = aabbResult.Value; + Vector3 size = aabb.Max - aabb.Min; + float minExtent = MathF.Min(size.X, MathF.Min(size.Y, size.Z)); + + return displacementLength > minExtent * 0.5f; + } + + /// + /// Computes the time of impact between two colliders given their start positions + /// and displacements over the frame. Returns a TOI in [0, 1] if collision found. + /// First samples the trajectory to find an overlap window, then refines with binary search. + /// + public static Result ComputeToi( + Collider colliderA, Vector3 startPosA, Quaternion rotA, Vector3 displacementA, + Collider colliderB, Vector3 startPosB, Quaternion rotB, Vector3 displacementB) + { + // Phase 1: Linear sampling to find the first overlap window + float firstOverlapT = -1f; + float lastNonOverlapT = 0f; + + for (int i = 0; i <= SampleCount; i++) + { + float t = (float)i / SampleCount; + Vector3 posA = startPosA + displacementA * t; + Vector3 posB = startPosB + displacementB * t; + + Result intersectResult = GjkEpa.Intersects( + colliderA, posA, rotA, + colliderB, posB, rotB); + + if (intersectResult.IsFailure) + { + return Result.Fail(ErrorCodes.CcdSweepFailed); + } + + if (intersectResult.Value) + { + firstOverlapT = t; + break; + } + + lastNonOverlapT = t; + } + + if (firstOverlapT < 0f) + { + return Result.Fail(ErrorCodes.CcdSweepFailed); + } + + // Phase 2: Binary search between lastNonOverlapT and firstOverlapT + float lower = lastNonOverlapT; + float upper = firstOverlapT; + + for (int i = 0; i < MaxIterations; i++) + { + if (upper - lower < MathHelper.Epsilon) + { + break; + } + + float mid = (lower + upper) * 0.5f; + + Vector3 posA = startPosA + displacementA * mid; + Vector3 posB = startPosB + displacementB * mid; + + Result intersectResult = GjkEpa.Intersects( + colliderA, posA, rotA, + colliderB, posB, rotB); + + if (intersectResult.IsFailure) + { + return Result.Fail(ErrorCodes.CcdSweepFailed); + } + + if (intersectResult.Value) + { + upper = mid; + } + else + { + lower = mid; + } + } + + return Result.Ok(lower); + } +} diff --git a/src/Seed.Engine/Physics/CcdSystem.cs b/src/Seed.Engine/Physics/CcdSystem.cs new file mode 100644 index 0000000..9833f9e --- /dev/null +++ b/src/Seed.Engine/Physics/CcdSystem.cs @@ -0,0 +1,208 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Foundation.Memory; +using Seed.Engine.Physics.BroadPhase; +using Seed.Engine.Physics.Ccd; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that performs continuous collision detection for fast-moving bodies. +/// Runs in the Physics frame phase, after IntegrationSystem. +/// Detects tunneling by checking if a body's displacement exceeds its collider size, +/// then sweeps for the time of impact and corrects the position. +/// +public sealed class CcdSystem : ISystem +{ + private readonly QueryDescription _query; + private readonly float _fixedDt; + + /// + /// Initializes a new with the specified fixed time step. + /// + public CcdSystem(float fixedDt) + { + _fixedDt = fixedDt; + _query = new QueryBuilder() + .WithWrite() + .WithWrite() + .WithRead() + .WithRead() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _query; + + /// + public void Execute(World world) + { + int rbTypeId = ComponentRegistry.Get().TypeId; + int velTypeId = ComponentRegistry.Get().TypeId; + int ltTypeId = ComponentRegistry.Get().TypeId; + int ccTypeId = ComponentRegistry.Get().TypeId; + + // Collect fast-moving entities + var fastEntities = new NativeList(32); + var fastPositions = new NativeList(32); + var fastDisplacements = new NativeList(32); + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_query.Matches(storage.Archetype)) + { + continue; + } + + int rbIdx = storage.GetComponentIndex(rbTypeId); + int velIdx = storage.GetComponentIndex(velTypeId); + int ltIdx = storage.GetComponentIndex(ltTypeId); + int ccIdx = storage.GetComponentIndex(ccTypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + int count = chunk.Count; + + for (int i = 0; i < count; i++) + { + ref RigidBody rb = ref chunk.GetComponent(rbIdx, i); + if (rb.IsKinematic) + { + continue; + } + + ref Velocity vel = ref chunk.GetComponent(velIdx, i); + ref LocalTransform lt = ref chunk.GetComponent(ltIdx, i); + ref ColliderComponent cc = ref chunk.GetComponent(ccIdx, i); + + Vector3 displacement = vel.Linear * _fixedDt; + + if (CcdSweep.NeedsCcd(cc.Value, lt.Position, lt.Rotation, displacement)) + { + Entity entity = chunk.GetEntity(i); + // Revert position to start of frame + Vector3 startPos = lt.Position - displacement; + fastEntities.Add(entity); + fastPositions.Add(startPos); + fastDisplacements.Add(displacement); + } + } + } + } + + // For each fast entity, sweep against all other colliders + for (int fi = 0; fi < fastEntities.Length; fi++) + { + Entity fastEntity = fastEntities[fi]; + if (!world.IsAlive(fastEntity)) + { + continue; + } + + ref ColliderComponent fastCc = ref world.GetComponent(fastEntity); + ref LocalTransform fastLt = ref world.GetComponent(fastEntity); + ref Velocity fastVel = ref world.GetComponent(fastEntity); + + Vector3 startPos = fastPositions[fi]; + Vector3 displacement = fastDisplacements[fi]; + + float earliestToi = 1f; + Vector3 contactNormal = Vector3.Zero; + bool found = false; + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_query.Matches(storage.Archetype)) + { + continue; + } + + int ccIdx = storage.GetComponentIndex(ccTypeId); + int ltIdx = storage.GetComponentIndex(ltTypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + int count = chunk.Count; + + for (int i = 0; i < count; i++) + { + Entity other = chunk.GetEntity(i); + if (other.Equals(fastEntity)) + { + continue; + } + + ref ColliderComponent otherCc = + ref chunk.GetComponent(ccIdx, i); + ref LocalTransform otherLt = + ref chunk.GetComponent(ltIdx, i); + + Result toiResult = CcdSweep.ComputeToi( + fastCc.Value, startPos, fastLt.Rotation, displacement, + otherCc.Value, otherLt.Position, otherLt.Rotation, Vector3.Zero); + + if (toiResult.IsSuccess && toiResult.Value < earliestToi) + { + earliestToi = toiResult.Value; + + // Get contact normal at a slightly overlapping position + float contactT = earliestToi + + (1f - earliestToi) * 0.1f; + Vector3 contactPos = startPos + displacement * contactT; + Result contactResult = GjkEpa.ComputeContact( + fastCc.Value, contactPos, fastLt.Rotation, + otherCc.Value, otherLt.Position, otherLt.Rotation, + out ContactPoint cp); + + if (contactResult.IsSuccess && contactResult.Value) + { + contactNormal = cp.Normal; + found = true; + } + else + { + // Use displacement direction as fallback normal + float dispLen = displacement.Length; + if (dispLen > MathHelper.Epsilon) + { + contactNormal = -(displacement / dispLen); + } + found = true; + } + } + } + } + } + + if (found) + { + // Move to TOI position + fastLt.Position = startPos + displacement * earliestToi; + + // Reflect velocity along contact normal + float vn = Vector3.Dot(fastVel.Linear, contactNormal); + if (vn < 0f) + { + fastVel.Linear = fastVel.Linear - contactNormal * (1.5f * vn); + } + } + } + + fastEntities.Dispose(); + fastPositions.Dispose(); + fastDisplacements.Dispose(); + } +} From a90eb1632f58b44eec9452e0f9d50efce77597d4 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 12:09:24 +0900 Subject: [PATCH 06/14] =?UTF-8?q?Vulkan=20Y-flip:=20CreatePerspective?= =?UTF-8?q?=E3=81=AEyScale=E3=82=92=E7=AC=A6=E5=8F=B7=E5=8F=8D=E8=BB=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Vulkan NDCは+Y=画面下のため、射影行列のM22を負にする必要がある。 FrontFace.Clockwiseは既にY-flip前提で設定済みだった。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Foundation/Mathematics/Matrix4x4.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Seed.Engine/Foundation/Mathematics/Matrix4x4.cs b/src/Seed.Engine/Foundation/Mathematics/Matrix4x4.cs index 8e16110..4ab79db 100644 --- a/src/Seed.Engine/Foundation/Mathematics/Matrix4x4.cs +++ b/src/Seed.Engine/Foundation/Mathematics/Matrix4x4.cs @@ -142,8 +142,8 @@ public static Matrix4x4 CreateLookAt(Vector3 eye, Vector3 target, Vector3 up) public static Matrix4x4 CreatePerspective( float fovRadians, float aspectRatio, float near, float far) { - float yScale = 1f / MathF.Tan(fovRadians * 0.5f); - float xScale = yScale / aspectRatio; + float yScale = -1f / MathF.Tan(fovRadians * 0.5f); + float xScale = -yScale / aspectRatio; float range = far / (near - far); return new Matrix4x4( From 278fe173dc84ae16658e3d84fcbcbc4003612f94 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 12:09:30 +0900 Subject: [PATCH 07/14] =?UTF-8?q?=E3=83=87=E3=83=A2=E7=94=A8=E5=8B=95?= =?UTF-8?q?=E7=9A=84=E3=82=AD=E3=83=A5=E3=83=BC=E3=83=96=E8=BF=BD=E5=8A=A0?= =?UTF-8?q?=20(RigidBody+Velocity+IntegrationSystem)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 3つ目のキューブにRigidBody・Velocityコンポーネントを付与し、 IntegrationSystemを登録して重力落下を確認できるようにした。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine.App/Program.cs | 50 +++++++++++++++++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) diff --git a/src/Seed.Engine.App/Program.cs b/src/Seed.Engine.App/Program.cs index ed5a336..45ec08a 100644 --- a/src/Seed.Engine.App/Program.cs +++ b/src/Seed.Engine.App/Program.cs @@ -10,6 +10,7 @@ using Seed.Engine.GameLogic; using Seed.Engine.Graphics; using Seed.Engine.Graphics.Vulkan; +using Seed.Engine.Physics; using Seed.Engine.Platform.Input; using Seed.Engine.Platform.Window; using Seed.Engine.Platform.Window.Glfw; @@ -552,6 +553,7 @@ private static void Main() scheduler.AddSystem(new MovementSystem()); scheduler.AddSystem(new CharacterPhysicsSystem()); + scheduler.AddSystem(new IntegrationSystem(1f / 60f)); scheduler.AddSystem(new TransformSystem()); scheduler.AddSystem(new CameraViewSystem( (float)window.Width / window.Height)); @@ -1218,6 +1220,52 @@ private static void CreateCubeEntity( }; } + private static void CreateDynamicCubeEntity( + World world, int meshId, int materialId, Vector3 position) + { + var ltType = ComponentRegistry.Get(); + var ltwType = ComponentRegistry.Get(); + var meshType = ComponentRegistry.Get(); + var matType = ComponentRegistry.Get(); + var boundsType = ComponentRegistry.Get(); + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + ReadOnlySpan types = + [ltType, ltwType, meshType, matType, boundsType, rbType, velType]; + + Entity entity = world.CreateEntity(types); + world.GetComponent(entity) = + LocalTransform.FromPosition(position); + world.GetComponent(entity) = new LocalToWorld + { + Value = Matrix4x4.Identity, + }; + world.GetComponent(entity) = new MeshReference + { + MeshId = meshId, + }; + world.GetComponent(entity) = new MaterialReference + { + MaterialId = materialId, + }; + world.GetComponent(entity) = new WorldBounds + { + Value = new AABB(position - new Vector3(0.5f, 0.5f, 0.5f), + position + new Vector3(0.5f, 0.5f, 0.5f)), + }; + world.GetComponent(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(entity) = new Velocity(); + } + private static void RunMainLoop( SystemScheduler scheduler, World world, @@ -1238,7 +1286,7 @@ private static void RunMainLoop( new Vector3(0f, 0f, -3f)); CreateCubeEntity(world, cubeMeshId, materialId, new Vector3(2f, 0f, -4f)); - CreateCubeEntity(world, cubeMeshId, materialId, + CreateDynamicCubeEntity(world, cubeMeshId, materialId, new Vector3(-2f, 1f, -5f)); CreatePlayerEntity(world); From 10ca970a0dae585936d7cfabcbb3c37bc0a08cd3 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 12:12:56 +0900 Subject: [PATCH 08/14] =?UTF-8?q?=E8=A1=9D=E7=AA=81=E6=A4=9C=E5=87=BA?= =?UTF-8?q?=E3=83=87=E3=83=A2=E8=BF=BD=E5=8A=A0=20(BroadPhase+NarrowPhase+?= =?UTF-8?q?ConstraintSolver=E7=B5=B1=E5=90=88)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 全キューブにColliderComponent(Box)を付与し、物理シングルトン エンティティ(BroadPhaseResult/NarrowPhaseResult/ConstraintSolverResult)を 作成。静的キューブを落下キューブの直下に配置し衝突・反発を確認可能に。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine.App/Program.cs | 31 ++++++++++++++++++++++++++++--- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/src/Seed.Engine.App/Program.cs b/src/Seed.Engine.App/Program.cs index 45ec08a..58e34f8 100644 --- a/src/Seed.Engine.App/Program.cs +++ b/src/Seed.Engine.App/Program.cs @@ -11,6 +11,7 @@ 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; @@ -553,6 +554,9 @@ private static void Main() scheduler.AddSystem(new MovementSystem()); scheduler.AddSystem(new CharacterPhysicsSystem()); + 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( @@ -1196,7 +1200,9 @@ private static void CreateCubeEntity( var meshType = ComponentRegistry.Get(); var matType = ComponentRegistry.Get(); var boundsType = ComponentRegistry.Get(); - ReadOnlySpan types = [ltType, ltwType, meshType, matType, boundsType]; + var ccType = ComponentRegistry.Get(); + ReadOnlySpan types = + [ltType, ltwType, meshType, matType, boundsType, ccType]; Entity entity = world.CreateEntity(types); world.GetComponent(entity) = @@ -1218,6 +1224,10 @@ 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(entity) = new ColliderComponent + { + Value = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)), + }; } private static void CreateDynamicCubeEntity( @@ -1228,10 +1238,11 @@ private static void CreateDynamicCubeEntity( var meshType = ComponentRegistry.Get(); var matType = ComponentRegistry.Get(); var boundsType = ComponentRegistry.Get(); + var ccType = ComponentRegistry.Get(); var rbType = ComponentRegistry.Get(); var velType = ComponentRegistry.Get(); ReadOnlySpan types = - [ltType, ltwType, meshType, matType, boundsType, rbType, velType]; + [ltType, ltwType, meshType, matType, boundsType, ccType, rbType, velType]; Entity entity = world.CreateEntity(types); world.GetComponent(entity) = @@ -1253,6 +1264,10 @@ private static void CreateDynamicCubeEntity( Value = new AABB(position - new Vector3(0.5f, 0.5f, 0.5f), position + new Vector3(0.5f, 0.5f, 0.5f)), }; + world.GetComponent(entity) = new ColliderComponent + { + Value = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)), + }; world.GetComponent(entity) = new RigidBody { Mass = 1.0f, @@ -1280,10 +1295,11 @@ 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, -2f, -5f)); CreateCubeEntity(world, cubeMeshId, materialId, new Vector3(2f, 0f, -4f)); CreateDynamicCubeEntity(world, cubeMeshId, materialId, @@ -1333,6 +1349,15 @@ private static void CreateSingletonEntity( } } + private static void CreatePhysicsSingletonEntity(World world) + { + var bpType = ComponentRegistry.Get(); + var npType = ComponentRegistry.Get(); + var csType = ComponentRegistry.Get(); + ReadOnlySpan types = [bpType, npType, csType]; + world.CreateEntity(types); + } + private static void CreatePlatformTagEntity( World world, RenderMode renderMode) { From 9440191cce8495f8d150b3f23a90ca25c403322a Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 13:01:06 +0900 Subject: [PATCH 09/14] =?UTF-8?q?=E7=89=A9=E7=90=86=E3=83=91=E3=82=A4?= =?UTF-8?q?=E3=83=97=E3=83=A9=E3=82=A4=E3=83=B3=E5=88=86=E5=89=B2:=20?= =?UTF-8?q?=E5=8A=9B=E7=A9=8D=E5=88=86=E3=81=A8=E4=BD=8D=E7=BD=AE=E7=A9=8D?= =?UTF-8?q?=E5=88=86=E3=82=92=E5=88=86=E9=9B=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit IntegrationSystemから重力・減衰の適用を分離し、 ForceIntegrationSystem (力→速度) と IntegrationSystem (速度→位置) に分割。 ConstraintSolver前後で正しいセミインプリシットオイラー積分順序を実現。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine.App/Program.cs | 1 + .../Physics/IntegrationSystemTests.cs | 46 ++++++++++- .../Physics/ForceIntegrationSystem.cs | 78 +++++++++++++++++++ .../Physics/Integration/Integrator.cs | 34 ++++++++ src/Seed.Engine/Physics/IntegrationSystem.cs | 12 ++- 5 files changed, 160 insertions(+), 11 deletions(-) create mode 100644 src/Seed.Engine/Physics/ForceIntegrationSystem.cs diff --git a/src/Seed.Engine.App/Program.cs b/src/Seed.Engine.App/Program.cs index 58e34f8..ebf39dd 100644 --- a/src/Seed.Engine.App/Program.cs +++ b/src/Seed.Engine.App/Program.cs @@ -554,6 +554,7 @@ 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)); diff --git a/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs b/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs index f23fd17..81c90a5 100644 --- a/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs +++ b/src/Seed.Engine.Tests/Physics/IntegrationSystemTests.cs @@ -22,7 +22,7 @@ public void Phase_IsPhysics() } [Fact] - public void Execute_DynamicBody_FallsUnderGravity() + public void Execute_WithVelocity_UpdatesPosition() { // Given var world = new World(); @@ -35,7 +35,10 @@ public void Execute_DynamicBody_FallsUnderGravity() Entity entity = world.CreateEntity(entityTypes); world.GetComponent(entity) = RigidBody.CreateDynamic(1.0f); - world.GetComponent(entity) = new Velocity(); + world.GetComponent(entity) = new Velocity + { + Linear = new Vector3(0f, -5f, 0f), + }; world.GetComponent(entity) = LocalTransform.FromPosition(new Vector3(0f, 10f, 0f)); @@ -44,12 +47,13 @@ public void Execute_DynamicBody_FallsUnderGravity() // When system.Execute(world); - // Then + // Then: position should move down based on velocity ref LocalTransform lt = ref world.GetComponent(entity); Assert.True(lt.Position.Y < 10f); + // Velocity should NOT change (gravity is handled by ForceIntegrationSystem) ref Velocity vel = ref world.GetComponent(entity); - Assert.True(vel.Linear.Y < 0f); + AssertHelper.ApproximatelyEqual(-5f, vel.Linear.Y, 0.01f); } [Fact] @@ -116,4 +120,38 @@ public void Execute_WithLinearVelocity_MovesPosition() ref LocalTransform lt = ref world.GetComponent(entity); AssertHelper.ApproximatelyEqual(1f, lt.Position.X, 0.01f); } + + [Fact] + public void FullPipeline_ForcesThenPosition_FallsUnderGravity() + { + // Given + var world = new World(); + using var _ = world; + + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [rbType, velType, ltType]; + + Entity entity = world.CreateEntity(entityTypes); + world.GetComponent(entity) = RigidBody.CreateDynamic(1.0f); + world.GetComponent(entity) = new Velocity(); + world.GetComponent(entity) = + LocalTransform.FromPosition(new Vector3(0f, 10f, 0f)); + + float dt = 1f / 60f; + var forceSystem = new ForceIntegrationSystem(dt); + var positionSystem = new IntegrationSystem(dt); + + // When: run forces then position (standard physics pipeline order) + forceSystem.Execute(world); + positionSystem.Execute(world); + + // Then + ref Velocity vel = ref world.GetComponent(entity); + Assert.True(vel.Linear.Y < 0f); + + ref LocalTransform lt = ref world.GetComponent(entity); + Assert.True(lt.Position.Y < 10f); + } } diff --git a/src/Seed.Engine/Physics/ForceIntegrationSystem.cs b/src/Seed.Engine/Physics/ForceIntegrationSystem.cs new file mode 100644 index 0000000..5fc5177 --- /dev/null +++ b/src/Seed.Engine/Physics/ForceIntegrationSystem.cs @@ -0,0 +1,78 @@ +using Seed.Engine.Ecs; +using Seed.Engine.Ecs.Components; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Integration; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that applies gravity and damping to velocity. +/// Runs in the Physics frame phase, before collision detection. +/// +public sealed class ForceIntegrationSystem : ISystem +{ + private readonly QueryDescription _query; + private readonly float _fixedDt; + + /// + /// Initializes a new with the specified fixed time step. + /// + public ForceIntegrationSystem(float fixedDt) + { + _fixedDt = fixedDt; + _query = new QueryBuilder() + .WithWrite() + .WithRead() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _query; + + /// + public void Execute(World world) + { + int rbTypeId = ComponentRegistry.Get().TypeId; + int velTypeId = ComponentRegistry.Get().TypeId; + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_query.Matches(storage.Archetype)) + { + continue; + } + + int rbIdx = storage.GetComponentIndex(rbTypeId); + int velIdx = storage.GetComponentIndex(velTypeId); + + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + int count = chunk.Count; + + for (int i = 0; i < count; i++) + { + ref RigidBody rb = ref chunk.GetComponent(rbIdx, i); + ref Velocity vel = ref chunk.GetComponent(velIdx, i); + + if (rb.IsKinematic) + { + continue; + } + + Vector3 linearVel = vel.Linear; + Integrator.ApplyLinearForces( + ref linearVel, + rb.InverseMass, rb.GravityScale, rb.LinearDamping, + Integrator.DefaultGravity, _fixedDt); + + vel.Linear = linearVel; + } + } + } + } +} diff --git a/src/Seed.Engine/Physics/Integration/Integrator.cs b/src/Seed.Engine/Physics/Integration/Integrator.cs index f2d9b89..a4ade77 100644 --- a/src/Seed.Engine/Physics/Integration/Integrator.cs +++ b/src/Seed.Engine/Physics/Integration/Integrator.cs @@ -33,6 +33,40 @@ public static void IntegrateLinear( position = position + linearVelocity * dt; } + /// + /// Applies gravity and damping to linear velocity without updating position. + /// + public static void ApplyLinearForces( + ref Vector3 linearVelocity, + float inverseMass, float gravityScale, float linearDamping, + Vector3 gravity, float dt) + { + if (inverseMass <= 0f) + { + return; + } + + linearVelocity = linearVelocity + gravity * gravityScale * dt; + + float dampingFactor = MathF.Max(0f, 1f - linearDamping * dt); + linearVelocity = linearVelocity * dampingFactor; + } + + /// + /// Updates position from the current linear velocity. + /// + public static void IntegrateLinearPosition( + ref Vector3 position, Vector3 linearVelocity, + float inverseMass, float dt) + { + if (inverseMass <= 0f) + { + return; + } + + position = position + linearVelocity * dt; + } + /// /// Integrates angular velocity: applies damping, freeze mask, then updates rotation. /// diff --git a/src/Seed.Engine/Physics/IntegrationSystem.cs b/src/Seed.Engine/Physics/IntegrationSystem.cs index a14ad3c..85696a0 100644 --- a/src/Seed.Engine/Physics/IntegrationSystem.cs +++ b/src/Seed.Engine/Physics/IntegrationSystem.cs @@ -6,8 +6,9 @@ namespace Seed.Engine.Physics; /// -/// ECS system that integrates velocity into position and rotation for all rigid bodies. +/// ECS system that integrates solved velocity into position and rotation. /// Runs in the Physics frame phase, after ConstraintSolverSystem. +/// Gravity and damping are applied separately by . /// public sealed class IntegrationSystem : ISystem { @@ -69,11 +70,9 @@ public void Execute(World world) } Vector3 position = lt.Position; - Vector3 linearVel = vel.Linear; - Integrator.IntegrateLinear( - ref position, ref linearVel, - rb.InverseMass, rb.GravityScale, rb.LinearDamping, - Integrator.DefaultGravity, _fixedDt); + Integrator.IntegrateLinearPosition( + ref position, vel.Linear, + rb.InverseMass, _fixedDt); Quaternion rotation = lt.Rotation; Vector3 angularVel = vel.Angular; @@ -81,7 +80,6 @@ public void Execute(World world) ref rotation, ref angularVel, rb.AngularDamping, rb.FreezeRotation, _fixedDt); - vel.Linear = linearVel; vel.Angular = angularVel; lt.Position = position; lt.Rotation = rotation; From ad1a3598277cbf1107a49b21dc21271b0894c4aa Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 13:01:19 +0900 Subject: [PATCH 10/14] =?UTF-8?q?=E7=89=A9=E7=90=86=E3=82=B7=E3=82=B9?= =?UTF-8?q?=E3=83=86=E3=83=A0=E3=81=AE=E3=82=B9=E3=82=B1=E3=82=B8=E3=83=A5?= =?UTF-8?q?=E3=83=BC=E3=83=AA=E3=83=B3=E3=82=B0=E3=82=AF=E3=82=A8=E3=83=AA?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BroadPhase/NarrowPhase/ConstraintSolverの各システムに 全コンポーネント依存を含むスケジューリングクエリを追加。 SystemSchedulerのバッチ構築で正しい実行順序を保証。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Physics/BroadPhaseSystem.cs | 9 ++++++++- src/Seed.Engine/Physics/ConstraintSolverSystem.cs | 12 +++++++++++- src/Seed.Engine/Physics/NarrowPhaseSystem.cs | 10 +++++++++- src/Seed.Engine/Physics/Solver/PgsSolver.cs | 2 +- 4 files changed, 29 insertions(+), 4 deletions(-) diff --git a/src/Seed.Engine/Physics/BroadPhaseSystem.cs b/src/Seed.Engine/Physics/BroadPhaseSystem.cs index 704761e..acc7ebb 100644 --- a/src/Seed.Engine/Physics/BroadPhaseSystem.cs +++ b/src/Seed.Engine/Physics/BroadPhaseSystem.cs @@ -21,6 +21,7 @@ public sealed class BroadPhaseSystem : ISystem private const int StackAllocThreshold = 256; private readonly QueryDescription _colliderQuery; private readonly QueryDescription _resultQuery; + private readonly QueryDescription _schedulingQuery; private BvhTree _bvh; private NativeList _pairs; private bool _initialized; @@ -38,13 +39,19 @@ public BroadPhaseSystem() _resultQuery = new QueryBuilder() .WithWrite() .Build(); + + _schedulingQuery = new QueryBuilder() + .WithRead() + .WithRead() + .WithWrite() + .Build(); } /// public FramePhase Phase => FramePhase.Physics; /// - public QueryDescription GetQuery() => _colliderQuery; + public QueryDescription GetQuery() => _schedulingQuery; /// public unsafe void Execute(World world) diff --git a/src/Seed.Engine/Physics/ConstraintSolverSystem.cs b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs index ee6db6c..f8c46f5 100644 --- a/src/Seed.Engine/Physics/ConstraintSolverSystem.cs +++ b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs @@ -23,6 +23,7 @@ public sealed class ConstraintSolverSystem : ISystem private readonly QueryDescription _narrowPhaseQuery; private readonly QueryDescription _rigidBodyQuery; private readonly QueryDescription _solverResultQuery; + private readonly QueryDescription _schedulingQuery; private readonly float _fixedDt; private NativeList _constraints; private bool _initialized; @@ -47,13 +48,21 @@ public ConstraintSolverSystem(float fixedDt) _solverResultQuery = new QueryBuilder() .WithWrite() .Build(); + + _schedulingQuery = new QueryBuilder() + .WithWrite() + .WithRead() + .WithRead() + .WithRead() + .WithWrite() + .Build(); } /// public FramePhase Phase => FramePhase.Physics; /// - public QueryDescription GetQuery() => _rigidBodyQuery; + public QueryDescription GetQuery() => _schedulingQuery; /// public unsafe void Execute(World world) @@ -179,6 +188,7 @@ public unsafe void Execute(World world) vel.Angular = angularVels[i]; } } + } linearVels.Dispose(); diff --git a/src/Seed.Engine/Physics/NarrowPhaseSystem.cs b/src/Seed.Engine/Physics/NarrowPhaseSystem.cs index de722b9..be99242 100644 --- a/src/Seed.Engine/Physics/NarrowPhaseSystem.cs +++ b/src/Seed.Engine/Physics/NarrowPhaseSystem.cs @@ -17,6 +17,7 @@ public sealed class NarrowPhaseSystem : ISystem private readonly QueryDescription _broadPhaseQuery; private readonly QueryDescription _colliderQuery; private readonly QueryDescription _narrowPhaseQuery; + private readonly QueryDescription _schedulingQuery; private NativeList _contacts; private NativeList _contactPairs; private bool _initialized; @@ -38,13 +39,20 @@ public NarrowPhaseSystem() _narrowPhaseQuery = new QueryBuilder() .WithWrite() .Build(); + + _schedulingQuery = new QueryBuilder() + .WithRead() + .WithRead() + .WithRead() + .WithWrite() + .Build(); } /// public FramePhase Phase => FramePhase.Physics; /// - public QueryDescription GetQuery() => _colliderQuery; + public QueryDescription GetQuery() => _schedulingQuery; /// public unsafe void Execute(World world) diff --git a/src/Seed.Engine/Physics/Solver/PgsSolver.cs b/src/Seed.Engine/Physics/Solver/PgsSolver.cs index 74d78af..c510c8c 100644 --- a/src/Seed.Engine/Physics/Solver/PgsSolver.cs +++ b/src/Seed.Engine/Physics/Solver/PgsSolver.cs @@ -156,7 +156,7 @@ private static void SolveNormal( baumgarte = BaumgarteBeta / dt * (c.PenetrationDepth - PenetrationSlop); } - // Restitution bias + // Restitution bias (skip for slow contacts to avoid jitter) float restitutionBias = 0f; if (vn < -1f) { From f8133ecf2600f29fcc74323f43e32817286ef6d3 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 13:01:31 +0900 Subject: [PATCH 11/14] =?UTF-8?q?EPA=E6=8E=A5=E8=A7=A6=E6=B3=95=E7=B7=9A?= =?UTF-8?q?=E3=81=AE=E6=96=B9=E5=90=91=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit EPAの面法線はMinkowski差分ポリトープの外向きだが、 ContactPointの規約 (BからAへ向く) とは逆方向だった。 法線が反転していたためソルバーが分離中と誤判定し、 衝突応答が効かず物体が貫通していた。 Co-Authored-By: Claude Opus 4.6 --- .../Physics/Collision/GjkEpaTests.cs | 93 +++++++++++++++++++ src/Seed.Engine/Physics/Collision/GjkEpa.cs | 2 +- 2 files changed, 94 insertions(+), 1 deletion(-) diff --git a/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs index 1d206f8..28e5fc2 100644 --- a/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs +++ b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs @@ -181,4 +181,97 @@ public void Intersects_SamePosition_ReturnsTrue() Assert.True(resultWrap.IsSuccess); Assert.True(resultWrap.Value); } + + [Fact] + public void ComputeContact_OverlappingBoxes_NormalPointsFromBToA() + { + // Given: A is above B, overlapping by 0.2 on Y axis + var a = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var b = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var posA = new Vector3(0f, 0.8f, 0f); + var posB = new Vector3(0f, 0f, 0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, posA, Quaternion.Identity, + b, posB, Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + Assert.True(contact.PenetrationDepth > 0f); + // Normal should point from B towards A (upward, positive Y) + Assert.True(contact.Normal.Y > 0.9f, + $"Expected normal Y > 0.9 (from B to A), got {contact.Normal.Y}"); + } + + [Fact] + public void ComputeContact_OverlappingBoxes_ReversePairOrder_NormalPointsFromBToA() + { + // Given: A is below B (reversed pair order) + var a = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var b = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var posA = new Vector3(0f, 0f, 0f); + var posB = new Vector3(0f, 0.8f, 0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, posA, Quaternion.Identity, + b, posB, Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + Assert.True(contact.PenetrationDepth > 0f); + // Normal should point from B towards A (downward, negative Y) + Assert.True(contact.Normal.Y < -0.9f, + $"Expected normal Y < -0.9 (from B to A), got {contact.Normal.Y}"); + } + + [Fact] + public void ComputeContact_OverlappingSpheres_NormalPointsFromBToA() + { + // Given: A is to the right of B, overlapping + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + var posA = new Vector3(1.5f, 0f, 0f); + var posB = new Vector3(0f, 0f, 0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, posA, Quaternion.Identity, + b, posB, Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + Assert.True(contact.PenetrationDepth > 0f); + // Normal should point from B towards A (positive X) + Assert.True(contact.Normal.X > 0.9f, + $"Expected normal X > 0.9 (from B to A), got {contact.Normal.X}"); + } + + [Fact] + public void ComputeContact_OverlappingBoxes_PenetrationDepthCorrect() + { + // Given: boxes at distance 0.8, each has half-extent 0.5 → overlap = 0.2 + var a = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var b = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var posA = new Vector3(0f, 0.8f, 0f); + var posB = new Vector3(0f, 0f, 0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, posA, Quaternion.Identity, + b, posB, Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + AssertHelper.ApproximatelyEqual(0.2f, contact.PenetrationDepth, 0.01f); + } } diff --git a/src/Seed.Engine/Physics/Collision/GjkEpa.cs b/src/Seed.Engine/Physics/Collision/GjkEpa.cs index 77ad635..9e7a083 100644 --- a/src/Seed.Engine/Physics/Collision/GjkEpa.cs +++ b/src/Seed.Engine/Physics/Collision/GjkEpa.cs @@ -357,7 +357,7 @@ private static Result RunEpa( contact = new ContactPoint( pointOnAResult.Value, pointOnBResult.Value, - contactNormal, penetrationDepth); + -contactNormal, penetrationDepth); vertices.Dispose(); faces.Dispose(); return Result.Ok(true); From 0b77322cc98898a09074785ea550e949804e0234 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 14:27:18 +0900 Subject: [PATCH 12/14] =?UTF-8?q?PGS=E3=82=BD=E3=83=AB=E3=83=90=E3=83=BC?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3:=20=E3=83=9D=E3=82=A4=E3=83=B3=E3=83=88?= =?UTF-8?q?=E3=83=9E=E3=82=B9=E8=BF=91=E4=BC=BC=20+=20=E4=BD=8D=E7=BD=AE?= =?UTF-8?q?=E8=A3=9C=E6=AD=A3=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 慣性テンソル未実装下でソルバーが正しく動作するよう修正: - ComputeEffectiveMass: 角速度項を除去 - ComputeRelativeVelocity: 線速度のみ使用 - ApplyImpulse: 角速度変更を除去 - ConstraintSolverSystemに直接位置補正を追加 - DefaultRestitution: 0.3→0.0 (ポイントマス近似との整合性) Co-Authored-By: Claude Opus 4.6 --- .../Physics/ConstraintSolverSystem.cs | 38 ++++++++++++++++++- src/Seed.Engine/Physics/Solver/PgsSolver.cs | 23 ++++------- 2 files changed, 45 insertions(+), 16 deletions(-) diff --git a/src/Seed.Engine/Physics/ConstraintSolverSystem.cs b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs index f8c46f5..fbcc5e8 100644 --- a/src/Seed.Engine/Physics/ConstraintSolverSystem.cs +++ b/src/Seed.Engine/Physics/ConstraintSolverSystem.cs @@ -17,8 +17,10 @@ namespace Seed.Engine.Physics; /// public sealed class ConstraintSolverSystem : ISystem { - private const float DefaultRestitution = 0.3f; + private const float DefaultRestitution = 0.0f; private const float DefaultFriction = 0.5f; + private const float PositionBeta = 0.4f; + private const float PositionSlop = 0.005f; private readonly QueryDescription _narrowPhaseQuery; private readonly QueryDescription _rigidBodyQuery; @@ -189,6 +191,40 @@ public unsafe void Execute(World world) } } + // Position correction: directly resolve penetration + for (int i = 0; i < _constraints.Length; i++) + { + ref ContactConstraint c = ref _constraints.AsSpan()[i]; + float correction = MathF.Max(0f, c.PenetrationDepth - PositionSlop) * PositionBeta; + if (correction <= 0f) + { + continue; + } + + int idxA2 = entityIndexA[i]; + int idxB2 = entityIndexB[i]; + float totalInvMass = invMasses[idxA2] + invMasses[idxB2]; + if (totalInvMass <= 0f) + { + continue; + } + + Vector3 posCorrection = c.Normal * (correction / totalInvMass); + + Entity eA = c.EntityA; + if (world.IsAlive(eA) && world.HasComponent(eA) && invMasses[idxA2] > 0f) + { + ref LocalTransform ltA = ref world.GetComponent(eA); + ltA.Position = ltA.Position + posCorrection * invMasses[idxA2]; + } + + Entity eB = c.EntityB; + if (world.IsAlive(eB) && world.HasComponent(eB) && invMasses[idxB2] > 0f) + { + ref LocalTransform ltB = ref world.GetComponent(eB); + ltB.Position = ltB.Position - posCorrection * invMasses[idxB2]; + } + } } linearVels.Dispose(); diff --git a/src/Seed.Engine/Physics/Solver/PgsSolver.cs b/src/Seed.Engine/Physics/Solver/PgsSolver.cs index c510c8c..17e2e02 100644 --- a/src/Seed.Engine/Physics/Solver/PgsSolver.cs +++ b/src/Seed.Engine/Physics/Solver/PgsSolver.cs @@ -49,20 +49,14 @@ public static void ComputeTangents(Vector3 normal, out Vector3 tangent1, out Vec /// /// Computes the effective mass for a contact constraint direction. + /// Uses point-mass approximation (no angular inertia) until a proper + /// inertia tensor is implemented. /// public static float ComputeEffectiveMass( float inverseMassA, float inverseMassB, Vector3 rA, Vector3 rB, Vector3 direction) { - // For point masses (ignoring inertia tensor): - // effectiveMass = 1 / (invMassA + invMassB + cross(rA, n)^2 * invIA + cross(rB, n)^2 * invIB) - // Simplified: using unit inertia approximation - Vector3 crossA = Vector3.Cross(rA, direction); - Vector3 crossB = Vector3.Cross(rB, direction); - - float invMassSum = inverseMassA + inverseMassB - + Vector3.Dot(crossA, crossA) * inverseMassA - + Vector3.Dot(crossB, crossB) * inverseMassB; + float invMassSum = inverseMassA + inverseMassB; if (invMassSum < MathHelper.Epsilon) { @@ -224,9 +218,9 @@ private static Vector3 ComputeRelativeVelocity( Vector3 linVelA, Vector3 angVelA, Vector3 rA, Vector3 linVelB, Vector3 angVelB, Vector3 rB) { - Vector3 velA = linVelA + Vector3.Cross(angVelA, rA); - Vector3 velB = linVelB + Vector3.Cross(angVelB, rB); - return velA - velB; + // Point-mass approximation: ignore angular velocity contribution + // until a proper inertia tensor is implemented. + return linVelA - linVelB; } private static void ApplyImpulse( @@ -236,10 +230,9 @@ private static void ApplyImpulse( { Vector3 impulse = direction * lambda; + // Point-mass approximation: only modify linear velocity. + // Angular impulse requires a proper inertia tensor. linVelA = linVelA + impulse * invMassA; - angVelA = angVelA + Vector3.Cross(rA, impulse) * invMassA; - linVelB = linVelB - impulse * invMassB; - angVelB = angVelB - Vector3.Cross(rB, impulse) * invMassB; } } From de72be043bc2a35e9b388357c4f89bf4b63a5312 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 14:27:25 +0900 Subject: [PATCH 13/14] =?UTF-8?q?=E7=89=A9=E7=90=86=E3=83=86=E3=82=B9?= =?UTF-8?q?=E3=83=88=E8=BF=BD=E5=8A=A0:=20=E9=9D=9E=E5=AF=BE=E7=A7=B0?= =?UTF-8?q?=E3=83=9C=E3=83=83=E3=82=AF=E3=82=B9=E8=A1=9D=E7=AA=81=20+=20?= =?UTF-8?q?=E3=83=91=E3=82=A4=E3=83=97=E3=83=A9=E3=82=A4=E3=83=B3=E7=B5=B1?= =?UTF-8?q?=E5=90=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit GjkEpaTests: 小ボックスが大プラットフォームに衝突するケース7件 PhysicsPipelineTests: BroadPhase→NarrowPhase→Solver→Integrationの エンドツーエンドテスト6件 (落下・停止・衝突検出・ソルバー単体) Co-Authored-By: Claude Opus 4.6 --- .../Physics/Collision/GjkEpaTests.cs | 136 ++++++ .../Physics/PhysicsPipelineTests.cs | 398 ++++++++++++++++++ 2 files changed, 534 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/PhysicsPipelineTests.cs diff --git a/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs index 28e5fc2..eba0f6d 100644 --- a/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs +++ b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs @@ -274,4 +274,140 @@ public void ComputeContact_OverlappingBoxes_PenetrationDepthCorrect() Assert.True(resultWrap.Value); AssertHelper.ApproximatelyEqual(0.2f, contact.PenetrationDepth, 0.01f); } + + [Fact] + public void Intersects_AsymmetricBoxes_SmallOnLarge_ReturnsTrue() + { + // Given: small box (0.5) sitting on large platform (2, 0.5, 2) + // Small box bottom at Y=-2.0, platform top at Y=-2.0 → touching + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posSmall = new Vector3(-2f, -1.5f, -5f); + var posLarge = new Vector3(-2f, -2.5f, -5f); + + // When + Result resultWrap = GjkEpa.Intersects( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity); + + // Then: boundary case, should detect intersection + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void Intersects_AsymmetricBoxes_SlightPenetration_ReturnsTrue() + { + // Given: small box slightly penetrating the large platform + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posSmall = new Vector3(-2f, -1.535f, -5f); // bottom at -2.035 + var posLarge = new Vector3(-2f, -2.5f, -5f); // top at -2.0 + + // When + Result resultWrap = GjkEpa.Intersects( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void ComputeContact_AsymmetricBoxes_SlightPenetration_CorrectNormalAndDepth() + { + // Given: small box slightly penetrating the large platform + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posSmall = new Vector3(-2f, -1.535f, -5f); // bottom at -2.035 + var posLarge = new Vector3(-2f, -2.5f, -5f); // top at -2.0 + + // When + Result resultWrap = GjkEpa.ComputeContact( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + // Penetration ≈ 0.035 (bottom of small at -2.035, top of large at -2.0) + AssertHelper.ApproximatelyEqual(0.035f, contact.PenetrationDepth, 0.01f); + // Normal should point from large (B) towards small (A) → upward + Assert.True(contact.Normal.Y > 0.9f, + $"Expected normal Y > 0.9 (upward), got {contact.Normal.Y}"); + } + + [Fact] + public void ComputeContact_AsymmetricBoxes_CenteredXZ_CorrectContact() + { + // Given: boxes perfectly aligned in X/Z (worst case for GJK initial direction) + // small bottom = -1.55 - 0.5 = -2.05, large top = -2.5 + 0.5 = -2.0 → overlap 0.05 + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posSmall = new Vector3(0f, -1.55f, 0f); + var posLarge = new Vector3(0f, -2.5f, 0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity, + out ContactPoint contact); + + // Then: overlap of 0.05 + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + AssertHelper.ApproximatelyEqual(0.05f, contact.PenetrationDepth, 0.01f); + Assert.True(contact.Normal.Y > 0.9f, + $"Expected normal Y > 0.9 (upward), got {contact.Normal.Y}"); + } + + [Fact] + public void Intersects_AsymmetricBoxes_BarelyOverlapping_ReturnsTrue() + { + // Given: small box barely overlapping the large platform (1mm penetration) + // small bottom = -1.501 - 0.5 = -2.001, large top = -2.0 → overlap 0.001 + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posSmall = new Vector3(-2f, -1.501f, -5f); + var posLarge = new Vector3(-2f, -2.5f, -5f); + + // When + Result resultWrap = GjkEpa.Intersects( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void ComputeContact_AsymmetricBoxes_MultiplePenetrationDepths() + { + // Test multiple penetration depths to verify consistency + var small = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)); + var large = Collider.CreateBox(new Vector3(2f, 0.5f, 2f)); + var posLarge = new Vector3(-2f, -2.5f, -5f); // top at -2.0 + + float[] yPositions = [-1.51f, -1.55f, -1.6f, -1.8f, -2.0f]; + foreach (float y in yPositions) + { + var posSmall = new Vector3(-2f, y, -5f); + float expectedDepth = (-2.0f) - (y - 0.5f); // platform top - small bottom + + Result resultWrap = GjkEpa.ComputeContact( + small, posSmall, Quaternion.Identity, + large, posLarge, Quaternion.Identity, + out ContactPoint contact); + + Assert.True(resultWrap.IsSuccess, $"Failed at y={y}"); + Assert.True(resultWrap.Value, $"No collision at y={y}, expected depth={expectedDepth}"); + Assert.True(contact.PenetrationDepth > 0f, + $"Zero depth at y={y}, expected {expectedDepth}"); + Assert.True(contact.Normal.Y > 0.5f, + $"Wrong normal at y={y}: Y={contact.Normal.Y}"); + } + } } diff --git a/src/Seed.Engine.Tests/Physics/PhysicsPipelineTests.cs b/src/Seed.Engine.Tests/Physics/PhysicsPipelineTests.cs new file mode 100644 index 0000000..2edb6bb --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/PhysicsPipelineTests.cs @@ -0,0 +1,398 @@ +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; + +namespace Seed.Engine.Tests.Physics; + +public class PhysicsPipelineTests +{ + private const float Dt = 1f / 60f; + + /// + /// Creates the physics singleton entity with BroadPhaseResult, NarrowPhaseResult, + /// and ConstraintSolverResult components. + /// + private static void CreatePhysicsSingleton(World world) + { + var bpType = ComponentRegistry.Get(); + var npType = ComponentRegistry.Get(); + var csType = ComponentRegistry.Get(); + ReadOnlySpan types = [bpType, npType, csType]; + world.CreateEntity(types); + } + + /// + /// Creates a static box entity with collider and transform. + /// + private static Entity CreateStaticBox( + World world, Vector3 position, Vector3 halfExtents) + { + var ltType = ComponentRegistry.Get(); + var ccType = ComponentRegistry.Get(); + ReadOnlySpan types = [ltType, ccType]; + + Entity entity = world.CreateEntity(types); + world.GetComponent(entity) = + LocalTransform.FromPosition(position); + world.GetComponent(entity) = new ColliderComponent + { + Value = Collider.CreateBox(halfExtents), + }; + return entity; + } + + /// + /// Creates a dynamic box entity with collider, transform, rigidbody, and velocity. + /// + private static Entity CreateDynamicBox( + World world, Vector3 position, Vector3 halfExtents, float mass = 1.0f) + { + var ltType = ComponentRegistry.Get(); + var ccType = ComponentRegistry.Get(); + var rbType = ComponentRegistry.Get(); + var velType = ComponentRegistry.Get(); + ReadOnlySpan types = [ltType, ccType, rbType, velType]; + + Entity entity = world.CreateEntity(types); + world.GetComponent(entity) = + LocalTransform.FromPosition(position); + world.GetComponent(entity) = new ColliderComponent + { + Value = Collider.CreateBox(halfExtents), + }; + world.GetComponent(entity) = new RigidBody + { + Mass = mass, + InverseMass = 1f / mass, + LinearDamping = 0.01f, + AngularDamping = 0.05f, + GravityScale = 1.0f, + IsKinematic = false, + FreezeRotation = new Vector3(0f, 0f, 0f), + }; + world.GetComponent(entity) = new Velocity(); + return entity; + } + + /// + /// Runs one physics frame: ForceIntegration → BroadPhase → NarrowPhase → Solver → Integration. + /// + private static void RunPhysicsFrame( + World world, + ForceIntegrationSystem forceSystem, + BroadPhaseSystem broadPhase, + NarrowPhaseSystem narrowPhase, + ConstraintSolverSystem solver, + IntegrationSystem integration) + { + forceSystem.Execute(world); + broadPhase.Execute(world); + narrowPhase.Execute(world); + solver.Execute(world); + integration.Execute(world); + } + + [Fact] + public void DynamicCube_FallsOntoLargePlatform_StopsAbovePlatform() + { + // Given: dynamic cube above a large static platform + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + // Large platform: top at Y = -2.5 + 0.5 = -2.0 + CreateStaticBox(world, + new Vector3(-2f, -2.5f, -5f), + new Vector3(2f, 0.5f, 2f)); + + // Dynamic cube: bottom at Y = 1 - 0.5 = 0.5 (far above platform) + Entity dynamicCube = CreateDynamicBox(world, + new Vector3(-2f, 1f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + var forceSystem = new ForceIntegrationSystem(Dt); + var broadPhase = new BroadPhaseSystem(); + var narrowPhase = new NarrowPhaseSystem(); + var solver = new ConstraintSolverSystem(Dt); + var integration = new IntegrationSystem(Dt); + + // When: run 300 frames (5 seconds at 60fps) + float platformTop = -2.0f; + float cubeHalfY = 0.5f; + bool everCollided = false; + + for (int frame = 0; frame < 300; frame++) + { + RunPhysicsFrame(world, forceSystem, broadPhase, narrowPhase, solver, integration); + + ref LocalTransform lt = ref world.GetComponent(dynamicCube); + float cubeBottom = lt.Position.Y - cubeHalfY; + + // Track if collision ever happened (cube bounced or stopped) + ref Velocity vel = ref world.GetComponent(dynamicCube); + if (cubeBottom <= platformTop + 0.1f && vel.Linear.Y >= 0f) + { + everCollided = true; + } + + // Safety: if cube falls way below the platform, the collision failed + if (cubeBottom < platformTop - 2.0f) + { + Assert.Fail( + $"Cube fell through platform at frame {frame}. " + + $"Position Y={lt.Position.Y:F4}, Velocity Y={vel.Linear.Y:F4}, " + + $"CubeBottom={cubeBottom:F4}, PlatformTop={platformTop:F4}"); + } + } + + // Then: cube should have collided and settled near the platform top + Assert.True(everCollided, "Cube never collided with the platform"); + + ref LocalTransform finalLt = ref world.GetComponent(dynamicCube); + float finalBottom = finalLt.Position.Y - cubeHalfY; + + // Cube bottom should be near the platform top (within tolerance) + Assert.True(finalBottom > platformTop - 0.2f, + $"Cube fell too far below platform. Bottom={finalBottom:F4}, PlatformTop={platformTop:F4}"); + Assert.True(finalBottom < platformTop + 0.5f, + $"Cube is too far above platform. Bottom={finalBottom:F4}, PlatformTop={platformTop:F4}"); + } + + [Fact] + public unsafe void BroadPhase_DetectsOverlappingBoxes() + { + // Given: two overlapping boxes + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + // Static box at origin, half-ext (0.5, 0.5, 0.5) + CreateStaticBox(world, new Vector3(0f, 0f, 0f), new Vector3(0.5f, 0.5f, 0.5f)); + + // Dynamic box overlapping at (0.5, 0, 0), half-ext (0.5, 0.5, 0.5) + CreateDynamicBox(world, new Vector3(0.5f, 0f, 0f), new Vector3(0.5f, 0.5f, 0.5f)); + + var broadPhase = new BroadPhaseSystem(); + + // When + broadPhase.Execute(world); + + // Then: check BroadPhaseResult singleton + var bpType = ComponentRegistry.Get(); + bool found = false; + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!storage.Archetype.Contains(bpType.TypeId)) + { + continue; + } + + int bpIdx = storage.GetComponentIndex(bpType.TypeId); + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + if (chunk.Count > 0) + { + ref BroadPhaseResult result = ref chunk.GetComponent(bpIdx, 0); + Assert.True(result.Count > 0, + $"BroadPhase found 0 pairs. Expected at least 1."); + found = true; + } + } + } + + Assert.True(found, "BroadPhaseResult singleton not found"); + } + + [Fact] + public unsafe void NarrowPhase_DetectsContactForOverlappingBoxes() + { + // Given: two overlapping boxes + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + CreateStaticBox(world, new Vector3(0f, 0f, 0f), new Vector3(0.5f, 0.5f, 0.5f)); + CreateDynamicBox(world, new Vector3(0.5f, 0f, 0f), new Vector3(0.5f, 0.5f, 0.5f)); + + var broadPhase = new BroadPhaseSystem(); + var narrowPhase = new NarrowPhaseSystem(); + + // When + broadPhase.Execute(world); + narrowPhase.Execute(world); + + // Then: check NarrowPhaseResult + var npType = ComponentRegistry.Get(); + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!storage.Archetype.Contains(npType.TypeId)) + { + continue; + } + + int npIdx = storage.GetComponentIndex(npType.TypeId); + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + if (chunk.Count > 0) + { + ref NarrowPhaseResult result = ref chunk.GetComponent(npIdx, 0); + Assert.True(result.Count > 0, + $"NarrowPhase found 0 contacts. Expected at least 1."); + return; + } + } + } + + Assert.Fail("NarrowPhaseResult singleton not found"); + } + + [Fact] + public unsafe void CollisionDetection_TracksDuringFall() + { + // Given: dynamic cube falling toward a static platform + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + CreateStaticBox(world, + new Vector3(-2f, -2.5f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + Entity dynamicCube = CreateDynamicBox(world, + new Vector3(-2f, 1f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + var forceSystem = new ForceIntegrationSystem(Dt); + var broadPhase = new BroadPhaseSystem(); + var narrowPhase = new NarrowPhaseSystem(); + var solver = new ConstraintSolverSystem(Dt); + var integration = new IntegrationSystem(Dt); + + // When: run 200 frames + for (int frame = 0; frame < 200; frame++) + { + RunPhysicsFrame(world, forceSystem, broadPhase, narrowPhase, solver, integration); + + ref LocalTransform lt = ref world.GetComponent(dynamicCube); + if (lt.Position.Y - 0.5f < -4.0f) + { + Assert.Fail($"Cube fell through at frame {frame}."); + } + } + + // Then: verify collision detection occurred at some point + // Run one more broad+narrow pass at the resting position to verify + broadPhase.Execute(world); + narrowPhase.Execute(world); + + // Cube should have settled near the platform + ref LocalTransform finalLt = ref world.GetComponent(dynamicCube); + Assert.True(finalLt.Position.Y - 0.5f > -2.2f, + $"Cube fell too far below platform. Y={finalLt.Position.Y:F4}"); + } + + [Fact] + public unsafe void Solver_StopsOverlappingFallingCube() + { + // Given: cube pre-overlapping a platform with downward velocity + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + CreateStaticBox(world, + new Vector3(-2f, -2.5f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + Entity dynamicCube = CreateDynamicBox(world, + new Vector3(-2f, -1.55f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + world.GetComponent(dynamicCube) = new Velocity + { + Linear = new Vector3(0f, -7f, 0f), + }; + + var broadPhase = new BroadPhaseSystem(); + var narrowPhase = new NarrowPhaseSystem(); + var solver = new ConstraintSolverSystem(Dt); + + // When: run collision detection + solver only + broadPhase.Execute(world); + narrowPhase.Execute(world); + + float velYBefore = world.GetComponent(dynamicCube).Linear.Y; + solver.Execute(world); + float velYAfter = world.GetComponent(dynamicCube).Linear.Y; + + // Then: solver should have stopped the cube (velocity >= 0) + Assert.True(velYAfter > velYBefore, + $"Solver did not push cube up. VelBefore={velYBefore:F4}, VelAfter={velYAfter:F4}"); + Assert.True(velYAfter >= 0f, + $"Cube still falling after solver. VelAfter={velYAfter:F4}"); + } + + [Fact] + public void DynamicCube_FallsOntoEqualSizePlatform_StopsAbovePlatform() + { + // Given: same test but with equal-size boxes (known working case for comparison) + var world = new World(); + using var _ = world; + + CreatePhysicsSingleton(world); + + // Platform: top at Y = -2.5 + 0.5 = -2.0 + CreateStaticBox(world, + new Vector3(-2f, -2.5f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + // Dynamic cube starts above + Entity dynamicCube = CreateDynamicBox(world, + new Vector3(-2f, 1f, -5f), + new Vector3(0.5f, 0.5f, 0.5f)); + + var forceSystem = new ForceIntegrationSystem(Dt); + var broadPhase = new BroadPhaseSystem(); + var narrowPhase = new NarrowPhaseSystem(); + var solver = new ConstraintSolverSystem(Dt); + var integration = new IntegrationSystem(Dt); + + float platformTop = -2.0f; + float cubeHalfY = 0.5f; + + // When: run 300 frames + for (int frame = 0; frame < 300; frame++) + { + RunPhysicsFrame(world, forceSystem, broadPhase, narrowPhase, solver, integration); + + ref LocalTransform lt = ref world.GetComponent(dynamicCube); + float cubeBottom = lt.Position.Y - cubeHalfY; + + if (cubeBottom < platformTop - 2.0f) + { + ref Velocity vel = ref world.GetComponent(dynamicCube); + Assert.Fail( + $"Cube fell through platform at frame {frame}. " + + $"Position Y={lt.Position.Y:F4}, Velocity Y={vel.Linear.Y:F4}"); + } + } + + ref LocalTransform finalLt = ref world.GetComponent(dynamicCube); + float finalBottom = finalLt.Position.Y - cubeHalfY; + Assert.True(finalBottom > platformTop - 0.2f, + $"Cube fell too far below. Bottom={finalBottom:F4}"); + } +} From b67da82084e5f971ced061c79f47ce7bd4361a30 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Sat, 7 Mar 2026 14:27:30 +0900 Subject: [PATCH 14/14] =?UTF-8?q?=E3=83=87=E3=83=A2=E6=9B=B4=E6=96=B0=20+?= =?UTF-8?q?=20roadmap.md=20v0.11=E3=82=92=E5=AE=8C=E4=BA=86=E3=81=AB?= =?UTF-8?q?=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit CreateCubeEntityにhalfExtentsパラメータ追加、 下部キューブを大型プラットフォーム(2x0.5x2)に変更 Co-Authored-By: Claude Opus 4.6 --- docs/roadmap.md | 8 ++++---- src/Seed.Engine.App/Program.cs | 15 +++++++++------ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/docs/roadmap.md b/docs/roadmap.md index ff64548..ec85a7a 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -163,10 +163,10 @@ ゴール: オブジェクトが物理法則に従って動く -- [ ] PGS拘束ソルバー -- [ ] リジッドボディ (質量、減衰、重力スケール、キネマティック) -- [ ] ConvexHullコリジョン -- [ ] CCD (高速移動体のトンネリング防止) +- [x] PGS拘束ソルバー +- [x] リジッドボディ (質量、減衰、重力スケール、キネマティック) +- [x] ConvexHullコリジョン +- [x] CCD (高速移動体のトンネリング防止) 完動品としての価値: オブジェクトが衝突・反発・落下・積み重なる。高速移動体もすり抜けない。 diff --git a/src/Seed.Engine.App/Program.cs b/src/Seed.Engine.App/Program.cs index ebf39dd..8e3fda9 100644 --- a/src/Seed.Engine.App/Program.cs +++ b/src/Seed.Engine.App/Program.cs @@ -1194,8 +1194,11 @@ private static void Main() } private static void CreateCubeEntity( - World world, int meshId, int materialId, Vector3 position) + 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(); var ltwType = ComponentRegistry.Get(); var meshType = ComponentRegistry.Get(); @@ -1207,7 +1210,8 @@ private static void CreateCubeEntity( Entity entity = world.CreateEntity(types); world.GetComponent(entity) = - LocalTransform.FromPosition(position); + LocalTransform.FromPositionRotationScale( + position, Quaternion.Identity, he * 2f); world.GetComponent(entity) = new LocalToWorld { Value = Matrix4x4.Identity, @@ -1222,12 +1226,11 @@ private static void CreateCubeEntity( }; world.GetComponent(entity) = new WorldBounds { - Value = new AABB(position - new Vector3(0.5f, 0.5f, 0.5f), - position + new Vector3(0.5f, 0.5f, 0.5f)), + Value = new AABB(position - he, position + he), }; world.GetComponent(entity) = new ColliderComponent { - Value = Collider.CreateBox(new Vector3(0.5f, 0.5f, 0.5f)), + Value = Collider.CreateBox(he), }; } @@ -1300,7 +1303,7 @@ private static void RunMainLoop( CreatePlatformTagEntity(world, renderMode); CreateCubeEntity(world, cubeMeshId, materialId, - new Vector3(-2f, -2f, -5f)); + new Vector3(-2f, -2.5f, -5f), new Vector3(2f, 0.5f, 2f)); CreateCubeEntity(world, cubeMeshId, materialId, new Vector3(2f, 0f, -4f)); CreateDynamicCubeEntity(world, cubeMeshId, materialId,