Merge pull request #415 from Wolftousen/master

Add Rope Joints

7 files changed, 442 insertions, 1 deletions

diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index aa3782b..6fecc9e 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -26,6 +26,7 @@ mod pyramid2;
 mod restitution2;
 mod sensor2;
 mod trimesh2;
+mod rope_joints2;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -75,6 +76,7 @@ pub fn main() {
         ("Polyline", polyline2::init_world),
         ("Pyramid", pyramid2::init_world),
         ("Restitution", restitution2::init_world),
+        ("Rope Joints", rope_joints2::init_world),
         ("Sensor", sensor2::init_world),
         ("Trimesh", trimesh2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
diff --git a/examples2d/rope_joints2.rs b/examples2d/rope_joints2.rs
new file mode 100644
index 0000000..c17f592
--- /dev/null
+++ b/examples2d/rope_joints2.rs
@@ -0,0 +1,62 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 0.75;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![-ground_size - ground_height, ground_size]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![ground_size + ground_height, ground_size]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Character we will control manually.
+     */
+    let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![0.0, 0.3]);
+    let character_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.15, 0.3);
+    colliders.insert_with_parent(collider, character_handle, &mut bodies);
+
+    /*
+     * Tethered Ball
+     */
+    let rad = 0.04;
+
+    let rigid_body = RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(vector![1.0, 1.0]);
+    let child_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::ball(rad);
+    colliders.insert_with_parent(collider, child_handle, &mut bodies);
+
+    let joint = RopeJointBuilder::new().local_anchor2(point![0.0, 0.0]).limits([2.0, 2.0]);
+    impulse_joints.insert(character_handle, child_handle, joint, true);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.set_character_body(character_handle);
+    testbed.look_at(point![0.0, 1.0], 100.0);
+}
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 194d7cb..e51e185 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -42,6 +42,7 @@ mod one_way_platforms3;
 mod platform3;
 mod primitives3;
 mod restitution3;
+mod rope_joints3;
 mod sensor3;
 mod trimesh3;
 mod vehicle_controller3;
@@ -100,6 +101,7 @@ pub fn main() {
         ("One-way platforms", one_way_platforms3::init_world),
         ("Platform", platform3::init_world),
         ("Restitution", restitution3::init_world),
+        ("Rope Joints", rope_joints3::init_world),
         ("Sensor", sensor3::init_world),
         ("TriMesh", trimesh3::init_world),
         ("Vehicle controller", vehicle_controller3::init_world),
diff --git a/examples3d/rope_joints3.rs b/examples3d/rope_joints3.rs
new file mode 100644
index 0000000..c72e40b
--- /dev/null
+++ b/examples3d/rope_joints3.rs
@@ -0,0 +1,76 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 0.75;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![-ground_size - ground_height, ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_height, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![ground_size + ground_height, ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_height, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, ground_height, -ground_size - ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, ground_height, ground_size + ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Character we will control manually.
+     */
+
+    let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![0.0, 0.3, 0.0]);
+    let character_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.15, 0.3, 0.15);
+    colliders.insert_with_parent(collider, character_handle, &mut bodies);
+    
+    testbed.set_initial_body_color(character_handle, [255. / 255., 131. / 255., 244.0 / 255.]);
+
+    /*
+     * Tethered Ball
+     */
+    let rad = 0.04;
+
+    let rigid_body = RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(vector![1.0, 1.0, 0.0]);
+    let child_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::ball(rad);
+    colliders.insert_with_parent(collider, child_handle, &mut bodies);
+
+    let joint = RopeJointBuilder::new().local_anchor2(point![0.0, 0.0, 0.0]).limits([2.0, 2.0]);
+    impulse_joints.insert(character_handle, child_handle, joint, true);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.set_character_body(character_handle);
+    testbed.look_at(point![10.0, 10.0, 10.0], point![0.0, 0.0, 0.0]);
+}
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index cbd9649..649b857 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint};
+use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
 use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
 use crate::utils::{WBasis, WReal};
 
@@ -521,6 +521,12 @@ impl GenericJoint {
         PrismaticJoint,
         JointAxesMask::LOCKED_PRISMATIC_AXES
     );
+    joint_conversion_methods!(
+        as_rope,
+        as_rope_mut,
+        RopeJoint,
+        JointAxesMask::FREE_FIXED_AXES
+    );
 
     #[cfg(feature = "dim3")]
     joint_conversion_methods!(
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index bb35927..93cb0ab 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -5,6 +5,7 @@ pub use self::motor_model::MotorModel;
 pub use self::multibody_joint::*;
 pub use self::prismatic_joint::*;
 pub use self::revolute_joint::*;
+pub use self::rope_joint::*;
 
 #[cfg(feature = "dim3")]
 pub use self::spherical_joint::*;
@@ -16,6 +17,7 @@ mod motor_model;
 mod multibody_joint;
 mod prismatic_joint;
 mod revolute_joint;
+mod rope_joint;
 
 #[cfg(feature = "dim3")]
 mod spherical_joint;
diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs
new file mode 100644
index 0000000..3b7317c
--- /dev/null
+++ b/src/dynamics/joint/rope_joint.rs
@@ -0,0 +1,291 @@
+use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
+use crate::dynamics::{JointAxis, MotorModel};
+use crate::math::{Point, Real, UnitVector};
+
+use super::{JointLimits, JointMotor};
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
+/// A rope joint, limits the maximum distance between two bodies
+pub struct RopeJoint {
+    /// The underlying joint data.
+    pub data: GenericJoint,
+}
+
+impl RopeJoint {
+    /// Creates a new rope joint limiting the max distance between to bodies
+    pub fn new() -> Self {
+        let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES)
+            .coupled_axes(JointAxesMask::LIN_AXES)
+            .build();
+        Self { data }
+    }
+
+    /// The underlying generic joint.
+    pub fn data(&self) -> &GenericJoint {
+        &self.data
+    }
+
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
+    /// The joint’s anchor, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.data.local_anchor1()
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    /// The joint’s anchor, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.data.local_anchor2()
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+
+    /// The principal axis of the joint, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_axis1(&self) -> UnitVector<Real> {
+        self.data.local_axis1()
+    }
+
+    /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
+    pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
+        self.data.set_local_axis1(axis1);
+        self
+    }
+
+    /// The principal axis of the joint, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_axis2(&self) -> UnitVector<Real> {
+        self.data.local_axis2()
+    }
+
+    /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
+    pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
+        self.data.set_local_axis2(axis2);
+        self
+    }
+
+    /// The motor affecting the joint’s translational degree of freedom.
+    #[must_use]
+    pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
+        self.data.motor(axis)
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
+        self.data.set_motor_model(JointAxis::X, model);
+        self.data.set_motor_model(JointAxis::Y, model);
+        #[cfg(feature = "dim3")]
+        self.data.set_motor_model(JointAxis::Z, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
+        self.data
+            .set_motor_velocity(JointAxis::X, target_vel, factor);
+        self.data
+            .set_motor_velocity(JointAxis::Y, target_vel, factor);
+        #[cfg(feature = "dim3")]
+        self.data
+            .set_motor_velocity(JointAxis::Z, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn set_motor_position(
+        &mut self,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor_position(JointAxis::X, target_pos, stiffness, damping);
+        self.data
+            .set_motor_position(JointAxis::Y, target_pos, stiffness, damping);
+        #[cfg(feature = "dim3")]
+        self.data
+            .set_motor_position(JointAxis::Z, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn set_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
+        self.data
+            .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
+        #[cfg(feature = "dim3")]
+        self.data
+            .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    /// Sets the maximum force the motor can deliver.
+    pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
+        self.data.set_motor_max_force(JointAxis::X, max_force);
+        self.data.set_motor_max_force(JointAxis::Y, max_force);
+        #[cfg(feature = "dim3")]
+        self.data.set_motor_max_force(JointAxis::Z, max_force);
+        self
+    }
+
+    /// The limit maximum distance attached bodies can translate.
+    #[must_use]
+    pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
+        self.data.limits(axis)
+    }
+
+    /// Sets the `[min,max]` limit distances attached bodies can translate.
+    pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
+        self.data.set_limits(JointAxis::X, limits);
+        self.data.set_limits(JointAxis::Y, limits);
+        #[cfg(feature = "dim3")]
+        self.data.set_limits(JointAxis::Z, limits);
+        self
+    }
+}
+
+impl Into<GenericJoint> for RopeJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+/// Create rope joints using the builder pattern.
+///
+/// A rope joint, limits the maximum distance between two bodies.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct RopeJointBuilder(pub RopeJoint);
+
+impl RopeJointBuilder {
+    /// Creates a new builder for rope joints.
+    ///
+    /// This axis is expressed in the local-space of both rigid-bodies.
+    pub fn new() -> Self {
+        Self(RopeJoint::new())
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.0.set_local_anchor1(anchor1);
+        self
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.0.set_local_anchor2(anchor2);
+        self
+    }
+
+    /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
+        self.0.set_local_axis1(axis1);
+        self
+    }
+
+    /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
+        self.0.set_local_axis2(axis2);
+        self
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
+    pub fn motor_model(mut self, model: MotorModel) -> Self {
+        self.0.set_motor_model(model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    #[must_use]
+    pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
+        self.0.set_motor_velocity(target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    #[must_use]
+    pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
+        self.0.set_motor_position(target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    #[must_use]
+    pub fn set_motor(
+        mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.0.set_motor(target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    /// Sets the maximum force the motor can deliver.
+    #[must_use]
+    pub fn motor_max_force(mut self, max_force: Real) -> Self {
+        self.0.set_motor_max_force(max_force);
+        self
+    }
+
+    /// Sets the `[min,max]` limit distances attached bodies can translate.
+    #[must_use]
+    pub fn limits(mut self, limits: [Real; 2]) -> Self {
+        self.0.set_limits(limits);
+        self
+    }
+
+    /// Builds the rope joint.
+    #[must_use]
+    pub fn build(self) -> RopeJoint {
+        self.0
+    }
+}
+
+impl Into<GenericJoint> for RopeJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
+    }
+}