diff options
| -rw-r--r-- | src/dynamics/solver/categorization.rs | 12 | ||||
| -rw-r--r-- | src/dynamics/solver/island_solver.rs | 2 | ||||
| -rw-r--r-- | src/dynamics/solver/position_solver.rs | 60 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_solver.rs | 39 |
4 files changed, 39 insertions, 74 deletions
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index d0a2d0f..69fe7df 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -5,9 +5,7 @@ pub(crate) fn categorize_position_contacts( bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_point_point_ground: &mut Vec<ContactManifoldIndex>, out_plane_point_ground: &mut Vec<ContactManifoldIndex>, - out_point_point: &mut Vec<ContactManifoldIndex>, out_plane_point: &mut Vec<ContactManifoldIndex>, ) { for manifold_i in manifold_indices { @@ -16,15 +14,9 @@ pub(crate) fn categorize_position_contacts( let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i), - } + out_plane_point_ground.push(*manifold_i) } else { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i), - } + out_plane_point.push(*manifold_i) } } } diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 7ce142a..1b40634 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -29,6 +29,7 @@ impl IslandSolver { ) { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.velocity_assembly_time.resume(); + self.position_solver.part.constraints.clear(); self.velocity_solver.init_constraints( island_id, params, @@ -37,6 +38,7 @@ impl IslandSolver { &manifold_indices, joints, &joint_indices, + &mut self.position_solver.part.constraints, ); counters.solver.velocity_assembly_time.pause(); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index be70d74..d610f3a 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -33,13 +33,9 @@ impl PositionSolverJointPart { } pub(crate) struct PositionSolverPart { - pub point_point_manifolds: Vec<ContactManifoldIndex>, pub plane_point_manifolds: Vec<ContactManifoldIndex>, - pub point_point_ground_manifolds: Vec<ContactManifoldIndex>, pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>, - pub point_point_groups: InteractionGroups, pub plane_point_groups: InteractionGroups, - pub point_point_ground_groups: InteractionGroups, pub plane_point_ground_groups: InteractionGroups, pub constraints: Vec<AnyPositionConstraint>, } @@ -47,13 +43,9 @@ pub(crate) struct PositionSolverPart { impl PositionSolverPart { pub fn new() -> Self { Self { - point_point_manifolds: Vec::new(), plane_point_manifolds: Vec::new(), - point_point_ground_manifolds: Vec::new(), plane_point_ground_manifolds: Vec::new(), - point_point_groups: InteractionGroups::new(), plane_point_groups: InteractionGroups::new(), - point_point_ground_groups: InteractionGroups::new(), plane_point_ground_groups: InteractionGroups::new(), constraints: Vec::new(), } @@ -62,7 +54,7 @@ impl PositionSolverPart { pub(crate) struct PositionSolver { positions: Vec<Isometry<f32>>, - part: PositionSolverPart, + pub part: PositionSolverPart, joint_part: PositionSolverJointPart, } @@ -85,8 +77,8 @@ impl PositionSolver { joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], ) { - self.part - .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + // self.part + // .init_constraints(island_id, params, bodies, manifolds, manifold_indices); self.joint_part.init_constraints( island_id, params, @@ -134,27 +126,16 @@ impl PositionSolverPart { manifolds_all: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], ) { - self.point_point_ground_manifolds.clear(); self.plane_point_ground_manifolds.clear(); - self.point_point_manifolds.clear(); self.plane_point_manifolds.clear(); categorize_position_contacts( bodies, manifolds_all, manifold_indices, - &mut self.point_point_ground_manifolds, &mut self.plane_point_ground_manifolds, - &mut self.point_point_manifolds, &mut self.plane_point_manifolds, ); - self.point_point_groups.clear_groups(); - self.point_point_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.point_point_manifolds, - ); self.plane_point_groups.clear_groups(); self.plane_point_groups.group_manifolds( island_id, @@ -163,13 +144,6 @@ impl PositionSolverPart { &self.plane_point_manifolds, ); - self.point_point_ground_groups.clear_groups(); - self.point_point_ground_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.point_point_ground_manifolds, - ); self.plane_point_ground_groups.clear_groups(); self.plane_point_ground_groups.group_manifolds( island_id, @@ -189,13 +163,6 @@ impl PositionSolverPart { params, bodies, manifolds_all, - &self.point_point_groups.grouped_interactions, - &mut self.constraints, - ); - compute_grouped_constraints( - params, - bodies, - manifolds_all, &self.plane_point_groups.grouped_interactions, &mut self.constraints, ); @@ -204,13 +171,6 @@ impl PositionSolverPart { params, bodies, manifolds_all, - &self.point_point_groups.nongrouped_interactions, - &mut self.constraints, - ); - compute_nongrouped_constraints( - params, - bodies, - manifolds_all, &self.plane_point_groups.nongrouped_interactions, &mut self.constraints, ); @@ -224,13 +184,6 @@ impl PositionSolverPart { params, bodies, manifolds_all, - &self.point_point_ground_groups.grouped_interactions, - &mut self.constraints, - ); - compute_grouped_ground_constraints( - params, - bodies, - manifolds_all, &self.plane_point_ground_groups.grouped_interactions, &mut self.constraints, ); @@ -239,13 +192,6 @@ impl PositionSolverPart { params, bodies, manifolds_all, - &self.point_point_ground_groups.nongrouped_interactions, - &mut self.constraints, - ); - compute_nongrouped_ground_constraints( - params, - bodies, - manifolds_all, &self.plane_point_ground_groups.nongrouped_interactions, &mut self.constraints, ); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 593eb0f..f091fc2 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -4,6 +4,10 @@ use super::{ #[cfg(feature = "simd-is-enabled")] use super::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts}; +use crate::dynamics::solver::{ + AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint, + WPositionGroundConstraint, +}; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, @@ -37,9 +41,16 @@ impl VelocitySolver { manifold_indices: &[ContactManifoldIndex], joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], + position_constraints: &mut Vec<AnyPositionConstraint>, ) { - self.contact_part - .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + self.contact_part.init_constraints( + island_id, + params, + bodies, + manifolds, + manifold_indices, + position_constraints, + ); self.joint_part.init_constraints( island_id, params, @@ -173,19 +184,25 @@ impl VelocitySolverPart<AnyVelocityConstraint> { bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], + position_constraints: &mut Vec<AnyPositionConstraint>, ) { self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); self.constraints.clear(); #[cfg(feature = "simd-is-enabled")] { - self.compute_grouped_constraints(params, bodies, manifolds); + self.compute_grouped_constraints(params, bodies, manifolds, position_constraints); } - self.compute_nongrouped_constraints(params, bodies, manifolds); + self.compute_nongrouped_constraints(params, bodies, manifolds, position_constraints); #[cfg(feature = "simd-is-enabled")] { - self.compute_grouped_ground_constraints(params, bodies, manifolds); + self.compute_grouped_ground_constraints( + params, + bodies, + manifolds, + position_constraints, + ); } - self.compute_nongrouped_ground_constraints(params, bodies, manifolds); + self.compute_nongrouped_ground_constraints(params, bodies, manifolds, position_constraints); } #[cfg(feature = "simd-is-enabled")] @@ -194,6 +211,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec<AnyPositionConstraint>, ) { for manifolds_i in self .interaction_groups @@ -210,6 +228,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { &mut self.constraints, true, ); + WPositionConstraint::generate(params, manifolds, bodies, constraints_all, true); } } @@ -218,6 +237,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec<AnyPositionConstraint>, ) { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; @@ -229,6 +249,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { &mut self.constraints, true, ); + PositionConstraint::generate(params, manifold, bodies, constraints_all, true); } } @@ -238,6 +259,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec<AnyPositionConstraint>, ) { for manifolds_i in self .ground_interaction_groups @@ -254,6 +276,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { &mut self.constraints, true, ); + WPositionGroundConstraint::generate(params, manifolds, bodies, constraints_all, true); } } @@ -262,6 +285,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec<AnyPositionConstraint>, ) { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; @@ -272,7 +296,8 @@ impl VelocitySolverPart<AnyVelocityConstraint> { bodies, &mut self.constraints, true, - ) + ); + PositionGroundConstraint::generate(params, manifold, bodies, constraints_all, true) } } } |