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

Refactor the constraints solver code.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-31 10:32:34 +01:00
parent 348a339fe3
commit 64507a68e1
6 changed files with 435 additions and 651 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,9 +1,15 @@
use super::{PositionSolver, VelocitySolver};
use crate::counters::Counters;
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
velocity_solver: VelocitySolver,
position_solver: PositionSolver,
}
@@ -11,6 +17,8 @@ pub struct IslandSolver {
impl IslandSolver {
pub fn new() -> Self {
Self {
contact_constraints: SolverConstraints::new(),
joint_constraints: SolverConstraints::new(),
velocity_solver: VelocitySolver::new(),
position_solver: PositionSolver::new(),
}
@@ -29,35 +37,23 @@ 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,
bodies,
manifolds,
&manifold_indices,
joints,
&joint_indices,
&mut self.position_solver.part.constraints,
);
self.contact_constraints
.init(island_id, params, bodies, manifolds, manifold_indices);
self.joint_constraints
.init(island_id, params, bodies, joints, joint_indices);
counters.solver.velocity_assembly_time.pause();
counters.solver.velocity_resolution_time.resume();
self.velocity_solver
.solve_constraints(island_id, params, bodies, manifolds, joints);
counters.solver.velocity_resolution_time.pause();
counters.solver.position_assembly_time.resume();
self.position_solver.init_constraints(
self.velocity_solver.solve(
island_id,
params,
bodies,
manifolds,
&manifold_indices,
joints,
&joint_indices,
&mut self.contact_constraints.velocity_constraints,
&mut self.joint_constraints.velocity_constraints,
);
counters.solver.position_assembly_time.pause();
counters.solver.velocity_resolution_time.pause();
}
counters.solver.velocity_update_time.resume();
@@ -67,8 +63,13 @@ impl IslandSolver {
if manifold_indices.len() != 0 || joint_indices.len() != 0 {
counters.solver.position_resolution_time.resume();
self.position_solver
.solve_constraints(island_id, params, bodies);
self.position_solver.solve(
island_id,
params,
bodies,
&self.contact_constraints.position_constraints,
&self.joint_constraints.position_constraints,
);
counters.solver.position_resolution_time.pause();
}
}

4
src/dynamics/solver/mod.rs
View File

@@ -9,6 +9,8 @@ pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
#[cfg(not(feature = "parallel"))]
pub(self) use self::position_solver::PositionSolver;
#[cfg(not(feature = "parallel"))]
pub(self) use self::solver_constraints::SolverConstraints;
#[cfg(not(feature = "parallel"))]
pub(self) use self::velocity_solver::VelocitySolver;
pub(self) use delta_vel::DeltaVel;
pub(self) use interaction_groups::*;
@@ -46,6 +48,8 @@ mod position_ground_constraint;
mod position_ground_constraint_wide;
#[cfg(not(feature = "parallel"))]
mod position_solver;
#[cfg(not(feature = "parallel"))]
mod solver_constraints;
mod velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod velocity_constraint_wide;

18
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -20,7 +20,7 @@ pub(crate) enum VelocityConstraintDesc {
GroundGrouped([usize; SIMD_WIDTH]),
}
pub(crate) struct ParallelVelocitySolverPart<Constraint> {
pub(crate) struct ParallelSolverConstraints<Constraint> {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
@@ -30,7 +30,7 @@ pub(crate) struct ParallelVelocitySolverPart<Constraint> {
pub parallel_desc_groups: Vec<usize>,
}
impl<Constraint> ParallelVelocitySolverPart<Constraint> {
impl<Constraint> ParallelSolverConstraints<Constraint> {
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
@@ -46,7 +46,7 @@ impl<Constraint> ParallelVelocitySolverPart<Constraint> {
macro_rules! impl_init_constraints_group {
($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
impl ParallelVelocitySolverPart<$Constraint> {
impl ParallelSolverConstraints<$Constraint> {
pub fn init_constraints_groups(
&mut self,
island_id: usize,
@@ -181,7 +181,7 @@ impl_init_constraints_group!(
weight
);
impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
impl ParallelSolverConstraints<AnyVelocityConstraint> {
fn fill_constraints(
&mut self,
thread: &ThreadContext,
@@ -219,7 +219,7 @@ impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
}
}
impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
fn fill_constraints(
&mut self,
thread: &ThreadContext,
@@ -262,15 +262,15 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
}
pub(crate) struct ParallelVelocitySolver {
part: ParallelVelocitySolverPart<AnyVelocityConstraint>,
joint_part: ParallelVelocitySolverPart<AnyJointVelocityConstraint>,
part: ParallelSolverConstraints<AnyVelocityConstraint>,
joint_part: ParallelSolverConstraints<AnyJointVelocityConstraint>,
}
impl ParallelVelocitySolver {
pub fn new() -> Self {
Self {
part: ParallelVelocitySolverPart::new(),
joint_part: ParallelVelocitySolverPart::new(),
part: ParallelSolverConstraints::new(),
joint_part: ParallelSolverConstraints::new(),
}
}

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

@@ -12,85 +12,24 @@ use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
pub(crate) struct PositionSolverJointPart {
pub nonground_joints: Vec<JointIndex>,
pub ground_joints: Vec<JointIndex>,
pub nonground_joint_groups: InteractionGroups,
pub ground_joint_groups: InteractionGroups,
pub constraints: Vec<AnyJointPositionConstraint>,
}
impl PositionSolverJointPart {
pub fn new() -> Self {
Self {
nonground_joints: Vec::new(),
ground_joints: Vec::new(),
nonground_joint_groups: InteractionGroups::new(),
ground_joint_groups: InteractionGroups::new(),
constraints: Vec::new(),
}
}
}
pub(crate) struct PositionSolverPart {
pub plane_point_manifolds: Vec<ContactManifoldIndex>,
pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
pub plane_point_groups: InteractionGroups,
pub plane_point_ground_groups: InteractionGroups,
pub constraints: Vec<AnyPositionConstraint>,
}
impl PositionSolverPart {
pub fn new() -> Self {
Self {
plane_point_manifolds: Vec::new(),
plane_point_ground_manifolds: Vec::new(),
plane_point_groups: InteractionGroups::new(),
plane_point_ground_groups: InteractionGroups::new(),
constraints: Vec::new(),
}
}
}
pub(crate) struct PositionSolver {
positions: Vec<Isometry<f32>>,
pub part: PositionSolverPart,
joint_part: PositionSolverJointPart,
}
impl PositionSolver {
pub fn new() -> Self {
Self {
positions: Vec::new(),
part: PositionSolverPart::new(),
joint_part: PositionSolverJointPart::new(),
}
}
pub fn init_constraints(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
self.joint_part.init_constraints(
island_id,
params,
bodies,
joints,
joint_constraint_indices,
);
}
pub fn solve_constraints(
pub fn solve(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
contact_constraints: &[AnyPositionConstraint],
joint_constraints: &[AnyJointPositionConstraint],
) {
self.positions.clear();
self.positions.extend(
@@ -100,11 +39,11 @@ impl PositionSolver {
);
for _ in 0..params.max_position_iterations {
for constraint in &self.joint_part.constraints {
for constraint in joint_constraints {
constraint.solve(params, &mut self.positions)
}
for constraint in &self.part.constraints {
for constraint in contact_constraints {
constraint.solve(params, &mut self.positions)
}
}
@@ -114,201 +53,3 @@ impl PositionSolver {
});
}
}
impl PositionSolverJointPart {
pub fn init_constraints(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
self.ground_joints.clear();
self.nonground_joints.clear();
categorize_joints(
bodies,
joints,
joint_constraint_indices,
&mut self.ground_joints,
&mut self.nonground_joints,
);
self.nonground_joint_groups.clear_groups();
self.nonground_joint_groups
.group_joints(island_id, bodies, joints, &self.nonground_joints);
self.ground_joint_groups.clear_groups();
self.ground_joint_groups
.group_joints(island_id, bodies, joints, &self.ground_joints);
/*
* Init ground joint constraints.
*/
self.constraints.clear();
compute_nongrouped_joint_ground_constraints(
params,
bodies,
joints,
&self.ground_joint_groups.nongrouped_interactions,
&mut self.constraints,
);
#[cfg(feature = "simd-is-enabled")]
{
compute_grouped_joint_ground_constraints(
params,
bodies,
joints,
&self.ground_joint_groups.grouped_interactions,
&mut self.constraints,
);
}
/*
* Init non-ground joint constraints.
*/
compute_nongrouped_joint_constraints(
params,
bodies,
joints,
&self.nonground_joint_groups.nongrouped_interactions,
&mut self.constraints,
);
#[cfg(feature = "simd-is-enabled")]
{
compute_grouped_joint_constraints(
params,
bodies,
joints,
&self.nonground_joint_groups.grouped_interactions,
&mut self.constraints,
);
}
}
}
fn compute_nongrouped_constraints(
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
output: &mut Vec<AnyPositionConstraint>,
) {
for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
PositionConstraint::generate(params, manifold, bodies, output, true)
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
output: &mut Vec<AnyPositionConstraint>,
) {
for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WPositionConstraint::generate(params, manifolds, bodies, output, true)
}
}
fn compute_nongrouped_ground_constraints(
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
output: &mut Vec<AnyPositionConstraint>,
) {
for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
PositionGroundConstraint::generate(params, manifold, bodies, output, true)
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
output: &mut Vec<AnyPositionConstraint>,
) {
for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WPositionGroundConstraint::generate(params, manifolds, bodies, output, true);
}
}
fn compute_nongrouped_joint_ground_constraints(
_params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
joint_indices: &[JointIndex],
output: &mut Vec<AnyJointPositionConstraint>,
) {
for joint_i in joint_indices {
let joint = &joints_all[*joint_i].weight;
let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
output.push(pos_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
_params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
joint_indices: &[JointIndex],
output: &mut Vec<AnyJointPositionConstraint>,
) {
for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
if let Some(pos_constraint) =
AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
{
output.push(pos_constraint);
} else {
for joint in joints.iter() {
output.push(AnyJointPositionConstraint::from_joint_ground(
*joint, bodies,
))
}
}
}
}
fn compute_nongrouped_joint_constraints(
_params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
joint_indices: &[JointIndex],
output: &mut Vec<AnyJointPositionConstraint>,
) {
for joint_i in joint_indices {
let joint = &joints_all[*joint_i];
let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies);
output.push(pos_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
_params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
joint_indices: &[JointIndex],
output: &mut Vec<AnyJointPositionConstraint>,
) {
for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) {
output.push(pos_constraint);
} else {
for joint in joints.iter() {
output.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
}
}
}
}

383
src/dynamics/solver/solver_constraints.rs Normal file
View File

@@ -0,0 +1,383 @@
use super::{
AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
};
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use crate::utils::WAngularInertia;
pub(crate) struct SolverConstraints<VelocityConstraint, PositionConstraint> {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
pub position_constraints: Vec<PositionConstraint>,
}
impl<VelocityConstraint, PositionConstraint>
SolverConstraints<VelocityConstraint, PositionConstraint>
{
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
ground_interactions: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
velocity_constraints: Vec::new(),
position_constraints: Vec::new(),
}
}
}
impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_contacts(
bodies,
manifolds,
manifold_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD contact resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.velocity_constraints.clear();
self.position_constraints.clear();
self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_constraints(params, bodies, manifolds);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_ground_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifolds_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WVelocityConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.velocity_constraints,
true,
);
WPositionConstraint::generate(
params,
manifolds,
bodies,
&mut self.position_constraints,
true,
);
}
}
fn compute_nongrouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.velocity_constraints,
true,
);
PositionConstraint::generate(
params,
manifold,
bodies,
&mut self.position_constraints,
true,
);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifolds_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WVelocityGroundConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.velocity_constraints,
true,
);
WPositionGroundConstraint::generate(
params,
manifolds,
bodies,
&mut self.position_constraints,
true,
);
}
}
fn compute_nongrouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.velocity_constraints,
true,
);
PositionGroundConstraint::generate(
params,
manifold,
bodies,
&mut self.position_constraints,
true,
)
}
}
}
impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
// Generate constraints for joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_joints(
bodies,
joints,
joint_constraint_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
self.velocity_constraints.clear();
self.position_constraints.clear();
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD joint resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
self.compute_nongrouped_joint_ground_constraints(params, bodies, joints);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_ground_constraints(params, bodies, joints);
}
self.compute_nongrouped_joint_constraints(params, bodies, joints);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_constraints(params, bodies, joints);
}
}
fn compute_nongrouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies);
self.velocity_constraints.push(vel_constraint);
let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
self.position_constraints.push(pos_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
params, joints_id, joints, bodies,
);
self.velocity_constraints.push(vel_constraint);
if let Some(pos_constraint) =
AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
{
self.position_constraints.push(pos_constraint);
} else {
for joint in joints.iter() {
self.position_constraints
.push(AnyJointPositionConstraint::from_joint_ground(
*joint, bodies,
))
}
}
}
}
fn compute_nongrouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies);
self.velocity_constraints.push(vel_constraint);
let pos_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
self.position_constraints.push(pos_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let vel_constraint =
AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
self.velocity_constraints.push(vel_constraint);
if let Some(pos_constraint) =
AnyJointPositionConstraint::from_wide_joint(joints, bodies)
{
self.position_constraints.push(pos_constraint);
} else {
for joint in joints.iter() {
self.position_constraints
.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
}
}
}
}
}

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

@@ -5,8 +5,8 @@ use super::{
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint,
WPositionGroundConstraint,
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
};
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
@@ -19,54 +19,24 @@ use crate::utils::WAngularInertia;
pub(crate) struct VelocitySolver {
pub mj_lambdas: Vec<DeltaVel<f32>>,
pub contact_part: VelocitySolverPart<AnyVelocityConstraint>,
pub joint_part: VelocitySolverPart<AnyJointVelocityConstraint>,
}
impl VelocitySolver {
pub fn new() -> Self {
Self {
mj_lambdas: Vec::new(),
contact_part: VelocitySolverPart::new(),
joint_part: VelocitySolverPart::new(),
}
}
pub fn init_constraints(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
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,
position_constraints,
);
self.joint_part.init_constraints(
island_id,
params,
bodies,
joints,
joint_constraint_indices,
)
}
pub fn solve_constraints(
pub fn solve(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
joint_constraints: &mut [AnyJointVelocityConstraint],
) {
self.mj_lambdas.clear();
self.mj_lambdas
@@ -75,11 +45,11 @@ impl VelocitySolver {
/*
* Warmstart constraints.
*/
for constraint in &self.joint_part.constraints {
for constraint in &*joint_constraints {
constraint.warmstart(&mut self.mj_lambdas[..]);
}
for constraint in &self.contact_part.constraints {
for constraint in &*contact_constraints {
constraint.warmstart(&mut self.mj_lambdas[..]);
}
@@ -87,11 +57,11 @@ impl VelocitySolver {
* Solve constraints.
*/
for _ in 0..params.max_velocity_iterations {
for constraint in &mut self.joint_part.constraints {
for constraint in &mut *joint_constraints {
constraint.solve(&mut self.mj_lambdas[..]);
}
for constraint in &mut self.contact_part.constraints {
for constraint in &mut *contact_constraints {
constraint.solve(&mut self.mj_lambdas[..]);
}
}
@@ -104,327 +74,12 @@ impl VelocitySolver {
});
// Write impulses back into the manifold structures.
for constraint in &self.joint_part.constraints {
for constraint in &*joint_constraints {
constraint.writeback_impulses(joints_all);
}
for constraint in &self.contact_part.constraints {
for constraint in &*contact_constraints {
constraint.writeback_impulses(manifolds_all);
}
}
}
pub(crate) struct VelocitySolverPart<Constraint> {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub constraints: Vec<Constraint>,
}
impl<Constraint> VelocitySolverPart<Constraint> {
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
ground_interactions: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
constraints: Vec::new(),
}
}
}
impl VelocitySolverPart<AnyVelocityConstraint> {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_contacts(
bodies,
manifolds,
manifold_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD contact resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init_constraints(
&mut self,
island_id: usize,
params: &IntegrationParameters,
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, position_constraints);
}
self.compute_nongrouped_constraints(params, bodies, manifolds, position_constraints);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_ground_constraints(
params,
bodies,
manifolds,
position_constraints,
);
}
self.compute_nongrouped_ground_constraints(params, bodies, manifolds, position_constraints);
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
constraints_all: &mut Vec<AnyPositionConstraint>,
) {
for manifolds_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WVelocityConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.constraints,
true,
);
WPositionConstraint::generate(params, manifolds, bodies, constraints_all, true);
}
}
fn compute_nongrouped_constraints(
&mut self,
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];
VelocityConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.constraints,
true,
);
PositionConstraint::generate(params, manifold, bodies, constraints_all, true);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
constraints_all: &mut Vec<AnyPositionConstraint>,
) {
for manifolds_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
WVelocityGroundConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.constraints,
true,
);
WPositionGroundConstraint::generate(params, manifolds, bodies, constraints_all, true);
}
}
fn compute_nongrouped_ground_constraints(
&mut self,
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];
VelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.constraints,
true,
);
PositionGroundConstraint::generate(params, manifold, bodies, constraints_all, true)
}
}
}
impl VelocitySolverPart<AnyJointVelocityConstraint> {
pub fn init_constraints(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
// Generate constraints for joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_joints(
bodies,
joints,
joint_constraint_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
self.constraints.clear();
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD joint resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
self.compute_nongrouped_joint_ground_constraints(params, bodies, joints);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_ground_constraints(params, bodies, joints);
}
self.compute_nongrouped_joint_constraints(params, bodies, joints);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_constraints(params, bodies, joints);
}
}
fn compute_nongrouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies);
self.constraints.push(vel_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
params, joints_id, joints, bodies,
);
self.constraints.push(vel_constraint);
}
}
fn compute_nongrouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies);
self.constraints.push(vel_constraint);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let vel_constraint =
AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
self.constraints.push(vel_constraint);
}
}
}