path: root/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix3x2, Matrix5, Vector5};
#[cfg(feature = "dim2")]
use {
    na::{Matrix2, Vector2},
    parry::utils::SdpMatrix2,
};

#[cfg(feature = "dim2")]
const LIN_IMPULSE_DIM: usize = 1;
#[cfg(feature = "dim3")]
const LIN_IMPULSE_DIM: usize = 2;

#[derive(Debug)]
pub(crate) struct PrismaticVelocityConstraint {
    mj_lambda1: usize,
    mj_lambda2: usize,

    joint_id: JointIndex,

    r1: Vector<Real>,
    r2: Vector<Real>,

    #[cfg(feature = "dim3")]
    inv_lhs: Matrix5<Real>,
    #[cfg(feature = "dim3")]
    rhs: Vector5<Real>,
    #[cfg(feature = "dim3")]
    impulse: Vector5<Real>,

    #[cfg(feature = "dim2")]
    inv_lhs: Matrix2<Real>,
    #[cfg(feature = "dim2")]
    rhs: Vector2<Real>,
    #[cfg(feature = "dim2")]
    impulse: Vector2<Real>,

    motor_axis1: Vector<Real>,
    motor_axis2: Vector<Real>,
    motor_impulse: Real,
    motor_rhs: Real,
    motor_inv_lhs: Real,
    motor_max_impulse: Real,

    limits_active: bool,
    limits_impulse: Real,
    /// World-coordinate direction of the limit force on rb2.
    /// The force direction on rb1 is opposite (Newton's third law)..
    limits_forcedir2: Vector<Real>,
    limits_rhs: Real,
    limits_inv_lhs: Real,
    /// min/max applied impulse due to limits
    limits_impulse_limits: (Real, Real),

    #[cfg(feature = "dim2")]
    basis1: Vector2<Real>,
    #[cfg(feature = "dim3")]
    basis1: Matrix3x2<Real>,

    im1: Real,
    im2: Real,

    ii1_sqrt: AngularInertia<Real>,
    ii2_sqrt: AngularInertia<Real>,
}

impl PrismaticVelocityConstraint {
    pub fn from_params(
        params: &IntegrationParameters,
        joint_id: JointIndex,
        rb1: (
            &RigidBodyPosition,
            &RigidBodyVelocity,
            &RigidBodyMassProps,
            &RigidBodyIds,
        ),
        rb2: (
            &RigidBodyPosition,
            &RigidBodyVelocity,
            &RigidBodyMassProps,
            &RigidBodyIds,
        ),
        joint: &PrismaticJoint,
    ) -> Self {
        let (poss1, vels1, mprops1, ids1) = rb1;
        let (poss2, vels2, mprops2, ids2) = rb2;

        // Linear part.
        let anchor1 = poss1.position * joint.local_anchor1;
        let anchor2 = poss2.position * joint.local_anchor2;
        let axis1 = poss1.position * joint.local_axis1;
        let axis2 = poss2.position * joint.local_axis2;

        #[cfg(feature = "dim2")]
        let basis1 = poss1.position * joint.basis1[0];
        #[cfg(feature = "dim3")]
        let basis1 = Matrix3x2::from_columns(&[
            poss1.position * joint.basis1[0],
            poss1.position * joint.basis1[1],
        ]);

        let im1 = mprops1.effective_inv_mass;
        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
        let r1 = anchor1 - mprops1.world_com;
        let r1_mat = r1.gcross_matrix();

        let im2 = mprops2.effective_inv_mass;
        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
        let r2 = anchor2 - mprops2.world_com;
        let r2_mat = r2.gcross_matrix();

        #[allow(unused_mut)] // For 2D.
        let mut lhs;

        #[cfg(feature = "dim3")]
        {
            let r1_mat_b1 = r1_mat * basis1;
            let r2_mat_b1 = r2_mat * basis1;

            lhs = Matrix5::zeros();
            let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
                + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
            let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
            let lhs11 = (ii1 + ii2).into_matrix();
            lhs.fixed_slice_mut::<2, 2>(0, 0)
                .copy_from(&lhs00.into_matrix());
            lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
            lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
        }

        #[cfg(feature = "dim2")]
        {
            let b1r1 = basis1.