nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Minor island solver simplification

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-29 11:48:31 +01:00
committed by Sébastien Crozet
parent 0bb0e412e6
commit e740493b98
2 changed files with 44 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
src/dynamics/solver/island_solver.rs
View File

@@ -11,7 +11,7 @@ use crate::dynamics::{
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
use crate::prelude::MultibodyJointSet;
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
@@ -51,113 +51,54 @@ impl IslandSolver {
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
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;
}
}
}
// 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();
}
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,
multibody_joints,
impulse_joints,
joint_indices,
);
counters.solver.velocity_assembly_time.pause();
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,
multibody_joints,
impulse_joints,
joint_indices,
);
counters.solver.velocity_assembly_time.pause();
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve(
island_id,
params,
islands,
bodies,
multibody_joints,
manifolds,
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();
} else {
self.contact_constraints.clear();
self.joint_constraints.clear();
counters.solver.velocity_update_time.resume();
for handle in islands.active_island(island_id) {
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(params.dt);
multibody.forward_kinematics(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);
bodies.set_internal(handle.0, new_vels);
bodies.set_internal(handle.0, new_poss);
}
}
counters.solver.velocity_update_time.pause();
}
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve(
island_id,
params,
islands,
bodies,
multibody_joints,
manifolds,
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();
}
}

3
src/dynamics/solver/velocity_solver.rs
View File

@@ -9,7 +9,7 @@ use crate::dynamics::{
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::prelude::{RigidBodyActivation, RigidBodyDamping, RigidBodyPosition};
use crate::prelude::{RigidBodyDamping, RigidBodyPosition};
use crate::utils::WAngularInertia;
use na::DVector;
@@ -47,7 +47,6 @@ impl VelocitySolver {
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyDamping>,
{
let cfm_factor = params.cfm_factor();