Implement multibody joints and the new solver

1 files changed, 61 insertions, 0 deletions

diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs
new file mode 100644
index 0000000..e5a6549
--- /dev/null
+++ b/src/dynamics/joint/motor_model.rs
@@ -0,0 +1,61 @@
+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 MotorModel {
+    /// 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 Default for MotorModel {
+    fn default() -> Self {
+        MotorModel::VelocityBased
+    }
+}
+
+impl MotorModel {
+    /// Combines the coefficients used for solving the spring equation.
+    ///
+    /// Returns the new coefficients (stiffness, damping, gamma, 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 {
+            MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
+            MotorModel::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)
+            } // MotorModel::ForceBased => {
+              //     let effective_stiffness = stiffness * dt;
+              //     let effective_damping = damping * dt;
+              //     let gamma = effective_stiffness * dt + effective_damping;
+              //     (effective_stiffness, effective_damping, gamma, false)
+              // }
+        }
+    }
+}