Merge pull request #321 from NoiseStudio/feature/311/simple-rigidbody…

…-and-sphere-collider

Simple rigidbody and sphere collider

NoiseEngine.Generator/Jobs/EntitySystemIncrementalGenerator.cs

@@ -408,6 +408,8 @@ public class EntitySystemIncrementalGenerator : IIncrementalGenerator {
                 .Append(i).AppendLine(".Inner is not null)");
             builder.AppendIndentation(4).Append("data.Changed.Add((observers").Append(i).Append(", changed").Append(i)
                 .AppendLine(".Inner));");
+
+            i++;
         }
 
         if (threadStorageType is not null) {

NoiseEngine.InternalGenerator/Jobs/EntityWorldSourceGenerator.cs

@@ -221,11 +221,11 @@ public class EntityWorldSourceGenerator : ISourceGenerator {
 
                 builder.AppendIndentation(2).AppendLine("if (!TryGetArchetype(hashCode, out Archetype? archetype)) {");
                 builder.AppendIndentation(3).AppendLine(
-                    "archetype = CreateArchetype(hashCode, new (Type type, int size, int affectiveHashCode)[] {"
+                    "archetype = CreateArchetype(hashCode, new (Type type, int affectiveHashCode)[] {"
                 );
                 for (int j = 1; j <= i; j++) {
-                    builder.AppendIndentation(4).Append("(typeof(T").Append(j).Append("), Unsafe.SizeOf<T").Append(j)
-                        .Append(">(), IAffectiveComponent.GetAffectiveHashCode(component").Append(j).Append("))")
+                    builder.AppendIndentation(4).Append("(typeof(T").Append(j)
+                        .Append("), IAffectiveComponent.GetAffectiveHashCode(component").Append(j).Append("))")
                         .AppendLine(j == i ? "" : ",");
                 }
                 builder.AppendIndentation(3).AppendLine("});").AppendLine();

NoiseEngine.Tests/Jobs/ArchetypeTest.cs

@@ -32,4 +32,15 @@ public class ArchetypeTest : ApplicationTestEnvironment {
         Assert.False(archetype.TryReadAnyRecord(out _));
     }
 
+    [Fact]
+    public void PointerAlignment() {
+        MockComponentA a = MockComponentA.TestValueB;
+        using Entity entity = EntityWorld.Spawn(MockComponentG.TestValueA, a);
+
+        Assert.True(entity.TryGet(out MockComponentG g));
+        Assert.Equal(MockComponentG.TestValueA, g);
+        Assert.True(entity.TryGet(out MockComponentA? a2));
+        Assert.Equal(a, a2!);
+    }
+
 }

NoiseEngine.Tests/Jobs/EntitySystemTest.cs

@@ -44,6 +44,9 @@ public class EntitySystemTest : ApplicationTestEnvironment {
 
         systemB.AddDependency(systemA);
 
+        systemA.ExecuteAndWait();
+        systemB.ExecuteAndWait();
+
         Assert.False(systemB.TryExecute());
         systemA.ExecuteAndWait();
         Assert.True(systemB.TryExecuteAndWait());

NoiseEngine.Tests/Jobs/MockComponentD.cs

@@ -1,5 +1,4 @@
 using NoiseEngine.Jobs;
-using System;
 
 namespace NoiseEngine.Tests.Jobs;
 

NoiseEngine.Tests/Jobs/MockComponentG.cs

@@ -0,0 +1,9 @@
+using NoiseEngine.Jobs;
+
+namespace NoiseEngine.Tests.Jobs;
+
+internal record struct MockComponentG(float ValueA, bool ValueB, int ValueC) : IComponent {
+
+    public static MockComponentG TestValueA => new MockComponentG(114.114f, true, 456373525);
+
+}

NoiseEngine.Tests/Mathematics/Matrix3x3Test.cs

@@ -0,0 +1,59 @@
+using NoiseEngine.Mathematics;
+using System;
+using System.Numerics;
+
+namespace NoiseEngine.Tests.Mathematics;
+
+public class Matrix3x3Test {
+
+    [Theory]
+    [InlineData(new float[] { 2, 2, 1, -3, 0, 4, 1, -1, 5 }, 49)]
+    [InlineData(new float[] { 4, 6, 5, 2, 1, 3, 7, 9, 8 }, 9)]
+    public void Determinant(float[] init, float expected) {
+        Matrix3x3<float> matrix = FromSpan<float>(init);
+        Assert.Equal(expected, matrix.Determinant());
+    }
+
+    [Theory]
+    [InlineData(
+        new float[] { 1, 2, -1, 2, 1, 2, -1, 2, 1 },
+        new float[] { 3f / 16f, 1f / 4f, -5f / 16f, 1f / 4f, 0f, 1f / 4f, -5f / 16f, 1f / 4f, 3 / 16f }
+    )]
+    [InlineData(
+        new float[] { 4, 6, 5, 2, 1, 3, 7, 9, 8 },
+        new float[] { -19f / 9f, -1f / 3f, 13f / 9f, 5f / 9f, -1f / 3f, -2f / 9f, 11f / 9f, 2f / 3f, -8f / 9f }
+    )]
+    public void TryInvert(float[] init, float[]? expected) {
+        Matrix3x3<float> matrix = FromSpan<float>(init);
+        if (expected is null) {
+            Assert.False(matrix.TryInvert(out _));
+        } else {
+            Assert.True(matrix.TryInvert(out Matrix3x3<float> invertedMatrix));
+            Assert.Equal(FromSpan<float>(expected), invertedMatrix);
+        }
+    }
+
+    [Theory]
+    [InlineData(
+        new float[] { 3, 7, 12, 5, 8, 15, 6, 9, 12 },
+        new float[] { 1, 4, 7 },
+        new float[] { 65, 102, 156 }
+    )]
+    public void MultipleByVector3(float[] init, float[] vector, float[] expected) {
+        Matrix3x3<float> matrix = FromSpan<float>(init);
+        Assert.Equal(Vector3FromSpan<float>(expected), matrix * Vector3FromSpan<float>(vector));
+    }
+
+    private Matrix3x3<T> FromSpan<T>(ReadOnlySpan<T> data) where T : INumber<T> {
+        return new Matrix3x3<T>(
+            new Vector3<T>(data[0], data[1], data[2]),
+            new Vector3<T>(data[3], data[4], data[5]),
+            new Vector3<T>(data[6], data[7], data[8])
+        );
+    }
+
+    private Vector3<T> Vector3FromSpan<T>(ReadOnlySpan<T> data) where T : INumber<T> {
+        return new Vector3<T>(data[0], data[1], data[2]);
+    }
+
+}

NoiseEngine.Tests/Physics/PhysicsTest.cs

@@ -0,0 +1,52 @@
+using NoiseEngine.Components;
+using NoiseEngine.DeveloperTools.Systems;
+using NoiseEngine.Mathematics;
+using NoiseEngine.Physics;
+using NoiseEngine.Tests.Environments;
+using NoiseEngine.Tests.Fixtures;
+using System.Threading;
+
+namespace NoiseEngine.Tests.Physics;
+
+public class PhysicsTest : ApplicationTestEnvironment {
+
+    public PhysicsTest(ApplicationFixture fixture) : base(fixture) {
+    }
+
+    /*[FactRequire(TestRequirements.Graphics | TestRequirements.Gui)]
+    public void SimpleScene() {
+        ApplicationScene scene = new ApplicationScene();
+        Window window = Fixture.GetWindow("Physics!");
+        Camera camera = new Camera(scene) {
+            RenderTarget = window,
+            RenderLoop = new PerformanceRenderLoop()
+        };
+        scene.AddFrameDependentSystem(new PhysicsTestActivatorSystem(scene, window));
+
+        scene.Spawn(
+            new TransformComponent(
+                new Vector3<float>(0, -105, 0), Quaternion<float>.Identity, new Vector3<float>(200, 200, 200)
+            ),
+            new MeshRendererComponent(scene.Primitive.GetSphereMesh(), scene.Primitive.DefaultMaterial),
+            new ColliderComponent(new SphereCollider())
+        );
+
+        for (int x = 0; x < 1; x += 2) {
+            for (int y = 0; y < 40; y += 2) {
+                for (int z = 0; z < 1; z += 2) {
+                    scene.Spawn(
+                        new TransformComponent(new Vector3<float>(x, y * 3 + 4.5f, z)),
+                        new MeshRendererComponent(scene.Primitive.GetSphereMesh(), scene.Primitive.DefaultMaterial),
+                        new RigidBodyComponent(),
+                        new ColliderComponent(new SphereCollider())
+                    );
+                }
+            }
+        }
+
+        DebugMovementSystem.InitializeTo(camera);
+        while (!window.IsDisposed)
+            Thread.Sleep(10);
+    }*/
+
+}

NoiseEngine.Tests/Physics/PhysicsTestActivatorSystem.cs

@@ -0,0 +1,65 @@
+using NoiseEngine.Components;
+using NoiseEngine.Inputs;
+using NoiseEngine.Jobs;
+using NoiseEngine.Mathematics;
+using NoiseEngine.Physics;
+using NoiseEngine.Physics.Collision;
+using NoiseEngine.Physics.FrameSmoothing;
+using NoiseEngine.Physics.Internal;
+using NoiseEngine.Physics.Simulation;
+using System;
+
+namespace NoiseEngine.Tests.Physics;
+
+internal partial class PhysicsTestActivatorSystem : EntitySystem {
+
+    private readonly ApplicationScene scene;
+    private readonly Window window;
+
+    public PhysicsTestActivatorSystem(ApplicationScene scene, Window window) {
+        this.scene = scene;
+        this.window = window;
+    }
+
+    protected override void OnUpdate() {
+        if (!window.Input.Pressed(Key.F1))
+            return;
+        Enabled = false;
+
+        scene.Primitive.CreateCube(
+            new Vector3<float>(0, 0, 15),
+            Quaternion.EulerDegrees(new Vector3<float>(0, 90, 0)),
+            new Vector3<float>(10, 10, 10)
+        );
+
+        scene.AddFrameDependentSystem(new RigidBodyFrameSmoothingSystem());
+        double cycleTime = 20;
+        CollisionSpace space = new CollisionSpace();
+        ContactPointsBuffer contactPoints = new ContactPointsBuffer();
+
+        SimulationSystem simulationSystem = new SimulationSystem(space);
+        CollisionDetectionSystem collisionDetectionSystem = new CollisionDetectionSystem(space, contactPoints);
+        CollisionResolveSystem collisionResolveSystem = new CollisionResolveSystem(contactPoints);
+        ImmovableColliderRegisterSystem immovableColliderRegisterSystem = new ImmovableColliderRegisterSystem(space);
+
+        simulationSystem.AddDependency(collisionResolveSystem);
+        immovableColliderRegisterSystem.AddDependency(collisionDetectionSystem);
+
+        collisionDetectionSystem.AddDependency(simulationSystem);
+        collisionDetectionSystem.AddDependency(immovableColliderRegisterSystem);
+
+        collisionResolveSystem.AddDependency(collisionDetectionSystem);
+
+        scene.AddSystem(simulationSystem, cycleTime);
+        scene.AddSystem(immovableColliderRegisterSystem, cycleTime);
+        scene.AddSystem(collisionDetectionSystem, cycleTime);
+        scene.AddSystem(collisionResolveSystem, cycleTime);
+
+        RigidBodyInitializerSystem initalizer = new RigidBodyInitializerSystem();
+        scene.AddSystem(initalizer, cycleTime);
+    }
+
+    private void OnUpdateEntity() {
+    }
+
+}

NoiseEngine/ApplicationScene.cs

@@ -1,5 +1,6 @@
 using NoiseEngine.Collections.Concurrent;
 using NoiseEngine.Jobs;
+using NoiseEngine.Physics;
 using NoiseEngine.Primitives;
 using NoiseEngine.Rendering;
 using System;
@@ -16,6 +17,8 @@ public class ApplicationScene : EntityWorld {
     private GraphicsDevice? graphicsDevice;
     private PrimitiveCreator? primitive;
 
+    public PhysicsSettings PhysicsSettings { get; init; } = new PhysicsSettings();
+
     public GraphicsDevice GraphicsDevice {
         get {
             if (graphicsDevice is null)

NoiseEngine/DeveloperTools/Systems/DebugMovementSystem.cs

@@ -3,6 +3,7 @@
 using NoiseEngine.Inputs;
 using NoiseEngine.Jobs;
 using NoiseEngine.Mathematics;
+using NoiseEngine.Physics;
 using System;
 
 namespace NoiseEngine.DeveloperTools.Systems;

NoiseEngine/Jobs/Archetype.cs

@@ -3,6 +3,7 @@
 using System;
 using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Diagnostics.CodeAnalysis;
 using System.Linq;
 using System.Runtime.CompilerServices;
@@ -31,41 +32,76 @@ internal class Archetype {
     internal (Type type, int size, int affectiveHashCode)[] ComponentTypes { get; }
     internal Dictionary<Type, nint> Offsets { get; } = new Dictionary<Type, nint>();
     internal Dictionary<Type, int> HashCodes { get; } = new Dictionary<Type, int>();
+    internal Dictionary<Type, (nint offset, int size)> ExtendedInformation = new Dictionary<Type, (nint, int)>();
     internal ConcurrentDictionary<Type, ChangedObserverContext[]> ChangedObserversLookup { get; } =
         new ConcurrentDictionary<Type, ChangedObserverContext[]>();
 
-    public Archetype(EntityWorld world, int hashCode, (Type type, int size, int affectiveHashCode)[] componentTypes) {
+    public Archetype(
+        EntityWorld world, int hashCode, IReadOnlyList<(Type type, int affectiveHashCode)> componentTypes
+    ) {
         componentTypes = componentTypes.OrderBy(x => x.type.FullName).ToArray();
 
         World = world;
         HashCode = hashCode;
-        ComponentTypes = componentTypes;
-
-        Offsets.Add(typeof(EntityInternalComponent), 0);
-        nint offset = Unsafe.SizeOf<EntityInternalComponent>();
-        foreach ((Type type, int size, _) in componentTypes) {
-            Offsets.Add(type, offset);
-            offset += size;
-        }
+        ComponentTypes = new (Type, int, int)[componentTypes.Count];
 
         Type? columnType = null;
-        for (int i = componentTypes.Length - 1; i >= 0; i--) {
-            if (columnType is null)
-                columnType = componentTypes[i].type;
-            else
-                columnType = typeof(ArchetypeColumn<,>).MakeGenericType(componentTypes[i].type, columnType);
+        for (int i = componentTypes.Count - 1; i >= 0; i--) {
+            if (columnType is null) {
+                columnType = WrapReference(componentTypes[i].type);
+            } else {
+                columnType = typeof(ArchetypeColumn<,>).MakeGenericType(
+                    WrapReference(componentTypes[i].type), columnType
+                );
+            }
         }
 
-        if (columnType is null)
+        if (columnType is null) {
             columnType = typeof(EntityInternalComponent);
-        else
+            Offsets.Add(typeof(EntityInternalComponent), 0);
+            RecordSize = Unsafe.SizeOf<EntityInternalComponent>();
+        } else {
             columnType = typeof(ArchetypeColumn<,>).MakeGenericType(typeof(EntityInternalComponent), columnType);
+            ArchetypeColumn<nint, nint> offsets = (ArchetypeColumn<nint, nint>)columnType.GetMethod(
+                nameof(ArchetypeColumn<nint, nint>.GetOffsets)
+            )!.Invoke(null, null)!;
+
+            Offsets.Add(typeof(EntityInternalComponent), offsets.Element1);
+            nint offset = offsets.Element2;
+
+            Type columnTypeFragment = columnType;
+            (Type type, int affectiveHashCode) componentType;
+
+            for (int i = 0; i < componentTypes.Count - 1; i++) {
+                columnTypeFragment = columnTypeFragment.GetField(
+                    nameof(ArchetypeColumn<nint, nint>.Element2)
+                )!.FieldType;
+                offsets = (ArchetypeColumn<nint, nint>)columnTypeFragment.GetMethod(
+                    nameof(ArchetypeColumn<nint, nint>.GetOffsets)
+                )!.Invoke(null, null)!;
+
+                componentType = componentTypes[i];
+                Offsets.Add(componentType.type, offset + offsets.Element1);
+                ComponentTypes[i] =
+                    (componentType.type, (int)(offsets.Element2 - offsets.Element1), componentType.affectiveHashCode);
+
+                offset += offsets.Element2;
+            }
+
+            RecordSize = (nint)columnType.GetMethod(nameof(ArchetypeColumn<nint, nint>.GetSize))!.Invoke(null, null)!;
+
+            componentType = componentTypes[componentTypes.Count - 1];
+            Offsets.Add(componentType.type, offset);
+            ComponentTypes[componentTypes.Count - 1] =
+                (componentType.type, (int)(RecordSize - offset), componentType.affectiveHashCode);
+        }
 
         this.columnType = columnType;
-        RecordSize = offset;
 
-        foreach ((Type type, _, int affectiveHashCode) in componentTypes)
+        foreach ((Type type, int size, int affectiveHashCode) in ComponentTypes) {
             HashCodes.Add(type, type.GetHashCode() + affectiveHashCode * 16777619);
+            ExtendedInformation.Add(type, (Offsets[type], size));
+        }
 
         // Enqueue affective systems.
         foreach (AffectiveSystem affectiveSystem in world.AffectiveSystems)
@@ -82,6 +118,12 @@ internal class Archetype {
         return typeof(T).GetHashCode();
     }
 
+    private static Type WrapReference(Type type) {
+        if (type.IsValueType)
+            return type;
+        return typeof(ArchetypeColumnReferenceWrapper<>).MakeGenericType(type);
+    }
+
     public void Initialize() {
         if (isInitialized.Exchange(true))
             return;
@@ -129,6 +171,7 @@ internal class Archetype {
     }
 
     public void ReleaseRecord(ArchetypeChunk chunk, nint index) {
+        Debug.Assert(index % RecordSize == 0);
         releasedRecords.Enqueue((chunk, index));
     }