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, RigidBody,
};
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, 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 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: [&RigidBody; SIMD_WIDTH],
        rbs2: [&RigidBody; SIMD_WIDTH],
        cparams: [&PrismaticJoint; SIMD_WIDTH],
    ) -> Self {
        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
        let ii1_sqrt = AngularInertia::<SimdReal>::from(
            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
        );
        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];

        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
        let ii2_sqrt = AngularInertia::<SimdReal>::from(
            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
        );
        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];

        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
        let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
        let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);

        #[cfg(feature = "dim2")]
        let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
        #[cfg(feature = "dim3")]
        let local_basis1 = [
            Vector::from