From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Jan 2022 14:47:40 +0100 Subject: Implement multibody joints and the new solver --- src/dynamics/solver/island_solver.rs | 151 ++++++++++++++++++----------------- 1 file changed, 78 insertions(+), 73 deletions(-) (limited to 'src/dynamics/solver/island_solver.rs') diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 912fe1d..a862480 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,9 +1,8 @@ -use super::{PositionSolver, VelocitySolver}; +use super::VelocitySolver; use crate::counters::Counters; use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, - AnyVelocityConstraint, SolverConstraints, + AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints, }; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, @@ -11,12 +10,12 @@ use crate::dynamics::{ }; use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::prelude::{MultibodyJointSet, RigidBodyActivation}; pub struct IslandSolver { - contact_constraints: SolverConstraints, - joint_constraints: SolverConstraints, + contact_constraints: SolverConstraints, + joint_constraints: SolverConstraints, velocity_solver: VelocitySolver, - position_solver: PositionSolver, } impl Default for IslandSolver { @@ -31,33 +30,10 @@ impl IslandSolver { contact_constraints: SolverConstraints::new(), joint_constraints: SolverConstraints::new(), velocity_solver: VelocitySolver::new(), - position_solver: PositionSolver::new(), } } - pub fn solve_position_constraints( - &mut self, - island_id: usize, - islands: &IslandManager, - counters: &mut Counters, - params: &IntegrationParameters, - bodies: &mut Bodies, - ) where - Bodies: ComponentSet + ComponentSetMut, - { - counters.solver.position_resolution_time.resume(); - self.position_solver.solve( - island_id, - params, - islands, - bodies, - &self.contact_constraints.position_constraints, - &self.joint_constraints.position_constraints, - ); - counters.solver.position_resolution_time.pause(); - } - - pub fn init_constraints_and_solve_velocity_constraints( + pub fn init_and_solve( &mut self, island_id: usize, counters: &mut Counters, @@ -66,31 +42,64 @@ impl IslandSolver { bodies: &mut Bodies, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - joints: &mut [JointGraphEdge], + impulse_joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], + multibody_joints: &mut MultibodyJointSet, ) where Bodies: ComponentSet + ComponentSetMut + ComponentSetMut - + ComponentSet + + ComponentSetMut + + ComponentSetMut + ComponentSet + ComponentSet + ComponentSet, { - let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; + let mut has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; + if !has_constraints { + // Check if the multibody_joints have internal constraints. + for handle in islands.active_island(island_id) { + if let Some(link) = multibody_joints.rigid_body_link(*handle) { + let multibody = multibody_joints.get_multibody(link.multibody).unwrap(); + + if (link.id == 0 || link.id == 1 && !multibody.root_is_dynamic) + && multibody.has_active_internal_constraints() + { + has_constraints = true; + break; + } + } + } + } if has_constraints { + // Init the solver id for multibody_joints. + // We need that for building the constraints. + let mut solver_id = 0; + for (_, multibody) in multibody_joints.multibodies.iter_mut() { + multibody.solver_id = solver_id; + solver_id += multibody.ndofs(); + } + counters.solver.velocity_assembly_time.resume(); self.contact_constraints.init( island_id, params, islands, bodies, + multibody_joints, manifolds, manifold_indices, ); - self.joint_constraints - .init(island_id, params, islands, bodies, joints, joint_indices); + self.joint_constraints.init( + island_id, + params, + islands, + bodies, + multibody_joints, + impulse_joints, + joint_indices, + ); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); @@ -99,57 +108,53 @@ impl IslandSolver { params, islands, bodies, + multibody_joints, manifolds, - joints, + impulse_joints, &mut self.contact_constraints.velocity_constraints, + &mut self.contact_constraints.generic_velocity_constraints, + &self.contact_constraints.generic_jacobians, &mut self.joint_constraints.velocity_constraints, + &self.joint_constraints.generic_jacobians, ); counters.solver.velocity_resolution_time.pause(); - - counters.solver.velocity_update_time.resume(); - - for handle in islands.active_island(island_id) { - let (poss, vels, damping, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let mut new_poss = *poss; - let new_vels = vels.apply_damping(params.dt, damping); - new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); - - bodies.set_internal(handle.0, new_vels); - bodies.set_internal(handle.0, new_poss); - } - - counters.solver.velocity_update_time.pause(); } else { self.contact_constraints.clear(); self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); for handle in islands.active_island(island_id) { - // Since we didn't run the velocity solver we need to integrate the accelerations here - let (poss, vels, forces, damping, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); + if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() { + let multibody = multibody_joints + .get_multibody_mut_internal(link.multibody) + .unwrap(); + + if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { + let accels = &multibody.accelerations; + multibody.velocities.axpy(params.dt, accels, 1.0); + multibody.integrate_next(params.dt); + multibody.forward_kinematics_next(bodies, false); + } + } else { + // Since we didn't run the velocity solver we need to integrate the accelerations here + let (poss, vels, forces, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); - let mut new_poss = *poss; - let new_vels = forces - .integrate(params.dt, vels, mprops) - .apply_damping(params.dt, &damping); - new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); + let mut new_poss = *poss; + let new_vels = forces + .integrate(params.dt, vels, mprops) + .apply_damping(params.dt, &damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); - bodies.set_internal(handle.0, new_vels); - bodies.set_internal(handle.0, new_poss); + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } } counters.solver.velocity_update_time.pause(); } -- cgit