From e2fbd2a58a74fd1e603240b7908756b3a7f9006a Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:19 +0900 Subject: [PATCH 1/7] =?UTF-8?q?AABB.SurfaceArea=E8=BF=BD=E5=8A=A0=E3=80=81?= =?UTF-8?q?Physics=20ErrorCodes=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BVHのSAHヒューリスティックに必要なSurfaceAreaプロパティと、 物理ドメインのResult用エラーコードを追加。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Foundation/Error/ErrorCode.cs | 19 +++++++++++++++++++ .../Foundation/Mathematics/AABB.cs | 12 ++++++++++++ 2 files changed, 31 insertions(+) diff --git a/src/Seed.Engine/Foundation/Error/ErrorCode.cs b/src/Seed.Engine/Foundation/Error/ErrorCode.cs index 3f1f7d54..e8bbcefb 100644 --- a/src/Seed.Engine/Foundation/Error/ErrorCode.cs +++ b/src/Seed.Engine/Foundation/Error/ErrorCode.cs @@ -210,4 +210,23 @@ public static class ErrorCodes /// OpenXR action set attach failed. public static readonly ErrorCode XrActionAttachFailed = new(519, "OpenXR action set attach failed"); + + // Physics (600-699) + /// GJK algorithm did not converge. + public static readonly ErrorCode GjkDidNotConverge = new(600, "GJK algorithm did not converge"); + + /// EPA algorithm did not converge. + public static readonly ErrorCode EpaDidNotConverge = new(601, "EPA algorithm did not converge"); + + /// Invalid collider type encountered. + public static readonly ErrorCode InvalidColliderType = new(602, "Invalid collider type"); + + /// BVH construction failed. + public static readonly ErrorCode BvhConstructionFailed = new(603, "BVH construction failed"); + + /// Raycast direction is zero-length. + public static readonly ErrorCode InvalidRayDirection = new(604, "Raycast direction is zero-length"); + + /// Spatial hash grid cell size is invalid. + public static readonly ErrorCode InvalidCellSize = new(605, "Spatial hash grid cell size is invalid"); } diff --git a/src/Seed.Engine/Foundation/Mathematics/AABB.cs b/src/Seed.Engine/Foundation/Mathematics/AABB.cs index 0cfcd93e..324d2988 100644 --- a/src/Seed.Engine/Foundation/Mathematics/AABB.cs +++ b/src/Seed.Engine/Foundation/Mathematics/AABB.cs @@ -32,6 +32,18 @@ public AABB(Vector3 min, Vector3 max) /// public Vector3 Extents => (Max - Min) * 0.5f; + /// + /// Gets the surface area of the bounding box. Used by BVH SAH heuristic. + /// + public float SurfaceArea + { + get + { + Vector3 size = Max - Min; + return 2f * (size.X * size.Y + size.Y * size.Z + size.Z * size.X); + } + } + /// /// Determines whether this AABB contains a point. /// From 37fc9c3a97cacbdc0d1e625494c2048b3aaeff3a Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:26 +0900 Subject: [PATCH 2/7] =?UTF-8?q?=E3=82=B3=E3=83=AA=E3=82=B8=E3=83=A7?= =?UTF-8?q?=E3=83=B3=E5=BD=A2=E7=8A=B6=E3=81=A8GJK+EPA=E7=8B=AD=E5=9F=9F?= =?UTF-8?q?=E3=83=95=E3=82=A7=E3=83=BC=E3=82=BA=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit tagged union方式のCollider (Sphere/Box/Capsule)、 GJKサポート関数、GJK衝突判定+EPA貫通情報抽出を実装。 全てunmanaged structでフレームループ0-allocation制約を遵守。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Physics/Collision/BoxShape.cs | 56 ++ .../Physics/Collision/CapsuleShape.cs | 63 +++ src/Seed.Engine/Physics/Collision/Collider.cs | 46 ++ .../Physics/Collision/ColliderBounds.cs | 65 +++ .../Physics/Collision/ColliderData.cs | 25 + .../Physics/Collision/ColliderType.cs | 16 + .../Physics/Collision/CollisionPair.cs | 60 +++ .../Physics/Collision/ContactPoint.cs | 71 +++ src/Seed.Engine/Physics/Collision/GjkEpa.cs | 480 ++++++++++++++++++ .../Physics/Collision/GjkSupport.cs | 96 ++++ src/Seed.Engine/Physics/Collision/Ray.cs | 68 +++ .../Physics/Collision/RaycastHit.cs | 72 +++ src/Seed.Engine/Physics/Collision/Simplex.cs | 40 ++ 13 files changed, 1158 insertions(+) create mode 100644 src/Seed.Engine/Physics/Collision/BoxShape.cs create mode 100644 src/Seed.Engine/Physics/Collision/CapsuleShape.cs create mode 100644 src/Seed.Engine/Physics/Collision/Collider.cs create mode 100644 src/Seed.Engine/Physics/Collision/ColliderBounds.cs create mode 100644 src/Seed.Engine/Physics/Collision/ColliderData.cs create mode 100644 src/Seed.Engine/Physics/Collision/ColliderType.cs create mode 100644 src/Seed.Engine/Physics/Collision/CollisionPair.cs create mode 100644 src/Seed.Engine/Physics/Collision/ContactPoint.cs create mode 100644 src/Seed.Engine/Physics/Collision/GjkEpa.cs create mode 100644 src/Seed.Engine/Physics/Collision/GjkSupport.cs create mode 100644 src/Seed.Engine/Physics/Collision/Ray.cs create mode 100644 src/Seed.Engine/Physics/Collision/RaycastHit.cs create mode 100644 src/Seed.Engine/Physics/Collision/Simplex.cs diff --git a/src/Seed.Engine/Physics/Collision/BoxShape.cs b/src/Seed.Engine/Physics/Collision/BoxShape.cs new file mode 100644 index 00000000..ea03c5c3 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/BoxShape.cs @@ -0,0 +1,56 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// An oriented box collision shape defined by half-extents along each axis. +/// +public readonly struct BoxShape : IEquatable +{ + /// The half-extents of the box along each local axis. + public readonly Vector3 HalfExtents; + + /// + /// Initializes a new . + /// + public BoxShape(Vector3 halfExtents) + { + HalfExtents = halfExtents; + } + + /// + public bool Equals(BoxShape other) + { + return HalfExtents.Equals(other.HalfExtents); + } + + /// + public override bool Equals(object? obj) + { + return obj is BoxShape other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HalfExtents.GetHashCode(); + } + + public static bool operator ==(BoxShape left, BoxShape right) + { + return left.Equals(right); + } + + public static bool operator !=(BoxShape left, BoxShape right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"BoxShape(HalfExtents: {HalfExtents})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/CapsuleShape.cs b/src/Seed.Engine/Physics/Collision/CapsuleShape.cs new file mode 100644 index 00000000..af7d56b2 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/CapsuleShape.cs @@ -0,0 +1,63 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A capsule collision shape defined by a radius and half-height. +/// The capsule is oriented along the local Y axis. +/// Total height = 2 * (HalfHeight + Radius). +/// +public readonly struct CapsuleShape : IEquatable +{ + /// The radius of the capsule hemispheres and cylinder. + public readonly float Radius; + + /// The half-height of the cylindrical segment (excluding hemispheres). + public readonly float HalfHeight; + + /// + /// Initializes a new . + /// + public CapsuleShape(float radius, float halfHeight) + { + Radius = radius; + HalfHeight = halfHeight; + } + + /// + public bool Equals(CapsuleShape other) + { + return MathHelper.ApproximatelyEqual(Radius, other.Radius) + && MathHelper.ApproximatelyEqual(HalfHeight, other.HalfHeight); + } + + /// + public override bool Equals(object? obj) + { + return obj is CapsuleShape other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HashCode.Combine(Radius, HalfHeight); + } + + public static bool operator ==(CapsuleShape left, CapsuleShape right) + { + return left.Equals(right); + } + + public static bool operator !=(CapsuleShape left, CapsuleShape right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"CapsuleShape(Radius: {Radius}, HalfHeight: {HalfHeight})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/Collider.cs b/src/Seed.Engine/Physics/Collision/Collider.cs new file mode 100644 index 00000000..ff7d11a3 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/Collider.cs @@ -0,0 +1,46 @@ +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A tagged union representing a collision shape. Combines a +/// discriminator with shape data stored in a union. +/// +public struct Collider +{ + /// The type of collision shape. + public ColliderType Type; + + /// The shape data (interpreted based on ). + public ColliderData Data; + + /// + /// Creates a sphere collider. + /// + public static Collider CreateSphere(float radius) + { + var collider = new Collider { Type = ColliderType.Sphere }; + collider.Data.Sphere = new Sphere(Vector3.Zero, radius); + return collider; + } + + /// + /// Creates a box collider. + /// + public static Collider CreateBox(Vector3 halfExtents) + { + var collider = new Collider { Type = ColliderType.Box }; + collider.Data.Box = new BoxShape(halfExtents); + return collider; + } + + /// + /// Creates a capsule collider. + /// + public static Collider CreateCapsule(float radius, float halfHeight) + { + var collider = new Collider { Type = ColliderType.Capsule }; + collider.Data.Capsule = new CapsuleShape(radius, halfHeight); + return collider; + } +} diff --git a/src/Seed.Engine/Physics/Collision/ColliderBounds.cs b/src/Seed.Engine/Physics/Collision/ColliderBounds.cs new file mode 100644 index 00000000..dd1513f8 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/ColliderBounds.cs @@ -0,0 +1,65 @@ +using System; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Computes world-space AABBs from collider shapes and transforms. +/// +public static class ColliderBounds +{ + /// + /// Computes the world-space AABB for a collider at the given position and rotation. + /// + public static Result ComputeAABB(Collider collider, Vector3 position, Quaternion rotation) + { + return collider.Type switch + { + ColliderType.Sphere => Result.Ok(ComputeSphereAABB(collider.Data.Sphere, position)), + ColliderType.Box => Result.Ok(ComputeBoxAABB(collider.Data.Box, position, rotation)), + ColliderType.Capsule => Result.Ok( + ComputeCapsuleAABB(collider.Data.Capsule, position, rotation)), + _ => Result.Fail(ErrorCodes.InvalidColliderType), + }; + } + + private static AABB ComputeSphereAABB(Sphere sphere, Vector3 position) + { + float r = sphere.Radius; + var extent = new Vector3(r, r, r); + return new AABB(position - extent, position + extent); + } + + private static AABB ComputeBoxAABB(BoxShape box, Vector3 position, Quaternion rotation) + { + Vector3 he = box.HalfExtents; + + Vector3 axisX = rotation.RotateVector(Vector3.UnitX); + Vector3 axisY = rotation.RotateVector(Vector3.UnitY); + Vector3 axisZ = rotation.RotateVector(Vector3.UnitZ); + + float ex = MathF.Abs(axisX.X) * he.X + MathF.Abs(axisY.X) * he.Y + MathF.Abs(axisZ.X) * he.Z; + float ey = MathF.Abs(axisX.Y) * he.X + MathF.Abs(axisY.Y) * he.Y + MathF.Abs(axisZ.Y) * he.Z; + float ez = MathF.Abs(axisX.Z) * he.X + MathF.Abs(axisY.Z) * he.Y + MathF.Abs(axisZ.Z) * he.Z; + + var extent = new Vector3(ex, ey, ez); + return new AABB(position - extent, position + extent); + } + + private static AABB ComputeCapsuleAABB(CapsuleShape capsule, Vector3 position, Quaternion rotation) + { + Vector3 up = rotation.RotateVector(Vector3.UnitY); + Vector3 topCenter = position + up * capsule.HalfHeight; + Vector3 bottomCenter = position - up * capsule.HalfHeight; + + float r = capsule.Radius; + var radiusExtent = new Vector3(r, r, r); + + AABB topAABB = new AABB(topCenter - radiusExtent, topCenter + radiusExtent); + AABB bottomAABB = new AABB(bottomCenter - radiusExtent, bottomCenter + radiusExtent); + + return topAABB.Merge(bottomAABB); + } +} diff --git a/src/Seed.Engine/Physics/Collision/ColliderData.cs b/src/Seed.Engine/Physics/Collision/ColliderData.cs new file mode 100644 index 00000000..7021e4c5 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/ColliderData.cs @@ -0,0 +1,25 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A union of all supported collision shapes. Uses explicit layout +/// to overlay shape data in the same memory region. +/// +[StructLayout(LayoutKind.Explicit)] +public struct ColliderData +{ + /// Sphere shape data. + [FieldOffset(0)] + public Sphere Sphere; + + /// Box shape data. + [FieldOffset(0)] + public BoxShape Box; + + /// Capsule shape data. + [FieldOffset(0)] + public CapsuleShape Capsule; +} diff --git a/src/Seed.Engine/Physics/Collision/ColliderType.cs b/src/Seed.Engine/Physics/Collision/ColliderType.cs new file mode 100644 index 00000000..2f121cb5 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/ColliderType.cs @@ -0,0 +1,16 @@ +namespace Seed.Engine.Physics.Collision; + +/// +/// Enumerates the supported collision shape types. +/// +public enum ColliderType : byte +{ + /// A sphere shape. + Sphere = 0, + + /// An axis-aligned box shape. + Box = 1, + + /// A capsule shape oriented along the local Y axis. + Capsule = 2, +} diff --git a/src/Seed.Engine/Physics/Collision/CollisionPair.cs b/src/Seed.Engine/Physics/Collision/CollisionPair.cs new file mode 100644 index 00000000..266f945f --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/CollisionPair.cs @@ -0,0 +1,60 @@ +using System; + +using Seed.Engine.Ecs; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Represents a pair of entities that may be colliding. +/// +public readonly struct CollisionPair : IEquatable +{ + /// The first entity in the pair. + public readonly Entity EntityA; + + /// The second entity in the pair. + public readonly Entity EntityB; + + /// + /// Initializes a new . + /// + public CollisionPair(Entity entityA, Entity entityB) + { + EntityA = entityA; + EntityB = entityB; + } + + /// + public bool Equals(CollisionPair other) + { + return EntityA.Equals(other.EntityA) && EntityB.Equals(other.EntityB); + } + + /// + public override bool Equals(object? obj) + { + return obj is CollisionPair other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HashCode.Combine(EntityA, EntityB); + } + + public static bool operator ==(CollisionPair left, CollisionPair right) + { + return left.Equals(right); + } + + public static bool operator !=(CollisionPair left, CollisionPair right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"CollisionPair({EntityA}, {EntityB})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/ContactPoint.cs b/src/Seed.Engine/Physics/Collision/ContactPoint.cs new file mode 100644 index 00000000..36d7075c --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/ContactPoint.cs @@ -0,0 +1,71 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Describes a contact point resulting from a collision between two shapes. +/// +public readonly struct ContactPoint : IEquatable +{ + /// The contact point on shape A in world space. + public readonly Vector3 PointOnA; + + /// The contact point on shape B in world space. + public readonly Vector3 PointOnB; + + /// The contact normal pointing from B towards A. + public readonly Vector3 Normal; + + /// The penetration depth (positive when overlapping). + public readonly float PenetrationDepth; + + /// + /// Initializes a new . + /// + public ContactPoint(Vector3 pointOnA, Vector3 pointOnB, Vector3 normal, float penetrationDepth) + { + PointOnA = pointOnA; + PointOnB = pointOnB; + Normal = normal; + PenetrationDepth = penetrationDepth; + } + + /// + public bool Equals(ContactPoint other) + { + return PointOnA.Equals(other.PointOnA) + && PointOnB.Equals(other.PointOnB) + && Normal.Equals(other.Normal) + && MathHelper.ApproximatelyEqual(PenetrationDepth, other.PenetrationDepth); + } + + /// + public override bool Equals(object? obj) + { + return obj is ContactPoint other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HashCode.Combine(PointOnA, PointOnB, Normal, PenetrationDepth); + } + + public static bool operator ==(ContactPoint left, ContactPoint right) + { + return left.Equals(right); + } + + public static bool operator !=(ContactPoint left, ContactPoint right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"ContactPoint(Normal: {Normal}, Depth: {PenetrationDepth})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/GjkEpa.cs b/src/Seed.Engine/Physics/Collision/GjkEpa.cs new file mode 100644 index 00000000..77ad6359 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/GjkEpa.cs @@ -0,0 +1,480 @@ +using System; + +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Implements the GJK (Gilbert-Johnson-Keerthi) algorithm for collision detection +/// and the EPA (Expanding Polytope Algorithm) for contact information extraction. +/// +public static class GjkEpa +{ + private const int GjkMaxIterations = 64; + private const int EpaMaxIterations = 64; + private const float EpaEpsilon = 1e-4f; + + /// + /// Tests whether two colliders intersect using the GJK algorithm. + /// + public static Result Intersects( + Collider colliderA, Vector3 posA, Quaternion rotA, + Collider colliderB, Vector3 posB, Quaternion rotB) + { + var simplex = new Simplex(); + Vector3 direction = posB - posA; + if (direction.LengthSquared < MathHelper.Epsilon * MathHelper.Epsilon) + { + direction = Vector3.UnitX; + } + + Result supportResult = GjkSupport.MinkowskiSupport( + colliderA, posA, rotA, colliderB, posB, rotB, direction); + if (supportResult.IsFailure) + { + return Result.Fail(supportResult.Error); + } + + Vector3 support = supportResult.Value; + simplex.Push(support); + direction = -support; + + for (int i = 0; i < GjkMaxIterations; i++) + { + supportResult = GjkSupport.MinkowskiSupport( + colliderA, posA, rotA, colliderB, posB, rotB, direction); + if (supportResult.IsFailure) + { + return Result.Fail(supportResult.Error); + } + + support = supportResult.Value; + + if (Vector3.Dot(support, direction) < 0f) + { + return Result.Ok(false); + } + + simplex.Push(support); + + if (DoSimplex(ref simplex, ref direction)) + { + return Result.Ok(true); + } + } + + return Result.Ok(false); + } + + /// + /// Computes collision contact information using GJK + EPA. + /// Returns a Result containing true if the shapes are intersecting, + /// with the contact point filled in. + /// + public static Result ComputeContact( + Collider colliderA, Vector3 posA, Quaternion rotA, + Collider colliderB, Vector3 posB, Quaternion rotB, + out ContactPoint contact) + { + contact = default; + + var simplex = new Simplex(); + Vector3 direction = posB - posA; + if (direction.LengthSquared < MathHelper.Epsilon * MathHelper.Epsilon) + { + direction = Vector3.UnitX; + } + + Result supportResult = GjkSupport.MinkowskiSupport( + colliderA, posA, rotA, colliderB, posB, rotB, direction); + if (supportResult.IsFailure) + { + return Result.Fail(supportResult.Error); + } + + Vector3 support = supportResult.Value; + simplex.Push(support); + direction = -support; + + for (int i = 0; i < GjkMaxIterations; i++) + { + supportResult = GjkSupport.MinkowskiSupport( + colliderA, posA, rotA, colliderB, posB, rotB, direction); + if (supportResult.IsFailure) + { + return Result.Fail(supportResult.Error); + } + + support = supportResult.Value; + + if (Vector3.Dot(support, direction) < 0f) + { + return Result.Ok(false); + } + + simplex.Push(support); + + if (DoSimplex(ref simplex, ref direction)) + { + return RunEpa( + colliderA, posA, rotA, colliderB, posB, rotB, + ref simplex, out contact); + } + } + + return Result.Ok(false); + } + + private static bool DoSimplex(ref Simplex simplex, ref Vector3 direction) + { + return simplex.Count switch + { + 2 => DoSimplexLine(ref simplex, ref direction), + 3 => DoSimplexTriangle(ref simplex, ref direction), + 4 => DoSimplexTetrahedron(ref simplex, ref direction), + _ => false, + }; + } + + private static bool DoSimplexLine(ref Simplex simplex, ref Vector3 direction) + { + Vector3 a = simplex.A; + Vector3 b = simplex.B; + Vector3 ab = b - a; + Vector3 ao = -a; + + if (Vector3.Dot(ab, ao) > 0f) + { + direction = Vector3.Cross(Vector3.Cross(ab, ao), ab); + if (direction.LengthSquared < MathHelper.Epsilon * MathHelper.Epsilon) + { + direction = PerpendicularTo(ab); + } + } + else + { + simplex.B = a; + simplex.Count = 1; + direction = ao; + } + + return false; + } + + private static bool DoSimplexTriangle(ref Simplex simplex, ref Vector3 direction) + { + Vector3 a = simplex.A; + Vector3 b = simplex.B; + Vector3 c = simplex.C; + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ao = -a; + Vector3 abc = Vector3.Cross(ab, ac); + + Vector3 abcCrossAc = Vector3.Cross(abc, ac); + if (Vector3.Dot(abcCrossAc, ao) > 0f) + { + if (Vector3.Dot(ac, ao) > 0f) + { + simplex.A = a; + simplex.B = c; + simplex.Count = 2; + direction = Vector3.Cross(Vector3.Cross(ac, ao), ac); + if (direction.LengthSquared < MathHelper.Epsilon * MathHelper.Epsilon) + { + direction = PerpendicularTo(ac); + } + } + else + { + simplex.B = b; + simplex.Count = 2; + return DoSimplexLine(ref simplex, ref direction); + } + } + else + { + Vector3 abCrossAbc = Vector3.Cross(ab, abc); + if (Vector3.Dot(abCrossAbc, ao) > 0f) + { + simplex.Count = 2; + return DoSimplexLine(ref simplex, ref direction); + } + else + { + if (Vector3.Dot(abc, ao) > 0f) + { + direction = abc; + } + else + { + Vector3 temp = simplex.B; + simplex.B = simplex.C; + simplex.C = temp; + direction = -abc; + } + } + } + + return false; + } + + private static bool DoSimplexTetrahedron(ref Simplex simplex, ref Vector3 direction) + { + Vector3 a = simplex.A; + Vector3 b = simplex.B; + Vector3 c = simplex.C; + Vector3 d = simplex.D; + Vector3 ao = -a; + + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ad = d - a; + + Vector3 abc = Vector3.Cross(ab, ac); + Vector3 acd = Vector3.Cross(ac, ad); + Vector3 adb = Vector3.Cross(ad, ab); + + if (Vector3.Dot(abc, ao) > 0f) + { + simplex.A = a; + simplex.B = b; + simplex.C = c; + simplex.Count = 3; + return DoSimplexTriangle(ref simplex, ref direction); + } + + if (Vector3.Dot(acd, ao) > 0f) + { + simplex.A = a; + simplex.B = c; + simplex.C = d; + simplex.Count = 3; + return DoSimplexTriangle(ref simplex, ref direction); + } + + if (Vector3.Dot(adb, ao) > 0f) + { + simplex.A = a; + simplex.B = d; + simplex.C = b; + simplex.Count = 3; + return DoSimplexTriangle(ref simplex, ref direction); + } + + return true; + } + + private static Vector3 PerpendicularTo(Vector3 v) + { + if (MathF.Abs(v.X) < MathF.Abs(v.Y)) + { + return Vector3.Cross(v, Vector3.UnitX); + } + return Vector3.Cross(v, Vector3.UnitY); + } + + private struct EpaFace + { + public int A; + public int B; + public int C; + public Vector3 Normal; + public float Distance; + } + + private static Result RunEpa( + Collider colliderA, Vector3 posA, Quaternion rotA, + Collider colliderB, Vector3 posB, Quaternion rotB, + ref Simplex simplex, + out ContactPoint contact) + { + contact = default; + + var vertices = new NativeList(64); + var faces = new NativeList(64); + + vertices.Add(simplex.A); + vertices.Add(simplex.B); + vertices.Add(simplex.C); + vertices.Add(simplex.D); + + AddFace(ref faces, ref vertices, 0, 1, 2); + AddFace(ref faces, ref vertices, 0, 2, 3); + AddFace(ref faces, ref vertices, 0, 3, 1); + AddFace(ref faces, ref vertices, 1, 3, 2); + + for (int iteration = 0; iteration < EpaMaxIterations; iteration++) + { + int closestFace = FindClosestFace(ref faces); + if (closestFace < 0) + { + vertices.Dispose(); + faces.Dispose(); + return Result.Ok(false); + } + + EpaFace face = faces[closestFace]; + Vector3 normal = face.Normal; + float distance = face.Distance; + + Result supportResult = GjkSupport.MinkowskiSupport( + colliderA, posA, rotA, colliderB, posB, rotB, normal); + if (supportResult.IsFailure) + { + vertices.Dispose(); + faces.Dispose(); + return Result.Fail(supportResult.Error); + } + + Vector3 support = supportResult.Value; + float supportDist = Vector3.Dot(support, normal); + + if (supportDist - distance < EpaEpsilon) + { + Vector3 contactNormal = normal; + float penetrationDepth = distance; + + Result pointOnAResult = GjkSupport.Support( + colliderA, posA, rotA, contactNormal); + if (pointOnAResult.IsFailure) + { + vertices.Dispose(); + faces.Dispose(); + return Result.Fail(pointOnAResult.Error); + } + + Result pointOnBResult = GjkSupport.Support( + colliderB, posB, rotB, -contactNormal); + if (pointOnBResult.IsFailure) + { + vertices.Dispose(); + faces.Dispose(); + return Result.Fail(pointOnBResult.Error); + } + + contact = new ContactPoint( + pointOnAResult.Value, pointOnBResult.Value, + contactNormal, penetrationDepth); + vertices.Dispose(); + faces.Dispose(); + return Result.Ok(true); + } + + int newVertIdx = vertices.Length; + vertices.Add(support); + + var edgeList = new NativeList(32); + + int writeIdx = 0; + for (int i = 0; i < faces.Length; i++) + { + EpaFace f = faces[i]; + if (Vector3.Dot(f.Normal, support - vertices[f.A]) > 0f) + { + AddEdge(ref edgeList, f.A, f.B); + AddEdge(ref edgeList, f.B, f.C); + AddEdge(ref edgeList, f.C, f.A); + } + else + { + if (writeIdx != i) + { + faces[writeIdx] = f; + } + writeIdx++; + } + } + + // Trim removed faces by rebuilding to the compacted length + while (faces.Length > writeIdx) + { + faces.RemoveAt(faces.Length - 1); + } + + for (int i = 0; i < edgeList.Length; i++) + { + long edge = edgeList[i]; + int edgeA = (int)(edge >> 32); + int edgeB = (int)(edge & 0xFFFFFFFF); + AddFace(ref faces, ref vertices, edgeA, edgeB, newVertIdx); + } + + edgeList.Dispose(); + } + + vertices.Dispose(); + faces.Dispose(); + return Result.Ok(false); + } + + private static void AddFace( + ref NativeList faces, ref NativeList vertices, + int a, int b, int c) + { + Vector3 ab = vertices[b] - vertices[a]; + Vector3 ac = vertices[c] - vertices[a]; + Vector3 normal = Vector3.Cross(ab, ac); + float len = normal.Length; + if (len < MathHelper.Epsilon) + { + return; + } + normal = normal / len; + + float dist = Vector3.Dot(normal, vertices[a]); + if (dist < 0f) + { + normal = -normal; + dist = -dist; + int temp = b; + b = c; + c = temp; + } + + faces.Add(new EpaFace + { + A = a, + B = b, + C = c, + Normal = normal, + Distance = dist, + }); + } + + private static int FindClosestFace(ref NativeList faces) + { + int closest = -1; + float minDist = float.MaxValue; + + for (int i = 0; i < faces.Length; i++) + { + if (faces[i].Distance < minDist) + { + minDist = faces[i].Distance; + closest = i; + } + } + + return closest; + } + + private static void AddEdge(ref NativeList edges, int a, int b) + { + long forward = ((long)a << 32) | (uint)b; + long reverse = ((long)b << 32) | (uint)a; + + for (int i = edges.Length - 1; i >= 0; i--) + { + if (edges[i] == reverse) + { + edges.RemoveAt(i); + return; + } + } + + edges.Add(forward); + } +} diff --git a/src/Seed.Engine/Physics/Collision/GjkSupport.cs b/src/Seed.Engine/Physics/Collision/GjkSupport.cs new file mode 100644 index 00000000..c5a0c2b1 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/GjkSupport.cs @@ -0,0 +1,96 @@ +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Provides support functions for GJK/EPA algorithms. +/// A support function returns the farthest point on a shape in a given direction. +/// +public static class GjkSupport +{ + /// + /// Returns the support point for a collider shape in world space. + /// + public static Result Support( + Collider collider, Vector3 position, Quaternion rotation, Vector3 direction) + { + return collider.Type switch + { + ColliderType.Sphere => Result.Ok( + SphereSupport(collider.Data.Sphere, position, direction)), + ColliderType.Box => Result.Ok( + BoxSupport(collider.Data.Box, position, rotation, direction)), + ColliderType.Capsule => Result.Ok( + CapsuleSupport(collider.Data.Capsule, position, rotation, direction)), + _ => Result.Fail(ErrorCodes.InvalidColliderType), + }; + } + + /// + /// Returns the Minkowski difference support point for two colliders. + /// + public static Result MinkowskiSupport( + Collider colliderA, Vector3 posA, Quaternion rotA, + Collider colliderB, Vector3 posB, Quaternion rotB, + Vector3 direction) + { + Result resultA = Support(colliderA, posA, rotA, direction); + if (resultA.IsFailure) + { + return resultA; + } + + Result resultB = Support(colliderB, posB, rotB, -direction); + if (resultB.IsFailure) + { + return resultB; + } + + return Result.Ok(resultA.Value - resultB.Value); + } + + private static Vector3 SphereSupport(Sphere sphere, Vector3 position, Vector3 direction) + { + float lengthSq = direction.LengthSquared; + if (lengthSq < MathHelper.Epsilon * MathHelper.Epsilon) + { + return position; + } + return position + direction.Normalize() * sphere.Radius; + } + + private static Vector3 BoxSupport( + BoxShape box, Vector3 position, Quaternion rotation, Vector3 direction) + { + Vector3 localDir = rotation.Conjugate().RotateVector(direction); + + Vector3 he = box.HalfExtents; + Vector3 localSupport = new Vector3( + localDir.X >= 0f ? he.X : -he.X, + localDir.Y >= 0f ? he.Y : -he.Y, + localDir.Z >= 0f ? he.Z : -he.Z); + + return position + rotation.RotateVector(localSupport); + } + + private static Vector3 CapsuleSupport( + CapsuleShape capsule, Vector3 position, Quaternion rotation, Vector3 direction) + { + Vector3 up = rotation.RotateVector(Vector3.UnitY); + Vector3 topCenter = position + up * capsule.HalfHeight; + Vector3 bottomCenter = position - up * capsule.HalfHeight; + + float dotTop = Vector3.Dot(topCenter, direction); + float dotBottom = Vector3.Dot(bottomCenter, direction); + + Vector3 sphereCenter = dotTop >= dotBottom ? topCenter : bottomCenter; + + float lengthSq = direction.LengthSquared; + if (lengthSq < MathHelper.Epsilon * MathHelper.Epsilon) + { + return sphereCenter; + } + return sphereCenter + direction.Normalize() * capsule.Radius; + } +} diff --git a/src/Seed.Engine/Physics/Collision/Ray.cs b/src/Seed.Engine/Physics/Collision/Ray.cs new file mode 100644 index 00000000..4baf9027 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/Ray.cs @@ -0,0 +1,68 @@ +using System; + +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A ray defined by an origin point and a normalized direction. +/// +public readonly struct Ray : IEquatable +{ + /// The starting point of the ray. + public readonly Vector3 Origin; + + /// The normalized direction of the ray. + public readonly Vector3 Direction; + + /// + /// Initializes a new . + /// + public Ray(Vector3 origin, Vector3 direction) + { + Origin = origin; + Direction = direction; + } + + /// + /// Returns the point along the ray at the given distance. + /// + public Vector3 GetPoint(float distance) + { + return Origin + Direction * distance; + } + + /// + public bool Equals(Ray other) + { + return Origin.Equals(other.Origin) && Direction.Equals(other.Direction); + } + + /// + public override bool Equals(object? obj) + { + return obj is Ray other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HashCode.Combine(Origin, Direction); + } + + public static bool operator ==(Ray left, Ray right) + { + return left.Equals(right); + } + + public static bool operator !=(Ray left, Ray right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"Ray(Origin: {Origin}, Direction: {Direction})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/RaycastHit.cs b/src/Seed.Engine/Physics/Collision/RaycastHit.cs new file mode 100644 index 00000000..508b068b --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/RaycastHit.cs @@ -0,0 +1,72 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// Describes the result of a raycast hitting a collider. +/// +public readonly struct RaycastHit : IEquatable +{ + /// The entity that was hit. + public readonly Entity Entity; + + /// The world-space hit point. + public readonly Vector3 Point; + + /// The surface normal at the hit point. + public readonly Vector3 Normal; + + /// The distance from the ray origin to the hit point. + public readonly float Distance; + + /// + /// Initializes a new . + /// + public RaycastHit(Entity entity, Vector3 point, Vector3 normal, float distance) + { + Entity = entity; + Point = point; + Normal = normal; + Distance = distance; + } + + /// + public bool Equals(RaycastHit other) + { + return Entity.Equals(other.Entity) + && Point.Equals(other.Point) + && Normal.Equals(other.Normal) + && MathHelper.ApproximatelyEqual(Distance, other.Distance); + } + + /// + public override bool Equals(object? obj) + { + return obj is RaycastHit other && Equals(other); + } + + /// + public override int GetHashCode() + { + return HashCode.Combine(Entity, Point, Normal, Distance); + } + + public static bool operator ==(RaycastHit left, RaycastHit right) + { + return left.Equals(right); + } + + public static bool operator !=(RaycastHit left, RaycastHit right) + { + return !left.Equals(right); + } + + /// + public override string ToString() + { + return $"RaycastHit(Entity: {Entity}, Distance: {Distance})"; + } +} diff --git a/src/Seed.Engine/Physics/Collision/Simplex.cs b/src/Seed.Engine/Physics/Collision/Simplex.cs new file mode 100644 index 00000000..92ee3e04 --- /dev/null +++ b/src/Seed.Engine/Physics/Collision/Simplex.cs @@ -0,0 +1,40 @@ +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.Collision; + +/// +/// A simplex used by the GJK algorithm, storing up to 4 vertices +/// in Minkowski difference space. +/// +public struct Simplex +{ + /// Vertex A (most recently added). + public Vector3 A; + + /// Vertex B. + public Vector3 B; + + /// Vertex C. + public Vector3 C; + + /// Vertex D. + public Vector3 D; + + /// The number of vertices in the simplex (1-4). + public int Count; + + /// + /// Pushes a new vertex as the most recent point (A), shifting existing vertices. + /// + public void Push(Vector3 point) + { + D = C; + C = B; + B = A; + A = point; + if (Count < 4) + { + Count++; + } + } +} From 56c5230bd6c965410f54d6331682d67638ae2df5 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:31 +0900 Subject: [PATCH 3/7] =?UTF-8?q?BVH=E5=BA=83=E5=9F=9F=E3=83=95=E3=82=A7?= =?UTF-8?q?=E3=83=BC=E3=82=BA=E3=81=A8SpatialHashGrid=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit SAHベースBVH (IntroSortでO(n log n)保証) と OverlapSphere用のグリッドベース空間ハッシュを実装。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Physics/BroadPhase/BvhNode.cs | 27 + src/Seed.Engine/Physics/BroadPhase/BvhTree.cs | 488 ++++++++++++++++++ .../Physics/BroadPhase/SpatialHashGrid.cs | 150 ++++++ 3 files changed, 665 insertions(+) create mode 100644 src/Seed.Engine/Physics/BroadPhase/BvhNode.cs create mode 100644 src/Seed.Engine/Physics/BroadPhase/BvhTree.cs create mode 100644 src/Seed.Engine/Physics/BroadPhase/SpatialHashGrid.cs diff --git a/src/Seed.Engine/Physics/BroadPhase/BvhNode.cs b/src/Seed.Engine/Physics/BroadPhase/BvhNode.cs new file mode 100644 index 00000000..2e27c160 --- /dev/null +++ b/src/Seed.Engine/Physics/BroadPhase/BvhNode.cs @@ -0,0 +1,27 @@ +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.BroadPhase; + +/// +/// A node in the BVH tree. Internal nodes have two children; +/// leaf nodes store an entity index. +/// +public struct BvhNode +{ + /// The axis-aligned bounding box for this node. + public AABB Bounds; + + /// Index of the left child node, or -1 for leaf nodes. + public int LeftChild; + + /// Index of the right child node, or -1 for leaf nodes. + public int RightChild; + + /// Index into the entity array for leaf nodes, or -1 for internal nodes. + public int EntityIndex; + + /// + /// Returns true if this node is a leaf (has no children). + /// + public readonly bool IsLeaf => LeftChild == -1 && RightChild == -1; +} diff --git a/src/Seed.Engine/Physics/BroadPhase/BvhTree.cs b/src/Seed.Engine/Physics/BroadPhase/BvhTree.cs new file mode 100644 index 00000000..cdde1790 --- /dev/null +++ b/src/Seed.Engine/Physics/BroadPhase/BvhTree.cs @@ -0,0 +1,488 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Foundation.Memory; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Physics.BroadPhase; + +/// +/// A bounding volume hierarchy built using the Surface Area Heuristic (SAH). +/// Stored as a flat array of nodes for cache-friendly traversal. +/// Rebuilt from scratch each frame. +/// +public struct BvhTree : IDisposable +{ + private NativeArray _nodes; + private int _nodeCount; + private int _rootIndex; + private bool _disposed; + + /// + /// Gets the number of nodes in the tree. + /// + public readonly int NodeCount => _nodeCount; + + /// + /// Gets the root node index. + /// + public readonly int RootIndex => _rootIndex; + + /// + /// Builds the BVH from a set of AABBs and corresponding entity indices. + /// + public void Build(ReadOnlySpan bounds, ReadOnlySpan entityIndices) + { + int count = bounds.Length; + if (count == 0) + { + _nodeCount = 0; + _rootIndex = -1; + return; + } + + int maxNodes = 2 * count - 1; + if (_nodes.Length < maxNodes) + { + if (_nodes.Length > 0) + { + _nodes.Dispose(); + } + _nodes = new NativeArray(maxNodes); + } + + _nodeCount = 0; + + Span indices = stackalloc int[count]; + for (int i = 0; i < count; i++) + { + indices[i] = entityIndices[i]; + } + + Span mutableBounds = stackalloc AABB[count]; + bounds.CopyTo(mutableBounds); + + _rootIndex = BuildRecursive(mutableBounds, indices, 0, count); + } + + /// + /// Queries the BVH for all leaf nodes whose AABB intersects the given query AABB. + /// Appends matching entity indices to the results list. + /// + public readonly void QueryAABB(AABB queryBounds, ref NativeList results) + { + if (_rootIndex < 0 || _nodeCount == 0) + { + return; + } + + Span stack = stackalloc int[64]; + int stackTop = 0; + stack[stackTop++] = _rootIndex; + + while (stackTop > 0) + { + int nodeIdx = stack[--stackTop]; + BvhNode node = _nodes[nodeIdx]; + + if (!node.Bounds.Intersects(queryBounds)) + { + continue; + } + + if (node.IsLeaf) + { + results.Add(node.EntityIndex); + } + else + { + if (node.LeftChild >= 0 && stackTop < 64) + { + stack[stackTop++] = node.LeftChild; + } + if (node.RightChild >= 0 && stackTop < 64) + { + stack[stackTop++] = node.RightChild; + } + } + } + } + + /// + /// Queries the BVH for all leaf nodes whose AABB is hit by the given ray. + /// Uses the slab method for ray-AABB intersection. + /// + public readonly void QueryRay(Ray ray, float maxDistance, ref NativeList results) + { + if (_rootIndex < 0 || _nodeCount == 0) + { + return; + } + + Vector3 invDir = new Vector3( + 1f / ray.Direction.X, + 1f / ray.Direction.Y, + 1f / ray.Direction.Z); + + Span stack = stackalloc int[64]; + int stackTop = 0; + stack[stackTop++] = _rootIndex; + + while (stackTop > 0) + { + int nodeIdx = stack[--stackTop]; + BvhNode node = _nodes[nodeIdx]; + + if (!RayIntersectsAABB(ray.Origin, invDir, node.Bounds, maxDistance)) + { + continue; + } + + if (node.IsLeaf) + { + results.Add(node.EntityIndex); + } + else + { + if (node.LeftChild >= 0 && stackTop < 64) + { + stack[stackTop++] = node.LeftChild; + } + if (node.RightChild >= 0 && stackTop < 64) + { + stack[stackTop++] = node.RightChild; + } + } + } + } + + /// + /// Finds all overlapping pairs of leaf nodes in the BVH. + /// + public readonly void FindOverlappingPairs(ref NativeList pairs, ReadOnlySpan entities) + { + if (_rootIndex < 0 || _nodeCount == 0) + { + return; + } + + FindPairsRecursive(_rootIndex, _rootIndex, ref pairs, entities); + } + + /// + public void Dispose() + { + if (!_disposed && _nodes.Length > 0) + { + _nodes.Dispose(); + _disposed = true; + } + } + + private int BuildRecursive(Span bounds, Span indices, int start, int end) + { + int count = end - start; + int nodeIdx = _nodeCount++; + + AABB totalBounds = bounds[start]; + for (int i = start + 1; i < end; i++) + { + totalBounds = totalBounds.Merge(bounds[i]); + } + + if (count == 1) + { + _nodes[nodeIdx] = new BvhNode + { + Bounds = totalBounds, + LeftChild = -1, + RightChild = -1, + EntityIndex = indices[start], + }; + return nodeIdx; + } + + int bestAxis = 0; + int bestSplit = start + count / 2; + float bestCost = float.MaxValue; + + for (int axis = 0; axis < 3; axis++) + { + SortByAxis(bounds, indices, start, end, axis); + + for (int split = start + 1; split < end; split++) + { + AABB leftBounds = bounds[start]; + for (int i = start + 1; i < split; i++) + { + leftBounds = leftBounds.Merge(bounds[i]); + } + + AABB rightBounds = bounds[split]; + for (int i = split + 1; i < end; i++) + { + rightBounds = rightBounds.Merge(bounds[i]); + } + + float cost = (split - start) * leftBounds.SurfaceArea + + (end - split) * rightBounds.SurfaceArea; + + if (cost < bestCost) + { + bestCost = cost; + bestAxis = axis; + bestSplit = split; + } + } + } + + SortByAxis(bounds, indices, start, end, bestAxis); + + _nodes[nodeIdx] = new BvhNode + { + Bounds = totalBounds, + LeftChild = -1, + RightChild = -1, + EntityIndex = -1, + }; + + int left = BuildRecursive(bounds, indices, start, bestSplit); + int right = BuildRecursive(bounds, indices, bestSplit, end); + + var node = _nodes[nodeIdx]; + node.LeftChild = left; + node.RightChild = right; + _nodes[nodeIdx] = node; + + return nodeIdx; + } + + private const int InsertionSortThreshold = 16; + + private static void SortByAxis(Span bounds, Span indices, int start, int end, int axis) + { + int count = end - start; + if (count <= 1) + { + return; + } + + int depthLimit = 2 * FloorLog2(count); + IntroSort(bounds, indices, start, end - 1, axis, depthLimit); + } + + private static int FloorLog2(int n) + { + int result = 0; + while (n > 1) + { + n >>= 1; + result++; + } + return result; + } + + private static void IntroSort( + Span bounds, Span indices, int lo, int hi, int axis, int depthLimit) + { + while (hi - lo + 1 > InsertionSortThreshold) + { + if (depthLimit == 0) + { + HeapSort(bounds, indices, lo, hi, axis); + return; + } + depthLimit--; + + int pivot = Partition(bounds, indices, lo, hi, axis); + IntroSort(bounds, indices, pivot + 1, hi, axis, depthLimit); + hi = pivot - 1; + } + + InsertionSort(bounds, indices, lo, hi, axis); + } + + private static void InsertionSort( + Span bounds, Span indices, int lo, int hi, int axis) + { + for (int i = lo + 1; i <= hi; i++) + { + AABB keyBounds = bounds[i]; + int keyIndex = indices[i]; + float keyValue = GetAxisCenter(keyBounds, axis); + + int j = i - 1; + while (j >= lo && GetAxisCenter(bounds[j], axis) > keyValue) + { + bounds[j + 1] = bounds[j]; + indices[j + 1] = indices[j]; + j--; + } + + bounds[j + 1] = keyBounds; + indices[j + 1] = keyIndex; + } + } + + private static int Partition( + Span bounds, Span indices, int lo, int hi, int axis) + { + // Median-of-three pivot selection + int mid = lo + (hi - lo) / 2; + if (GetAxisCenter(bounds[mid], axis) < GetAxisCenter(bounds[lo], axis)) + { + (bounds[lo], bounds[mid]) = (bounds[mid], bounds[lo]); + (indices[lo], indices[mid]) = (indices[mid], indices[lo]); + } + if (GetAxisCenter(bounds[hi], axis) < GetAxisCenter(bounds[lo], axis)) + { + (bounds[lo], bounds[hi]) = (bounds[hi], bounds[lo]); + (indices[lo], indices[hi]) = (indices[hi], indices[lo]); + } + if (GetAxisCenter(bounds[mid], axis) < GetAxisCenter(bounds[hi], axis)) + { + (bounds[mid], bounds[hi]) = (bounds[hi], bounds[mid]); + (indices[mid], indices[hi]) = (indices[hi], indices[mid]); + } + + float pivotValue = GetAxisCenter(bounds[hi], axis); + int i = lo - 1; + + for (int j = lo; j < hi; j++) + { + if (GetAxisCenter(bounds[j], axis) <= pivotValue) + { + i++; + (bounds[i], bounds[j]) = (bounds[j], bounds[i]); + (indices[i], indices[j]) = (indices[j], indices[i]); + } + } + + i++; + (bounds[i], bounds[hi]) = (bounds[hi], bounds[i]); + (indices[i], indices[hi]) = (indices[hi], indices[i]); + return i; + } + + private static void HeapSort( + Span bounds, Span indices, int lo, int hi, int axis) + { + int count = hi - lo + 1; + for (int i = count / 2 - 1; i >= 0; i--) + { + SiftDown(bounds, indices, lo, i, count, axis); + } + + for (int i = count - 1; i > 0; i--) + { + (bounds[lo], bounds[lo + i]) = (bounds[lo + i], bounds[lo]); + (indices[lo], indices[lo + i]) = (indices[lo + i], indices[lo]); + SiftDown(bounds, indices, lo, 0, i, axis); + } + } + + private static void SiftDown( + Span bounds, Span indices, int lo, int root, int count, int axis) + { + while (true) + { + int child = 2 * root + 1; + if (child >= count) + { + break; + } + + if (child + 1 < count + && GetAxisCenter(bounds[lo + child], axis) < GetAxisCenter(bounds[lo + child + 1], axis)) + { + child++; + } + + if (GetAxisCenter(bounds[lo + root], axis) >= GetAxisCenter(bounds[lo + child], axis)) + { + break; + } + + (bounds[lo + root], bounds[lo + child]) = (bounds[lo + child], bounds[lo + root]); + (indices[lo + root], indices[lo + child]) = (indices[lo + child], indices[lo + root]); + root = child; + } + } + + private static float GetAxisCenter(AABB aabb, int axis) + { + Vector3 center = aabb.Center; + return axis switch + { + 0 => center.X, + 1 => center.Y, + _ => center.Z, + }; + } + + private readonly void FindPairsRecursive( + int nodeA, int nodeB, ref NativeList pairs, ReadOnlySpan entities) + { + BvhNode a = _nodes[nodeA]; + BvhNode b = _nodes[nodeB]; + + if (!a.Bounds.Intersects(b.Bounds)) + { + return; + } + + if (a.IsLeaf && b.IsLeaf) + { + if (nodeA != nodeB) + { + int idxA = a.EntityIndex; + int idxB = b.EntityIndex; + if (idxA < idxB) + { + pairs.Add(new CollisionPair(entities[idxA], entities[idxB])); + } + } + return; + } + + if (a.IsLeaf) + { + FindPairsRecursive(nodeA, b.LeftChild, ref pairs, entities); + FindPairsRecursive(nodeA, b.RightChild, ref pairs, entities); + } + else if (b.IsLeaf) + { + FindPairsRecursive(a.LeftChild, nodeB, ref pairs, entities); + FindPairsRecursive(a.RightChild, nodeB, ref pairs, entities); + } + else + { + FindPairsRecursive(a.LeftChild, b.LeftChild, ref pairs, entities); + FindPairsRecursive(a.LeftChild, b.RightChild, ref pairs, entities); + FindPairsRecursive(a.RightChild, b.LeftChild, ref pairs, entities); + FindPairsRecursive(a.RightChild, b.RightChild, ref pairs, entities); + } + } + + private static bool RayIntersectsAABB(Vector3 origin, Vector3 invDir, AABB aabb, float maxDist) + { + float t1 = (aabb.Min.X - origin.X) * invDir.X; + float t2 = (aabb.Max.X - origin.X) * invDir.X; + float tmin = MathF.Min(t1, t2); + float tmax = MathF.Max(t1, t2); + + t1 = (aabb.Min.Y - origin.Y) * invDir.Y; + t2 = (aabb.Max.Y - origin.Y) * invDir.Y; + tmin = MathF.Max(tmin, MathF.Min(t1, t2)); + tmax = MathF.Min(tmax, MathF.Max(t1, t2)); + + t1 = (aabb.Min.Z - origin.Z) * invDir.Z; + t2 = (aabb.Max.Z - origin.Z) * invDir.Z; + tmin = MathF.Max(tmin, MathF.Min(t1, t2)); + tmax = MathF.Min(tmax, MathF.Max(t1, t2)); + + return tmax >= MathF.Max(tmin, 0f) && tmin < maxDist; + } +} diff --git a/src/Seed.Engine/Physics/BroadPhase/SpatialHashGrid.cs b/src/Seed.Engine/Physics/BroadPhase/SpatialHashGrid.cs new file mode 100644 index 00000000..c867e218 --- /dev/null +++ b/src/Seed.Engine/Physics/BroadPhase/SpatialHashGrid.cs @@ -0,0 +1,150 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; + +namespace Seed.Engine.Physics.BroadPhase; + +/// +/// A grid-based spatial hash for efficient range queries such as OverlapSphere. +/// Each cell stores a list of entity indices. Rebuilt from scratch each frame. +/// +public struct SpatialHashGrid : IDisposable +{ + private NativeHashMap _cellHeads; + private NativeList _nextLinks; + private NativeList _entityIndices; + private float _cellSize; + private float _inverseCellSize; + private int _entryCount; + private bool _disposed; + + /// + /// Initializes a new with the specified cell size. + /// + public SpatialHashGrid(float cellSize, int initialCapacity) + { + _cellSize = cellSize; + _inverseCellSize = 1f / cellSize; + _cellHeads = new NativeHashMap(initialCapacity); + _nextLinks = new NativeList(initialCapacity); + _entityIndices = new NativeList(initialCapacity); + _entryCount = 0; + _disposed = false; + } + + /// + /// Gets the cell size. + /// + public readonly float CellSize => _cellSize; + + /// + /// Clears all entries, preparing for a new frame. + /// + public void Clear() + { + _cellHeads.Clear(); + _nextLinks.Clear(); + _entityIndices.Clear(); + _entryCount = 0; + } + + /// + /// Inserts an entity with the given AABB into the grid. + /// The entity is placed into every cell its AABB overlaps. + /// + public void Insert(int entityIndex, AABB bounds) + { + int minX = (int)MathF.Floor(bounds.Min.X * _inverseCellSize); + int minY = (int)MathF.Floor(bounds.Min.Y * _inverseCellSize); + int minZ = (int)MathF.Floor(bounds.Min.Z * _inverseCellSize); + int maxX = (int)MathF.Floor(bounds.Max.X * _inverseCellSize); + int maxY = (int)MathF.Floor(bounds.Max.Y * _inverseCellSize); + int maxZ = (int)MathF.Floor(bounds.Max.Z * _inverseCellSize); + + for (int x = minX; x <= maxX; x++) + { + for (int y = minY; y <= maxY; y++) + { + for (int z = minZ; z <= maxZ; z++) + { + int cellHash = HashCell(x, y, z); + int entryIdx = _entryCount; + _entryCount++; + + _entityIndices.Add(entityIndex); + + int prevHead = -1; + if (_cellHeads.TryGetValue(cellHash, out int existing)) + { + prevHead = existing; + } + + _nextLinks.Add(prevHead); + _cellHeads[cellHash] = entryIdx; + } + } + } + } + + /// + /// Queries all entity indices within cells that overlap the given sphere. + /// Results may contain duplicates which the caller must filter. + /// + public readonly void QuerySphere( + Vector3 center, float radius, ref NativeList results) + { + float r = radius; + int minX = (int)MathF.Floor((center.X - r) * _inverseCellSize); + int minY = (int)MathF.Floor((center.Y - r) * _inverseCellSize); + int minZ = (int)MathF.Floor((center.Z - r) * _inverseCellSize); + int maxX = (int)MathF.Floor((center.X + r) * _inverseCellSize); + int maxY = (int)MathF.Floor((center.Y + r) * _inverseCellSize); + int maxZ = (int)MathF.Floor((center.Z + r) * _inverseCellSize); + + for (int x = minX; x <= maxX; x++) + { + for (int y = minY; y <= maxY; y++) + { + for (int z = minZ; z <= maxZ; z++) + { + int cellHash = HashCell(x, y, z); + if (!_cellHeads.TryGetValue(cellHash, out int entryIdx)) + { + continue; + } + + while (entryIdx >= 0) + { + results.Add(_entityIndices[entryIdx]); + entryIdx = _nextLinks[entryIdx]; + } + } + } + } + } + + /// + public void Dispose() + { + if (!_disposed) + { + _cellHeads.Dispose(); + _nextLinks.Dispose(); + _entityIndices.Dispose(); + _disposed = true; + } + } + + private static int HashCell(int x, int y, int z) + { + unchecked + { + int hash = x * 73856093; + hash ^= y * 19349663; + hash ^= z * 83492791; + return hash; + } + } +} From a8c252b332d7ab7a3ca32bdeda458aad6cf08f9d Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:35 +0900 Subject: [PATCH 4/7] =?UTF-8?q?=E3=83=AC=E3=82=A4=E3=82=AD=E3=83=A3?= =?UTF-8?q?=E3=82=B9=E3=83=88API=E8=BF=BD=E5=8A=A0=20(Raycast,=20RaycastAl?= =?UTF-8?q?l,=20SphereCast,=20OverlapSphere)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BVHベースのRay/Sphere交差テストと、 SpatialHashGridベースのOverlapSphereクエリを実装。 Co-Authored-By: Claude Opus 4.6 --- src/Seed.Engine/Physics/Query/PhysicsWorld.cs | 197 +++++++++++++ .../Physics/Query/RayShapeIntersection.cs | 279 ++++++++++++++++++ 2 files changed, 476 insertions(+) create mode 100644 src/Seed.Engine/Physics/Query/PhysicsWorld.cs create mode 100644 src/Seed.Engine/Physics/Query/RayShapeIntersection.cs diff --git a/src/Seed.Engine/Physics/Query/PhysicsWorld.cs b/src/Seed.Engine/Physics/Query/PhysicsWorld.cs new file mode 100644 index 00000000..9aa48f4a --- /dev/null +++ b/src/Seed.Engine/Physics/Query/PhysicsWorld.cs @@ -0,0 +1,197 @@ +using System; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.BroadPhase; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Physics.Query; + +/// +/// Provides high-level physics query APIs: Raycast, RaycastAll, SphereCast, OverlapSphere. +/// Uses BVH for ray queries and SpatialHashGrid for sphere overlap queries. +/// +public static class PhysicsWorld +{ + /// + /// Casts a ray and returns the closest hit. + /// + public static bool Raycast( + Ray ray, float maxDistance, + ref BvhTree bvh, + ReadOnlySpan entities, + ReadOnlySpan colliders, + ReadOnlySpan positions, + ReadOnlySpan rotations, + out RaycastHit hit) + { + hit = default; + + var candidates = new NativeList(32); + bvh.QueryRay(ray, maxDistance, ref candidates); + + bool found = false; + float closestDist = maxDistance; + + for (int i = 0; i < candidates.Length; i++) + { + int idx = candidates[i]; + Result rayResult = RayShapeIntersection.RayCollider( + ray, colliders[idx], positions[idx], rotations[idx], + out float dist, out Vector3 normal); + if (rayResult.IsSuccess && rayResult.Value) + { + if (dist < closestDist) + { + closestDist = dist; + hit = new RaycastHit( + entities[idx], ray.GetPoint(dist), normal, dist); + found = true; + } + } + } + + candidates.Dispose(); + return found; + } + + /// + /// Casts a ray and returns all hits within max distance, sorted by distance. + /// + public static void RaycastAll( + Ray ray, float maxDistance, + ref BvhTree bvh, + ReadOnlySpan entities, + ReadOnlySpan colliders, + ReadOnlySpan positions, + ReadOnlySpan rotations, + ref NativeList results) + { + var candidates = new NativeList(32); + bvh.QueryRay(ray, maxDistance, ref candidates); + + for (int i = 0; i < candidates.Length; i++) + { + int idx = candidates[i]; + Result rayResult = RayShapeIntersection.RayCollider( + ray, colliders[idx], positions[idx], rotations[idx], + out float dist, out Vector3 normal); + if (rayResult.IsSuccess && rayResult.Value) + { + if (dist <= maxDistance) + { + results.Add(new RaycastHit( + entities[idx], ray.GetPoint(dist), normal, dist)); + } + } + } + + candidates.Dispose(); + SortHitsByDistance(ref results); + } + + /// + /// Casts a sphere along a ray and returns the closest hit. + /// Implemented by expanding collider AABBs by the sphere radius and then raycasting. + /// + public static bool SphereCast( + Ray ray, float sphereRadius, float maxDistance, + ref BvhTree bvh, + ReadOnlySpan entities, + ReadOnlySpan colliders, + ReadOnlySpan positions, + ReadOnlySpan rotations, + out RaycastHit hit) + { + hit = default; + + var candidates = new NativeList(32); + bvh.QueryRay(ray, maxDistance + sphereRadius, ref candidates); + + bool found = false; + float closestDist = maxDistance; + + for (int i = 0; i < candidates.Length; i++) + { + int idx = candidates[i]; + + float expandedRadius = sphereRadius; + if (colliders[idx].Type == ColliderType.Sphere) + { + expandedRadius += colliders[idx].Data.Sphere.Radius; + } + + if (RayShapeIntersection.RaySphere( + ray, positions[idx], expandedRadius, + out float dist, out Vector3 normal)) + { + if (dist < closestDist) + { + closestDist = dist; + hit = new RaycastHit( + entities[idx], ray.GetPoint(dist), normal, dist); + found = true; + } + } + } + + candidates.Dispose(); + return found; + } + + /// + /// Returns all entity indices within a sphere using the spatial hash grid. + /// + public static void OverlapSphere( + Vector3 center, float radius, + ref SpatialHashGrid grid, + ReadOnlySpan entities, + ReadOnlySpan bounds, + ref NativeList results) + { + var candidates = new NativeList(32); + grid.QuerySphere(center, radius, ref candidates); + + Sphere querySphere = new Sphere(center, radius); + + var seen = new NativeHashMap(candidates.Length > 0 ? candidates.Length : 4); + + for (int i = 0; i < candidates.Length; i++) + { + int idx = candidates[i]; + ulong entityId = entities[idx].Id; + + if (seen.ContainsKey(entityId)) + { + continue; + } + seen[entityId] = 1; + + if (bounds[idx].Intersects(querySphere)) + { + results.Add(entities[idx]); + } + } + + seen.Dispose(); + candidates.Dispose(); + } + + private static void SortHitsByDistance(ref NativeList hits) + { + for (int i = 0; i < hits.Length - 1; i++) + { + for (int j = 0; j < hits.Length - 1 - i; j++) + { + if (hits[j].Distance > hits[j + 1].Distance) + { + RaycastHit temp = hits[j]; + hits[j] = hits[j + 1]; + hits[j + 1] = temp; + } + } + } + } +} diff --git a/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs b/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs new file mode 100644 index 00000000..89ff10ff --- /dev/null +++ b/src/Seed.Engine/Physics/Query/RayShapeIntersection.cs @@ -0,0 +1,279 @@ +using System; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Physics.Query; + +/// +/// Provides ray-shape intersection tests for Sphere, Box, and Capsule shapes. +/// +public static class RayShapeIntersection +{ + /// + /// Tests a ray against a sphere at the given position. + /// Returns true if the ray hits, with distance and normal. + /// + public static bool RaySphere( + Ray ray, Vector3 sphereCenter, float sphereRadius, + out float distance, out Vector3 normal) + { + distance = 0f; + normal = Vector3.Zero; + + Vector3 oc = ray.Origin - sphereCenter; + float b = Vector3.Dot(oc, ray.Direction); + float c = Vector3.Dot(oc, oc) - sphereRadius * sphereRadius; + + float discriminant = b * b - c; + if (discriminant < 0f) + { + return false; + } + + float sqrtDisc = MathF.Sqrt(discriminant); + float t = -b - sqrtDisc; + + if (t < 0f) + { + t = -b + sqrtDisc; + } + + if (t < 0f) + { + return false; + } + + distance = t; + Vector3 hitPoint = ray.GetPoint(t); + normal = (hitPoint - sphereCenter).Normalize(); + return true; + } + + /// + /// Tests a ray against an oriented box. + /// + public static bool RayBox( + Ray ray, Vector3 boxCenter, Quaternion boxRotation, Vector3 halfExtents, + out float distance, out Vector3 normal) + { + distance = 0f; + normal = Vector3.Zero; + + Quaternion invRot = boxRotation.Conjugate(); + Vector3 localOrigin = invRot.RotateVector(ray.Origin - boxCenter); + Vector3 localDir = invRot.RotateVector(ray.Direction); + + float tmin = float.NegativeInfinity; + float tmax = float.PositiveInfinity; + int hitAxis = 0; + float hitSign = 1f; + + for (int axis = 0; axis < 3; axis++) + { + float origin = GetComponent(localOrigin, axis); + float dir = GetComponent(localDir, axis); + float extent = GetComponent(halfExtents, axis); + + if (MathF.Abs(dir) < MathHelper.Epsilon) + { + if (origin < -extent || origin > extent) + { + return false; + } + continue; + } + + float invD = 1f / dir; + float t1 = (-extent - origin) * invD; + float t2 = (extent - origin) * invD; + + float sign = 1f; + if (t1 > t2) + { + (t1, t2) = (t2, t1); + sign = -1f; + } + + if (t1 > tmin) + { + tmin = t1; + hitAxis = axis; + hitSign = -sign; + } + + tmax = MathF.Min(tmax, t2); + + if (tmin > tmax) + { + return false; + } + } + + if (tmin < 0f) + { + return false; + } + + distance = tmin; + + Vector3 localNormal = hitAxis switch + { + 0 => new Vector3(hitSign, 0f, 0f), + 1 => new Vector3(0f, hitSign, 0f), + _ => new Vector3(0f, 0f, hitSign), + }; + + normal = boxRotation.RotateVector(localNormal); + return true; + } + + /// + /// Tests a ray against a capsule. + /// + public static bool RayCapsule( + Ray ray, Vector3 capsuleCenter, Quaternion capsuleRotation, + float capsuleRadius, float capsuleHalfHeight, + out float distance, out Vector3 normal) + { + distance = 0f; + normal = Vector3.Zero; + + Vector3 up = capsuleRotation.RotateVector(Vector3.UnitY); + Vector3 topCenter = capsuleCenter + up * capsuleHalfHeight; + Vector3 bottomCenter = capsuleCenter - up * capsuleHalfHeight; + + float bestDist = float.MaxValue; + Vector3 bestNormal = Vector3.Zero; + bool hit = false; + + if (RayCylinder(ray, bottomCenter, topCenter, capsuleRadius, out float cylDist, out Vector3 cylNormal)) + { + if (cylDist < bestDist) + { + bestDist = cylDist; + bestNormal = cylNormal; + hit = true; + } + } + + if (RaySphere(ray, topCenter, capsuleRadius, out float topDist, out Vector3 topNorm)) + { + if (topDist < bestDist) + { + bestDist = topDist; + bestNormal = topNorm; + hit = true; + } + } + + if (RaySphere(ray, bottomCenter, capsuleRadius, out float botDist, out Vector3 botNorm)) + { + if (botDist < bestDist) + { + bestDist = botDist; + bestNormal = botNorm; + hit = true; + } + } + + if (hit) + { + distance = bestDist; + normal = bestNormal; + } + + return hit; + } + + /// + /// Tests a ray against a collider at the given transform. + /// + public static Result RayCollider( + Ray ray, Collider collider, Vector3 position, Quaternion rotation, + out float distance, out Vector3 normal) + { + switch (collider.Type) + { + case ColliderType.Sphere: + return Result.Ok(RaySphere( + ray, position, collider.Data.Sphere.Radius, out distance, out normal)); + case ColliderType.Box: + return Result.Ok(RayBox( + ray, position, rotation, collider.Data.Box.HalfExtents, + out distance, out normal)); + case ColliderType.Capsule: + return Result.Ok(RayCapsule( + ray, position, rotation, + collider.Data.Capsule.Radius, collider.Data.Capsule.HalfHeight, + out distance, out normal)); + default: + distance = 0f; + normal = Vector3.Zero; + return Result.Fail(ErrorCodes.InvalidColliderType); + } + } + + private static bool RayCylinder( + Ray ray, Vector3 p1, Vector3 p2, float radius, + out float distance, out Vector3 normal) + { + distance = 0f; + normal = Vector3.Zero; + + Vector3 d = p2 - p1; + Vector3 m = ray.Origin - p1; + Vector3 n = ray.Direction; + + float md = Vector3.Dot(m, d); + float nd = Vector3.Dot(n, d); + float dd = Vector3.Dot(d, d); + + float mn = Vector3.Dot(m, n); + float mm = Vector3.Dot(m, m); + + float a = dd * Vector3.Dot(n, n) - nd * nd; + float b = dd * mn - nd * md; + float c = dd * (mm - radius * radius) - md * md; + + if (MathF.Abs(a) < MathHelper.Epsilon) + { + return false; + } + + float discriminant = b * b - a * c; + if (discriminant < 0f) + { + return false; + } + + float t = (-b - MathF.Sqrt(discriminant)) / a; + if (t < 0f) + { + return false; + } + + float y = md + t * nd; + if (y < 0f || y > dd) + { + return false; + } + + distance = t; + Vector3 hitPoint = ray.GetPoint(t); + Vector3 onAxis = p1 + d * (y / dd); + normal = (hitPoint - onAxis).Normalize(); + return true; + } + + private static float GetComponent(Vector3 v, int axis) + { + return axis switch + { + 0 => v.X, + 1 => v.Y, + _ => v.Z, + }; + } +} From a6c6a577d2f3bc5c57ae188d1c2d8e5a87344e40 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:42 +0900 Subject: [PATCH 5/7] =?UTF-8?q?=E8=A1=9D=E7=AA=81=E6=A4=9C=E5=87=BAECS?= =?UTF-8?q?=E3=82=B7=E3=82=B9=E3=83=86=E3=83=A0=E7=B5=B1=E5=90=88=20(Broad?= =?UTF-8?q?Phase+NarrowPhase)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BroadPhaseSystem (BVH構築+ペア抽出) と NarrowPhaseSystem (GJK+EPA実行) をPhysicsフェーズに追加。 Co-Authored-By: Claude Opus 4.6 --- .../Ecs/Components/BroadPhaseResult.cs | 23 +++ .../Ecs/Components/ColliderComponent.cs | 17 ++ .../Ecs/Components/NarrowPhaseResult.cs | 28 +++ src/Seed.Engine/Physics/BroadPhaseSystem.cs | 189 ++++++++++++++++++ src/Seed.Engine/Physics/NarrowPhaseSystem.cs | 171 ++++++++++++++++ 5 files changed, 428 insertions(+) create mode 100644 src/Seed.Engine/Ecs/Components/BroadPhaseResult.cs create mode 100644 src/Seed.Engine/Ecs/Components/ColliderComponent.cs create mode 100644 src/Seed.Engine/Ecs/Components/NarrowPhaseResult.cs create mode 100644 src/Seed.Engine/Physics/BroadPhaseSystem.cs create mode 100644 src/Seed.Engine/Physics/NarrowPhaseSystem.cs diff --git a/src/Seed.Engine/Ecs/Components/BroadPhaseResult.cs b/src/Seed.Engine/Ecs/Components/BroadPhaseResult.cs new file mode 100644 index 00000000..dca32e96 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/BroadPhaseResult.cs @@ -0,0 +1,23 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Ecs.Components; + +/// +/// Singleton ECS component holding a pointer to the broad phase collision pair results. +/// Uses an unsafe pointer to a NativeList to satisfy unmanaged struct constraints. +/// +[StructLayout(LayoutKind.Sequential)] +public unsafe struct BroadPhaseResult : IComponent +{ + /// + /// Pointer to the NativeList of CollisionPairs from the broad phase. + /// + public CollisionPair* Pairs; + + /// + /// Number of collision pairs. + /// + public int Count; +} diff --git a/src/Seed.Engine/Ecs/Components/ColliderComponent.cs b/src/Seed.Engine/Ecs/Components/ColliderComponent.cs new file mode 100644 index 00000000..249c96e7 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/ColliderComponent.cs @@ -0,0 +1,17 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Ecs.Components; + +/// +/// ECS component that attaches a collision shape to an entity. +/// +[StructLayout(LayoutKind.Sequential)] +public struct ColliderComponent : IComponent +{ + /// + /// The collider shape and type data. + /// + public Collider Value; +} diff --git a/src/Seed.Engine/Ecs/Components/NarrowPhaseResult.cs b/src/Seed.Engine/Ecs/Components/NarrowPhaseResult.cs new file mode 100644 index 00000000..0606afb8 --- /dev/null +++ b/src/Seed.Engine/Ecs/Components/NarrowPhaseResult.cs @@ -0,0 +1,28 @@ +using System.Runtime.InteropServices; + +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Ecs.Components; + +/// +/// Singleton ECS component holding a pointer to the narrow phase contact point results. +/// Uses an unsafe pointer to satisfy unmanaged struct constraints. +/// +[StructLayout(LayoutKind.Sequential)] +public unsafe struct NarrowPhaseResult : IComponent +{ + /// + /// Pointer to the array of ContactPoints from the narrow phase. + /// + public ContactPoint* Contacts; + + /// + /// Pointer to the array of CollisionPairs corresponding to each contact. + /// + public CollisionPair* Pairs; + + /// + /// Number of contact results. + /// + public int Count; +} diff --git a/src/Seed.Engine/Physics/BroadPhaseSystem.cs b/src/Seed.Engine/Physics/BroadPhaseSystem.cs new file mode 100644 index 00000000..704761ee --- /dev/null +++ b/src/Seed.Engine/Physics/BroadPhaseSystem.cs @@ -0,0 +1,189 @@ +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.Collision; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that builds a BVH from all entities with colliders and +/// extracts overlapping pairs for the narrow phase. +/// Runs in the Physics frame phase. +/// +public sealed class BroadPhaseSystem : ISystem +{ + private const int StackAllocThreshold = 256; + private readonly QueryDescription _colliderQuery; + private readonly QueryDescription _resultQuery; + private BvhTree _bvh; + private NativeList _pairs; + private bool _initialized; + + /// + /// Initializes a new . + /// + public BroadPhaseSystem() + { + _colliderQuery = new QueryBuilder() + .WithRead() + .WithRead() + .Build(); + + _resultQuery = new QueryBuilder() + .WithWrite() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _colliderQuery; + + /// + public unsafe void Execute(World world) + { + if (!_initialized) + { + _bvh = new BvhTree(); + _pairs = new NativeList(256); + _initialized = true; + } + + _pairs.Clear(); + + int totalEntities = 0; + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_colliderQuery.Matches(storage.Archetype)) + { + continue; + } + for (int c = 0; c < storage.ChunkCount; c++) + { + ref Chunk chunk = ref storage.GetChunk(c); + totalEntities += chunk.Count; + } + } + + if (totalEntities == 0) + { + UpdateResult(world); + return; + } + + if (totalEntities <= StackAllocThreshold) + { + Span bounds = stackalloc AABB[totalEntities]; + Span entityIndices = stackalloc int[totalEntities]; + Span entities = stackalloc Entity[totalEntities]; + int idx = FillEntityData(world, bounds, entityIndices, entities); + _bvh.Build(bounds[..idx], entityIndices[..idx]); + _bvh.FindOverlappingPairs(ref _pairs, entities[..idx]); + } + else + { + var boundsHeap = new NativeArray(totalEntities); + var indicesHeap = new NativeArray(totalEntities); + var entitiesHeap = new NativeArray(totalEntities); + int idx = FillEntityData( + world, boundsHeap.AsSpan(), indicesHeap.AsSpan(), entitiesHeap.AsSpan()); + _bvh.Build(boundsHeap.AsSpan()[..idx], indicesHeap.AsSpan()[..idx]); + _bvh.FindOverlappingPairs(ref _pairs, entitiesHeap.AsSpan()[..idx]); + boundsHeap.Dispose(); + indicesHeap.Dispose(); + entitiesHeap.Dispose(); + } + + UpdateResult(world); + } + + private int FillEntityData( + World world, Span bounds, Span entityIndices, Span entities) + { + int idx = 0; + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_colliderQuery.Matches(storage.Archetype)) + { + continue; + } + + int ccIdx = storage.GetComponentIndex( + ComponentRegistry.Get().TypeId); + int ltIdx = storage.GetComponentIndex( + ComponentRegistry.Get().TypeId); + + 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 ColliderComponent cc = ref chunk.GetComponent(ccIdx, i); + ref LocalTransform lt = ref chunk.GetComponent(ltIdx, i); + + Result aabbResult = ColliderBounds.ComputeAABB( + cc.Value, lt.Position, lt.Rotation); + if (aabbResult.IsFailure) + { + continue; + } + + bounds[idx] = aabbResult.Value; + entityIndices[idx] = idx; + entities[idx] = chunk.GetEntity(i); + idx++; + } + } + } + + return idx; + } + + private unsafe void UpdateResult(World world) + { + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_resultQuery.Matches(storage.Archetype)) + { + continue; + } + + int brIdx = 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 BroadPhaseResult result = ref chunk.GetComponent(brIdx, 0); + if (_pairs.Length > 0) + { + fixed (CollisionPair* ptr = &_pairs.AsSpan()[0]) + { + result.Pairs = ptr; + } + } + else + { + result.Pairs = null; + } + result.Count = _pairs.Length; + return; + } + } + } + } +} diff --git a/src/Seed.Engine/Physics/NarrowPhaseSystem.cs b/src/Seed.Engine/Physics/NarrowPhaseSystem.cs new file mode 100644 index 00000000..de722b92 --- /dev/null +++ b/src/Seed.Engine/Physics/NarrowPhaseSystem.cs @@ -0,0 +1,171 @@ +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.Physics.Collision; + +namespace Seed.Engine.Physics; + +/// +/// ECS system that runs GJK+EPA on collision pairs from the broad phase +/// to produce precise contact points. +/// Runs in the Physics frame phase, after BroadPhaseSystem. +/// +public sealed class NarrowPhaseSystem : ISystem +{ + private readonly QueryDescription _broadPhaseQuery; + private readonly QueryDescription _colliderQuery; + private readonly QueryDescription _narrowPhaseQuery; + private NativeList _contacts; + private NativeList _contactPairs; + private bool _initialized; + + /// + /// Initializes a new . + /// + public NarrowPhaseSystem() + { + _broadPhaseQuery = new QueryBuilder() + .WithRead() + .Build(); + + _colliderQuery = new QueryBuilder() + .WithRead() + .WithRead() + .Build(); + + _narrowPhaseQuery = new QueryBuilder() + .WithWrite() + .Build(); + } + + /// + public FramePhase Phase => FramePhase.Physics; + + /// + public QueryDescription GetQuery() => _colliderQuery; + + /// + public unsafe void Execute(World world) + { + if (!_initialized) + { + _contacts = new NativeList(256); + _contactPairs = new NativeList(256); + _initialized = true; + } + + _contacts.Clear(); + _contactPairs.Clear(); + + CollisionPair* broadPairs = null; + int broadCount = 0; + + for (int s = 0; s < world.Storages.Count; s++) + { + var storage = world.Storages[s]; + if (!_broadPhaseQuery.Matches(storage.Archetype)) + { + continue; + } + + int brIdx = 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 BroadPhaseResult br = ref chunk.GetComponent(brIdx, 0); + broadPairs = br.Pairs; + broadCount = br.Count; + break; + } + } + if (broadCount > 0) break; + } + + if (broadCount == 0 || broadPairs == null) + { + UpdateResult(world); + return; + } + + for (int p = 0; p < broadCount; p++) + { + CollisionPair pair = broadPairs[p]; + + if (!world.IsAlive(pair.EntityA) || !world.IsAlive(pair.EntityB)) + { + continue; + } + + if (!world.HasComponent(pair.EntityA) + || !world.HasComponent(pair.EntityB)) + { + continue; + } + + ref ColliderComponent ccA = ref world.GetComponent(pair.EntityA); + ref LocalTransform ltA = ref world.GetComponent(pair.EntityA); + ref ColliderComponent ccB = ref world.GetComponent(pair.EntityB); + ref LocalTransform ltB = ref world.GetComponent(pair.EntityB); + + Result contactResult = GjkEpa.ComputeContact( + ccA.Value, ltA.Position, ltA.Rotation, + ccB.Value, ltB.Position, ltB.Rotation, + out ContactPoint contact); + if (contactResult.IsSuccess && contactResult.Value) + { + _contacts.Add(contact); + _contactPairs.Add(pair); + } + } + + UpdateResult(world); + } + + private unsafe void UpdateResult(World world) + { + 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 result = ref chunk.GetComponent(npIdx, 0); + if (_contacts.Length > 0) + { + fixed (ContactPoint* cPtr = &_contacts.AsSpan()[0]) + { + result.Contacts = cPtr; + } + fixed (CollisionPair* pPtr = &_contactPairs.AsSpan()[0]) + { + result.Pairs = pPtr; + } + } + else + { + result.Contacts = null; + result.Pairs = null; + } + result.Count = _contacts.Length; + return; + } + } + } + } +} From 30da96a0e5efa5257c6d204fb0a2fd859f5fcbc7 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:46 +0900 Subject: [PATCH 6/7] =?UTF-8?q?=E8=A1=9D=E7=AA=81=E6=A4=9C=E5=87=BA?= =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88=E8=BF=BD=E5=8A=A0=20(72=E3=83=86?= =?UTF-8?q?=E3=82=B9=E3=83=88)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit GJK+EPA、BVH、SpatialHashGrid、レイキャスト、 ECSシステム統合の単体テストを網羅的に追加。 Co-Authored-By: Claude Opus 4.6 --- .../Physics/BroadPhase/BvhTreeTests.cs | 173 ++++++++++++++++ .../BroadPhase/SpatialHashGridTests.cs | 89 +++++++++ .../Physics/BroadPhaseSystemTests.cs | 128 ++++++++++++ .../Physics/Collision/BoxShapeTests.cs | 42 ++++ .../Physics/Collision/CapsuleShapeTests.cs | 42 ++++ .../Physics/Collision/ColliderBoundsTests.cs | 80 ++++++++ .../Physics/Collision/ColliderTests.cs | 44 +++++ .../Physics/Collision/ContactPointTests.cs | 38 ++++ .../Physics/Collision/GjkEpaTests.cs | 184 ++++++++++++++++++ .../Physics/Collision/GjkSupportTests.cs | 97 +++++++++ .../Physics/Collision/RayTests.cs | 60 ++++++ .../Physics/Collision/SimplexTests.cs | 75 +++++++ .../Physics/NarrowPhaseSystemTests.cs | 117 +++++++++++ .../Physics/Query/PhysicsWorldTests.cs | 145 ++++++++++++++ .../Query/RayShapeIntersectionTests.cs | 132 +++++++++++++ 15 files changed, 1446 insertions(+) create mode 100644 src/Seed.Engine.Tests/Physics/BroadPhase/BvhTreeTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/BroadPhase/SpatialHashGridTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/BroadPhaseSystemTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/BoxShapeTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/CapsuleShapeTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/ColliderBoundsTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/ColliderTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/ContactPointTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/GjkSupportTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/RayTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Collision/SimplexTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/NarrowPhaseSystemTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Query/PhysicsWorldTests.cs create mode 100644 src/Seed.Engine.Tests/Physics/Query/RayShapeIntersectionTests.cs diff --git a/src/Seed.Engine.Tests/Physics/BroadPhase/BvhTreeTests.cs b/src/Seed.Engine.Tests/Physics/BroadPhase/BvhTreeTests.cs new file mode 100644 index 00000000..a2ab7c84 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/BroadPhase/BvhTreeTests.cs @@ -0,0 +1,173 @@ +using System; +using Xunit; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.BroadPhase; +using Seed.Engine.Physics.Collision; + +namespace Seed.Engine.Tests.Physics.BroadPhase; + +public class BvhTreeTests +{ + [Fact] + public void Build_SingleElement_CreatesLeafNode() + { + // Given + var bvh = new BvhTree(); + Span bounds = [new AABB(Vector3.Zero, Vector3.One)]; + Span indices = [0]; + + // When + bvh.Build(bounds, indices); + + // Then + Assert.Equal(1, bvh.NodeCount); + + bvh.Dispose(); + } + + [Fact] + public void Build_MultipleElements_CreatesTree() + { + // Given + var bvh = new BvhTree(); + Span bounds = + [ + new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f)), + new AABB(new Vector3(5f, 0f, 0f), new Vector3(6f, 1f, 1f)), + new AABB(new Vector3(10f, 0f, 0f), new Vector3(11f, 1f, 1f)), + ]; + Span indices = [0, 1, 2]; + + // When + bvh.Build(bounds, indices); + + // Then: 3 leaves + internal nodes + Assert.True(bvh.NodeCount >= 3); + + bvh.Dispose(); + } + + [Fact] + public void QueryAABB_Intersecting_ReturnsMatches() + { + // Given + var bvh = new BvhTree(); + Span bounds = + [ + new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f)), + new AABB(new Vector3(5f, 0f, 0f), new Vector3(6f, 1f, 1f)), + new AABB(new Vector3(10f, 0f, 0f), new Vector3(11f, 1f, 1f)), + ]; + Span indices = [0, 1, 2]; + bvh.Build(bounds, indices); + + var queryBounds = new AABB(new Vector3(4f, 0f, 0f), new Vector3(7f, 1f, 1f)); + var results = new NativeList(8); + + // When + bvh.QueryAABB(queryBounds, ref results); + + // Then: only index 1 should match + Assert.True(results.Length >= 1); + + results.Dispose(); + bvh.Dispose(); + } + + [Fact] + public void QueryAABB_NonIntersecting_ReturnsEmpty() + { + // Given + var bvh = new BvhTree(); + Span bounds = + [ + new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f)), + ]; + Span indices = [0]; + bvh.Build(bounds, indices); + + var queryBounds = new AABB(new Vector3(50f, 50f, 50f), new Vector3(51f, 51f, 51f)); + var results = new NativeList(8); + + // When + bvh.QueryAABB(queryBounds, ref results); + + // Then + Assert.Equal(0, results.Length); + + results.Dispose(); + bvh.Dispose(); + } + + [Fact] + public void QueryRay_HitsLeaf_ReturnsMatch() + { + // Given + var bvh = new BvhTree(); + Span bounds = + [ + new AABB(new Vector3(4f, -1f, -1f), new Vector3(6f, 1f, 1f)), + ]; + Span indices = [0]; + bvh.Build(bounds, indices); + + var ray = new Ray(Vector3.Zero, Vector3.UnitX); + var results = new NativeList(8); + + // When + bvh.QueryRay(ray, 100f, ref results); + + // Then + Assert.Equal(1, results.Length); + Assert.Equal(0, results[0]); + + results.Dispose(); + bvh.Dispose(); + } + + [Fact] + public void FindOverlappingPairs_OverlappingLeaves_ReturnsPair() + { + // Given + var bvh = new BvhTree(); + Span bounds = + [ + new AABB(new Vector3(0f, 0f, 0f), new Vector3(2f, 2f, 2f)), + new AABB(new Vector3(1f, 0f, 0f), new Vector3(3f, 2f, 2f)), + ]; + Span indices = [0, 1]; + bvh.Build(bounds, indices); + + Entity e0 = new Entity(0, 1); + Entity e1 = new Entity(1, 1); + ReadOnlySpan entities = [e0, e1]; + var pairs = new NativeList(8); + + // When + bvh.FindOverlappingPairs(ref pairs, entities); + + // Then + Assert.Equal(1, pairs.Length); + + pairs.Dispose(); + bvh.Dispose(); + } + + [Fact] + public void Build_Empty_HandlesGracefully() + { + // Given + var bvh = new BvhTree(); + + // When + bvh.Build(ReadOnlySpan.Empty, ReadOnlySpan.Empty); + + // Then + Assert.Equal(0, bvh.NodeCount); + + bvh.Dispose(); + } +} diff --git a/src/Seed.Engine.Tests/Physics/BroadPhase/SpatialHashGridTests.cs b/src/Seed.Engine.Tests/Physics/BroadPhase/SpatialHashGridTests.cs new file mode 100644 index 00000000..7ad9845a --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/BroadPhase/SpatialHashGridTests.cs @@ -0,0 +1,89 @@ +using Xunit; + +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.BroadPhase; + +namespace Seed.Engine.Tests.Physics.BroadPhase; + +public class SpatialHashGridTests +{ + [Fact] + public void Insert_And_QuerySphere_FindsEntity() + { + // Given + var grid = new SpatialHashGrid(2.0f, 16); + var bounds = new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f)); + grid.Insert(0, bounds); + + var results = new NativeList(8); + + // When + grid.QuerySphere(new Vector3(0.5f, 0.5f, 0.5f), 1.0f, ref results); + + // Then + Assert.True(results.Length >= 1); + + results.Dispose(); + grid.Dispose(); + } + + [Fact] + public void QuerySphere_FarAway_ReturnsEmpty() + { + // Given + var grid = new SpatialHashGrid(2.0f, 16); + var bounds = new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f)); + grid.Insert(0, bounds); + + var results = new NativeList(8); + + // When + grid.QuerySphere(new Vector3(100f, 100f, 100f), 1.0f, ref results); + + // Then + Assert.Equal(0, results.Length); + + results.Dispose(); + grid.Dispose(); + } + + [Fact] + public void Clear_RemovesAllEntries() + { + // Given + var grid = new SpatialHashGrid(2.0f, 16); + grid.Insert(0, new AABB(Vector3.Zero, Vector3.One)); + + // When + grid.Clear(); + var results = new NativeList(8); + grid.QuerySphere(new Vector3(0.5f, 0.5f, 0.5f), 1.0f, ref results); + + // Then + Assert.Equal(0, results.Length); + + results.Dispose(); + grid.Dispose(); + } + + [Fact] + public void Insert_MultipleEntities_AllFound() + { + // Given + var grid = new SpatialHashGrid(2.0f, 16); + grid.Insert(0, new AABB(new Vector3(0f, 0f, 0f), new Vector3(1f, 1f, 1f))); + grid.Insert(1, new AABB(new Vector3(0.5f, 0f, 0f), new Vector3(1.5f, 1f, 1f))); + + var results = new NativeList(8); + + // When + grid.QuerySphere(new Vector3(0.5f, 0.5f, 0.5f), 2.0f, ref results); + + // Then + Assert.True(results.Length >= 2); + + results.Dispose(); + grid.Dispose(); + } +} diff --git a/src/Seed.Engine.Tests/Physics/BroadPhaseSystemTests.cs b/src/Seed.Engine.Tests/Physics/BroadPhaseSystemTests.cs new file mode 100644 index 00000000..b6612f78 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/BroadPhaseSystemTests.cs @@ -0,0 +1,128 @@ +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 BroadPhaseSystemTests +{ + [Fact] + public void Phase_IsPhysics() + { + // Given / When + var system = new BroadPhaseSystem(); + + // Then + Assert.Equal(FramePhase.Physics, system.Phase); + } + + [Fact] + public unsafe void Execute_OverlappingEntities_ProducesPairs() + { + // Given + var world = new World(); + using var _ = world; + + var brType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes = [brType]; + Entity singleton = world.CreateEntity(singletonTypes); + + var ccType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [ccType, ltType]; + + Entity e0 = world.CreateEntity(entityTypes); + Entity e1 = world.CreateEntity(entityTypes); + + world.GetComponent(e0) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e0) = + LocalTransform.FromPosition(new Vector3(0f, 0f, 0f)); + + world.GetComponent(e1) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e1) = + LocalTransform.FromPosition(new Vector3(1f, 0f, 0f)); + + var system = new BroadPhaseSystem(); + + // When + system.Execute(world); + + // Then + ref BroadPhaseResult result = ref world.GetComponent(singleton); + Assert.True(result.Count > 0); + } + + [Fact] + public unsafe void Execute_NonOverlappingEntities_ProducesNoPairs() + { + // Given + var world = new World(); + using var _ = world; + + var brType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes = [brType]; + Entity singleton = world.CreateEntity(singletonTypes); + + var ccType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [ccType, ltType]; + + Entity e0 = world.CreateEntity(entityTypes); + Entity e1 = world.CreateEntity(entityTypes); + + world.GetComponent(e0) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e0) = + LocalTransform.FromPosition(new Vector3(0f, 0f, 0f)); + + world.GetComponent(e1) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e1) = + LocalTransform.FromPosition(new Vector3(100f, 0f, 0f)); + + var system = new BroadPhaseSystem(); + + // When + system.Execute(world); + + // Then + ref BroadPhaseResult result = ref world.GetComponent(singleton); + Assert.Equal(0, result.Count); + } + + [Fact] + public unsafe void Execute_NoEntities_ProducesNoPairs() + { + // Given + var world = new World(); + using var _ = world; + + var brType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes = [brType]; + Entity singleton = world.CreateEntity(singletonTypes); + + var system = new BroadPhaseSystem(); + + // When + system.Execute(world); + + // Then + ref BroadPhaseResult result = ref world.GetComponent(singleton); + Assert.Equal(0, result.Count); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/BoxShapeTests.cs b/src/Seed.Engine.Tests/Physics/Collision/BoxShapeTests.cs new file mode 100644 index 00000000..460b0613 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/BoxShapeTests.cs @@ -0,0 +1,42 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class BoxShapeTests +{ + [Fact] + public void Constructor_StoresHalfExtents() + { + // Given / When + var box = new BoxShape(new Vector3(1f, 2f, 3f)); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 3f), box.HalfExtents); + } + + [Fact] + public void Equals_SameValues_ReturnsTrue() + { + // Given + var a = new BoxShape(new Vector3(1f, 2f, 3f)); + var b = new BoxShape(new Vector3(1f, 2f, 3f)); + + // Then + Assert.True(a.Equals(b)); + } + + [Fact] + public void Equals_DifferentValues_ReturnsFalse() + { + // Given + var a = new BoxShape(new Vector3(1f, 2f, 3f)); + var b = new BoxShape(new Vector3(4f, 5f, 6f)); + + // Then + Assert.False(a.Equals(b)); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/CapsuleShapeTests.cs b/src/Seed.Engine.Tests/Physics/Collision/CapsuleShapeTests.cs new file mode 100644 index 00000000..1874ab0e --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/CapsuleShapeTests.cs @@ -0,0 +1,42 @@ +using Xunit; + +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class CapsuleShapeTests +{ + [Fact] + public void Constructor_StoresValues() + { + // Given / When + var capsule = new CapsuleShape(0.5f, 1.0f); + + // Then + AssertHelper.ApproximatelyEqual(0.5f, capsule.Radius); + AssertHelper.ApproximatelyEqual(1.0f, capsule.HalfHeight); + } + + [Fact] + public void Equals_SameValues_ReturnsTrue() + { + // Given + var a = new CapsuleShape(0.5f, 1.0f); + var b = new CapsuleShape(0.5f, 1.0f); + + // Then + Assert.True(a.Equals(b)); + } + + [Fact] + public void Equals_DifferentValues_ReturnsFalse() + { + // Given + var a = new CapsuleShape(0.5f, 1.0f); + var b = new CapsuleShape(0.3f, 0.8f); + + // Then + Assert.False(a.Equals(b)); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/ColliderBoundsTests.cs b/src/Seed.Engine.Tests/Physics/Collision/ColliderBoundsTests.cs new file mode 100644 index 00000000..39fc7979 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/ColliderBoundsTests.cs @@ -0,0 +1,80 @@ +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 ColliderBoundsTests +{ + [Fact] + public void ComputeAABB_Sphere_ReturnsCorrectBounds() + { + // Given + var collider = Collider.CreateSphere(1.0f); + var position = new Vector3(5f, 5f, 5f); + + // When + Result result = ColliderBounds.ComputeAABB(collider, position, Quaternion.Identity); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(4f, 4f, 4f), result.Value.Min); + AssertHelper.ApproximatelyEqual(new Vector3(6f, 6f, 6f), result.Value.Max); + } + + [Fact] + public void ComputeAABB_Box_IdentityRotation_ReturnsCorrectBounds() + { + // Given + var collider = Collider.CreateBox(new Vector3(1f, 2f, 3f)); + var position = new Vector3(0f, 0f, 0f); + + // When + Result result = ColliderBounds.ComputeAABB(collider, position, Quaternion.Identity); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(-1f, -2f, -3f), result.Value.Min); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 3f), result.Value.Max); + } + + [Fact] + public void ComputeAABB_Box_Rotated90Degrees_ExpandsBounds() + { + // Given + var collider = Collider.CreateBox(new Vector3(1f, 0.5f, 0.5f)); + var position = Vector3.Zero; + var rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.HalfPi); + + // When + Result result = ColliderBounds.ComputeAABB(collider, position, rotation); + + // Then: rotated 90 degrees around Y, X extent becomes Z extent and vice versa + Assert.True(result.IsSuccess); + Assert.True(result.Value.Max.Z > 0.9f); + } + + [Fact] + public void ComputeAABB_Capsule_IdentityRotation_ReturnsCorrectBounds() + { + // Given + var collider = Collider.CreateCapsule(0.5f, 1.0f); + var position = Vector3.Zero; + + // When + Result result = ColliderBounds.ComputeAABB(collider, position, Quaternion.Identity); + + // Then: capsule oriented along Y, total half-height = 1.0 + 0.5 = 1.5 + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(-0.5f, result.Value.Min.X); + AssertHelper.ApproximatelyEqual(-1.5f, result.Value.Min.Y); + AssertHelper.ApproximatelyEqual(-0.5f, result.Value.Min.Z); + AssertHelper.ApproximatelyEqual(0.5f, result.Value.Max.X); + AssertHelper.ApproximatelyEqual(1.5f, result.Value.Max.Y); + AssertHelper.ApproximatelyEqual(0.5f, result.Value.Max.Z); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/ColliderTests.cs b/src/Seed.Engine.Tests/Physics/Collision/ColliderTests.cs new file mode 100644 index 00000000..46e09773 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/ColliderTests.cs @@ -0,0 +1,44 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class ColliderTests +{ + [Fact] + public void CreateSphere_SetsCorrectTypeAndData() + { + // When + var collider = Collider.CreateSphere(2.0f); + + // Then + Assert.Equal(ColliderType.Sphere, collider.Type); + AssertHelper.ApproximatelyEqual(2.0f, collider.Data.Sphere.Radius); + } + + [Fact] + public void CreateBox_SetsCorrectTypeAndData() + { + // When + var collider = Collider.CreateBox(new Vector3(1f, 2f, 3f)); + + // Then + Assert.Equal(ColliderType.Box, collider.Type); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 3f), collider.Data.Box.HalfExtents); + } + + [Fact] + public void CreateCapsule_SetsCorrectTypeAndData() + { + // When + var collider = Collider.CreateCapsule(0.5f, 1.0f); + + // Then + Assert.Equal(ColliderType.Capsule, collider.Type); + AssertHelper.ApproximatelyEqual(0.5f, collider.Data.Capsule.Radius); + AssertHelper.ApproximatelyEqual(1.0f, collider.Data.Capsule.HalfHeight); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/ContactPointTests.cs b/src/Seed.Engine.Tests/Physics/Collision/ContactPointTests.cs new file mode 100644 index 00000000..a2fba475 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/ContactPointTests.cs @@ -0,0 +1,38 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class ContactPointTests +{ + [Fact] + public void Constructor_StoresAllFields() + { + // Given / When + var contact = new ContactPoint( + new Vector3(1f, 0f, 0f), + new Vector3(-1f, 0f, 0f), + new Vector3(1f, 0f, 0f), + 0.5f); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(1f, 0f, 0f), contact.PointOnA); + AssertHelper.ApproximatelyEqual(new Vector3(-1f, 0f, 0f), contact.PointOnB); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 0f, 0f), contact.Normal); + AssertHelper.ApproximatelyEqual(0.5f, contact.PenetrationDepth); + } + + [Fact] + public void Equals_SameValues_ReturnsTrue() + { + // Given + var a = new ContactPoint(Vector3.Zero, Vector3.UnitX, Vector3.UnitY, 1f); + var b = new ContactPoint(Vector3.Zero, Vector3.UnitX, Vector3.UnitY, 1f); + + // Then + Assert.True(a.Equals(b)); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs new file mode 100644 index 00000000..1d206f82 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/GjkEpaTests.cs @@ -0,0 +1,184 @@ +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 GjkEpaTests +{ + [Fact] + public void Intersects_OverlappingSpheres_ReturnsTrue() + { + // Given + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + + // When + Result resultWrap = GjkEpa.Intersects( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(1f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void Intersects_NonOverlappingSpheres_ReturnsFalse() + { + // Given + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + + // When + Result resultWrap = GjkEpa.Intersects( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(5f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.False(resultWrap.Value); + } + + [Fact] + public void Intersects_OverlappingBoxes_ReturnsTrue() + { + // Given + var a = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + var b = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + + // When + Result resultWrap = GjkEpa.Intersects( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(1f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void Intersects_NonOverlappingBoxes_ReturnsFalse() + { + // Given + var a = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + var b = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + + // When + Result resultWrap = GjkEpa.Intersects( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(5f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.False(resultWrap.Value); + } + + [Fact] + public void Intersects_SphereAndBox_Overlapping_ReturnsTrue() + { + // Given + var sphere = Collider.CreateSphere(1.0f); + var box = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + + // When + Result resultWrap = GjkEpa.Intersects( + sphere, new Vector3(0f, 0f, 0f), Quaternion.Identity, + box, new Vector3(1.5f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void Intersects_CapsuleAndSphere_Overlapping_ReturnsTrue() + { + // Given + var capsule = Collider.CreateCapsule(0.5f, 1.0f); + var sphere = Collider.CreateSphere(0.5f); + + // When + Result resultWrap = GjkEpa.Intersects( + capsule, new Vector3(0f, 0f, 0f), Quaternion.Identity, + sphere, new Vector3(0.8f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void ComputeContact_OverlappingSpheres_ReturnsContactInfo() + { + // Given + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(1.5f, 0f, 0f), Quaternion.Identity, + out ContactPoint contact); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + Assert.True(contact.PenetrationDepth > 0f); + } + + [Fact] + public void ComputeContact_NonOverlapping_ReturnsFalse() + { + // Given + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + + // When + Result resultWrap = GjkEpa.ComputeContact( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(5f, 0f, 0f), Quaternion.Identity, + out _); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.False(resultWrap.Value); + } + + [Fact] + public void Intersects_OverlappingCapsules_ReturnsTrue() + { + // Given + var a = Collider.CreateCapsule(0.5f, 1.0f); + var b = Collider.CreateCapsule(0.5f, 1.0f); + + // When + Result resultWrap = GjkEpa.Intersects( + a, new Vector3(0f, 0f, 0f), Quaternion.Identity, + b, new Vector3(0.5f, 0f, 0f), Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } + + [Fact] + public void Intersects_SamePosition_ReturnsTrue() + { + // Given + var a = Collider.CreateSphere(1.0f); + var b = Collider.CreateSphere(1.0f); + + // When + Result resultWrap = GjkEpa.Intersects( + a, Vector3.Zero, Quaternion.Identity, + b, Vector3.Zero, Quaternion.Identity); + + // Then + Assert.True(resultWrap.IsSuccess); + Assert.True(resultWrap.Value); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/GjkSupportTests.cs b/src/Seed.Engine.Tests/Physics/Collision/GjkSupportTests.cs new file mode 100644 index 00000000..f1d83e32 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/GjkSupportTests.cs @@ -0,0 +1,97 @@ +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 GjkSupportTests +{ + [Fact] + public void Support_Sphere_ReturnsPointOnSurface() + { + // Given + var collider = Collider.CreateSphere(2.0f); + var position = new Vector3(1f, 0f, 0f); + var direction = Vector3.UnitX; + + // When + Result result = GjkSupport.Support(collider, position, Quaternion.Identity, direction); + + // Then: center(1,0,0) + normalize(1,0,0) * 2 = (3,0,0) + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(3f, 0f, 0f), result.Value); + } + + [Fact] + public void Support_Box_ReturnsCorner() + { + // Given + var collider = Collider.CreateBox(new Vector3(1f, 1f, 1f)); + var position = Vector3.Zero; + var direction = new Vector3(1f, 1f, 1f); + + // When + Result result = GjkSupport.Support(collider, position, Quaternion.Identity, direction); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 1f, 1f), result.Value); + } + + [Fact] + public void Support_Box_NegativeDirection_ReturnsOppositeCorner() + { + // Given + var collider = Collider.CreateBox(new Vector3(1f, 2f, 3f)); + var position = Vector3.Zero; + var direction = new Vector3(-1f, -1f, -1f); + + // When + Result result = GjkSupport.Support(collider, position, Quaternion.Identity, direction); + + // Then + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(-1f, -2f, -3f), result.Value); + } + + [Fact] + public void Support_Capsule_DirectionUp_ReturnsTopSphereSurface() + { + // Given + var collider = Collider.CreateCapsule(0.5f, 1.0f); + var position = Vector3.Zero; + var direction = Vector3.UnitY; + + // When + Result result = GjkSupport.Support(collider, position, Quaternion.Identity, direction); + + // Then: topCenter(0,1,0) + normalize(0,1,0) * 0.5 = (0, 1.5, 0) + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(0f, 1.5f, 0f), result.Value); + } + + [Fact] + public void MinkowskiSupport_TwoSpheres_ReturnsMinkowskiDifferencePoint() + { + // Given + var colliderA = Collider.CreateSphere(1.0f); + var colliderB = Collider.CreateSphere(1.0f); + var posA = new Vector3(-2f, 0f, 0f); + var posB = new Vector3(2f, 0f, 0f); + var direction = Vector3.UnitX; + + // When + Result result = GjkSupport.MinkowskiSupport( + colliderA, posA, Quaternion.Identity, + colliderB, posB, Quaternion.Identity, + direction); + + // Then: supportA = (-2+1, 0, 0) = (-1,0,0), supportB = (2-1, 0, 0) = (1,0,0) + // Minkowski = (-1,0,0) - (1,0,0) = (-2,0,0) + Assert.True(result.IsSuccess); + AssertHelper.ApproximatelyEqual(new Vector3(-2f, 0f, 0f), result.Value); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/RayTests.cs b/src/Seed.Engine.Tests/Physics/Collision/RayTests.cs new file mode 100644 index 00000000..dc0de3a9 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/RayTests.cs @@ -0,0 +1,60 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class RayTests +{ + [Fact] + public void GetPoint_ReturnsCorrectPosition() + { + // Given + var ray = new Ray(new Vector3(1f, 2f, 3f), new Vector3(0f, 0f, 1f)); + + // When + Vector3 point = ray.GetPoint(5f); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 8f), point); + } + + [Fact] + public void GetPoint_AtZeroDistance_ReturnsOrigin() + { + // Given + var ray = new Ray(new Vector3(1f, 2f, 3f), new Vector3(1f, 0f, 0f)); + + // When + Vector3 point = ray.GetPoint(0f); + + // Then + AssertHelper.ApproximatelyEqual(new Vector3(1f, 2f, 3f), point); + } + + [Fact] + public void Equals_SameValues_ReturnsTrue() + { + // Given + var a = new Ray(Vector3.Zero, Vector3.UnitX); + var b = new Ray(Vector3.Zero, Vector3.UnitX); + + // Then + Assert.True(a.Equals(b)); + Assert.True(a == b); + } + + [Fact] + public void Equals_DifferentValues_ReturnsFalse() + { + // Given + var a = new Ray(Vector3.Zero, Vector3.UnitX); + var b = new Ray(Vector3.Zero, Vector3.UnitY); + + // Then + Assert.False(a.Equals(b)); + Assert.True(a != b); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Collision/SimplexTests.cs b/src/Seed.Engine.Tests/Physics/Collision/SimplexTests.cs new file mode 100644 index 00000000..efe0c6dc --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Collision/SimplexTests.cs @@ -0,0 +1,75 @@ +using Xunit; + +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Collision; + +public class SimplexTests +{ + [Fact] + public void Push_FirstPoint_CountIsOne() + { + // Given + var simplex = new Simplex(); + + // When + simplex.Push(new Vector3(1f, 0f, 0f)); + + // Then + Assert.Equal(1, simplex.Count); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 0f, 0f), simplex.A); + } + + [Fact] + public void Push_TwoPoints_CountIsTwo() + { + // Given + var simplex = new Simplex(); + + // When + simplex.Push(new Vector3(1f, 0f, 0f)); + simplex.Push(new Vector3(0f, 1f, 0f)); + + // Then + Assert.Equal(2, simplex.Count); + AssertHelper.ApproximatelyEqual(new Vector3(0f, 1f, 0f), simplex.A); + AssertHelper.ApproximatelyEqual(new Vector3(1f, 0f, 0f), simplex.B); + } + + [Fact] + public void Push_FourPoints_CountIsFour() + { + // Given + var simplex = new Simplex(); + + // When + simplex.Push(new Vector3(1f, 0f, 0f)); + simplex.Push(new Vector3(0f, 1f, 0f)); + simplex.Push(new Vector3(0f, 0f, 1f)); + simplex.Push(new Vector3(-1f, 0f, 0f)); + + // Then + Assert.Equal(4, simplex.Count); + AssertHelper.ApproximatelyEqual(new Vector3(-1f, 0f, 0f), simplex.A); + } + + [Fact] + public void Push_FifthPoint_CountStaysAtFour() + { + // Given + var simplex = new Simplex(); + simplex.Push(new Vector3(1f, 0f, 0f)); + simplex.Push(new Vector3(0f, 1f, 0f)); + simplex.Push(new Vector3(0f, 0f, 1f)); + simplex.Push(new Vector3(-1f, 0f, 0f)); + + // When + simplex.Push(new Vector3(0f, -1f, 0f)); + + // Then + Assert.Equal(4, simplex.Count); + AssertHelper.ApproximatelyEqual(new Vector3(0f, -1f, 0f), simplex.A); + } +} diff --git a/src/Seed.Engine.Tests/Physics/NarrowPhaseSystemTests.cs b/src/Seed.Engine.Tests/Physics/NarrowPhaseSystemTests.cs new file mode 100644 index 00000000..d97bfef0 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/NarrowPhaseSystemTests.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; + +namespace Seed.Engine.Tests.Physics; + +public class NarrowPhaseSystemTests +{ + [Fact] + public void Phase_IsPhysics() + { + // Given / When + var system = new NarrowPhaseSystem(); + + // Then + Assert.Equal(FramePhase.Physics, system.Phase); + } + + [Fact] + public unsafe void Execute_WithOverlappingPairs_ProducesContacts() + { + // Given + var world = new World(); + using var _ = world; + + var brType = ComponentRegistry.Get(); + var npType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes = [brType, npType]; + Entity singleton = world.CreateEntity(singletonTypes); + + var ccType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [ccType, ltType]; + + Entity e0 = world.CreateEntity(entityTypes); + Entity e1 = world.CreateEntity(entityTypes); + + world.GetComponent(e0) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e0) = + LocalTransform.FromPosition(new Vector3(0f, 0f, 0f)); + + world.GetComponent(e1) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e1) = + LocalTransform.FromPosition(new Vector3(1f, 0f, 0f)); + + // Run broad phase first + var broadPhase = new BroadPhaseSystem(); + broadPhase.Execute(world); + + var narrowPhase = new NarrowPhaseSystem(); + + // When + narrowPhase.Execute(world); + + // Then + ref NarrowPhaseResult result = ref world.GetComponent(singleton); + Assert.True(result.Count > 0); + Assert.True(result.Contacts[0].PenetrationDepth > 0f); + } + + [Fact] + public unsafe void Execute_WithNoOverlap_ProducesNoContacts() + { + // Given + var world = new World(); + using var _ = world; + + var brType = ComponentRegistry.Get(); + var npType = ComponentRegistry.Get(); + ReadOnlySpan singletonTypes = [brType, npType]; + Entity singleton = world.CreateEntity(singletonTypes); + + var ccType = ComponentRegistry.Get(); + var ltType = ComponentRegistry.Get(); + ReadOnlySpan entityTypes = [ccType, ltType]; + + Entity e0 = world.CreateEntity(entityTypes); + Entity e1 = world.CreateEntity(entityTypes); + + world.GetComponent(e0) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e0) = + LocalTransform.FromPosition(new Vector3(0f, 0f, 0f)); + + world.GetComponent(e1) = new ColliderComponent + { + Value = Collider.CreateSphere(1.0f), + }; + world.GetComponent(e1) = + LocalTransform.FromPosition(new Vector3(100f, 0f, 0f)); + + var broadPhase = new BroadPhaseSystem(); + broadPhase.Execute(world); + + var narrowPhase = new NarrowPhaseSystem(); + + // When + narrowPhase.Execute(world); + + // Then + ref NarrowPhaseResult result = ref world.GetComponent(singleton); + Assert.Equal(0, result.Count); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Query/PhysicsWorldTests.cs b/src/Seed.Engine.Tests/Physics/Query/PhysicsWorldTests.cs new file mode 100644 index 00000000..366100a4 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Query/PhysicsWorldTests.cs @@ -0,0 +1,145 @@ +using System; +using Xunit; + +using Seed.Engine.Ecs; +using Seed.Engine.Foundation.Collections; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.BroadPhase; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Physics.Query; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Query; + +public class PhysicsWorldTests +{ + [Fact] + public void Raycast_HitsClosestObject() + { + // Given + var bvh = new BvhTree(); + Entity e0 = new Entity(0, 1); + Entity e1 = new Entity(1, 1); + + var colliders = new Collider[] + { + Collider.CreateSphere(1.0f), + Collider.CreateSphere(1.0f), + }; + var positions = new Vector3[] + { + new(5f, 0f, 0f), + new(10f, 0f, 0f), + }; + var rotations = new Quaternion[] + { + Quaternion.Identity, + Quaternion.Identity, + }; + + Span bounds = + [ + ColliderBounds.ComputeAABB(colliders[0], positions[0], rotations[0]).Value, + ColliderBounds.ComputeAABB(colliders[1], positions[1], rotations[1]).Value, + ]; + Span indices = [0, 1]; + bvh.Build(bounds, indices); + + var ray = new Ray(Vector3.Zero, Vector3.UnitX); + + // When + bool hit = PhysicsWorld.Raycast( + ray, 100f, ref bvh, + [e0, e1], colliders, positions, rotations, + out RaycastHit result); + + // Then + Assert.True(hit); + Assert.Equal(e0, result.Entity); + AssertHelper.ApproximatelyEqual(4.0f, result.Distance, 0.01f); + + bvh.Dispose(); + } + + [Fact] + public void RaycastAll_ReturnsAllHitsSorted() + { + // Given + var bvh = new BvhTree(); + Entity e0 = new Entity(0, 1); + Entity e1 = new Entity(1, 1); + + var colliders = new Collider[] + { + Collider.CreateSphere(1.0f), + Collider.CreateSphere(1.0f), + }; + var positions = new Vector3[] + { + new(10f, 0f, 0f), + new(5f, 0f, 0f), + }; + var rotations = new Quaternion[] + { + Quaternion.Identity, + Quaternion.Identity, + }; + + Span bounds = + [ + ColliderBounds.ComputeAABB(colliders[0], positions[0], rotations[0]).Value, + ColliderBounds.ComputeAABB(colliders[1], positions[1], rotations[1]).Value, + ]; + Span indices = [0, 1]; + bvh.Build(bounds, indices); + + var ray = new Ray(Vector3.Zero, Vector3.UnitX); + var results = new NativeList(8); + + // When + PhysicsWorld.RaycastAll( + ray, 100f, ref bvh, + [e0, e1], colliders, positions, rotations, + ref results); + + // Then + Assert.Equal(2, results.Length); + Assert.True(results[0].Distance <= results[1].Distance); + + results.Dispose(); + bvh.Dispose(); + } + + [Fact] + public void OverlapSphere_FindsEntitiesInRange() + { + // Given + var grid = new SpatialHashGrid(2.0f, 16); + Entity e0 = new Entity(0, 1); + Entity e1 = new Entity(1, 1); + + var boundsArr = new AABB[] + { + new(new Vector3(-1f, -1f, -1f), new Vector3(1f, 1f, 1f)), + new(new Vector3(10f, 10f, 10f), new Vector3(12f, 12f, 12f)), + }; + + grid.Insert(0, boundsArr[0]); + grid.Insert(1, boundsArr[1]); + + var results = new NativeList(8); + + // When + PhysicsWorld.OverlapSphere( + Vector3.Zero, 2.0f, ref grid, + [e0, e1], boundsArr, + ref results); + + // Then + Assert.Equal(1, results.Length); + Assert.Equal(e0, results[0]); + + results.Dispose(); + grid.Dispose(); + } +} diff --git a/src/Seed.Engine.Tests/Physics/Query/RayShapeIntersectionTests.cs b/src/Seed.Engine.Tests/Physics/Query/RayShapeIntersectionTests.cs new file mode 100644 index 00000000..550c9556 --- /dev/null +++ b/src/Seed.Engine.Tests/Physics/Query/RayShapeIntersectionTests.cs @@ -0,0 +1,132 @@ +using Xunit; + +using Seed.Engine.Foundation.Error; +using Seed.Engine.Foundation.Mathematics; +using Seed.Engine.Physics.Collision; +using Seed.Engine.Physics.Query; +using Seed.Engine.Tests.Foundation.Mathematics; + +namespace Seed.Engine.Tests.Physics.Query; + +public class RayShapeIntersectionTests +{ + [Fact] + public void RaySphere_Hit_ReturnsTrue() + { + // Given + var ray = new Ray(new Vector3(-5f, 0f, 0f), Vector3.UnitX); + var center = new Vector3(0f, 0f, 0f); + float radius = 1.0f; + + // When + bool result = RayShapeIntersection.RaySphere(ray, center, radius, out float dist, out Vector3 normal); + + // Then + Assert.True(result); + AssertHelper.ApproximatelyEqual(4.0f, dist, 0.01f); + AssertHelper.ApproximatelyEqual(-1f, normal.X, 0.01f); + } + + [Fact] + public void RaySphere_Miss_ReturnsFalse() + { + // Given + var ray = new Ray(new Vector3(-5f, 5f, 0f), Vector3.UnitX); + var center = Vector3.Zero; + float radius = 1.0f; + + // When + bool result = RayShapeIntersection.RaySphere(ray, center, radius, out _, out _); + + // Then + Assert.False(result); + } + + [Fact] + public void RayBox_Hit_ReturnsTrue() + { + // Given + var ray = new Ray(new Vector3(-5f, 0f, 0f), Vector3.UnitX); + var center = Vector3.Zero; + var halfExtents = new Vector3(1f, 1f, 1f); + + // When + bool result = RayShapeIntersection.RayBox( + ray, center, Quaternion.Identity, halfExtents, out float dist, out Vector3 normal); + + // Then + Assert.True(result); + AssertHelper.ApproximatelyEqual(4.0f, dist, 0.01f); + AssertHelper.ApproximatelyEqual(-1f, normal.X, 0.01f); + } + + [Fact] + public void RayBox_Miss_ReturnsFalse() + { + // Given + var ray = new Ray(new Vector3(-5f, 5f, 0f), Vector3.UnitX); + var center = Vector3.Zero; + var halfExtents = new Vector3(1f, 1f, 1f); + + // When + bool result = RayShapeIntersection.RayBox( + ray, center, Quaternion.Identity, halfExtents, out _, out _); + + // Then + Assert.False(result); + } + + [Fact] + public void RayCapsule_Hit_ReturnsTrue() + { + // Given + var ray = new Ray(new Vector3(-5f, 0f, 0f), Vector3.UnitX); + var center = Vector3.Zero; + float radius = 0.5f; + float halfHeight = 1.0f; + + // When + bool result = RayShapeIntersection.RayCapsule( + ray, center, Quaternion.Identity, radius, halfHeight, + out float dist, out Vector3 normal); + + // Then + Assert.True(result); + Assert.True(dist > 0f); + } + + [Fact] + public void RayCapsule_Miss_ReturnsFalse() + { + // Given + var ray = new Ray(new Vector3(-5f, 10f, 0f), Vector3.UnitX); + var center = Vector3.Zero; + float radius = 0.5f; + float halfHeight = 1.0f; + + // When + bool result = RayShapeIntersection.RayCapsule( + ray, center, Quaternion.Identity, radius, halfHeight, + out _, out _); + + // Then + Assert.False(result); + } + + [Fact] + public void RayCollider_Sphere_Hit_ReturnsTrue() + { + // Given + var ray = new Ray(new Vector3(-5f, 0f, 0f), Vector3.UnitX); + var collider = Collider.CreateSphere(1.0f); + + // When + Result result = RayShapeIntersection.RayCollider( + ray, collider, Vector3.Zero, Quaternion.Identity, out float dist, out _); + + // Then + Assert.True(result.IsSuccess); + Assert.True(result.Value); + AssertHelper.ApproximatelyEqual(4.0f, dist, 0.01f); + } +} From 5a9e1325495070b4bd3ffba87a10134abbd3dd04 Mon Sep 17 00:00:00 2001 From: HMasataka Date: Thu, 5 Mar 2026 22:19:50 +0900 Subject: [PATCH 7/7] =?UTF-8?q?roadmap.md=20v0.10=E3=82=92=E5=AE=8C?= =?UTF-8?q?=E4=BA=86=E3=81=AB=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- docs/roadmap.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/roadmap.md b/docs/roadmap.md index c6992319..ff64548f 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -148,10 +148,10 @@ ゴール: オブジェクト同士の衝突を正確に検出できる -- [ ] SpatialHashGrid -- [ ] BVH広域フェーズ -- [ ] GJK+EPA狭域フェーズ -- [ ] Raycast, RaycastAll, SphereCast, OverlapSphere +- [x] SpatialHashGrid +- [x] BVH広域フェーズ +- [x] GJK+EPA狭域フェーズ +- [x] Raycast, RaycastAll, SphereCast, OverlapSphere 完動品としての価値: オブジェクト同士の衝突が検出される。レイキャストで任意のオブジェクトを特定できる。