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

use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};

use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
    AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};

#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix3x2, Matrix5, Vector3, 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 WPrismaticVelocityConstraint {
    mj_lambda1: [usize; SIMD_WIDTH],
    mj_lambda2: [usize; SIMD_WIDTH],

    joint_id: [JointIndex; SIMD_WIDTH],

    r1: Vector<SimdReal>,
    r2: Vector<SimdReal>,

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

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

    limits_active: bool,
    limits_impulse: SimdReal,
    limits_forcedir2: Vector<SimdReal>,
    limits_rhs: SimdReal,
    limits_inv_lhs: SimdReal,
    limits_impulse_limits: (SimdReal, SimdReal),

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

    im1: SimdReal,
    im2: SimdReal,

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

impl WPrismaticVelocityConstraint {
    pub fn from_params(
        params: &IntegrationParameters,
        joint_id: [JointIndex; SIMD_WIDTH],
        rbs1: (
            [&RigidBodyPosition; SIMD_WIDTH],
            [&RigidBodyVelocity; SIMD_WIDTH],
            [&RigidBodyMassProps; SIMD_WIDTH],
            [&RigidBodyIds; SIMD_WIDTH],
        ),
        rbs2: (
            [&RigidBodyPosition; SIMD_WIDTH],
            [&RigidBodyVelocity; SIMD_WIDTH],
            [&RigidBodyMassProps; SIMD_WIDTH],
            [&RigidBodyIds; SIMD_WIDTH],
        ),
        cparams: [&PrismaticJoint; SIMD_WIDTH],
    ) -> Self {
        let (poss1, vels1, mprops1, ids1) = rbs1;
        let (poss2, vels2, mprops2, ids2) = rbs2;

        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
        ]);
        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];

        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
        ]);
        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];

        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
        let local_axis1 = Vector::from(gather!