rapier2d/dynamics/joint/motor_model.rs
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
use crate::math::Real;
/// The spring-like model used for constraints resolution.
#[derive(Default, 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)`
#[default]
AccelerationBased,
/// The solved spring-like equation is:
/// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
ForceBased,
}
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)
}
}
}
}