Complete the implementation of non-simd joint motor for the revolute joint.

5 files changed, 194 insertions, 22 deletions

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index fa35520..5d776b0 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -54,6 +54,13 @@ impl GenericJoint {
         }
     }
 
+    pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) {
+        self.min_velocity[dof as usize] = target_vel;
+        self.max_velocity[dof as usize] = target_vel;
+        self.min_impulse[dof as usize] = -max_force;
+        self.max_impulse[dof as usize] = max_force;
+    }
+
     pub fn free_dof(&mut self, dof: u8) {
         self.min_position[dof as usize] = -Real::MAX;
         self.max_position[dof as usize] = Real::MAX;
@@ -82,6 +89,18 @@ impl From<RevoluteJoint> for GenericJoint {
 
         let mut result = Self::new(local_anchor1, local_anchor2);
         result.free_dof(3);
+
+        if joint.motor_damping != 0.0 {
+            result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse);
+        }
+
+        result.impulse[0] = joint.impulse[0];
+        result.impulse[1] = joint.impulse[1];
+        result.impulse[2] = joint.impulse[2];
+        result.impulse[3] = joint.motor_impulse;
+        result.impulse[4] = joint.impulse[3];
+        result.impulse[5] = joint.impulse[4];
+
         result
     }
 }
@@ -92,6 +111,9 @@ impl From<BallJoint> for GenericJoint {
         let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
 
         let mut result = Self::new(local_anchor1, local_anchor2);
+        result.impulse[0] = joint.impulse[0];
+        result.impulse[1] = joint.impulse[1];
+        result.impulse[2] = joint.impulse[2];
         result.free_dof(3);
         result.free_dof(4);
         result.free_dof(5);
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 31c5e0a..fe77b4b 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -1,8 +1,6 @@
 #[cfg(feature = "dim3")]
 use crate::dynamics::RevoluteJoint;
-use crate::dynamics::{
-    BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle,
-};
+use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -19,7 +17,7 @@ pub enum JointParams {
     /// A revolute joint that removes all degrees of degrees of freedom between the affected
     /// bodies except for the translation along one axis.
     RevoluteJoint(RevoluteJoint),
-    GenericJoint(GenericJoint),
+    // GenericJoint(GenericJoint),
 }
 
 impl JointParams {
@@ -29,7 +27,7 @@ impl JointParams {
             JointParams::BallJoint(_) => 0,
             JointParams::FixedJoint(_) => 1,
             JointParams::PrismaticJoint(_) => 2,
-            JointParams::GenericJoint(_) => 3,
+            // JointParams::GenericJoint(_) => 3,
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(_) => 4,
         }
@@ -53,14 +51,14 @@ impl JointParams {
         }
     }
 
-    /// Gets a reference to the underlying generic joint, if `self` is one.
-    pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
-        if let JointParams::GenericJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
+    // /// Gets a reference to the underlying generic joint, if `self` is one.
+    // pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
+    //     if let JointParams::GenericJoint(j) = self {
+    //         Some(j)
+    //     } else {
+    //         None
+    //     }
+    // }
 
     /// Gets a reference to the underlying prismatic joint, if `self` is one.
     pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
@@ -94,11 +92,11 @@ impl From<FixedJoint> for JointParams {
     }
 }
 
-impl From<GenericJoint> for JointParams {
-    fn from(j: GenericJoint) -> Self {
-        JointParams::GenericJoint(j)
-    }
-}
+// impl From<GenericJoint> for JointParams {
+//     fn from(j: GenericJoint) -> Self {
+//         JointParams::GenericJoint(j)
+//     }
+// }
 
 #[cfg(feature = "dim3")]
 impl From<RevoluteJoint> for JointParams {
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index 92dd715..72a7483 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,18 +1,20 @@
 pub use self::ball_joint::BallJoint;
 pub use self::fixed_joint::FixedJoint;
-pub use self::generic_joint::GenericJoint;
+// pub use self::generic_joint::GenericJoint;
 pub use self::joint::{Joint, JointParams};
 pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
 pub use self::joint_set::{JointHandle, JointSet};
 pub use self::prismatic_joint::PrismaticJoint;
 #[cfg(feature = "dim3")]
 pub use self::revolute_joint::RevoluteJoint;
+pub use self::spring_model::SpringModel;
 
 mod ball_joint;
 mod fixed_joint;
-mod generic_joint;
+// mod generic_joint;
 mod joint;
 mod joint_set;
 mod prismatic_joint;
 #[cfg(feature = "dim3")]
 mod revolute_joint;
+mod spring_model;
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index ad7db0d..9af0ae6 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -1,6 +1,7 @@
-use crate::math::{Point, Real, Vector};
+use crate::dynamics::SpringModel;
+use crate::math::{Isometry, Point, Real, Vector};
 use crate::utils::WBasis;
-use na::{Unit, Vector5};
+use na::{RealField, Unit, Vector5};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -22,6 +23,28 @@ pub struct RevoluteJoint {
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     pub impulse: Vector5<Real>,
+    /// The target relative angular velocity the motor will attempt to reach.
+    pub motor_target_vel: Real,
+    /// The target relative angle along the joint axis the motor will attempt to reach.
+    pub motor_target_pos: Real,
+    /// The motor's stiffness.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_stiffness: Real,
+    /// The motor's damping.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_damping: Real,
+    /// The maximal impulse the motor is able to deliver.
+    pub motor_max_impulse: Real,
+    /// The angular impulse applied by the motor.
+    pub motor_impulse: Real,
+    /// The spring-like model used by the motor to reach the target velocity and .
+    pub motor_model: SpringModel,
+    // Used to handle cases where the position target ends up being more than pi radians away.
+    pub(crate) motor_last_angle: Real,
+    // The angular impulse expressed in world-space.
+    pub(crate) world_ang_impulse: Vector<Real>,
+    // The world-space orientation of the free axis of the first attached body.
+    pub(crate) prev_axis1: Vector<Real>,
 }
 
 impl RevoluteJoint {
@@ -41,6 +64,74 @@ impl RevoluteJoint {
             basis1: local_axis1.orthonormal_basis(),
             basis2: local_axis2.orthonormal_basis(),
             impulse: na::zero(),
+            world_ang_impulse: na::zero(),
+            motor_target_vel: 0.0,
+            motor_target_pos: 0.0,
+            motor_stiffness: 0.0,
+            motor_damping: 0.0,
+            motor_max_impulse: Real::MAX,
+            motor_impulse: 0.0,
+            prev_axis1: *local_axis1,
+            motor_model: SpringModel::VelocityBased,
+            motor_last_angle: 0.0,
         }
     }
+
+    pub fn configure_motor_model(&mut self, model: SpringModel) {
+        self.motor_model = model;
+    }
+
+    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
+        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    }
+
+    pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
+        self.configure_motor(target_pos, 0.0, stiffness, damping)
+    }
+
+    pub fn configure_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.motor_target_vel = target_vel;
+        self.motor_target_pos = target_pos;
+        self.motor_stiffness = stiffness;
+        self.motor_damping = damping;
+    }
+
+    /// Estimates the current position of the motor angle.
+    pub fn estimate_motor_angle(
+        &self,
+        body_pos1: &Isometry<Real>,
+        body_pos2: &Isometry<Real>,
+    ) -> Real {
+        let motor_axis1 = body_pos1 * self.local_axis1;
+        let ref1 = body_pos1 * self.basis1[0];
+        let ref2 = body_pos2 * self.basis2[0];
+
+        let last_angle_cycles = (self.motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
+
+        // Measure the position between 0 and 2-pi
+        let new_angle = if ref1.cross(&ref2).dot(&motor_axis1) < 0.0 {
+            Real::two_pi() - ref1.angle(&ref2)
+        } else {
+            ref1.angle(&ref2)
+        };
+
+        // The last angle between 0 and 2-pi
+        let last_angle_zero_two_pi = self.motor_last_angle - last_angle_cycles;
+
+        // Figure out the smallest angle differance.
+        let mut angle_diff = new_angle - last_angle_zero_two_pi;
+        if angle_diff > Real::pi() {
+            angle_diff -= Real::two_pi()
+        } else if angle_diff < -Real::pi() {
+            angle_diff += Real::two_pi()
+        }
+
+        self.motor_last_angle + angle_diff
+    }
 }
diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs
new file mode 100644
index 0000000..dd6035d
--- /dev/null
+++ b/src/dynamics/joint/spring_model.rs
@@ -0,0 +1,59 @@
+use crate::math::Real;
+
+/// The spring-like model used for constraints resolution.
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum SpringModel {
+    /// No equation is solved.
+    Disabled,
+    /// The solved spring-like equation is:
+    /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
+    ///
+    /// Here the `stiffness` is the ratio of position error to be solved at each timestep (like
+    /// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at
+    /// each timestep.
+    VelocityBased,
+    /// The solved spring-like equation is:
+    /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
+    AccelerationBased,
+    /// The solved spring-like equation is:
+    /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
+    ForceBased,
+}
+
+impl SpringModel {
+    /// Combines the coefficients used for solving the spring equation.
+    ///
+    /// Returns the new coefficients (stiffness, damping, inv_lhs_scale, keep_inv_lhs)
+    /// coefficients for the equivalent impulse-based equation. These new
+    /// coefficients must be used in the following way:
+    /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
+    /// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`.
+    /// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero.
+    pub fn combine_coefficients(
+        self,
+        dt: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> (Real, Real, Real, bool) {
+        match self {
+            SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
+            SpringModel::AccelerationBased => {
+                let effective_stiffness = stiffness * dt;
+                let effective_damping = damping * dt;
+                // TODO: Using gamma behaves very badly for some reasons.
+                // Maybe I got the formulation wrong, so let's keep it to 1.0 for now,
+                // and get back to this later.
+                // let gamma = effective_stiffness * dt + effective_damping;
+                (effective_stiffness, effective_damping, 1.0, true)
+            }
+            SpringModel::ForceBased => {
+                let effective_stiffness = stiffness * dt;
+                let effective_damping = damping * dt;
+                let gamma = effective_stiffness * dt + effective_damping;
+                (effective_stiffness, effective_damping, gamma, false)
+            }
+            SpringModel::Disabled => return (0.0, 0.0, 0.0, false),
+        }
+    }
+}