1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

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:
    /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
    AccelerationBased,
    /// The solved spring-like equation is:
    /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
    ForceBased,
}

impl Default for MotorModel {
    fn default() -> Self {
        MotorModel::AccelerationBased
    }
}

impl MotorModel {
    /// Combines the coefficients used for solving the spring equation.
    ///
    /// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain).
    pub fn combine_coefficients(
        self,
        dt: Real,
        stiffness: Real,
        damping: Real,
    ) -> (Real, Real, Real) {
        match self {
            MotorModel::AccelerationBased => {
                let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
                let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping);
                (erp_inv_dt, cfm_coeff, 0.0)
            }
            MotorModel::ForceBased => {
                let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
                let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping);
                (erp_inv_dt, 0.0, cfm_gain)
            }
        }
    }
}