fix: Moving static collidables under bepu

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs

@@ -321,6 +321,51 @@ public static void OnTriggerRemovalTest()
             RunGameTest(game);
         }
 
+        [Fact]
+        public static void StaticTriggerMoveTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                int staticEnter = 0, staticExit = 0;
+                int exitedFloor = 0;
+                var staticTrigger = new ContactEvents { NoContactResponse = true };
+                var floorTrigger = new ContactEvents { NoContactResponse = true };
+                var bodyA = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } };
+                var bodyB = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } };
+                var staticA = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() { Size = new(0.5f, 1, 0.5f) } } }, ContactEventHandler = staticTrigger } };
+                var floor = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() { Size = new(10, 1, 10) }  } }, ContactEventHandler = floorTrigger } };
+                staticTrigger.StartedTouching += (_, _) => staticEnter++;
+                staticTrigger.StoppedTouching += (_, _) => staticExit++;
+                floorTrigger.StoppedTouching += (_, _) => exitedFloor++;
+
+                floor.Transform.Position.Y = -10;
+                bodyA.Transform.Position.Y = 3;
+                bodyB.Transform.Position.X = 2;
+                bodyB.Transform.Position.Y = 3;
+
+                // Move the static component under the other entity to test that moving the static does still capture trigger events
+                staticTrigger.StartedTouching += (_, _) => staticA.Transform.Position = new Vector3(bodyB.Transform.Position.X, 0, 0);
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { bodyA, staticA, bodyB, floor });
+
+                var simulation = bodyA.GetSimulation();
+
+                while (exitedFloor == 0)
+                    await simulation.AfterUpdate();
+                await simulation.AfterUpdate();
+
+                Assert.Equal(2, staticEnter);
+
+                Assert.Equal(staticEnter, staticExit);
+
+                game.Exit();
+            });
+            RunGameTest(game);
+        }
+
         [Fact]
         public void ContactImpulseTest()
         {

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs

@@ -846,9 +846,7 @@ private static void InterpolateBody(BodyComponent body, float interpolationFacto
             // We may be able to get away with just a Lerp instead of Slerp, not sure if it needs to be normalized though at which point it may not be that much faster
             var interpolatedRotation = System.Numerics.Quaternion.Slerp(body.PreviousPose.Orientation, body.CurrentPose.Orientation, interpolationFactor).ToStride();
 
-            body.WorldToLocal(ref interpolatedPosition, ref interpolatedRotation);
-            body.Entity.Transform.Position = interpolatedPosition;
-            body.Entity.Transform.Rotation = interpolatedRotation;
+            body.UpdateTransformationComponent(interpolatedPosition, interpolatedRotation);
         }
     }
 
@@ -876,20 +874,10 @@ private static void SyncTransformsWithPhysics(in BodyReference body, BodyCompone
                 component.Parent.Entity.Transform.UpdateWorldMatrix();
             }
 
-            var localPosition = body.Pose.Position.ToStride();
-            var localRotation = body.Pose.Orientation.ToStride();
+            var worldPos = body.Pose.Position.ToStride();
+            var worldRot = body.Pose.Orientation.ToStride();
 
-            var entityTransform = component.Entity.Transform;
-            if (entityTransform.Parent is { } parent)
-            {
-                parent.WorldMatrix.Decompose(out Vector3 _, out Quaternion parentEntityRotation, out Vector3 parentEntityPosition);
-                var iRotation = Quaternion.Invert(parentEntityRotation);
-                localPosition = Vector3.Transform(localPosition - parentEntityPosition, iRotation);
-                localRotation = localRotation * iRotation;
-            }
-
-            entityTransform.Rotation = localRotation;
-            entityTransform.Position = localPosition - Vector3.Transform(component.CenterOfMass, localRotation);
+            component.UpdateTransformationComponent(worldPos, worldRot);
         }
     }
 

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs

@@ -238,14 +238,17 @@ public Vector3 AngularVelocity
     public Vector3 PreviousAngularVelocity { get; internal set; }
 
     /// <summary>
-    /// The position of this body in the physics scene, setting it will teleport this object to the position provided.
+    /// The position of this body in the physics scene, setting it will 'teleport' this object to the position provided.
     /// </summary>
     /// <remarks>
     /// Using this property to move objects around is not recommended,
     /// as it disregards any collider that may overlap with the body at this new position,
     /// you should make sure the area is clear to ensure this object does not become stuck in the scenery.<br/><br/>
-    /// This value is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>
+    /// This value is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/><br/><br/>
+    /// Writing to this property is equivalent to calling <see cref="Teleport"/>.<br/><br/>
+    /// This method overwrites this <see cref="Entity.Transform"/>'s data.
     /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="value"/> must be finite</exception>
     [DataMemberIgnore]
     public Vector3 Position
     {
@@ -260,8 +263,11 @@ public Vector3 Position
     /// <remarks>
     /// Using this property to move objects around is not recommended,
     /// as it disregards any collider that may overlap with the body at this new orientation,
-    /// you should make sure the area is clear to ensure this object does not become stuck in the scenery.
+    /// you should make sure the area is clear to ensure this object does not become stuck in the scenery.<br/><br/>
+    /// Writing to this property is equivalent to calling <see cref="Teleport"/>.<br/><br/>
+    /// This method overwrites this <see cref="Entity.Transform"/>'s data.
     /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="value"/> must be normalized</exception>
     [DataMemberIgnore]
     public Quaternion Orientation
     {
@@ -399,35 +405,34 @@ public void SetTargetPose(Vector3 targetPosition, Quaternion targetOrientation)
     /// Teleport this body into a new pose
     /// </summary>
     /// <remarks>
-    /// Using this function to move objects around is not recommended,
+    /// Using this method to move objects around is not recommended,
     /// as it disregards any collider that may overlap with the body at this new position,
     /// you should make sure the area is clear to ensure this object does not become stuck in the scenery.<br/><br/>
-    /// <paramref name="position"/> is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>
+    /// <paramref name="position"/> is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>.<br/><br/>
+    /// This method overwrites this <see cref="Entity.Transform"/>'s data.
     /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="orientation"/> must be normalized; <paramref name="position"/> must be finite</exception>
     public void Teleport(Vector3 position, Quaternion orientation)
     {
+        if (orientation.IsNormalized == false)
+            throw new ArgumentException($"{nameof(orientation)} must be normalized");
+
+        var positionNumerics = position.ToNumeric();
+        if (System.Numerics.Vector3.AllWhereAllBitsSet(System.Numerics.Vector3.IsFinite(positionNumerics)) == false)
+            throw new ArgumentException($"{nameof(position)} must be finite");
+
         if (BodyReference is { } bodyRef)
         {
-            bodyRef.Pose.Orientation = PreviousPose.Orientation = orientation.ToNumeric();
-            bodyRef.Pose.Position = PreviousPose.Position = position.ToNumeric();
+            bodyRef.Pose.Orientation = orientation.ToNumeric();
+            bodyRef.Pose.Position = positionNumerics;
             bodyRef.UpdateBounds();
-            CurrentPose = bodyRef.Pose; // Update interpolation data as well
+            PreviousPose = CurrentPose = bodyRef.Pose; // Update interpolation data as well
         }
 
-        WorldToLocal(ref position, ref orientation);
-        Entity.Transform.Position = position;
-        Entity.Transform.Rotation = orientation;
+        UpdateTransformationComponent(position, orientation);
     }
 
-    /// <summary>
-    /// Teleport this body into a new pose
-    /// </summary>
-    /// <remarks>
-    /// Using this function to move objects around is not recommended,
-    /// as it disregards any collider that may overlap with the body at this new position,
-    /// you should make sure the area is clear to ensure this object does not become stuck in the scenery.<br/><br/>
-    /// <paramref name="position"/> is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>
-    /// </remarks>
+    /// <inheritdoc cref="Teleport"/>
     [Obsolete($"This method will be removed in the future, use {nameof(Teleport)} instead")]
     public void SetPose(Vector3 position, Quaternion orientation) => Teleport(position, orientation);
 
@@ -507,23 +512,6 @@ protected override void DetachInner()
         BodyReference = null;
     }
 
-    /// <summary>
-    /// A special variant taking the center of mass into consideration
-    /// </summary>
-    internal void WorldToLocal(ref Vector3 worldPos, ref Quaternion worldRot)
-    {
-        var entityTransform = Entity.Transform;
-        if (entityTransform.Parent is { } parent)
-        {
-            parent.WorldMatrix.Decompose(out Vector3 _, out Quaternion parentEntityRotation, out Vector3 parentEntityPosition);
-            var iRotation = Quaternion.Invert(parentEntityRotation);
-            worldPos = Vector3.Transform(worldPos - parentEntityPosition, iRotation);
-            worldRot *= iRotation;
-        }
-
-        worldPos -= Vector3.Transform(CenterOfMass, worldRot);
-    }
-
     private static void SetParentForChildren(BodyComponent parent, TransformComponent root, BepuSimulation simulation)
     {
         foreach (var child in root.Children)

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs

@@ -279,6 +279,14 @@ internal void ReAttach(BepuSimulation onSimulation)
 
         Entity.Transform.UpdateWorldMatrix();
         Entity.Transform.WorldMatrix.Decompose(out _, out Quaternion collidableWorldRotation, out Vector3 collidableWorldTranslation);
+
+        if (collidableWorldRotation.IsNormalized == false)
+            throw new ArgumentException($"{nameof(collidableWorldRotation)} must be normalized");
+
+        var positionNumerics = collidableWorldTranslation.ToNumeric();
+        if (System.Numerics.Vector3.AllWhereAllBitsSet(System.Numerics.Vector3.IsFinite(positionNumerics)) == false)
+            throw new ArgumentException($"{nameof(collidableWorldTranslation)} must be finite");
+
         var pose = new NRigidPose((collidableWorldTranslation + collidableWorldRotation * CenterOfMass).ToNumeric(), collidableWorldRotation.ToNumeric());
 
         AttachInner(pose, shapeInertia, ShapeIndex);
@@ -466,4 +474,36 @@ internal void RayTest<TRayHitHandler>(
 
         Collider.RayTest(Simulation.Simulation.Shapes, ShapeIndex, Pose!.Value, new RayData { Origin = origin, Direction = dir }, ref maximumT, ref hitHandler, Simulation.BufferPool);
     }
+
+    /// <summary>
+    /// Updates <see cref="TransformComponent.Position"/> fields to match these world position and rotation
+    /// </summary>
+    /// <remarks>
+    /// Parent/Scene world matrix is expected to be up to date when calling this.<br/>
+    /// <see cref="CenterOfMass"/> is subtracted from the position; arguments are expected to be in bepu-space.
+    /// </remarks>
+    internal void UpdateTransformationComponent(Vector3 worldPos, Quaternion worldRot)
+    {
+        var entityTransform = Entity.Transform;
+        var parentMatrix = entityTransform.Parent?.WorldMatrix ?? Entity.Scene?.WorldMatrix ?? Matrix.Identity;
+        parentMatrix.Decompose(out Vector3 _, out Quaternion parentEntityRotation, out Vector3 parentEntityPosition);
+
+        var invParentRot = Quaternion.Invert(parentEntityRotation);
+        var localRot = worldRot * invParentRot;
+        var localPos = Vector3.Transform(worldPos - parentEntityPosition, invParentRot) - Vector3.Transform(CenterOfMass, localRot);
+
+        if (entityTransform.UseTRS)
+        {
+            entityTransform.Position = localPos;
+            entityTransform.Rotation = localRot;
+        }
+        else
+        {
+            Vector3 scale;
+            scale.X = entityTransform.LocalMatrix.Right.Length();
+            scale.Y = entityTransform.LocalMatrix.Up.Length();
+            scale.Z = entityTransform.LocalMatrix.Backward.Length();
+            Matrix.Transformation(ref scale, ref localRot, ref localPos, out entityTransform.LocalMatrix);
+        }
+    }
 }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs

@@ -22,26 +22,42 @@ public class StaticComponent : CollidableComponent
     /// <summary> Can be null when it isn't part of a simulation yet/anymore </summary>
     internal StaticReference? StaticReference { get; private set; } = null;
 
+    /// <summary>
+    /// The position of this static in the physics scene, setting it will 'teleport' this object to the position provided.
+    /// </summary>
+    /// <remarks>
+    /// This is performed automatically by the engine at the end of the frame when this entity's <see cref="TransformComponent"/> has been moved,
+    /// you may call this manually earlier if needed.<br/><br/>
+    /// Moving statics is very inefficient, do not write to this property every frame.<br/><br/>
+    /// This value is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>.<br/><br/>
+    /// Writing to this property is equivalent to calling <see cref="Teleport"/>.<br/><br/>
+    /// This property overwrites this <see cref="Entity.Transform"/>'s data.
+    /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="value"/> must be finite</exception>
     [DataMemberIgnore]
     public Vector3 Position
     {
         get => StaticReference?.Pose.Position.ToStride() ?? default;
-        set
-        {
-            if (StaticReference is {} staticRef)
-                staticRef.Pose.Position = value.ToNumeric();
-        }
+        set => Teleport(value, Orientation);
     }
 
+
+    /// <summary>
+    /// The rotation of this body in the physics scene, setting it will 'teleport' this object's rotation to the one provided.
+    /// </summary>
+    /// <remarks>
+    /// This is performed automatically by the engine at the end of the frame when this entity's <see cref="TransformComponent"/> has been moved,
+    /// you may call this manually earlier if needed.<br/><br/>
+    /// Moving statics is very inefficient, do not write to this property every frame.<br/><br/>
+    /// Writing to this property is equivalent to calling <see cref="Teleport"/>.<br/><br/>
+    /// This property overwrites this <see cref="Entity.Transform"/>'s data.
+    /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="value"/> must be normalized</exception>
     [DataMemberIgnore]
     public Quaternion Orientation
     {
         get => StaticReference?.Pose.Orientation.ToStride() ?? default;
-        set
-        {
-            if (StaticReference is {} staticRef)
-                staticRef.Pose.Orientation = value.ToNumeric();
-        }
+        set => Teleport(Position, value);
     }
 
     [DataMemberIgnore]
@@ -106,4 +122,38 @@ protected override void DetachInner()
 
         Processor.Statics.Remove(this);
     }
+
+    /// <summary>
+    /// Teleport this static into a new pose
+    /// </summary>
+    /// <remarks>
+    /// This is performed automatically by the engine at the end of the frame when this entity's <see cref="TransformComponent"/> has been moved,
+    /// you may call this manually earlier if needed<br/><br/>
+    /// <paramref name="position"/> is slightly offset from this entity's Transform <see cref="TransformComponent.Position"/> based on its <see cref="CollidableComponent.CenterOfMass"/>.<br/><br/>
+    /// This method overwrites this <see cref="Entity.Transform"/>'s data.
+    /// </remarks>
+    /// <exception cref="ArgumentException"><paramref name="orientation"/> must be normalized; <paramref name="position"/> must be finite</exception>
+    public void Teleport(Vector3 position, Quaternion orientation)
+    {
+        TeleportNoTransformUpdate(position, orientation);
+        UpdateTransformationComponent(position, orientation);
+    }
+
+    /// <inheritdoc cref="Teleport"/>
+    internal void TeleportNoTransformUpdate(Vector3 position, Quaternion orientation)
+    {
+        if (orientation.IsNormalized == false)
+            throw new ArgumentException($"{nameof(orientation)} must be normalized");
+
+        var positionNumerics = position.ToNumeric();
+        if (System.Numerics.Vector3.AllWhereAllBitsSet(System.Numerics.Vector3.IsFinite(positionNumerics)) == false)
+            throw new ArgumentException($"{nameof(position)} must be finite");
+
+        if (StaticReference is { } staticRef)
+        {
+            staticRef.Pose.Orientation = orientation.ToNumeric();
+            staticRef.Pose.Position = positionNumerics + CenterOfMass;
+            staticRef.UpdateBounds();
+        }
+    }
 }