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

use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
    JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
    JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{
    GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet,
    MultibodyLinkId, RigidBodySet,
};
use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
use crate::utils;
use crate::utils::IndexMut2;
use crate::utils::SimdDot;
use na::{DVector, SVector};

use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::ConstraintsCounts;
#[cfg(feature = "dim3")]
use crate::utils::SimdAngularInertia;
#[cfg(feature = "dim2")]
use na::Vector1;
use parry::math::Isometry;

#[derive(Copy, Clone)]
enum LinkOrBody {
    Link(MultibodyLinkId),
    Body(usize),
}

#[derive(Copy, Clone)]
pub struct JointGenericTwoBodyConstraintBuilder {
    link1: LinkOrBody,
    link2: LinkOrBody,
    joint_id: JointIndex,
    joint: GenericJoint,
    j_id: usize,
    // These are solver body for both joints, except that
    // the world_com is actually in local-space.
    local_body1: JointSolverBody<Real, 1>,
    local_body2: JointSolverBody<Real, 1>,
    multibodies_ndof: usize,
    constraint_id: usize,
}

impl JointGenericTwoBodyConstraintBuilder {
    pub fn invalid() -> Self {
        Self {
            link1: LinkOrBody::Body(usize::MAX),
            link2: LinkOrBody::Body(usize::MAX),
            joint_id: JointIndex::MAX,
            joint: GenericJoint::default(),
            j_id: usize::MAX,
            local_body1: JointSolverBody::invalid(),
            local_body2: JointSolverBody::invalid(),
            multibodies_ndof: usize::MAX,
            constraint_id: usize::MAX,
        }
    }

    pub fn generate(
        joint_id: JointIndex,
        joint: &ImpulseJoint,
        bodies: &RigidBodySet,
        multibodies: &MultibodyJointSet,
        out_builder: &mut Self,<