Add handle for rapier's generic constraints (#125)

Co-authored-by: Ocelot <FinntheRaider@gmail.com>

Author: tazercopter

common/src/main/java/dev/ryanhcode/sable/api/physics/constraint/generic/GenericConstraintConfiguration.java

@@ -0,0 +1,32 @@
+package dev.ryanhcode.sable.api.physics.constraint.generic;
+
+import dev.ryanhcode.sable.api.physics.constraint.ConstraintJointAxis;
+import dev.ryanhcode.sable.api.physics.constraint.PhysicsConstraintConfiguration;
+import org.joml.Quaterniondc;
+import org.joml.Vector3dc;
+
+import java.util.EnumSet;
+import java.util.Set;
+
+/**
+ * A configuration for a generic constraint, with per-axis hard locks and re-anchorable local frames.
+ *
+ * @param pos1         the position in world space assumed to be inside the plot of the first sub-level (ex. a block position).
+ * @param pos2         the position in world space assumed to be inside the plot of the second sub-level (ex. a block position).
+ * @param orientation1 the local orientation of the joint frame on the first sub-level.
+ * @param orientation2 the local orientation of the joint frame on the second sub-level.
+ * @param lockedAxes   the set of axes hard-locked by the solver; empty matches a free constraint.
+ * @since 1.1.0
+ */
+public record GenericConstraintConfiguration(
+        Vector3dc pos1,
+        Vector3dc pos2,
+        Quaterniondc orientation1,
+        Quaterniondc orientation2,
+        Set<ConstraintJointAxis> lockedAxes
+) implements PhysicsConstraintConfiguration<GenericConstraintHandle> {
+
+    public GenericConstraintConfiguration(final Vector3dc pos1, final Vector3dc pos2, final Quaterniondc orientation1, final Quaterniondc orientation2) {
+        this(pos1, pos2, orientation1, orientation2, EnumSet.noneOf(ConstraintJointAxis.class));
+    }
+}

common/src/main/java/dev/ryanhcode/sable/api/physics/constraint/generic/GenericConstraintHandle.java

@@ -0,0 +1,29 @@
+package dev.ryanhcode.sable.api.physics.constraint.generic;
+
+import dev.ryanhcode.sable.api.physics.constraint.PhysicsConstraintHandle;
+import org.joml.Quaterniondc;
+import org.joml.Vector3dc;
+
+/**
+ * A generic constraint between two bodies.
+ *
+ * @since 1.1.0
+ */
+public interface GenericConstraintHandle extends PhysicsConstraintHandle {
+
+    /**
+     * Sets the local frame on the first body.
+     *
+     * @param localPosition the local anchor position
+     * @param localRotation the local frame orientation
+     */
+    void setFrame1(Vector3dc localPosition, Quaterniondc localRotation);
+
+    /**
+     * Sets the local frame on the second body.
+     *
+     * @param localPosition the local anchor position
+     * @param localRotation the local frame orientation
+     */
+    void setFrame2(Vector3dc localPosition, Quaterniondc localRotation);
+}

common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/Rapier3D.java

@@ -369,6 +369,56 @@ public static native long addFreeConstraint(final int dimensionID,
                                                 double localOrientationZB,
                                                 double localOrientationWB);
 
+    /**
+     * Adds a generic constraint between two objects.
+     *
+     * @param id                 the object ID
+     * @param otherId            the other object ID
+     * @param localAnchorXA      the local anchor X on the first object
+     * @param localAnchorYA      the local anchor Y on the first object
+     * @param localAnchorZA      the local anchor Z on the first object
+     * @param localOrientationXA the local orientation X of the first object
+     * @param localOrientationYA the local orientation Y of the first object
+     * @param localOrientationZA the local orientation Z of the first object
+     * @param localOrientationWA the local orientation W of the first object
+     * @param localAnchorXB      the local anchor X on the second object
+     * @param localAnchorYB      the local anchor Y on the second object
+     * @param localAnchorZB      the local anchor Z on the second object
+     * @param localOrientationXB the local orientation X of the second object
+     * @param localOrientationYB the local orientation Y of the second object
+     * @param localOrientationZB the local orientation Z of the second object
+     * @param localOrientationWB the local orientation W of the second object
+     * @param lockedAxesMask     bit mask of locked axes; bit {@code n} corresponds to {@link dev.ryanhcode.sable.api.physics.constraint.ConstraintJointAxis#ordinal()}
+     */
+    @ApiStatus.Internal
+    public static native long addGenericConstraint(final int dimensionID,
+                                                   int id,
+                                                   int otherId,
+                                                   double localAnchorXA,
+                                                   double localAnchorYA,
+                                                   double localAnchorZA,
+                                                   double localOrientationXA,
+                                                   double localOrientationYA,
+                                                   double localOrientationZA,
+                                                   double localOrientationWA,
+                                                   double localAnchorXB,
+                                                   double localAnchorYB,
+                                                   double localAnchorZB,
+                                                   double localOrientationXB,
+                                                   double localOrientationYB,
+                                                   double localOrientationZB,
+                                                   double localOrientationWB,
+                                                   int lockedAxesMask);
+
+    /**
+     * Sets the local frame on one side of a constraint.
+     *
+     * @param handle the handle of the constraint
+     * @param side   {@code 0} for the first body, {@code 1} for the second body
+     */
+    @ApiStatus.Internal
+    public static native void setConstraintFrame(final int dimensionID, long handle, int side, double localPosX, double localPosY, double localPosZ, double localOrientationX, double localOrientationY, double localOrientationZ, double localOrientationW);
+
     /**
      * Sets if contacts are enabled between the two bodies in the constraint
      *

common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/RapierPhysicsPipeline.java

@@ -7,6 +7,7 @@
 import dev.ryanhcode.sable.api.physics.constraint.PhysicsConstraintHandle;
 import dev.ryanhcode.sable.api.physics.constraint.fixed.FixedConstraintConfiguration;
 import dev.ryanhcode.sable.api.physics.constraint.free.FreeConstraintConfiguration;
+import dev.ryanhcode.sable.api.physics.constraint.generic.GenericConstraintConfiguration;
 import dev.ryanhcode.sable.api.physics.constraint.rotary.RotaryConstraintConfiguration;
 import dev.ryanhcode.sable.api.physics.mass.MassTracker;
 import dev.ryanhcode.sable.api.physics.object.box.BoxHandle;
@@ -23,6 +24,7 @@
 import dev.ryanhcode.sable.physics.impl.rapier.collider.RapierVoxelColliderData;
 import dev.ryanhcode.sable.physics.impl.rapier.constraint.fixed.RapierFixedConstraintHandle;
 import dev.ryanhcode.sable.physics.impl.rapier.constraint.free.RapierFreeConstraintHandle;
+import dev.ryanhcode.sable.physics.impl.rapier.constraint.generic.RapierGenericConstraintHandle;
 import dev.ryanhcode.sable.physics.impl.rapier.constraint.rotary.RapierRotaryConstraintHandle;
 import dev.ryanhcode.sable.physics.impl.rapier.rope.RapierRopeHandle;
 import dev.ryanhcode.sable.sublevel.ServerSubLevel;
@@ -644,6 +646,10 @@ public <T extends PhysicsConstraintHandle> T addConstraint(@Nullable final Serve
             return (T) RapierFreeConstraintHandle.create(this.level, sublevelA, sublevelB, config);
         }
 
+        if (configuration instanceof final GenericConstraintConfiguration config) {
+            return (T) RapierGenericConstraintHandle.create(this.level, sublevelA, sublevelB, config);
+        }
+
         Sable.LOGGER.error("Unknown constraint configuration type: {}", configuration.getClass().getName());
         return null;
     }

common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/RapierConstraintHandle.java

@@ -3,9 +3,12 @@
 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 org.jetbrains.annotations.ApiStatus;
 import org.joml.Vector3d;
 
+@ApiStatus.Internal
 public abstract class RapierConstraintHandle implements PhysicsConstraintHandle {
+
     /**
      * The handle to use for {@link dev.ryanhcode.sable.physics.impl.rapier.Rapier3D} methods
      */
@@ -20,8 +23,9 @@ public abstract class RapierConstraintHandle implements PhysicsConstraintHandle
 
     /**
      * Creates a new constraint handle
+     *
      * @param sceneID the scene ID that this constraint is in
-     * @param handle the handle from the physics engine
+     * @param handle  the handle from the physics engine
      */
     protected RapierConstraintHandle(final int sceneID, final long handle) {
         this.sceneID = sceneID;
@@ -50,12 +54,12 @@ public void getJointImpulses(final Vector3d linearImpulseDest, final Vector3d an
     /**
      * Adds / sets a motor on this joint
      *
-     * @param axis        The axis on which the motor operates
-     * @param target      The target position along that axis [m | rad]
-     * @param stiffness   How stiff the motor should act, or P in the PD controller
-     * @param damping     How much damping the motor should have, or D in the PD controller
+     * @param axis          The axis on which the motor operates
+     * @param target        The target position along that axis [m | rad]
+     * @param stiffness     How stiff the motor should act, or P in the PD controller
+     * @param damping       How much damping the motor should have, or D in the PD controller
      * @param hasForceLimit If the motor should have a force limit
-     * @param maxForce    The maximum force the motor can apply
+     * @param maxForce      The maximum force the motor can apply
      */
     @Override
     public void setMotor(final ConstraintJointAxis axis, final double target, final double stiffness, final double damping, final boolean hasForceLimit, final double maxForce) {

common/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/constraint/generic/RapierGenericConstraintHandle.java

@@ -0,0 +1,83 @@
+package dev.ryanhcode.sable.physics.impl.rapier.constraint.generic;
+
+import dev.ryanhcode.sable.api.physics.constraint.ConstraintJointAxis;
+import dev.ryanhcode.sable.api.physics.constraint.generic.GenericConstraintConfiguration;
+import dev.ryanhcode.sable.api.physics.constraint.generic.GenericConstraintHandle;
+import dev.ryanhcode.sable.physics.impl.rapier.Rapier3D;
+import dev.ryanhcode.sable.physics.impl.rapier.constraint.RapierConstraintHandle;
+import dev.ryanhcode.sable.sublevel.ServerSubLevel;
+import net.minecraft.server.level.ServerLevel;
+import org.jetbrains.annotations.ApiStatus;
+import org.jetbrains.annotations.Nullable;
+import org.joml.Quaterniondc;
+import org.joml.Vector3dc;
+
+@ApiStatus.Internal
+public class RapierGenericConstraintHandle extends RapierConstraintHandle implements GenericConstraintHandle {
+
+    private static final int FRAME_SIDE_FIRST = 0;
+    private static final int FRAME_SIDE_SECOND = 1;
+
+    /**
+     * Creates a rapier constraint handle
+     */
+    public static RapierGenericConstraintHandle create(final ServerLevel serverLevel, @Nullable final ServerSubLevel sublevelA, @Nullable final ServerSubLevel sublevelB, final GenericConstraintConfiguration config) {
+        final int sceneID = Rapier3D.getID(serverLevel);
+
+        int lockedAxesMask = 0;
+        for (final ConstraintJointAxis axis : config.lockedAxes()) {
+            lockedAxesMask |= 1 << axis.ordinal();
+        }
+
+        final long handle = Rapier3D.addGenericConstraint(
+                sceneID,
+                sublevelA == null ? -1 : Rapier3D.getID(sublevelA),
+                sublevelB == null ? -1 : Rapier3D.getID(sublevelB),
+                config.pos1().x(),
+                config.pos1().y(),
+                config.pos1().z(),
+                config.orientation1().x(),
+                config.orientation1().y(),
+                config.orientation1().z(),
+                config.orientation1().w(),
+                config.pos2().x(),
+                config.pos2().y(),
+                config.pos2().z(),
+                config.orientation2().x(),
+                config.orientation2().y(),
+                config.orientation2().z(),
+                config.orientation2().w(),
+                lockedAxesMask
+        );
+
+        return new RapierGenericConstraintHandle(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) {
+        super(sceneID, handle);
+    }
+
+    @Override
+    public void setFrame1(final Vector3dc localPosition, final Quaterniondc localRotation) {
+        Rapier3D.setConstraintFrame(
+                this.sceneID, this.handle, FRAME_SIDE_FIRST,
+                localPosition.x(), localPosition.y(), localPosition.z(),
+                localRotation.x(), localRotation.y(), localRotation.z(), localRotation.w()
+        );
+    }
+
+    @Override
+    public void setFrame2(final Vector3dc localPosition, final Quaterniondc localRotation) {
+        Rapier3D.setConstraintFrame(
+                this.sceneID, this.handle, FRAME_SIDE_SECOND,
+                localPosition.x(), localPosition.y(), localPosition.z(),
+                localRotation.x(), localRotation.y(), localRotation.z(), localRotation.w()
+        );
+    }
+}