diff options
Diffstat (limited to 'src')
6 files changed, 14 insertions, 8 deletions
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 434e6fb..643facb 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -286,7 +286,7 @@ impl VelocityConstraint { let mut mj_lambda2 = DeltaVel::zero(); VelocityConstraintElement::warmstart_group( - &self.elements, + &self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, @@ -305,7 +305,7 @@ impl VelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityConstraintElement::solve_group( - &mut self.elements, + &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index e80d3dd..1324c35 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -30,6 +30,7 @@ impl<N: SimdRealField> VelocityConstraintTangentPart<N> { } } + #[inline] pub fn warmstart( &self, tangents1: [&Vector<N>; DIM - 1], @@ -50,6 +51,7 @@ impl<N: SimdRealField> VelocityConstraintTangentPart<N> { } } + #[inline] pub fn solve( &mut self, tangents1: [&Vector<N>; DIM - 1], @@ -193,6 +195,7 @@ impl<N: SimdRealField> VelocityConstraintElement<N> { } } + #[inline] pub fn warmstart_group( elements: &[Self], dir1: &Vector<N>, @@ -221,6 +224,7 @@ impl<N: SimdRealField> VelocityConstraintElement<N> { } } + #[inline] pub fn solve_group( elements: &mut [Self], dir1: &Vector<N>, diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 691a983..673af54 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -217,7 +217,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::warmstart_group( - &self.elements, + &self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, @@ -257,7 +257,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::solve_group( - &mut self.elements, + &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 3509dd5..cabce5e 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -196,7 +196,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = DeltaVel::zero(); VelocityGroundConstraintElement::warmstart_group( - &self.elements, + &self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, @@ -212,7 +212,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityGroundConstraintElement::solve_group( - &mut self.elements, + &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index 43e3e01..8bcdc82 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -28,6 +28,7 @@ impl<N: SimdRealField> VelocityGroundConstraintTangentPart<N> { } } + #[inline] pub fn warmstart( &self, tangents1: [&Vector<N>; DIM - 1], @@ -40,6 +41,7 @@ impl<N: SimdRealField> VelocityGroundConstraintTangentPart<N> { } } + #[inline] pub fn solve( &mut self, tangents1: [&Vector<N>; DIM - 1], diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 22da895..ba1c46a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -194,7 +194,7 @@ impl WVelocityGroundConstraint { }; VelocityGroundConstraintElement::warmstart_group( - &self.elements, + &self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, @@ -219,7 +219,7 @@ impl WVelocityGroundConstraint { }; VelocityGroundConstraintElement::solve_group( - &mut self.elements, + &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, |