use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{
self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector,
StorageMut, LU,
};
#[repr(C)]
#[derive(Copy, Clone, Debug, Default)]
struct Force {
linear: Vector<Real>,
angular: AngVector<Real>,
}
impl Force {
fn new(linear: Vector<Real>, angular: AngVector<Real>) -> Self {
Self { linear