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

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

#[cfg(feature = "dim2")]
type LinImpulseDim = na::U1;
#[cfg(feature = "dim3")]
type LinImpulseDim = na::U2;

#[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: &RigidBody,
        rb2: &RigidBody,
        joint: &PrismaticJoint,
    ) -> Self {
        // Linear part.
        let anchor1 = rb1.position * joint.local_anchor1;
        let anchor2 = rb2.position * joint.local_anchor2;
        let axis1 = rb1.position * joint.local_axis1;
        let axis2 = rb2.position * joint.local_axis2;
        #[cfg(feature = "dim2")]
        let basis1 = rb1.position * joint.basis1[0];
        #[cfg(feature = "dim3")]
        let basis1 = Matrix3x2::from_columns(&[
            rb1.position * joint.basis1[0],
            rb1.position * joint.basis1[1],
        ]);

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

        let im2 = rb2.effective_inv_mass;
        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
        let r2 = anchor2 - rb2.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::<U2, U2>(0, 0)
                .copy_from(&lhs00.into_matrix());
            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
            lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
        }

        #[cfg(feature = "dim2")]
        {
            let b1r1 = basis1.dot(&r1_mat);
            let b2r2 = basis1.dot(&r2_mat);
            let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
            let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
            let m22 = ii1 + ii2;
            lhs = SdpMatrix2::new(m11, m12, m22);
        }

        let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
        let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross