diff --git a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/RapierConstraintHandle.java b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/RapierConstraintHandle.java index 7adfc13a..17fd3b2d 100644 --- a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/RapierConstraintHandle.java +++ b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/RapierConstraintHandle.java @@ -3,7 +3,9 @@ import dev.ryanhcode.sable.api.physics.constraint.ConstraintJointAxis; import dev.ryanhcode.sable.api.physics.constraint.PhysicsConstraintHandle; import dev.ryanhcode.sable.physics.impl.rapier.Rapier3D; +import dev.ryanhcode.sable.sublevel.ServerSubLevel; import org.jetbrains.annotations.ApiStatus; +import org.jetbrains.annotations.Nullable; import org.joml.Vector3d; @ApiStatus.Internal @@ -21,6 +23,11 @@ public abstract class RapierConstraintHandle implements PhysicsConstraintHandle private final double[] impulseCache; + @Nullable + private final ServerSubLevel sublevelA; + @Nullable + private final ServerSubLevel sublevelB; + /** * Creates a new constraint handle * @@ -31,6 +38,29 @@ protected RapierConstraintHandle(final int sceneID, final long handle) { this.sceneID = sceneID; this.handle = handle; this.impulseCache = new double[6]; + + this.sublevelA = null; + this.sublevelB = null; + } + + /** + * Creates a new constraint handle + * + * @param sceneID the scene ID that this constraint is in + * @param handle the handle from the physics engine + */ + protected RapierConstraintHandle(final int sceneID, final long handle, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB) { + this.sceneID = sceneID; + this.handle = handle; + this.impulseCache = new double[6]; + + this.sublevelA = sublevelA; + this.sublevelB = sublevelB; + + if (this.sublevelA != null && this.sublevelB != null) { + this.sublevelA.addJointedSubLevels(this.sublevelB); + this.sublevelB.addJointedSubLevels(this.sublevelA); + } } /** @@ -72,6 +102,11 @@ public void setMotor(final ConstraintJointAxis axis, final double target, final @Override public void remove() { Rapier3D.removeConstraint(this.sceneID, this.handle); + + if (this.sublevelA != null && this.sublevelB != null) { + this.sublevelA.removeJointedSubLevels(this.sublevelB); + this.sublevelB.removeJointedSubLevels(this.sublevelA); + } } /** diff --git a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/fixed/RapierFixedConstraintHandle.java b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/fixed/RapierFixedConstraintHandle.java index 0fbcb28e..86832f9e 100644 --- a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/fixed/RapierFixedConstraintHandle.java +++ b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/fixed/RapierFixedConstraintHandle.java @@ -31,7 +31,7 @@ public static RapierFixedConstraintHandle create(final ServerLevel serverLevel, config.orientation().w() ); - return new RapierFixedConstraintHandle(sceneID, handle); + return new RapierFixedConstraintHandle(sceneID, handle, sublevelA, sublevelB); } /** @@ -44,4 +44,13 @@ public RapierFixedConstraintHandle(final int sceneID, final long handle) { super(sceneID, handle); } + /** + * Creates a new constraint handle + * + * @param sceneID the scene ID that this constraint is in + * @param handle the handle from the physics engine + */ + public RapierFixedConstraintHandle(final int sceneID, final long handle, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB) { + super(sceneID, handle, sublevelA, sublevelB); + } } diff --git a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/free/RapierFreeConstraintHandle.java b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/free/RapierFreeConstraintHandle.java index 4bfd6f90..8ebc5ab9 100644 --- a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/free/RapierFreeConstraintHandle.java +++ b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/free/RapierFreeConstraintHandle.java @@ -31,7 +31,7 @@ public static RapierFreeConstraintHandle create(final ServerLevel serverLevel, @ config.orientation().w() ); - return new RapierFreeConstraintHandle(sceneID, handle); + return new RapierFreeConstraintHandle(sceneID, handle, sublevelA, sublevelB); } /** @@ -44,4 +44,14 @@ public RapierFreeConstraintHandle(final int sceneID, final long handle) { super(sceneID, handle); } + /** + * Creates a new constraint handle + * + * @param sceneID the scene ID that this constraint is in + * @param handle the handle from the physics engine + */ + public RapierFreeConstraintHandle(final int sceneID, final long handle, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB) { + super(sceneID, handle, sublevelA, sublevelB); + } + } diff --git a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/generic/RapierGenericConstraintHandle.java b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/generic/RapierGenericConstraintHandle.java index 24160741..54780853 100644 --- a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/generic/RapierGenericConstraintHandle.java +++ b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/generic/RapierGenericConstraintHandle.java @@ -50,7 +50,7 @@ public static RapierGenericConstraintHandle create(final ServerLevel serverLevel lockedAxesMask ); - return new RapierGenericConstraintHandle(sceneID, handle); + return new RapierGenericConstraintHandle(sceneID, handle, sublevelA, sublevelB); } /** @@ -63,6 +63,16 @@ public RapierGenericConstraintHandle(final int sceneID, final long handle) { super(sceneID, handle); } + /** + * Creates a new constraint handle + * + * @param sceneID the scene ID that this constraint is in + * @param handle the handle from the physics engine + */ + public RapierGenericConstraintHandle(final int sceneID, final long handle, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB) { + super(sceneID, handle, sublevelA, sublevelB); + } + @Override public void setFrame1(final Vector3dc localPosition, final Quaterniondc localRotation) { Rapier3D.setConstraintFrame( diff --git a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/rotary/RapierRotaryConstraintHandle.java b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/rotary/RapierRotaryConstraintHandle.java index b30563d2..ae3900a7 100644 --- a/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/rotary/RapierRotaryConstraintHandle.java +++ b/common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/rotary/RapierRotaryConstraintHandle.java @@ -33,7 +33,7 @@ public static RapierRotaryConstraintHandle create(final ServerLevel serverLevel, config.normal2().z() ); - return new RapierRotaryConstraintHandle(sceneID, handle); + return new RapierRotaryConstraintHandle(sceneID, handle, sublevelA, sublevelB); } /** @@ -45,4 +45,14 @@ public static RapierRotaryConstraintHandle create(final ServerLevel serverLevel, public RapierRotaryConstraintHandle(final int sceneID, final long handle) { super(sceneID, handle); } + + /** + * Creates a new constraint handle + * + * @param sceneID the scene ID that this constraint is in + * @param handle the handle from the physics engine + */ + public RapierRotaryConstraintHandle(final int sceneID, final long handle, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB) { + super(sceneID, handle, sublevelA, sublevelB); + } } diff --git a/common/src/main/java/dev/ryanhcode/sable/sublevel/ServerSubLevel.java b/common/src/main/java/dev/ryanhcode/sable/sublevel/ServerSubLevel.java index ed65bf1f..a029826e 100644 --- a/common/src/main/java/dev/ryanhcode/sable/sublevel/ServerSubLevel.java +++ b/common/src/main/java/dev/ryanhcode/sable/sublevel/ServerSubLevel.java @@ -89,6 +89,20 @@ public class ServerSubLevel extends SubLevel implements PhysicsPipelineBody { private final FloatingBlockController floatingBlockController = new FloatingBlockController(this); private final ReactionWheelManager reactionWheelManager = new ReactionWheelManager(this); + + /** + * The Sublevels jointed to this sublevel + * This does not include the sublevel of "this" or its child sublevels. + */ + private final ObjectCollection jointedSubLevels = new ObjectOpenHashSet<>(); + + /** + * sub-levels that interact directly + * This also includes the child's jointed sub-level. + * This includes instances of `this`. + */ + private final ObjectCollection interactiveSubLevels = new ObjectOpenHashSet<>(); + /** * Nullable lazy map of force group -> force totals */ @@ -128,6 +142,8 @@ public class ServerSubLevel extends SubLevel implements PhysicsPipelineBody { */ private boolean trackIndividualQueuedForces = false; + private boolean updateInteractiveSubLevelsNextTick; + /** * Creates a new sub-level with the given parent level and pose. * @@ -143,6 +159,7 @@ public ServerSubLevel(final ServerLevel level, final int plotX, final int plotY, assert physicsSystem != null; this.runtimeId = physicsSystem.getNextRuntimeID(); + this.updateInteractiveSubLevels(); } /** @@ -295,6 +312,17 @@ public void applyQueuedForces(final SubLevelPhysicsSystem physicsSystem, final R */ @ApiStatus.Internal public void prePhysicsTick(final SubLevelPhysicsSystem physicsSystem, final RigidBodyHandle handle, final double timeStep) { + this.jointedSubLevels.forEach(jointedSubLevels -> { + if (jointedSubLevels.isRemoved()) { + this.removeJointedSubLevels(jointedSubLevels); + } + }); + + if (this.updateInteractiveSubLevelsNextTick) { + this.updateInteractiveSubLevels(); + this.updateInteractiveSubLevelsNextTick = false; + } + final ServerLevelPlot plot = this.getPlot(); for (final BlockEntitySubLevelActor actor : plot.getBlockEntityActors()) { actor.sable$physicsTick(this, handle, timeStep); @@ -387,6 +415,32 @@ public void prePhysicsTick(final SubLevelPhysicsSystem physicsSystem, final Rigi } } + private void updateInteractiveSubLevels() { + final Deque queue = new ArrayDeque<>(); + queue.add(this); + + final ObjectCollection allSubLevels = new ObjectOpenHashSet<>(); + while (!queue.isEmpty()) { + final ServerSubLevel currentSubLevel = queue.pop(); + allSubLevels.add(currentSubLevel); + + currentSubLevel.getJointedSubLevels().forEach(childSubLevel -> { + if (!allSubLevels.contains(childSubLevel)) { + queue.add(childSubLevel); + } + }); + } + allSubLevels.forEach(serverSubLevel -> { + serverSubLevel.interactiveSubLevels.clear(); + serverSubLevel.interactiveSubLevels.addAll(allSubLevels); + + //Display the size of allSubLevels for debugging. + //serverSubLevel.setName(String.valueOf(allSubLevels.size())); + //Display the size of jointedSubLevels for debugging. + //serverSubLevel.setName(String.valueOf(serverSubLevel.jointedSubLevels.size())); + }); + } + /** * Gets or creates a queued force group for the given force group. * @@ -504,6 +558,24 @@ public MassTracker getSelfMassTracker() { return this.massTracker.getSelfMassTracker(); } + public ObjectCollection getJointedSubLevels() { + return this.jointedSubLevels; + } + + public ObjectCollection getInteractiveSubLevels() { + return this.interactiveSubLevels; + } + + public void addJointedSubLevels(final ServerSubLevel subLevel) { + this.jointedSubLevels.add(subLevel); + this.updateInteractiveSubLevelsNextTick = true; + } + + public void removeJointedSubLevels(final ServerSubLevel subLevel) { + this.jointedSubLevels.remove(subLevel); + this.updateInteractiveSubLevelsNextTick = true; + } + /** * @return the last place this sub-level was saved */