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

Attempt to combine the position constraints initialization with the velocity constraints initialization.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-30 12:03:25 +01:00
parent 5876a262da
commit 7545e06cb1
4 changed files with 39 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/dynamics/solver/categorization.rs
View File

@@ -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)
}
}
}

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

@@ -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();

60
src/dynamics/solver/position_solver.rs
View File

@@ -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,
@@ -185,13 +159,6 @@ impl PositionSolverPart {
*/
#[cfg(feature = "simd-is-enabled")]
{
compute_grouped_constraints(
params,
bodies,
manifolds_all,
&self.point_point_groups.grouped_interactions,
&mut self.constraints,
);
compute_grouped_constraints(
params,
bodies,
@@ -200,13 +167,6 @@ impl PositionSolverPart {
&mut self.constraints,
);
}
compute_nongrouped_constraints(
params,
bodies,
manifolds_all,
&self.point_point_groups.nongrouped_interactions,
&mut self.constraints,
);
compute_nongrouped_constraints(
params,
bodies,
@@ -220,13 +180,6 @@ impl PositionSolverPart {
*/
#[cfg(feature = "simd-is-enabled")]
{
compute_grouped_ground_constraints(
params,
bodies,
manifolds_all,
&self.point_point_ground_groups.grouped_interactions,
&mut self.constraints,
);
compute_grouped_ground_constraints(
params,
bodies,
@@ -235,13 +188,6 @@ impl PositionSolverPart {
&mut self.constraints,
);
}
compute_nongrouped_ground_constraints(
params,
bodies,
manifolds_all,
&self.point_point_ground_groups.nongrouped_interactions,
&mut self.constraints,
);
compute_nongrouped_ground_constraints(
params,
bodies,

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

@@ -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)
}
}
}