diff options
Diffstat (limited to 'src/dynamics')
| -rw-r--r-- | src/dynamics/solver/island_solver.rs | 32 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_island_solver.rs | 2 | ||||
| -rw-r--r-- | src/dynamics/solver/position_solver.rs | 5 | ||||
| -rw-r--r-- | src/dynamics/solver/solver_constraints.rs | 9 |
4 files changed, 35 insertions, 13 deletions
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 6ebf402..c117457 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -24,7 +24,25 @@ impl IslandSolver { } } - pub fn solve_island( + pub fn solve_position_constraints( + &mut self, + island_id: usize, + counters: &mut Counters, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + counters.solver.position_resolution_time.resume(); + self.position_solver.solve( + island_id, + params, + 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( &mut self, island_id: usize, counters: &mut Counters, @@ -62,17 +80,9 @@ impl IslandSolver { rb.integrate_next_position(params.dt, true) }); counters.solver.velocity_update_time.pause(); - - counters.solver.position_resolution_time.resume(); - self.position_solver.solve( - island_id, - params, - bodies, - &self.contact_constraints.position_constraints, - &self.joint_constraints.position_constraints, - ); - counters.solver.position_resolution_time.pause(); } else { + self.contact_constraints.clear(); + self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { // Since we didn't run the velocity solver we need to integrate the accelerations here diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index f32f49f..bbdc3b9 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -200,11 +200,9 @@ impl ParallelIslandSolver { let dvel = &mut self.mj_lambdas[rb.active_set_offset]; dvel.linear += rb.force * (rb.effective_inv_mass * params.dt); - rb.force = na::zero(); // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor: dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; - rb.torque = na::zero(); } } } diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index ea92c59..2fa4aee 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -21,6 +21,11 @@ impl PositionSolver { contact_constraints: &[AnyPositionConstraint], joint_constraints: &[AnyJointPositionConstraint], ) { + if contact_constraints.is_empty() && joint_constraints.is_empty() { + // Nothing to do. + return; + } + self.positions.clear(); self.positions.extend( bodies diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index b9dd497..3a4ecb7 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -38,6 +38,15 @@ impl<VelocityConstraint, PositionConstraint> position_constraints: Vec::new(), } } + + pub fn clear(&mut self) { + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + self.interaction_groups.clear(); + self.ground_interaction_groups.clear(); + self.velocity_constraints.clear(); + self.position_constraints.clear(); + } } impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> { |