Add jointed sub-level detection

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);
+        }
     }
 
     /**

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);
+    }
 }

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);
+    }
+
 }

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(

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);
+    }
 }

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<ServerSubLevel> 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<ServerSubLevel> 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<ServerSubLevel> queue = new ArrayDeque<>();
+        queue.add(this);
+
+        final ObjectCollection<ServerSubLevel> 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<ServerSubLevel> getJointedSubLevels() {
+        return this.jointedSubLevels;
+    }
+
+    public ObjectCollection<ServerSubLevel> 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
      */