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

Finalize refactoring

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-20 12:29:57 +02:00
committed by Sébastien Crozet
parent 2b1374c596
commit f108520b5a
32 changed files with 707 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files

297
src/dynamics/ccd/ccd_solver.rs
View File

@@ -1,16 +1,10 @@
use super::TOIEntry;
use crate::dynamics::{
IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{
ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent,
NarrowPhase,
};
use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
use crate::prelude::{ActiveEvents, ColliderFlags};
use crate::prelude::ActiveEvents;
use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
use parry::utils::hashmap::HashMap;
use std::collections::BinaryHeap;
@@ -61,23 +55,18 @@ impl CCDSolver {
match impacts {
PredictedImpacts::Impacts(tois) => {
for (handle, toi) in tois {
let (rb_poss, vels, ccd, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyCcd,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let local_com = &mprops.local_mprops.local_com;
let rb = bodies.index_mut_internal(*handle);
let local_com = &rb.mprops.local_mprops.local_com;
let min_toi = (ccd.ccd_thickness
let min_toi = (rb.ccd.ccd_thickness
* 0.15
* crate::utils::inv(ccd.max_point_velocity(vels)))
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels)))
.min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi);
let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com);
bodies.map_mut_internal(handle.0, |rb_poss| {
rb_poss.next_position = new_pos;
});
let new_pos = rb
.vels
.integrate(toi.max(min_toi), &rb.pos.position, &local_com);
rb.pos.next_position = new_pos;
}
}
_ => {}
@@ -98,17 +87,16 @@ impl CCDSolver {
// println!("Checking CCD activation");
for handle in islands.active_dynamic_bodies() {
let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) =
bodies.index_bundle(handle.0);
if ccd.ccd_enabled {
let forces = if include_forces { Some(forces) } else { None };
let moving_fast = ccd.is_moving_fast(dt, vels, forces);
bodies.map_mut_internal(handle.0, |ccd| {
ccd.ccd_active = moving_fast;
});
let rb = bodies.index_mut_internal(*handle);
if rb.ccd.ccd_enabled {
let forces = if include_forces {
Some(&rb.forces)
} else {
None
};
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces);
rb.ccd.ccd_active = moving_fast;
ccd_active = ccd_active || moving_fast;
}
}
@@ -137,36 +125,31 @@ impl CCDSolver {
let mut min_toi = dt;
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
let rb1 = &bodies[*handle];
if rb_ccd1.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.vels,
&rb1.mprops,
);
let predicted_body_pos1 =
rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1);
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
for ch1 in &rb1.colliders.0 {
let co1 = &colliders[*ch1];
let co1_parent = co1
.parent
.as_ref()
.expect("Could not find the ColliderParent component.");
let (co_shape1, co_pos1, co_type1): (
&ColliderShape,
&ColliderPosition,
&ColliderType,
) = colliders.index_bundle(ch1.0);
if co_type1.is_sensor() {
if co1.is_sensor() {
continue; // Ignore sensors.
}
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.pos_wrt_parent;
let aabb1 = co1
.shape
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -182,21 +165,17 @@ impl CCDSolver {
)
.is_none()
{
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
let co_type1: &ColliderType = colliders.index(ch1.0);
let co_type2: &ColliderType = colliders.index(ch1.0);
let co1 = &colliders[*ch1];
let co2 = &colliders[*ch2];
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and sensors and apply collision groups filter.
if bh1 == bh2 // Ignore self-intersection.
|| (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors.
|| !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups.
|| !c1.3.solver_groups.test(c2.3.solver_groups)
if bh1 == bh2 // Ignore self-intersection.
|| (co1.is_sensor() || co2.is_sensor()) // Ignore sensors.
|| !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
|| !co1.flags.solver_groups.test(co2.flags.solver_groups)
// Apply solver groups.
{
return true;
@@ -208,16 +187,16 @@ impl CCDSolver {
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
let rb2 = bh2.and_then(|h| bodies.get(h));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, c2.3, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2,
co1,
co2,
Some(rb1),
rb2,
None,
None,
0.0,
@@ -271,29 +250,27 @@ impl CCDSolver {
*/
// TODO: don't iterate through all the colliders.
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
let rb1 = &bodies[*handle];
if rb_ccd1.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.vels,
&rb1.mprops,
);
let predicted_body_pos1 =
rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1);
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
for ch1 in &rb1.colliders.0 {
let co1 = &colliders[*ch1];
let co_parent1 = co1
.parent
.as_ref()
.expect("Could not find the ColliderParent component.");
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
colliders.index_bundle(ch1.0);
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
let aabb1 = co1
.shape
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -309,16 +286,15 @@ impl CCDSolver {
)
.is_none()
{
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
let co1 = &colliders[*ch1];
let co2 = &colliders[*ch2];
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersections and apply groups filter.
if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups)
if bh1 == bh2
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
{
return true;
}
@@ -329,17 +305,17 @@ impl CCDSolver {
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b1 = bh1.map(|h| bodies.index_bundle(h.0));
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
let rb1 = bh1.map(|h| &bodies[h]);
let rb2 = bh2.map(|h| &bodies[h]);
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, c2.3, co_parent2),
b1,
b2,
co1,
co2,
rb1,
rb2,
None,
None,
0.0,
@@ -381,17 +357,15 @@ impl CCDSolver {
while let Some(toi) = all_toi.pop() {
assert!(toi.toi <= dt);
let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
toi.b1.map(|b| bodies.index_bundle(b.0));
let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
toi.b2.map(|b| bodies.index_bundle(b.0));
let rb1 = toi.b1.and_then(|b| bodies.get(b));
let rb2 = toi.b2.and_then(|b| bodies.get(b));
let mut colliders_to_check = Vec::new();
let should_freeze1 = rb1.is_some()
&& rb1.unwrap().0.ccd_active
&& rb1.unwrap().ccd.ccd_active
&& !frozen.contains_key(&toi.b1.unwrap());
let should_freeze2 = rb2.is_some()
&& rb2.unwrap().0.ccd_active
&& rb2.unwrap().ccd.ccd_active
&& !frozen.contains_key(&toi.b2.unwrap());
if !should_freeze1 && !should_freeze2 {
@@ -413,12 +387,12 @@ impl CCDSolver {
if should_freeze1 {
let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0);
colliders_to_check.extend_from_slice(&rb1.unwrap().colliders.0);
}
if should_freeze2 {
let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0);
colliders_to_check.extend_from_slice(&rb2.unwrap().colliders.0);
}
let start_time = toi.toi;
@@ -426,39 +400,36 @@ impl CCDSolver {
// NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the
// ones we used above.
for ch1 in &colliders_to_check {
let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap();
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
colliders.index_bundle(ch1.0);
let co1 = &colliders[*ch1];
let co1_parent = co1.parent.as_ref().unwrap();
let rb1 = &bodies[co1_parent.handle];
let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0);
let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent;
let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1);
let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent;
let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
let co2 = &colliders[*ch2];
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and apply groups filter.
if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) {
if bh1 == bh2
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
{
return true;
}
let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h));
let b1: Option<(_, _, _, &RigidBodyCcd)> =
bh1.map(|h| bodies.index_bundle(h.0));
let b2: Option<(_, _, _, &RigidBodyCcd)> =
bh2.map(|h| bodies.index_bundle(h.0));
let rb1 = bh1.and_then(|h| bodies.get(h));
let rb2 = bh2.and_then(|h| bodies.get(h));
if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false))
&& (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false))
if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false))
&& (frozen2.is_some()
|| !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
{
// We already did a resweep.
return true;
@@ -474,10 +445,10 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, c2.3, co_parent2),
b1,
b2,
co1,
co2,
rb1,
rb2,
frozen1.copied(),
frozen2.copied(),
start_time,
@@ -500,20 +471,10 @@ impl CCDSolver {
// - If the intersection isn't active anymore, and it wasn't intersecting
// before, then we need to generate one interaction-start and one interaction-stop
// events because it will never be detected by the narrow-phase because of tunneling.
let (co_type1, co_pos1, co_shape1, co_flags1): (
&ColliderType,
&ColliderPosition,
&ColliderShape,
&ColliderFlags,
) = colliders.index_bundle(toi.c1.0);
let (co_type2, co_pos2, co_shape2, co_flags2): (
&ColliderType,
&ColliderPosition,
&ColliderShape,
&ColliderFlags,
) = colliders.index_bundle(toi.c2.0);
let co1 = &colliders[toi.c1];
let co2 = &colliders[toi.c2];
if !co_type1.is_sensor() && !co_type2.is_sensor() {
if !co1.is_sensor() && !co2.is_sensor() {
// TODO: this happens if we found a TOI between two non-sensor
// colliders with mismatching solver_flags. It is not clear
// what we should do in this case: we could report a
@@ -525,56 +486,46 @@ impl CCDSolver {
}
let co_next_pos1 = if let Some(b1) = toi.b1 {
let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap();
let (rb_pos1, rb_vels1, rb_mprops1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b1.0);
let local_com1 = &rb_mprops1.local_mprops.local_com;
let co_parent1: &ColliderParent = co1.parent.as_ref().unwrap();
let rb1 = &bodies[b1];
let local_com1 = &rb1.mprops.local_mprops.local_com;
let frozen1 = frozen.get(&b1);
let pos1 = frozen1
.map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1))
.unwrap_or(rb_pos1.next_position);
.map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
.unwrap_or(rb1.pos.next_position);
pos1 * co_parent1.pos_wrt_parent
} else {
co_pos1.0
co1.pos.0
};
let co_next_pos2 = if let Some(b2) = toi.b2 {
let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap();
let (rb_pos2, rb_vels2, rb_mprops2): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b2.0);
let local_com2 = &rb_mprops2.local_mprops.local_com;
let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap();
let rb2 = &bodies[b2];
let local_com2 = &rb2.mprops.local_mprops.local_com;
let frozen2 = frozen.get(&b2);
let pos2 = frozen2
.map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2))
.unwrap_or(rb_pos2.next_position);
.map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
.unwrap_or(rb2.pos.next_position);
pos2 * co_parent2.pos_wrt_parent
} else {
co_pos2.0
co2.pos.0
};
let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2);
let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos);
let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
let query_dispatcher = self.query_pipeline.query_dispatcher();
let intersect_before = query_dispatcher
.intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
.unwrap_or(false);
let intersect_after = query_dispatcher
.intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
.unwrap_or(false);
if !intersect_before
&& !intersect_after
&& (co_flags1.active_events | co_flags2.active_events)
&& (co1.flags.active_events | co2.flags.active_events)
.contains(ActiveEvents::COLLISION_EVENTS)
{
// Emit one intersection-started and one intersection-stopped event.

104
src/dynamics/ccd/toi_entry.rs
View File

@@ -1,9 +1,5 @@
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::dynamics::{RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -45,32 +41,10 @@ impl TOIEntry {
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
c1: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
&ColliderFlags,
Option<&ColliderParent>,
),
c2: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
&ColliderFlags,
Option<&ColliderParent>,
),
b1: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
b2: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
co1: &Collider,
co2: &Collider,
rb1: Option<&RigidBody>,
rb2: Option<&RigidBody>,
frozen1: Option<Real>,
frozen2: Option<Real>,
start_time: Real,
@@ -78,39 +52,36 @@ impl TOIEntry {
smallest_contact_dist: Real,
) -> Option<Self> {
assert!(start_time <= end_time);
if b1.is_none() && b2.is_none() {
if rb1.is_none() && rb2.is_none() {
return None;
}
let (co_type1, co_shape1, co_pos1, co_flags1, co_parent1) = c1;
let (co_type2, co_shape2, co_pos2, co_flags2, co_parent2) = c2;
let linvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
+ angvel1.abs() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
+ angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
#[cfg(feature = "dim3")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
+ angvel1.norm() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
+ angvel2.norm() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
// We may be slightly over-conservative by taking the `max(0.0)` here.
// But removing the `max` doesn't really affect performances so let's
// keep it since more conservatism is good at this stage.
let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
let thickness = (co1.shape.0.ccd_thickness() + co2.shape.0.ccd_thickness())
+ smallest_contact_dist.max(0.0);
let is_pseudo_intersection_test = co_type1.is_sensor()
|| co_type2.is_sensor()
|| !co_flags1.solver_groups.test(co_flags2.solver_groups);
let is_pseudo_intersection_test = co1.is_sensor()
|| co2.is_sensor()
|| !co1.flags.solver_groups.test(co2.flags.solver_groups);
if (end_time - start_time) * vel12 < thickness {
return None;
@@ -118,8 +89,8 @@ impl TOIEntry {
// Compute the TOI.
let identity = NonlinearRigidMotion::identity();
let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity);
let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity);
let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity);
let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -129,8 +100,8 @@ impl TOIEntry {
motion2.freeze(t);
}
let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0));
let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0));
let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0));
let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0));
// println!("start_time: {}", start_time);
@@ -146,9 +117,9 @@ impl TOIEntry {
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
co_shape1.as_ref(),
co1.shape.as_ref(),
&motion_c2,
co_shape2.as_ref(),
co2.shape.as_ref(),
start_time,
end_time,
stop_at_penetration,
@@ -160,31 +131,24 @@ impl TOIEntry {
Some(Self::new(
toi.toi,
ch1,
co_parent1.map(|p| p.handle),
co1.parent.map(|p| p.handle),
ch2,
co_parent2.map(|p| p.handle),
co2.parent.map(|p| p.handle),
is_pseudo_intersection_test,
0,
))
}
fn body_motion(
(poss, vels, mprops, ccd): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
),
) -> NonlinearRigidMotion {
if ccd.ccd_active {
fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion {
if rb.ccd.ccd_active {
NonlinearRigidMotion::new(
poss.position,
mprops.local_mprops.local_com,
vels.linvel,
vels.angvel,
rb.pos.position,
rb.mprops.local_mprops.local_com,
rb.vels.linvel,
rb.vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(poss.next_position)
NonlinearRigidMotion::constant_position(rb.pos.next_position)
}
}
}

100
src/dynamics/island_manager.rs
View File

@@ -47,12 +47,15 @@ impl IslandManager {
while i < active_set.len() {
let handle = active_set[i];
if bodies.get(handle.0).is_none() {
if bodies.get(handle).is_none() {
// This rigid-body no longer exists, so we need to remove it from the active set.
active_set.swap_remove(i);
if i < active_set.len() {
bodies.map_mut_internal(active_set[i].0, |rb_ids| rb_ids.active_set_id = i);
// Update the active_set_id for the body that has been swapped.
if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) {
swapped_rb.ids.active_set_id = i;
}
}
} else {
i += 1;
@@ -92,15 +95,13 @@ impl IslandManager {
// deleting a joint attached to an already-removed body) where we could be
// attempting to wake-up a rigid-body that has already been deleted.
if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
activation.wake_up(strong)
});
bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| {
if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) {
ids.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
});
let rb = bodies.index_mut_internal(handle);
rb.activation.wake_up(strong);
if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) {
rb.ids.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
}
@@ -162,25 +163,22 @@ impl IslandManager {
let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack;
let vels: &RigidBodyVelocity = bodies.index(h.0);
let sq_linvel = vels.linvel.norm_squared();
let sq_angvel = vels.angvel.gdot(vels.angvel);
let rb = bodies.index_mut_internal(h);
let sq_linvel = rb.vels.linvel.norm_squared();
let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
update_energy(activation, sq_linvel, sq_angvel, dt);
update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt);
if activation.time_since_can_sleep
>= RigidBodyActivation::default_time_until_sleep()
{
// Mark them as sleeping for now. This will
// be set to false during the graph traversal
// if it should not be put to sleep.
activation.sleeping = true;
can_sleep.push(h);
} else {
stack.push(h);
}
});
if rb.activation.time_since_can_sleep >= RigidBodyActivation::default_time_until_sleep()
{
// Mark them as sleeping for now. This will
// be set to false during the graph traversal
// if it should not be put to sleep.
rb.activation.sleeping = true;
can_sleep.push(h);
} else {
stack.push(h);
}
}
// Read all the contacts and push objects touching touching this rigid-body.
@@ -199,7 +197,7 @@ impl IslandManager {
(inter.collider1, inter.collider2),
*collider_handle,
);
if let Some(other_body) = colliders.get(other.0) {
if let Some(other_body) = colliders[other].parent {
stack.push(other_body.handle);
}
break;
@@ -212,15 +210,15 @@ impl IslandManager {
// Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() {
let (vels, rb_colliders): (&RigidBodyVelocity, _) = bodies.index_bundle(h.0);
let rb = &bodies[*h];
if vels.is_zero() {
if rb.vels.is_zero() {
// If the kinematic body does not move, it does not have
// to wake up any dynamic body.
continue;
}
push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
}
// println!("Selection: {}", instant::now() - t);
@@ -235,13 +233,9 @@ impl IslandManager {
let mut island_marker = self.stack.len().max(1) - 1;
while let Some(handle) = self.stack.pop() {
let (rb_status, rb_ids, rb_colliders): (
&RigidBodyType,
&RigidBodyIds,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
let rb = bodies.index_mut_internal(handle);
if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() {
if rb.ids.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through fixed bodies.
continue;
@@ -260,7 +254,7 @@ impl IslandManager {
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
for inter in impulse_joints.joints_with(handle) {
let other = crate::utils::select_other((inter.0, inter.1), handle);
@@ -271,16 +265,12 @@ impl IslandManager {
self.stack.push(other);
}
bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
activation.wake_up(false);
});
bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| {
ids.active_island_id = self.active_islands.len() - 1;
ids.active_set_id = self.active_dynamic_set.len();
ids.active_set_offset =
ids.active_set_id - self.active_islands[ids.active_island_id];
ids.active_set_timestamp = self.active_set_timestamp;
});
rb.activation.wake_up(false);
rb.ids.active_island_id = self.active_islands.len() - 1;
rb.ids.active_set_id = self.active_dynamic_set.len();
rb.ids.active_set_offset =
rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id];
rb.ids.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
}
@@ -293,13 +283,11 @@ impl IslandManager {
// );
// Actually put to sleep bodies which have not been detected as awake.
for h in &self.can_sleep {
let activation: &RigidBodyActivation = bodies.index(h.0);
if activation.sleeping {
bodies.set_internal(h.0, RigidBodyVelocity::zero());
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
activation.sleep()
});
for handle in &self.can_sleep {
let rb = bodies.index_mut_internal(*handle);
if rb.activation.sleeping {
rb.vels = RigidBodyVelocity::zero();
rb.activation.sleep();
}
}
}

30
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -3,10 +3,7 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio
use crate::data::arena::Arena;
use crate::data::Coarena;
use crate::dynamics::{
GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet,
RigidBodyType,
};
use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet};
/// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider set.
@@ -230,26 +227,17 @@ impl ImpulseJointSet {
// FIXME: don't iterate through all the interactions.
for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
let joint = &edge.weight;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let (status1, activation1, ids1): (
&RigidBodyType,
&RigidBodyActivation,
&RigidBodyIds,
) = bodies.index_bundle(joint.body1.0);
let (status2, activation2, ids2): (
&RigidBodyType,
&RigidBodyActivation,
&RigidBodyIds,
) = bodies.index_bundle(joint.body2.0);
if (status1.is_dynamic() || status2.is_dynamic())
&& (!status1.is_dynamic() || !activation1.sleeping)
&& (!status2.is_dynamic() || !activation2.sleeping)
if (rb1.is_dynamic() || rb2.is_dynamic())
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
{
let island_index = if !status1.is_dynamic() {
ids2.active_island_id
let island_index = if !rb1.is_dynamic() {
rb2.ids.active_island_id
} else {
ids1.active_island_id
rb1.ids.active_island_id
};
out[island_index].push(i);

70
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -1,8 +1,8 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle,
RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity,
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
@@ -376,36 +376,32 @@ impl Multibody {
for i in 0..self.links.len() {
let link = &self.links[i];
let (rb_vels, rb_mprops, rb_forces): (
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyForces,
) = bodies.index_bundle(link.rigid_body.0);
let rb = &bodies[link.rigid_body];
let mut acc = RigidBodyVelocity::zero();
if i != 0 {
let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_id];
let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
let parent_rb = &bodies[parent_link.rigid_body];
acc += self.workspace.accs[parent_id];
// The 2.0 originates from the two identical terms of Jdot (the terms become
// identical once they are multiplied by the generalized velocities).
acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel);
acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel);
#[cfg(feature = "dim3")]
{
acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel);
acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel);
}
acc.linvel += parent_rb_vels
acc.linvel += parent_rb
.vels
.angvel
.gcross(parent_rb_vels.angvel.gcross(link.shift02));
.gcross(parent_rb.vels.angvel.gcross(link.shift02));
acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02);
}
acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23));
acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23));
acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
self.workspace.accs[i] = acc;
@@ -413,12 +409,12 @@ impl Multibody {
// TODO: should gyroscopic forces already be computed by the rigid-body itself
// (at the same time that we add the gravity force)?
let gyroscopic;
let rb_inertia = rb_mprops.effective_angular_inertia();
let rb_mass = rb_mprops.effective_mass();
let rb_inertia = rb.mprops.effective_angular_inertia();
let rb_mass = rb.mprops.effective_mass();
#[cfg(feature = "dim3")]
{
gyroscopic = rb_vels.angvel.cross(&(rb_inertia * rb_vels.angvel));
gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel));
}
#[cfg(feature = "dim2")]
{
@@ -426,8 +422,8 @@ impl Multibody {
}
let external_forces = Force::new(
rb_forces.force - rb_mass.component_mul(&acc.linvel),
rb_forces.torque - gyroscopic - rb_inertia * acc.angvel,
rb.forces.force - rb_mass.component_mul(&acc.linvel),
rb.forces.torque - gyroscopic - rb_inertia * acc.angvel,
);
self.accelerations.gemv_tr(
1.0,
@@ -456,13 +452,12 @@ impl Multibody {
.jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
link.joint_velocity = joint_velocity;
bodies.set_internal(link.rigid_body.0, link.joint_velocity);
bodies.index_mut_internal(link.rigid_body).vels = link.joint_velocity;
for i in 1..self.links.len() {
let (link, parent_link) = self.links.get_mut_with_parent(i);
let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(parent_link.rigid_body.0);
let rb = &bodies[link.rigid_body];
let parent_rb = &bodies[parent_link.rigid_body];
let joint_velocity = link
.joint
@@ -470,12 +465,12 @@ impl Multibody {
link.joint_velocity = joint_velocity.transformed(
&(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation),
);
let mut new_rb_vels = *parent_rb_vels + link.joint_velocity;
let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift);
let mut new_rb_vels = parent_rb.vels + link.joint_velocity;
let shift = rb.mprops.world_com - parent_rb.mprops.world_com;
new_rb_vels.linvel += parent_rb.vels.angvel.gcross(shift);
new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23);
bodies.set_internal(link.rigid_body.0, new_rb_vels);
bodies.index_mut_internal(link.rigid_body).vels = new_rb_vels;
}
/*
@@ -563,10 +558,9 @@ impl Multibody {
for i in 0..self.links.len() {
let link = &self.links[i];
let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(link.rigid_body.0);
let rb_mass = rb_mprops.effective_mass();
let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix();
let rb = &bodies[link.rigid_body];
let rb_mass = rb.mprops.effective_mass();
let rb_inertia = rb.mprops.effective_angular_inertia().into_matrix();
let body_jacobian = &self.body_jacobians[i];
@@ -576,8 +570,8 @@ impl Multibody {
#[cfg(feature = "dim3")]
{
// Derivative of gyroscopic forces.
let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia
- (rb_inertia * rb_vels.angvel).gcross_matrix();
let gyroscopic_matrix = rb.vels.angvel.gcross_matrix() * rb_inertia
- (rb_inertia * rb.vels.angvel).gcross_matrix();
augmented_inertia += gyroscopic_matrix * dt;
}
@@ -604,10 +598,10 @@ impl Multibody {
if i != 0 {
let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_id];
let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
let parent_rb = &bodies[parent_link.rigid_body];
let parent_j = &self.body_jacobians[parent_id];
let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
let parent_w = parent_rb_vels.angvel.gcross_matrix();
let parent_w = parent_rb.vels.angvel.gcross_matrix();
let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id);
let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id);
@@ -620,7 +614,7 @@ impl Multibody {
coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0);
// JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
let dvel_cross = (rb_vels.angvel.gcross(link.shift02)
let dvel_cross = (rb.vels.angvel.gcross(link.shift02)
+ 2.0 * link.joint_velocity.linvel)
.gcross_matrix_tr();
coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
@@ -676,13 +670,13 @@ impl Multibody {
coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0);
// JDot
let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr();
let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr();
coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
// JDot/u * qdot
coriolis_v.gemm(
1.0,
&(rb_vels.angvel.gcross_matrix() * shift_cross_tr),
&(rb.vels.angvel.gcross_matrix() * shift_cross_tr),
&rb_j_w,
1.0,
);

25
src/dynamics/rigid_body_components.rs
View File

@@ -231,6 +231,7 @@ pub struct RigidBodyMassProps {
pub flags: LockedAxes,
/// The local mass properties of the rigid-body.
pub local_mprops: MassProperties,
/// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders.
pub additional_local_mprops: Option<Box<MassProperties>>,
/// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>,
@@ -307,11 +308,11 @@ impl RigidBodyMassProps {
.unwrap_or_else(MassProperties::default);
for handle in &attached_colliders.0 {
if let Some(co) = colliders.get(handle) {
if let Some(co) = colliders.get(*handle) {
if let Some(co_parent) = co.parent {
let to_add = co
.mprops
.mass_properties(&**co.shape)
.mass_properties(&*co.shape)
.transform_by(&co_parent.pos_wrt_parent);
self.local_mprops += to_add;
}
@@ -895,21 +896,17 @@ impl RigidBodyColliders {
) {
for handle in &self.0 {
// NOTE: the ColliderParent component must exist if we enter this method.
let co_parent: &ColliderParent = colliders
.get(handle.0)
.expect("Could not find the ColliderParent component.");
let new_pos = parent_pos * co_parent.pos_wrt_parent;
let co = colliders.index_mut_internal(*handle);
let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent;
if !co.changes.contains(ColliderChanges::MODIFIED) {
modified_colliders.push(*handle);
}
// Set the modification flag so we can benefit from the modification-tracking
// when updating the narrow-phase/broad-phase afterwards.
colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| {
if !co_changes.contains(ColliderChanges::MODIFIED) {
modified_colliders.push(*handle);
}
*co_changes |= ColliderChanges::POSITION;
});
colliders.set_internal(handle.0, ColliderPosition(new_pos));
co.changes |= ColliderChanges::POSITION;
co.pos = ColliderPosition(new_pos);
}
}
}

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

@@ -50,18 +50,18 @@ pub(crate) fn categorize_joints(
) {
for joint_i in joint_indices {
let joint = &impulse_joints[*joint_i].weight;
let status1 = bodies.index(joint.body1.0);
let status2 = bodies.index(joint.body2.0);
let rb1 = &bodies[joint.body1.0];
let rb2 = &bodies[joint.body2.0];
if multibody_joints.rigid_body_link(joint.body1).is_some()
|| multibody_joints.rigid_body_link(joint.body2).is_some()
{
if !status1.is_dynamic() || !status2.is_dynamic() {
if !rb1.is_dynamic() || !rb2.is_dynamic() {
generic_ground_joints.push(*joint_i);
} else {
generic_nonground_joints.push(*joint_i);
}
} else if !status1.is_dynamic() || !status2.is_dynamic() {
} else if !rb1.is_dynamic() || !rb2.is_dynamic() {
ground_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);

92
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -1,8 +1,5 @@
use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -42,18 +39,12 @@ impl GenericVelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): (
&RigidBodyIds,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyType,
) = bodies.index_bundle(handle1.0);
let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): (
&RigidBodyIds,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyType,
) = bodies.index_bundle(handle2.0);
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
let multibody1 = multibodies
.rigid_body_link(handle1)
@@ -63,15 +54,15 @@ impl GenericVelocityConstraint {
.map(|m| (&multibodies[m.multibody], m.id));
let mj_lambda1 = multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type1.is_dynamic() {
rb_ids1.active_set_offset
.unwrap_or(if type1.is_dynamic() {
rb1.ids.active_set_offset
} else {
0
});
let mj_lambda2 = multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type2.is_dynamic() {
rb_ids2.active_set_offset
.unwrap_or(if type2.is_dynamic() {
rb2.ids.active_set_offset
} else {
0
});
@@ -80,11 +71,8 @@ impl GenericVelocityConstraint {
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0)
+ multibody2.map(|m| m.0.ndofs()).unwrap_or(0);
@@ -109,13 +97,13 @@ impl GenericVelocityConstraint {
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: if rb_type1.is_dynamic() {
rb_mprops1.effective_inv_mass
im1: if type1.is_dynamic() {
mprops1.effective_inv_mass
} else {
na::zero()
},
im2: if rb_type2.is_dynamic() {
rb_mprops2.effective_inv_mass
im2: if type2.is_dynamic() {
mprops2.effective_inv_mass
} else {
na::zero()
},
@@ -129,11 +117,11 @@ impl GenericVelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - rb_mprops1.world_com;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -143,15 +131,15 @@ impl GenericVelocityConstraint {
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
let gcross1 = if rb_type1.is_dynamic() {
rb_mprops1
let gcross1 = if type1.is_dynamic() {
mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1)
} else {
na::zero()
};
let gcross2 = if rb_type2.is_dynamic() {
rb_mprops2
let gcross2 = if type2.is_dynamic() {
mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2)
} else {
@@ -170,8 +158,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type1.is_dynamic() {
force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
} else if type1.is_dynamic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
} else {
0.0
@@ -189,8 +177,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type2.is_dynamic() {
force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
} else if type2.is_dynamic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
} else {
0.0
@@ -224,8 +212,8 @@ impl GenericVelocityConstraint {
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
let gcross1 = if rb_type1.is_dynamic() {
rb_mprops1
let gcross1 = if type1.is_dynamic() {
mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1)
} else {
@@ -234,8 +222,8 @@ impl GenericVelocityConstraint {
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
let torque_dir2 = dp2.gcross(-tangents1[j]);
let gcross2 = if rb_type2.is_dynamic() {
rb_mprops2
let gcross2 = if type2.is_dynamic() {
mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2)
} else {
@@ -255,9 +243,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type1.is_dynamic() {
force_dir1
.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
} else if type1.is_dynamic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
} else {
0.0
@@ -275,9 +262,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type2.is_dynamic() {
force_dir1
.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
} else if type2.is_dynamic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
} else {
0.0
@@ -303,8 +289,8 @@ impl GenericVelocityConstraint {
// reduce all ops to nothing because its ndofs will be zero.
let generic_constraint_mask = (multibody1.is_some() as u8)
| ((multibody2.is_some() as u8) << 1)
| (!rb_type1.is_dynamic() as u8)
| ((!rb_type2.is_dynamic() as u8) << 1);
| (!type1.is_dynamic() as u8)
| ((!type2.is_dynamic() as u8) << 1);
let constraint = GenericVelocityConstraint {
velocity_constraint: constraint,

30
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -1,7 +1,5 @@
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross;
@@ -48,16 +46,15 @@ impl GenericVelocityGroundConstraint {
(-manifold.data.normal, 1.0)
};
let (rb_vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let rb2 = &bodies[handle2.unwrap()];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h))
@@ -68,11 +65,8 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
@@ -96,7 +90,7 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb_mprops2.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -107,10 +101,10 @@ impl GenericVelocityGroundConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;

55
src/dynamics/solver/interaction_groups.rs
View File

@@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
crate::data::BundleSet,
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap,
};
@@ -90,14 +89,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut())
{
let mut body_pair = interactions[*interaction_id].body_pair();
let is_fixed1 = body_pair
.0
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let is_fixed2 = body_pair
.1
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let representative = |handle: RigidBodyHandle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() {
@@ -119,28 +112,28 @@ impl ParallelInteractionGroups {
match (is_fixed1, is_fixed2) {
(false, false) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let rb1 = &bodies[body_pair.0.unwrap()];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask =
bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset];
bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset];
*color = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(true, false) => {
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let color_mask = bcolors[rb_ids2.active_set_offset];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb2.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(false, true) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let color_mask = bcolors[rb_ids1.active_set_offset];
let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
}
(true, true) => unreachable!(),
}
@@ -258,13 +251,11 @@ impl InteractionGroups {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight;
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body1.0);
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let rb1 = &bodies[interaction.body1];
let rb2 = &bodies[interaction.body2];
let is_fixed1 = !status1.is_dynamic();
let is_fixed2 = !status2.is_dynamic();
let is_fixed1 = !rb1.is_dynamic();
let is_fixed2 = !rb2.is_dynamic();
if is_fixed1 && is_fixed2 {
continue;
@@ -277,8 +268,8 @@ impl InteractionGroups {
}
let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = ids1.active_set_offset;
let i2 = ids2.active_set_offset;
let i1 = rb1.ids.active_set_offset;
let i2 = rb2.ids.active_set_offset;
let conflicts =
self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -421,15 +412,15 @@ impl InteractionGroups {
let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
(*data.0, data.1.active_set_offset)
let rb1 = &bodies[rb1];
(rb1.body_type, rb1.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
(*data.0, data.1.active_set_offset)
let rb2 = &bodies[rb2];
(rb2.body_type, rb2.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
};

129
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -6,8 +6,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity,
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
};
use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
@@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint {
) {
let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2;
let rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(joint.body1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(joint.body2.0);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
mj_lambda: [rb_ids1.active_set_offset],
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [rb1.ids.active_set_offset],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
let mb1 = multibodies
@@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| &bodies[impulse_joints[ii].body1].pos],
gather![|ii| &bodies[impulse_joints[ii].body1].vels],
gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
gather![|ii| &bodies[impulse_joints[ii].body1].ids],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| &bodies[impulse_joints[ii].body2].pos],
gather![|ii| &bodies[impulse_joints[ii].body2].vels],
gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
gather![|ii| &bodies[impulse_joints[ii].body2].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
@@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint {
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
let flipped = !bodies[handle2].is_dynamic();
let (local_frame1, local_frame2) = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
@@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint {
(joint.data.local_frame1, joint.data.local_frame2)
};
let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(handle2.0);
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let (rb_pos1, rb_vel1, rb_mprops1) = rb1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [crate::INVALID_USIZE],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
if let Some(mb2) = multibodies
@@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
let mut handles2 = gather![|ii| impulse_joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint {
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| &bodies[handles1[ii]].pos],
gather![|ii| &bodies[handles1[ii]].vels],
gather![|ii| &bodies[handles1[ii]].mprops],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| &bodies[handles2[ii]].pos],
gather![|ii| &bodies[handles2[ii]].vels],
gather![|ii| &bodies[handles2[ii]].mprops],
gather![|ii| &bodies[handles2[ii]].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;

13
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -7,8 +7,7 @@ use crate::dynamics::solver::{
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use na::DVector;
@@ -316,15 +315,13 @@ impl ParallelIslandSolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
let rb = &bodies[*handle];
let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
}
}
}

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

@@ -8,8 +8,7 @@ use crate::dynamics::solver::{
};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
MultibodyJointSet, RigidBodyHandle, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::{Real, SPATIAL_DIM};

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

@@ -2,8 +2,7 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadC
use crate::concurrent_loop;
use crate::dynamics::{
solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps,
RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
MultibodyJointSet, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
@@ -210,33 +209,22 @@ impl ParallelVelocitySolver {
multibody.velocities = prev_vels;
}
} else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
// Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
let mut new_vels = rb.vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_pos.next_position = new_vels.integrate(
new_vels = new_vels.apply_damping(params.dt, &rb.damping);
rb.pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
&rb.pos.position,
&rb.mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
}
}
}
@@ -321,23 +309,14 @@ impl ParallelVelocitySolver {
multibody.velocities += mj_lambdas;
}
} else {
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
rb.vels.linvel += dvel.linear;
rb.vels.angvel += dangvel;
rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
}
}
}

17
src/dynamics/solver/velocity_constraint.rs
View File

@@ -3,9 +3,7 @@ use crate::dynamics::solver::{
};
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
@@ -160,13 +158,14 @@ impl VelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let (ids1, vels1, mprops1): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.0);
let mj_lambda1 = ids1.active_set_offset;
let mj_lambda2 = ids2.active_set_offset;
let rb1 = &bodies[handle1];
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let mj_lambda1 = rb1.ids.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]

16
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -1,7 +1,9 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -49,12 +51,12 @@ impl WVelocityConstraint {
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);

16
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -7,9 +7,7 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -51,15 +49,15 @@ impl VelocityGroundConstraint {
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
@@ -67,7 +65,7 @@ impl VelocityGroundConstraint {
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = ids2.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
for (_l, manifold_points) in manifold
.data

14
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -2,7 +2,9 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -54,20 +56,20 @@ impl WVelocityGroundConstraint {
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| *bodies.index(h.0))
.map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::zero)
}];
let world_com1 = Point::from(gather![|ii| {
handles1[ii]
.map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).world_com)
.map(|h| bodies[h].mprops.world_com)
.unwrap_or_else(Point::origin)
}]);
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
gather![|ii| &bodies[handles2[ii].unwrap()].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
let flipped_sign = SimdReal::from(flipped);

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

@@ -1,9 +1,7 @@
use super::AnyJointVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet,
RigidBodyVelocity,
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
@@ -59,15 +57,15 @@ impl VelocitySolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
let rb = &bodies[*handle];
let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
dvel.angular +=
rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear +=
rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
}
}
@@ -151,33 +149,26 @@ impl VelocitySolver {
multibody.velocities = prev_vels;
}
} else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb
.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
// Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
let mut new_pos = rb.pos;
let mut new_vels = rb.vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_vels = new_vels.apply_damping(params.dt, &rb.damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
&rb.pos.position,
&rb.mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
rb.pos = new_pos;
}
}
@@ -234,23 +225,16 @@ impl VelocitySolver {
multibody.velocities += mj_lambdas;
}
} else {
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb
.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
rb.vels.linvel += dvel.linear;
rb.vels.angvel += dangvel;
rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
}
}