Skip to content

fix: Bepu transformation for bodies children of bodies #3013

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Eideren wants to merge 1 commit into
base: master
Choose a base branch
from
Open

fix: Bepu transformation for bodies children of bodies #3013

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
147 changes: 81 additions & 66 deletions sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -557,7 +557,7 @@ public ConversionEnum<ManagedConverter, OverlapInfoStack, OverlapInfo> OverlapIn
/// <summary>
/// Called by the BroadPhase.GetOverlaps to collect all encountered collidables.
/// </summary>
struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
private struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
{
public QuickList<CollidableReference> References;
//The enumerator never gets stored into unmanaged memory, so it's safe to include a reference type instance.
Expand All @@ -574,7 +574,7 @@ public bool LoopBody(CollidableReference reference)
/// <summary>
/// Provides callbacks for filtering and data collection to the CollisionBatcher we'll be using to test query shapes against the detected environment.
/// </summary>
struct BatcherCallbacks<T> : ICollisionCallbacks where T : IOverlapCollector
private struct BatcherCallbacks<T> : ICollisionCallbacks where T : IOverlapCollector
{
public required CollisionMask CollisionMask;
public required QuickList<CollidableReference> References;
Expand Down Expand Up @@ -605,7 +605,7 @@ public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where
}
}

unsafe void OverlapInner<TShape, TCollector>(in TShape shape, in SRigidPose pose, CollisionMask collisionMask, ref TCollector collector) where TShape : unmanaged, IConvexShape where TCollector : IOverlapCollector
private unsafe void OverlapInner<TShape, TCollector>(in TShape shape, in SRigidPose pose, CollisionMask collisionMask, ref TCollector collector) where TShape : unmanaged, IConvexShape where TCollector : IOverlapCollector
{
fixed (TShape* queryShapeData = &shape)
{
Expand Down Expand Up @@ -749,49 +749,14 @@ internal void Update(TimeSpan elapsed)

private void SyncActiveTransformsWithPhysics()
{
var job = new SyncTransformsJob(Simulation.Bodies, this);
if (ParallelUpdate)
{
Dispatcher.For(0, Simulation.Bodies.ActiveSet.Count, (i) => SyncTransformsWithPhysics(Simulation.Bodies.GetBodyReference(Simulation.Bodies.ActiveSet.IndexToHandle[i]), this));
Dispatcher.ForBatched(Simulation.Bodies.ActiveSet.Count, job);
}
else
{
for (int i = 0; i < Simulation.Bodies.ActiveSet.Count; i++)
{
SyncTransformsWithPhysics(Simulation.Bodies.GetBodyReference(Simulation.Bodies.ActiveSet.IndexToHandle[i]), this);
}
}

static void SyncTransformsWithPhysics(in BodyReference body, BepuSimulation bepuSim)
{
var collidable = bepuSim.GetComponent(body.Handle);

for (var item = collidable.Parent; item != null; item = item.Parent)
{
if (item.BodyReference is { } bRef)
{
// Have to go through our parents to make sure they're up to date since we're reading from the parent's world matrix
// This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing
SyncTransformsWithPhysics(bRef, bepuSim);
// This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc.
// It's not that likely but should still be documented as suboptimal somewhere
item.Entity.Transform.Parent.UpdateWorldMatrix();
}
}

var localPosition = body.Pose.Position.ToStride();
var localRotation = body.Pose.Orientation.ToStride();

var entityTransform = collidable.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(collidable.CenterOfMass, localRotation);
job.Process(0, Simulation.Bodies.ActiveSet.Count);
}
}

Expand All @@ -801,30 +766,63 @@ private void InterpolateTransforms()
// a value of 0.5 means that we're halfway to the next physics update, just have to wait for the same amount of time.
var interpolationFactor = (float)(_remainingUpdateTime.TotalSeconds / FixedTimeStep.TotalSeconds);
interpolationFactor = MathF.Min(interpolationFactor, 1f);
var job = new InterpolateTransformsJob(interpolationFactor, _interpolatedBodies);
if (ParallelUpdate)
{
Dispatcher.For(0, _interpolatedBodies.Count, i => InterpolateBody(_interpolatedBodies[i], interpolationFactor));
Dispatcher.ForBatched(_interpolatedBodies.Count, job);
}
else
{
foreach (var body in _interpolatedBodies)
job.Process(0, _interpolatedBodies.Count);
}
}

internal void Register(ISimulationUpdate simulationUpdateComponent)
{
Elider.AddToHandlers(simulationUpdateComponent, _simulationUpdateComponents);
}

internal bool Unregister(ISimulationUpdate simulationUpdateComponent)
{
return Elider.RemoveFromHandlers(simulationUpdateComponent, _simulationUpdateComponents);
}

internal void RegisterInterpolated(BodyComponent body)
{
_interpolatedBodies.Add(body);

Debug.Assert(body.BodyReference.HasValue);

body.PreviousPose = body.CurrentPose = body.BodyReference.Value.Pose;
}

internal void UnregisterInterpolated(BodyComponent body)
{
_interpolatedBodies.Remove(body);
}

private readonly struct InterpolateTransformsJob(float interpolationFactor, List<BodyComponent> bodies) : Dispatcher.IBatchJob
{
public void Process(int start, int endExclusive)
{
for (int i = start; i < endExclusive; i++)
{
InterpolateBody(body, interpolationFactor);
InterpolateBody(bodies[i], interpolationFactor);
}
}

static void InterpolateBody(BodyComponent body, float interpolationFactor)
private static void InterpolateBody(BodyComponent body, float interpolationFactor)
{
// Have to go through our parents to make sure they're up-to-date since we're reading from the parent's world matrix
// This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing
for (var item = body.Parent; item != null; item = item.Parent)
for (var parent = body.Parent; parent != null; parent = parent.Parent)
{
if (item is BodyComponent parentBody && parentBody.InterpolationMode != InterpolationMode.None)
if (parent.InterpolationMode != InterpolationMode.None)
{
InterpolateBody(parentBody, interpolationFactor); // This one will take care of his parents too.
InterpolateBody(parent, interpolationFactor); // This one will take care of his parents too.
// This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc.
// It's not that likely but should still be documented as suboptimal somewhere
parentBody.Entity.Transform.Parent.UpdateWorldMatrix();
parent.Entity.Transform.UpdateWorldMatrix();
break;
}
}
Expand All @@ -842,28 +840,45 @@ static void InterpolateBody(BodyComponent body, float interpolationFactor)
}
}

internal void Register(ISimulationUpdate simulationUpdateComponent)
private readonly struct SyncTransformsJob(Bodies bodies, BepuSimulation bepuSimulation) : Dispatcher.IBatchJob
{
Elider.AddToHandlers(simulationUpdateComponent, _simulationUpdateComponents);
}

internal bool Unregister(ISimulationUpdate simulationUpdateComponent)
{
return Elider.RemoveFromHandlers(simulationUpdateComponent, _simulationUpdateComponents);
}
public void Process(int start, int endExclusive)
{
for (int i = start; i < endExclusive; i++)
{
var bepuBody = bodies.GetBodyReference(bodies.ActiveSet.IndexToHandle[i]);
var strideBody = bepuSimulation.GetComponent(bepuBody);
SyncTransformsWithPhysics(bepuBody, strideBody);
}
}

internal void RegisterInterpolated(BodyComponent body)
{
_interpolatedBodies.Add(body);
private static void SyncTransformsWithPhysics(in BodyReference body, BodyComponent component)
{
if (component.Parent?.BodyReference is { } bRef)
{
// Have to go through our parents to make sure they're up to date since we're reading from the parent's world matrix
// This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing
SyncTransformsWithPhysics(bRef, component.Parent);
// This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc.
// It's not that likely but should still be documented as suboptimal somewhere
component.Parent.Entity.Transform.UpdateWorldMatrix();
}

Debug.Assert(body.BodyReference.HasValue);
var localPosition = body.Pose.Position.ToStride();
var localRotation = body.Pose.Orientation.ToStride();

body.PreviousPose = body.CurrentPose = body.BodyReference.Value.Pose;
}
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;
}

internal void UnregisterInterpolated(BodyComponent body)
{
_interpolatedBodies.Remove(body);
entityTransform.Rotation = localRotation;
entityTransform.Position = localPosition - Vector3.Transform(component.CenterOfMass, localRotation);
}
}

/// <summary>
Expand Down
Loading