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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,12 @@
use super::TOIEntry;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase};
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase,
};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
@@ -44,19 +50,34 @@ impl CCDSolver {
/// Apply motion-clamping to the bodies affected by the given `impacts`.
///
/// The `impacts` should be the result of a previous call to `self.predict_next_impacts`.
pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
pub fn clamp_motions<Bodies>(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts)
where
Bodies: ComponentSet<RigidBodyCcd>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
match impacts {
PredictedImpacts::Impacts(tois) => {
// println!("Num to clamp: {}", tois.len());
for (handle, toi) in tois {
if let Some(body) = bodies.get_mut_internal(*handle) {
let min_toi = (body.ccd_thickness
* 0.15
* crate::utils::inv(body.max_point_velocity()))
.min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi);
body.integrate_next_position(toi.max(min_toi));
}
let (rb_poss, vels, ccd, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyCcd,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let local_com = &mprops.mass_properties.local_com;
let min_toi = (ccd.ccd_thickness
* 0.15
* crate::utils::inv(ccd.max_point_velocity(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;
});
}
}
_ => {}
@@ -66,34 +87,64 @@ impl CCDSolver {
/// Updates the set of bodies that needs CCD to be resolved.
///
/// Returns `true` if any rigid-body must have CCD resolved.
pub fn update_ccd_active_flags(
pub fn update_ccd_active_flags<Bodies>(
&self,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
dt: Real,
include_forces: bool,
) -> bool {
) -> bool
where
Bodies: ComponentSetMut<RigidBodyCcd>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyForces>,
{
let mut ccd_active = false;
// println!("Checking CCD activation");
bodies.foreach_active_dynamic_body_mut_internal(|_, body| {
body.update_ccd_active_flag(dt, include_forces);
// println!("CCD is active: {}, for {:?}", ccd_active, handle);
ccd_active = ccd_active || body.is_ccd_active();
});
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;
});
ccd_active = ccd_active || moving_fast;
}
}
ccd_active
}
/// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
pub fn find_first_impact(
pub fn find_first_impact<Bodies, Colliders>(
&mut self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
) -> Option<Real> {
) -> Option<Real>
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>,
{
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -102,19 +153,37 @@ impl CCDSolver {
let mut pairs_seen = HashMap::default();
let mut min_toi = dt;
for (_, rb1) in bodies.iter_active_dynamic() {
if rb1.is_ccd_active() {
let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt);
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
for ch1 in &rb1.colliders {
let co1 = &colliders[*ch1];
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 co1.is_sensor() {
let predicted_body_pos1 =
rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
.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() {
continue; // Ignore sensors.
}
let aabb1 =
co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent()));
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -130,12 +199,17 @@ impl CCDSolver {
)
.is_none()
{
let c1 = colliders.get(*ch1).unwrap();
let c2 = colliders.get(*ch2).unwrap();
let bh1 = c1.parent();
let bh2 = c2.parent();
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
let co_type1: &ColliderType = colliders.index(ch1.0);
let co_type2: &ColliderType = colliders.index(ch1.0);
if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) {
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) {
// Ignore self-intersection and sensors.
return true;
}
@@ -146,16 +220,15 @@ impl CCDSolver {
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
c1,
c2,
b1,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2,
None,
None,
@@ -181,14 +254,27 @@ impl CCDSolver {
}
/// Outputs the set of bodies as well as their first time-of-impact event.
pub fn predict_impacts_at_next_positions(
pub fn predict_impacts_at_next_positions<Bodies, Colliders>(
&mut self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
events: &dyn EventHandler,
) -> PredictedImpacts {
) -> PredictedImpacts
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>,
{
let mut frozen = HashMap::<_, Real>::default();
let mut all_toi = BinaryHeap::new();
let mut pairs_seen = HashMap::default();
@@ -196,6 +282,7 @@ impl CCDSolver {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithNextPosition,
@@ -207,71 +294,94 @@ impl CCDSolver {
*
*/
// TODO: don't iterate through all the colliders.
for (ch1, co1) in colliders.iter() {
let rb1 = &bodies[co1.parent()];
if rb1.is_ccd_active() {
let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent()));
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
if ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
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 pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let c1 = colliders.get(ch1).unwrap();
let c2 = colliders.get(*ch2).unwrap();
let bh1 = c1.parent();
let bh2 = c2.parent();
let predicted_body_pos1 =
rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
if bh1 == bh2 {
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
.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);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
if *ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
let smallest_dist = narrow_phase
.contact_pair(ch1, *ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
ch1,
*ch2,
c1,
c2,
b1,
b2,
None,
None,
0.0,
// NOTE: we use dt here only once we know that
// there is at least one TOI before dt.
min_overstep,
smallest_dist,
) {
if toi.toi > dt {
min_overstep = min_overstep.min(toi.toi);
} else {
min_overstep = dt;
all_toi.push(toi);
if bh1 == bh2 {
// Ignore self-intersection.
return true;
}
let smallest_dist = narrow_phase
.contact_pair(*ch1, *ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2,
None,
None,
0.0,
// NOTE: we use dt here only once we know that
// there is at least one TOI before dt.
min_overstep,
smallest_dist,
) {
if toi.toi > dt {
min_overstep = min_overstep.min(toi.toi);
} else {
min_overstep = dt;
all_toi.push(toi);
}
}
}
}
true
});
true
});
}
}
}
@@ -293,19 +403,25 @@ impl CCDSolver {
while let Some(toi) = all_toi.pop() {
assert!(toi.toi <= dt);
let body1 = bodies.get(toi.b1).unwrap();
let body2 = bodies.get(toi.b2).unwrap();
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 mut colliders_to_check = Vec::new();
let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1);
let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2);
let should_freeze1 = rb1.is_some()
&& rb1.unwrap().0.ccd_active
&& !frozen.contains_key(&toi.b1.unwrap());
let should_freeze2 = rb2.is_some()
&& rb2.unwrap().0.ccd_active
&& !frozen.contains_key(&toi.b2.unwrap());
if !should_freeze1 && !should_freeze2 {
continue;
}
if toi.is_intersection_test {
// NOTE: this test is rendundant with the previous `if !should_freeze && ...`
// NOTE: this test is redundant with the previous `if !should_freeze && ...`
// but let's keep it to avoid tricky regressions if we end up swapping both
// `if` for some reasons in the future.
if should_freeze1 || should_freeze2 {
@@ -318,42 +434,51 @@ impl CCDSolver {
}
if should_freeze1 {
let _ = frozen.insert(toi.b1, toi.toi);
colliders_to_check.extend_from_slice(&body1.colliders);
let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0);
}
if should_freeze2 {
let _ = frozen.insert(toi.b2, toi.toi);
colliders_to_check.extend_from_slice(&body2.colliders);
let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0);
}
let start_time = toi.toi;
for ch1 in &colliders_to_check {
let co1 = &colliders[*ch1];
let rb1 = &bodies[co1.parent];
let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent()));
let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap();
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
colliders.index_bundle(ch1.0);
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);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
let c1 = colliders.get(*ch1).unwrap();
let c2 = colliders.get(*ch2).unwrap();
let bh1 = c1.parent();
let bh2 = c2.parent();
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 {
// Ignore self-intersection.
return true;
}
let frozen1 = frozen.get(&bh1);
let frozen2 = frozen.get(&bh2);
let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h));
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
let b1: Option<(_, _, _, &RigidBodyCcd)> =
bh1.map(|h| bodies.index_bundle(h.0));
let b2: Option<(_, _, _, &RigidBodyCcd)> =
bh1.map(|h| bodies.index_bundle(h.0));
if (frozen1.is_some() || !b1.is_ccd_active())
&& (frozen2.is_some() || !b2.is_ccd_active())
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))
{
// We already did a resweep.
return true;
@@ -369,8 +494,8 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
c1,
c2,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
b1,
b2,
frozen1.copied(),
@@ -395,30 +520,57 @@ 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 body1 = &bodies[toi.b1];
let body2 = &bodies[toi.b2];
let co1 = &colliders[toi.c1];
let co2 = &colliders[toi.c2];
let frozen1 = frozen.get(&toi.b1);
let frozen2 = frozen.get(&toi.b2);
let pos1 = frozen1
.map(|t| body1.integrate_velocity(*t))
.unwrap_or(body1.next_position);
let pos2 = frozen2
.map(|t| body2.integrate_velocity(*t))
.unwrap_or(body2.next_position);
let (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(toi.c1.0);
let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(toi.c2.0);
let prev_coll_pos12 = co1.position.inv_mul(&co2.position);
let next_coll_pos12 =
(pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent());
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.mass_properties.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);
pos1 * co_parent1.pos_wrt_parent
} else {
co_pos1.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.mass_properties.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);
pos2 * co_parent2.pos_wrt_parent
} else {
co_pos2.0
};
let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2);
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, co1.shape(), co2.shape())
.intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.unwrap_or(false);
let intersect_after = query_dispatcher
.intersection_test(&next_coll_pos12, co1.shape(), co2.shape())
.intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.unwrap_or(false);
if !intersect_before && !intersect_after {

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

@@ -1,5 +1,9 @@
use crate::dynamics::{RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher};
pub struct TOIEntry {
pub toi: Real,
pub c1: ColliderHandle,
pub b1: RigidBodyHandle,
pub b1: Option<RigidBodyHandle>,
pub c2: ColliderHandle,
pub b2: RigidBodyHandle,
pub b2: Option<RigidBodyHandle>,
pub is_intersection_test: bool,
pub timestamp: usize,
}
@@ -18,9 +22,9 @@ impl TOIEntry {
fn new(
toi: Real,
c1: ColliderHandle,
b1: RigidBodyHandle,
b1: Option<RigidBodyHandle>,
c2: ColliderHandle,
b2: RigidBodyHandle,
b2: Option<RigidBodyHandle>,
is_intersection_test: bool,
timestamp: usize,
) -> Self {
@@ -39,10 +43,30 @@ impl TOIEntry {
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
c1: &Collider,
c2: &Collider,
b1: &RigidBody,
b2: &RigidBody,
c1: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
c2: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
b1: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
b2: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
frozen1: Option<Real>,
frozen2: Option<Real>,
start_time: Real,
@@ -50,35 +74,46 @@ impl TOIEntry {
smallest_contact_dist: Real,
) -> Option<Self> {
assert!(start_time <= end_time);
if b1.is_none() && b2.is_none() {
return None;
}
let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel();
let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel();
let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel();
let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel();
let (co_type1, co_shape1, co_pos1, co_parent1) = c1;
let (co_type2, co_shape2, co_pos2, co_parent2) = c2;
let linvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.abs() * b1.ccd_max_dist
+ angvel2.abs() * b2.ccd_max_dist;
+ 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);
#[cfg(feature = "dim3")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.norm() * b1.ccd_max_dist
+ angvel2.norm() * b2.ccd_max_dist;
+ 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);
// 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 = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness())
let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
+ smallest_contact_dist.max(0.0);
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor();
if (end_time - start_time) * vel12 < thickness {
return None;
}
// Compute the TOI.
let mut motion1 = Self::body_motion(b1);
let mut motion2 = Self::body_motion(b2);
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);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -88,8 +123,8 @@ impl TOIEntry {
motion2.freeze(t);
}
let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
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));
// println!("start_time: {}", start_time);
@@ -105,9 +140,9 @@ impl TOIEntry {
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
c1.shape(),
co_shape1.as_ref(),
&motion_c2,
c2.shape(),
co_shape2.as_ref(),
start_time,
end_time,
stop_at_penetration,
@@ -119,24 +154,31 @@ impl TOIEntry {
Some(Self::new(
toi.toi,
ch1,
c1.parent(),
co_parent1.map(|p| p.handle),
ch2,
c2.parent(),
co_parent2.map(|p| p.handle),
is_intersection_test,
0,
))
}
fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
if body.is_ccd_active() {
fn body_motion(
(poss, vels, mprops, ccd): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
),
) -> NonlinearRigidMotion {
if ccd.ccd_active {
NonlinearRigidMotion::new(
body.position,
body.mass_properties.local_com,
body.linvel,
body.angvel,
poss.position,
mprops.mass_properties.local_com,
vels.linvel,
vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(body.next_position)
NonlinearRigidMotion::constant_position(poss.next_position)
}
}
}

14
src/dynamics/coefficient_combine_rule.rs
View File

@@ -20,17 +20,13 @@ pub enum CoefficientCombineRule {
Max,
}
impl CoefficientCombineRule {
pub(crate) fn from_value(val: u8) -> Self {
match val {
0 => CoefficientCombineRule::Average,
1 => CoefficientCombineRule::Min,
2 => CoefficientCombineRule::Multiply,
3 => CoefficientCombineRule::Max,
_ => panic!("Invalid coefficient combine rule."),
}
impl Default for CoefficientCombineRule {
fn default() -> Self {
CoefficientCombineRule::Average
}
}
impl CoefficientCombineRule {
pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real {
let effective_rule = rule_value1.max(rule_value2);

344
src/dynamics/island_manager.rs Normal file
View File

@@ -0,0 +1,344 @@
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{
Joint, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodyType,
RigidBodyVelocity,
};
use crate::geometry::{ColliderParent, InteractionGraph, NarrowPhase};
use crate::math::Real;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct IslandManager {
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
active_set_timestamp: u32,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>, // Workspace.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<RigidBodyHandle>, // Workspace.
}
impl IslandManager {
pub fn new() -> Self {
Self {
active_dynamic_set: vec![],
active_kinematic_set: vec![],
active_islands: vec![],
active_set_timestamp: 0,
can_sleep: vec![],
stack: vec![],
}
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
pub fn cleanup_removed_rigid_bodies(
&mut self,
bodies: &mut impl ComponentSetMut<RigidBodyIds>,
) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
let mut i = 0;
while i < active_set.len() {
let handle = active_set[i];
if bodies.get(handle.0).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);
}
} else {
i += 1;
}
}
}
}
pub fn rigid_body_removed(
&mut self,
removed_handle: RigidBodyHandle,
removed_ids: &RigidBodyIds,
bodies: &mut impl ComponentSetMut<RigidBodyIds>,
) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
active_set.swap_remove(removed_ids.active_set_id);
if let Some(replacement) = active_set.get(removed_ids.active_set_id) {
bodies.map_mut_internal(replacement.0, |ids| {
ids.active_set_id = removed_ids.active_set_id;
});
}
}
}
}
/// Forces the specified rigid-body to wake up if it is dynamic.
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up<Bodies>(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool)
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
// TODO: what about kinematic bodies?
let status: RigidBodyType = *bodies.index(handle.0);
if status.is_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);
}
});
}
}
/// Iter through all the active kinematic rigid-bodies on this set.
pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] {
&self.active_kinematic_set[..]
}
/// Iter through all the active dynamic rigid-bodies on this set.
pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] {
&self.active_dynamic_set[..]
}
#[cfg(not(feature = "parallel"))]
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
&self.active_dynamic_set[island_range]
}
#[inline(always)]
pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
self.active_dynamic_set
.iter()
.copied()
.chain(self.active_kinematic_set.iter().copied())
}
/*
#[cfg(feature = "parallel")]
#[inline(always)]
#[allow(dead_code)]
pub(crate) fn foreach_active_island_body_mut_internal_parallel<Set>(
&self,
island_id: usize,
bodies: &mut Set,
f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
) where
Set: ComponentSet<T>,
{
use std::sync::atomic::Ordering;
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
let bodies = std::sync::atomic::AtomicPtr::new(&mut bodies as *mut _);
self.active_dynamic_set[island_range]
.par_iter()
.for_each_init(
|| bodies.load(Ordering::Relaxed),
|bodies, handle| {
let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) };
if let Some(rb) = bodies.get_mut_internal(handle.0) {
f(*handle, rb)
}
},
);
}
*/
#[cfg(feature = "parallel")]
pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
self.active_islands[island_id]..self.active_islands[island_id + 1]
}
pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
&mut self,
bodies: &mut Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
min_island_size: usize,
) where
Bodies: ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetOption<ColliderParent>,
{
assert!(
min_island_size > 0,
"The minimum island size must be at least 1."
);
// Update the energy of every rigid body and
// keep only those that may not sleep.
// let t = instant::now();
self.active_set_timestamp += 1;
self.stack.clear();
self.can_sleep.clear();
// NOTE: the `.rev()` is here so that two successive timesteps preserve
// the order of the bodies in the `active_dynamic_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes
// debugging slightly nicer so we keep this rev.
for h in self.active_dynamic_set.drain(..).rev() {
let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack;
let vels: &RigidBodyVelocity = bodies.index(h.0);
let pseudo_kinetic_energy = vels.pseudo_kinetic_energy();
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
update_energy(activation, pseudo_kinetic_energy);
if activation.energy <= activation.threshold {
// 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);
}
});
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)]
fn push_contacting_bodies(
rb_colliders: &RigidBodyColliders,
colliders: &impl ComponentSetOption<ColliderParent>,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {
for collider_handle in &rb_colliders.0 {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if !manifold.data.solver_contacts.is_empty() {
let other = crate::utils::select_other(
(inter.0, inter.1),
*collider_handle,
);
if let Some(other_body) = colliders.get(other.0) {
stack.push(other_body.handle);
}
break;
}
}
}
}
}
}
// 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);
if 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);
}
// println!("Selection: {}", instant::now() - t);
// let t = instant::now();
// Propagation of awake state and awake island computation through the
// traversal of the interaction graph.
self.active_islands.clear();
self.active_islands.push(0);
// The max avoid underflow when the stack is empty.
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);
if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() {
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through static bodies.
continue;
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
// We are starting a new island.
self.active_islands.push(self.active_dynamic_set.len());
}
island_marker = self.stack.len();
}
// 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);
for inter in joint_graph.interactions_with(rb_ids.joint_graph_index) {
let other = crate::utils::select_other((inter.0, inter.1), handle);
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;
});
self.active_dynamic_set.push(handle);
}
self.active_islands.push(self.active_dynamic_set.len());
// println!(
// "Extraction: {}, num islands: {}",
// instant::now() - t,
// self.active_islands.len() - 1
// );
// 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()
});
}
}
}
}
fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) {
let mix_factor = 0.01;
let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy;
activation.energy = new_energy.min(activation.threshold.abs() * 4.0);
}

116
src/dynamics/joint/joint_set.rs
View File

@@ -2,31 +2,33 @@ use super::Joint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
use crate::data::arena::Arena;
use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
use crate::dynamics::{JointParams, RigidBodyHandle};
/// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider set.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct JointHandle(pub(crate) crate::data::arena::Index);
pub struct JointHandle(pub crate::data::arena::Index);
impl JointHandle {
/// Converts this handle into its (index, generation) components.
pub fn into_raw_parts(self) -> (usize, u64) {
pub fn into_raw_parts(self) -> (u32, u32) {
self.0.into_raw_parts()
}
/// Reconstructs an handle from its (index, generation) components.
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
pub fn from_raw_parts(id: u32, generation: u32) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid joint handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
crate::INVALID_U32,
crate::INVALID_U32,
))
}
}
@@ -157,7 +159,7 @@ impl JointSet {
/// Inserts a new joint into this set and retrieve its handle.
pub fn insert<J>(
&mut self,
bodies: &mut RigidBodySet,
bodies: &mut impl ComponentSetMut<RigidBodyIds>,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
joint_params: J,
@@ -177,57 +179,64 @@ impl JointSet {
params: joint_params.into(),
};
let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
let (rb1, rb2) = (
rb1.expect("Attempt to attach a joint to a non-existing body."),
rb2.expect("Attempt to attach a joint to a non-existing body."),
);
let mut graph_index1 = bodies.index(joint.body1.0).joint_graph_index;
let mut graph_index2 = bodies.index(joint.body2.0).joint_graph_index;
// NOTE: the body won't have a graph index if it does not
// have any joint attached.
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
{
rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index1) {
graph_index1 = self.joint_graph.graph.add_node(joint.body1);
bodies.map_mut_internal(joint.body1.0, |ids| ids.joint_graph_index = graph_index1);
}
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
{
rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index2) {
graph_index2 = self.joint_graph.graph.add_node(joint.body2);
bodies.map_mut_internal(joint.body2.0, |ids| ids.joint_graph_index = graph_index2);
}
let id = self
.joint_graph
.add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
self.joint_ids[handle] = id;
self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
JointHandle(handle)
}
/// Retrieve all the joints happening between two active bodies.
// NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
pub(crate) fn select_active_interactions(
pub(crate) fn select_active_interactions<Bodies>(
&self,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
out: &mut Vec<Vec<JointIndex>>,
) {
for out_island in &mut out[..bodies.num_islands()] {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyIds>,
{
for out_island in &mut out[..islands.num_islands()] {
out_island.clear();
}
// 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];
if (rb1.is_dynamic() || rb2.is_dynamic())
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
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)
{
let island_index = if !rb1.is_dynamic() {
rb2.active_island_id
let island_index = if !status1.is_dynamic() {
ids2.active_island_id
} else {
rb1.active_island_id
ids1.active_island_id
};
out[island_index].push(i);
@@ -239,22 +248,28 @@ impl JointSet {
///
/// If `wake_up` is set to `true`, then the bodies attached to this joint will be
/// automatically woken up.
pub fn remove(
pub fn remove<Bodies>(
&mut self,
handle: JointHandle,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
bodies: &mut Bodies,
wake_up: bool,
) -> Option<Joint> {
) -> Option<Joint>
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
let id = self.joint_ids.remove(handle.0)?;
let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
if wake_up {
// Wake-up the bodies attached to this joint.
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
bodies.wake_up(*rb_handle, true);
islands.wake_up(bodies, *rb_handle, true);
}
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
bodies.wake_up(*rb_handle, true);
islands.wake_up(bodies, *rb_handle, true);
}
}
@@ -267,11 +282,16 @@ impl JointSet {
removed_joint
}
pub(crate) fn remove_rigid_body(
pub(crate) fn remove_rigid_body<Bodies>(
&mut self,
deleted_id: RigidBodyGraphIndex,
bodies: &mut RigidBodySet,
) {
islands: &mut IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
// We have to delete each joint one by one in order to:
// - Wake-up the attached bodies.
@@ -292,16 +312,16 @@ impl JointSet {
}
// Wake up the attached bodies.
bodies.wake_up(h1, true);
bodies.wake_up(h2, true);
islands.wake_up(bodies, h1, true);
islands.wake_up(bodies, h2, true);
}
if let Some(other) = self.joint_graph.remove_node(deleted_id) {
// One rigid-body joint graph index may have been invalidated
// so we need to update it.
if let Some(replacement) = bodies.get_mut_internal(other) {
replacement.joint_graph_index = deleted_id;
}
bodies.map_mut_internal(other.0, |ids: &mut RigidBodyIds| {
ids.joint_graph_index = deleted_id;
});
}
}
}

19
src/dynamics/mod.rs
View File

@@ -3,6 +3,7 @@
pub use self::ccd::CCDSolver;
pub use self::coefficient_combine_rule::CoefficientCombineRule;
pub use self::integration_parameters::IntegrationParameters;
pub use self::island_manager::IslandManager;
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::joint::JointIndex;
#[cfg(feature = "dim3")]
@@ -17,19 +18,27 @@ pub use self::joint::{
PrismaticJoint,
SpringModel, // GenericJoint
};
pub(crate) use self::rigid_body::RigidBodyChanges;
pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
pub use self::rigid_body_components::*;
#[cfg(not(feature = "parallel"))]
pub(crate) use self::solver::IslandSolver;
#[cfg(feature = "parallel")]
pub(crate) use self::solver::ParallelIslandSolver;
pub use parry::mass_properties::MassProperties;
#[cfg(feature = "default-sets")]
pub use self::rigid_body::{RigidBody, RigidBodyBuilder};
#[cfg(feature = "default-sets")]
pub use self::rigid_body_set::{BodyPair, RigidBodySet};
mod ccd;
mod coefficient_combine_rule;
mod integration_parameters;
mod island_manager;
mod joint;
mod rigid_body;
mod rigid_body_set;
mod rigid_body_components;
mod solver;
#[cfg(feature = "default-sets")]
mod rigid_body;
#[cfg(feature = "default-sets")]
mod rigid_body_set;

728
src/dynamics/rigid_body.rs
View File

File diff suppressed because it is too large Load Diff

659
src/dynamics/rigid_body_components.rs Normal file
View File

@@ -0,0 +1,659 @@
use crate::data::{ComponentSetMut, ComponentSetOption};
use crate::dynamics::MassProperties;
use crate::geometry::{
ColliderChanges, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition,
ColliderShape, InteractionGraph, RigidBodyGraphIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Translation, Vector};
use crate::parry::partitioning::IndexedData;
use crate::utils::WDot;
use num::Zero;
/// The unique handle of a rigid body added to a `RigidBodySet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct RigidBodyHandle(pub crate::data::arena::Index);
impl RigidBodyHandle {
/// Converts this handle into its (index, generation) components.
pub fn into_raw_parts(self) -> (u32, u32) {
self.0.into_raw_parts()
}
/// Reconstructs an handle from its (index, generation) components.
pub fn from_raw_parts(id: u32, generation: u32) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid rigid-body handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_U32,
crate::INVALID_U32,
))
}
}
impl IndexedData for RigidBodyHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
/// The type of a body, governing the way it is affected by external forces.
#[deprecated(note = "renamed as RigidBodyType")]
pub type BodyStatus = RigidBodyType;
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// The status of a body, governing the way it is affected by external forces.
pub enum RigidBodyType {
/// A `RigidBodyType::Dynamic` body can be affected by all external forces.
Dynamic,
/// A `RigidBodyType::Static` body cannot be affected by external forces.
Static,
/// A `RigidBodyType::Kinematic` body cannot be affected by any external forces but can be controlled
/// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
///
/// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
/// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
/// modified by the user and is independent from any contact or joint it is involved in.
Kinematic,
// Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it?
// Disabled,
}
impl RigidBodyType {
pub fn is_static(self) -> bool {
self == RigidBodyType::Static
}
pub fn is_dynamic(self) -> bool {
self == RigidBodyType::Dynamic
}
pub fn is_kinematic(self) -> bool {
self == RigidBodyType::Kinematic
}
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags describing how the rigid-body has been modified by the user.
pub struct RigidBodyChanges: u32 {
const MODIFIED = 1 << 0;
const POSITION = 1 << 1;
const SLEEP = 1 << 2;
const COLLIDERS = 1 << 3;
const TYPE = 1 << 4;
}
}
impl Default for RigidBodyChanges {
fn default() -> Self {
RigidBodyChanges::empty()
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyPosition {
/// The world-space position of the rigid-body.
pub position: Isometry<Real>,
/// The next position of the rigid-body.
///
/// At the beginning of the timestep, and when the
/// timestep is complete we must have position == next_position
/// except for kinematic bodies.
///
/// The next_position is updated after the velocity and position
/// resolution. Then it is either validated (ie. we set position := set_position)
/// or clamped by CCD.
pub next_position: Isometry<Real>,
}
impl Default for RigidBodyPosition {
fn default() -> Self {
Self {
position: Isometry::identity(),
next_position: Isometry::identity(),
}
}
}
impl RigidBodyPosition {
#[must_use]
pub fn interpolate_velocity(&self, inv_dt: Real) -> RigidBodyVelocity {
let dpos = self.next_position * self.position.inverse();
let angvel;
#[cfg(feature = "dim2")]
{
angvel = dpos.rotation.angle() * inv_dt;
}
#[cfg(feature = "dim3")]
{
angvel = dpos.rotation.scaled_axis() * inv_dt;
}
let linvel = dpos.translation.vector * inv_dt;
RigidBodyVelocity { linvel, angvel }
}
#[must_use]
pub fn integrate_force_and_velocity(
&self,
dt: Real,
forces: &RigidBodyForces,
vels: &RigidBodyVelocity,
mprops: &RigidBodyMassProps,
) -> Isometry<Real> {
let new_vels = forces.integrate(dt, vels, mprops);
new_vels.integrate(dt, &self.position, &mprops.mass_properties.local_com)
}
}
impl<T> From<T> for RigidBodyPosition
where
Isometry<Real>: From<T>,
{
fn from(position: T) -> Self {
let position = position.into();
Self {
position,
next_position: position,
}
}
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub struct RigidBodyMassPropsFlags: u8 {
const TRANSLATION_LOCKED = 1 << 0;
const ROTATION_LOCKED_X = 1 << 1;
const ROTATION_LOCKED_Y = 1 << 2;
const ROTATION_LOCKED_Z = 1 << 3;
const ROTATION_LOCKED = Self::ROTATION_LOCKED_X.bits | Self::ROTATION_LOCKED_Y.bits | Self::ROTATION_LOCKED_Z.bits;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyMassProps {
/// Flags for locking rotation and translation.
pub flags: RigidBodyMassPropsFlags,
/// The local mass properties of the rigid-body.
pub mass_properties: MassProperties,
/// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>,
/// The inverse mass taking into account translation locking.
pub effective_inv_mass: Real,
/// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
/// taking into account rotation locking.
pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
}
impl Default for RigidBodyMassProps {
fn default() -> Self {
Self {
flags: RigidBodyMassPropsFlags::empty(),
mass_properties: MassProperties::zero(),
world_com: Point::origin(),
effective_inv_mass: 0.0,
effective_world_inv_inertia_sqrt: AngularInertia::zero(),
}
}
}
impl From<RigidBodyMassPropsFlags> for RigidBodyMassProps {
fn from(flags: RigidBodyMassPropsFlags) -> Self {
Self {
flags,
..Self::default()
}
}
}
impl RigidBodyMassProps {
#[must_use]
pub fn with_translations_locked(mut self) -> Self {
self.flags |= RigidBodyMassPropsFlags::TRANSLATION_LOCKED;
self
}
pub fn effective_mass(&self) -> Real {
crate::utils::inv(self.effective_inv_mass)
}
pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
self.world_com = self.mass_properties.world_com(&position);
self.effective_inv_mass = self.mass_properties.inv_mass;
self.effective_world_inv_inertia_sqrt = self
.mass_properties
.world_inv_inertia_sqrt(&position.rotation);
// Take into account translation/rotation locking.
if self
.flags
.contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED)
{
self.effective_inv_mass = 0.0;
}
#[cfg(feature = "dim2")]
{
if self
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
{
self.effective_world_inv_inertia_sqrt = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if self
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X)
{
self.effective_world_inv_inertia_sqrt.m11 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
}
if self
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y)
{
self.effective_world_inv_inertia_sqrt.m22 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
if self
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
{
self.effective_world_inv_inertia_sqrt.m33 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyVelocity {
/// The linear velocity of the rigid-body.
pub linvel: Vector<Real>,
/// The angular velocity of the rigid-body.
pub angvel: AngVector<Real>,
}
impl Default for RigidBodyVelocity {
fn default() -> Self {
Self::zero()
}
}
impl RigidBodyVelocity {
#[must_use]
pub fn zero() -> Self {
Self {
linvel: na::zero(),
angvel: na::zero(),
}
}
#[must_use]
pub fn pseudo_kinetic_energy(&self) -> Real {
self.linvel.norm_squared() + self.angvel.gdot(self.angvel)
}
#[must_use]
pub fn apply_damping(&self, dt: Real, damping: &RigidBodyDamping) -> Self {
RigidBodyVelocity {
linvel: self.linvel * (1.0 / (1.0 + dt * damping.linear_damping)),
angvel: self.angvel * (1.0 / (1.0 + dt * damping.angular_damping)),
}
}
#[must_use]
pub fn integrate(
&self,
dt: Real,
init_pos: &Isometry<Real>,
local_com: &Point<Real>,
) -> Isometry<Real> {
let com = init_pos * local_com;
let shift = Translation::from(com.coords);
let mut result =
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos;
result.rotation.renormalize_fast();
result
}
#[must_use]
pub fn is_zero(&self) -> bool {
self.linvel.is_zero() && self.angvel.is_zero()
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyDamping {
/// Damping factor for gradually slowing down the translational motion of the rigid-body.
pub linear_damping: Real,
/// Damping factor for gradually slowing down the angular motion of the rigid-body.
pub angular_damping: Real,
}
impl Default for RigidBodyDamping {
fn default() -> Self {
Self {
linear_damping: 0.0,
angular_damping: 0.0,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyForces {
/// Accumulation of external forces (only for dynamic bodies).
pub force: Vector<Real>,
/// Accumulation of external torques (only for dynamic bodies).
pub torque: AngVector<Real>,
pub gravity_scale: Real,
}
impl Default for RigidBodyForces {
fn default() -> Self {
Self {
force: na::zero(),
torque: na::zero(),
gravity_scale: 1.0,
}
}
}
impl RigidBodyForces {
#[must_use]
pub fn integrate(
&self,
dt: Real,
init_vels: &RigidBodyVelocity,
mprops: &RigidBodyMassProps,
) -> RigidBodyVelocity {
let linear_acc = self.force * mprops.effective_inv_mass;
let angular_acc = mprops.effective_world_inv_inertia_sqrt
* (mprops.effective_world_inv_inertia_sqrt * self.torque);
RigidBodyVelocity {
linvel: init_vels.linvel + linear_acc * dt,
angvel: init_vels.angvel + angular_acc * dt,
}
}
pub fn add_linear_acceleration(&mut self, gravity: &Vector<Real>, mass: Real) {
self.force += gravity * self.gravity_scale * mass;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyCcd {
pub ccd_thickness: Real,
pub ccd_max_dist: Real,
pub ccd_active: bool,
pub ccd_enabled: bool,
}
impl Default for RigidBodyCcd {
fn default() -> Self {
Self {
ccd_thickness: 0.0,
ccd_max_dist: 0.0,
ccd_active: false,
ccd_enabled: false,
}
}
}
impl RigidBodyCcd {
pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real {
#[cfg(feature = "dim2")]
return vels.linvel.norm() + vels.angvel.abs() * self.ccd_max_dist;
#[cfg(feature = "dim3")]
return vels.linvel.norm() + vels.angvel.norm() * self.ccd_max_dist;
}
pub fn is_moving_fast(
&self,
dt: Real,
vels: &RigidBodyVelocity,
forces: Option<&RigidBodyForces>,
) -> bool {
// NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
// should use `self.rb_ccd.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
// is the deepest contact (the contact with the largest penetration depth, i.e., the
// negative `dist` with the largest absolute value.
// However, getting this penetration depth assumes querying the contact graph from
// the narrow-phase, which can be pretty expensive. So we use the CCD thickness
// divided by 10 right now. We will see in practice if this value is OK or if we
// should use a smaller (to be less conservative) or larger divisor (to be more conservative).
let threshold = self.ccd_thickness / 10.0;
if let Some(forces) = forces {
let linear_part = (vels.linvel + forces.force * dt).norm();
#[cfg(feature = "dim2")]
let angular_part = (vels.angvel + forces.torque * dt).abs() * self.ccd_max_dist;
#[cfg(feature = "dim3")]
let angular_part = (vels.angvel + forces.torque * dt).norm() * self.ccd_max_dist;
let vel_with_forces = linear_part + angular_part;
vel_with_forces > threshold
} else {
self.max_point_velocity(vels) * dt > threshold
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyIds {
pub joint_graph_index: RigidBodyGraphIndex,
pub active_island_id: usize,
pub active_set_id: usize,
pub active_set_offset: usize,
pub active_set_timestamp: u32,
}
impl Default for RigidBodyIds {
fn default() -> Self {
Self {
joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
active_island_id: 0,
active_set_id: 0,
active_set_offset: 0,
active_set_timestamp: 0,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
pub struct RigidBodyColliders(pub Vec<ColliderHandle>);
impl Default for RigidBodyColliders {
fn default() -> Self {
Self(vec![])
}
}
impl RigidBodyColliders {
pub fn detach_collider(
&mut self,
rb_changes: &mut RigidBodyChanges,
co_handle: ColliderHandle,
) {
if let Some(i) = self.0.iter().position(|e| *e == co_handle) {
rb_changes.set(
RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
true,
);
self.0.swap_remove(i);
}
}
pub fn attach_collider(
&mut self,
rb_changes: &mut RigidBodyChanges,
rb_ccd: &mut RigidBodyCcd,
rb_mprops: &mut RigidBodyMassProps,
rb_pos: &RigidBodyPosition,
co_handle: ColliderHandle,
co_pos: &mut ColliderPosition,
co_parent: &ColliderParent,
co_shape: &ColliderShape,
co_mprops: &ColliderMassProperties,
) {
rb_changes.set(
RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
true,
);
co_pos.0 = rb_pos.position * co_parent.pos_wrt_parent;
rb_ccd.ccd_thickness = rb_ccd.ccd_thickness.min(co_shape.ccd_thickness());
let shape_bsphere = co_shape.compute_bounding_sphere(&co_parent.pos_wrt_parent);
rb_ccd.ccd_max_dist = rb_ccd
.ccd_max_dist
.max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
let mass_properties = co_mprops
.mass_properties(&**co_shape)
.transform_by(&co_parent.pos_wrt_parent);
self.0.push(co_handle);
rb_mprops.mass_properties += mass_properties;
rb_mprops.update_world_mass_properties(&rb_pos.position);
}
pub fn update_positions<Colliders>(
&self,
colliders: &mut Colliders,
modified_colliders: &mut Vec<ColliderHandle>,
parent_pos: &Isometry<Real>,
) where
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
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;
// 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));
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy)]
pub struct RigidBodyDominance(pub i8);
impl Default for RigidBodyDominance {
fn default() -> Self {
RigidBodyDominance(0)
}
}
impl RigidBodyDominance {
pub fn effective_group(&self, status: &RigidBodyType) -> i16 {
if status.is_dynamic() {
self.0 as i16
} else {
i8::MAX as i16 + 1
}
}
}
/// The rb_activation status of a body.
///
/// This controls whether a body is sleeping or not.
/// If the threshold is negative, the body never sleeps.
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct RigidBodyActivation {
/// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
pub threshold: Real,
/// The current pseudo-kinetic energy of the body.
pub energy: Real,
/// Is this body already sleeping?
pub sleeping: bool,
}
impl Default for RigidBodyActivation {
fn default() -> Self {
Self::new_active()
}
}
impl RigidBodyActivation {
/// The default amount of energy bellow which a body can be put to sleep by nphysics.
pub fn default_threshold() -> Real {
0.01
}
/// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
pub fn new_active() -> Self {
RigidBodyActivation {
threshold: Self::default_threshold(),
energy: Self::default_threshold() * 4.0,
sleeping: false,
}
}
/// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
pub fn new_inactive() -> Self {
RigidBodyActivation {
threshold: Self::default_threshold(),
energy: 0.0,
sleeping: true,
}
}
/// Returns `true` if the body is not asleep.
#[inline]
pub fn is_active(&self) -> bool {
self.energy != 0.0
}
#[inline]
pub fn wake_up(&mut self, strong: bool) {
self.sleeping = false;
if strong || self.energy == 0.0 {
self.energy = self.threshold.abs() * 2.0;
}
}
#[inline]
pub fn sleep(&mut self) {
self.energy = 0.0;
self.sleeping = true;
}
}

658
src/dynamics/rigid_body_set.rs
View File

@@ -1,48 +1,19 @@
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges};
use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
RigidBodyType,
};
use crate::dynamics::{
JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::ColliderSet;
use parry::partitioning::IndexedData;
use std::ops::{Index, IndexMut};
/// The unique handle of a rigid body added to a `RigidBodySet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
impl RigidBodyHandle {
/// Converts this handle into its (index, generation) components.
pub fn into_raw_parts(self) -> (usize, u64) {
self.0.into_raw_parts()
}
/// Reconstructs an handle from its (index, generation) components.
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid rigid-body handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
))
}
}
impl IndexedData for RigidBodyHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A pair of rigid body handles.
@@ -69,38 +40,75 @@ pub struct RigidBodySet {
// parallelism because the `Receiver` breaks the Sync impl.
// Could we avoid this?
pub(crate) bodies: Arena<RigidBody>,
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
// Set of inactive bodies which have been modified.
// This typically include static bodies which have been modified.
pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
active_set_timestamp: u32,
pub(crate) modified_bodies: Vec<RigidBodyHandle>,
pub(crate) modified_all_bodies: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>, // Workspace.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<RigidBodyHandle>, // Workspace.
}
macro_rules! impl_field_component_set(
($T: ty, $field: ident) => {
impl ComponentSetOption<$T> for RigidBodySet {
fn get(&self, handle: crate::data::Index) -> Option<&$T> {
self.get(RigidBodyHandle(handle)).map(|b| &b.$field)
}
}
impl ComponentSet<$T> for RigidBodySet {
fn size_hint(&self) -> usize {
self.len()
}
#[inline(always)]
fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
for (handle, body) in self.bodies.iter() {
f(handle, &body.$field)
}
}
}
impl ComponentSetMut<$T> for RigidBodySet {
fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) {
rb.$field = val;
}
}
#[inline(always)]
fn map_mut_internal<Result>(
&mut self,
handle: crate::data::Index,
f: impl FnOnce(&mut $T) -> Result,
) -> Option<Result> {
self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field))
}
}
}
);
impl_field_component_set!(RigidBodyPosition, rb_pos);
impl_field_component_set!(RigidBodyMassProps, rb_mprops);
impl_field_component_set!(RigidBodyVelocity, rb_vels);
impl_field_component_set!(RigidBodyDamping, rb_damping);
impl_field_component_set!(RigidBodyForces, rb_forces);
impl_field_component_set!(RigidBodyCcd, rb_ccd);
impl_field_component_set!(RigidBodyIds, rb_ids);
impl_field_component_set!(RigidBodyType, rb_type);
impl_field_component_set!(RigidBodyActivation, rb_activation);
impl_field_component_set!(RigidBodyColliders, rb_colliders);
impl_field_component_set!(RigidBodyDominance, rb_dominance);
impl_field_component_set!(RigidBodyChanges, changes);
impl RigidBodySet {
/// Create a new empty set of rigid bodies.
pub fn new() -> Self {
RigidBodySet {
bodies: Arena::new(),
active_dynamic_set: Vec::new(),
active_kinematic_set: Vec::new(),
modified_inactive_set: Vec::new(),
active_islands: Vec::new(),
active_set_timestamp: 0,
modified_bodies: Vec::new(),
modified_all_bodies: false,
can_sleep: Vec::new(),
stack: Vec::new(),
}
}
pub(crate) fn take_modified(&mut self) -> Vec<RigidBodyHandle> {
std::mem::replace(&mut self.modified_bodies, vec![])
}
/// The number of rigid bodies on this set.
pub fn len(&self) -> usize {
self.bodies.len()
@@ -121,18 +129,10 @@ impl RigidBodySet {
// Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one.
rb.reset_internal_references();
rb.changes.set(RigidBodyChanges::all(), true);
rb.changes_mut_internal().set(RigidBodyChanges::all(), true);
let handle = RigidBodyHandle(self.bodies.insert(rb));
self.modified_bodies.push(handle);
let rb = &mut self.bodies[handle.0];
if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
handle
}
@@ -140,6 +140,7 @@ impl RigidBodySet {
pub fn remove(
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
@@ -147,55 +148,23 @@ impl RigidBodySet {
/*
* Update active sets.
*/
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
if active_set.get(rb.active_set_id) == Some(&handle) {
active_set.swap_remove(rb.active_set_id);
if let Some(replacement) = active_set.get(rb.active_set_id) {
self.bodies[replacement.0].active_set_id = rb.active_set_id;
}
}
}
islands.rigid_body_removed(handle, &rb.rb_ids, self);
/*
* Remove colliders attached to this rigid-body.
*/
for collider in &rb.colliders {
colliders.remove(*collider, self, false);
for collider in rb.colliders() {
colliders.remove(*collider, islands, self, false);
}
/*
* Remove joints attached to this rigid-body.
*/
joints.remove_rigid_body(rb.joint_graph_index, self);
joints.remove_rigid_body(rb.rb_ids.joint_graph_index, islands, self);
Some(rb)
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
/// Forces the specified rigid-body to wake up if it is dynamic.
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
if let Some(rb) = self.bodies.get_mut(handle.0) {
// TODO: what about kinematic bodies?
if rb.is_dynamic() {
rb.wake_up(strong);
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
}
}
/// Gets the rigid-body with the given handle without a known generation.
///
/// This is useful when you know you want the rigid-body at position `i` but
@@ -224,12 +193,7 @@ impl RigidBodySet {
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
let handle = RigidBodyHandle(handle);
Self::mark_as_modified(
handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
Some((rb, handle))
}
@@ -238,14 +202,13 @@ impl RigidBodySet {
self.bodies.get(handle.0)
}
fn mark_as_modified(
pub(crate) fn mark_as_modified(
handle: RigidBodyHandle,
rb: &mut RigidBody,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_all_bodies: bool,
) {
if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
rb.changes = RigidBodyChanges::MODIFIED;
if !rb.changes().contains(RigidBodyChanges::MODIFIED) {
*rb.changes_mut_internal() = RigidBodyChanges::MODIFIED;
modified_bodies.push(handle);
}
}
@@ -254,12 +217,7 @@ impl RigidBodySet {
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Self::mark_as_modified(handle, result, &mut self.modified_bodies);
Some(result)
}
@@ -274,23 +232,10 @@ impl RigidBodySet {
handle: RigidBodyHandle,
) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Self::mark_as_modified(handle, result, &mut self.modified_bodies);
Some(result)
}
pub(crate) fn get2_mut_internal(
&mut self,
h1: RigidBodyHandle,
h2: RigidBodyHandle,
) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
self.bodies.get2_mut(h1.0, h2.0)
}
/// Iterates through all the rigid-bodies on this set.
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
@@ -300,431 +245,11 @@ impl RigidBodySet {
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
self.modified_bodies.clear();
self.modified_all_bodies = true;
self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
}
/// Iter through all the active kinematic rigid-bodies on this set.
pub fn iter_active_kinematic<'a>(
&'a self,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let bodies: &'a _ = &self.bodies;
self.active_kinematic_set
.iter()
.filter_map(move |h| Some((*h, bodies.get(h.0)?)))
}
/// Iter through all the active dynamic rigid-bodies on this set.
pub fn iter_active_dynamic<'a>(
&'a self,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let bodies: &'a _ = &self.bodies;
self.active_dynamic_set
.iter()
.filter_map(move |h| Some((*h, bodies.get(h.0)?)))
}
#[cfg(not(feature = "parallel"))]
pub(crate) fn iter_active_island<'a>(
&'a self,
island_id: usize,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
let bodies: &'a _ = &self.bodies;
self.active_dynamic_set[island_range]
.iter()
.filter_map(move |h| Some((*h, bodies.get(h.0)?)))
}
/// Applies the given function on all the active dynamic rigid-bodies
/// contained by this set.
#[inline(always)]
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn foreach_active_dynamic_body_mut(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
Self::mark_as_modified(
*handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb)
}
}
for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_dynamic_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_kinematic_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb)
}
}
}
#[inline(always)]
#[cfg(not(feature = "parallel"))]
pub(crate) fn foreach_active_island_body_mut_internal(
&mut self,
island_id: usize,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
for handle in &self.active_dynamic_set[island_range] {
if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb)
}
}
}
#[cfg(feature = "parallel")]
#[inline(always)]
#[allow(dead_code)]
pub(crate) fn foreach_active_island_body_mut_internal_parallel(
&mut self,
island_id: usize,
f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
) {
use std::sync::atomic::Ordering;
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
self.active_dynamic_set[island_range]
.par_iter()
.for_each_init(
|| bodies.load(Ordering::Relaxed),
|bodies, handle| {
let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
if let Some(rb) = bodies.get_mut(handle.0) {
f(*handle, rb)
}
},
);
}
// pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
// &self.active_dynamic_set
// }
pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
self.active_islands[island_id]..self.active_islands[island_id + 1]
}
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
&self.active_dynamic_set[self.active_island_range(island_id)]
}
// Utility function to avoid some borrowing issue in the `maintain` method.
fn maintain_one(
bodies: &mut Arena<RigidBody>,
colliders: &mut ColliderSet,
handle: RigidBodyHandle,
modified_inactive_set: &mut Vec<RigidBodyHandle>,
active_kinematic_set: &mut Vec<RigidBodyHandle>,
active_dynamic_set: &mut Vec<RigidBodyHandle>,
) {
enum FinalAction {
UpdateActiveKinematicSetId,
UpdateActiveDynamicSetId,
}
if let Some(rb) = bodies.get_mut(handle.0) {
let mut final_action = None;
// The body's status changed. We need to make sure
// it is on the correct active set.
if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
match rb.body_status() {
BodyStatus::Dynamic => {
// Remove from the active kinematic set if it was there.
if active_kinematic_set.get(rb.active_set_id) == Some(&handle) {
active_kinematic_set.swap_remove(rb.active_set_id);
final_action =
Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id));
}
// Add to the active dynamic set.
rb.wake_up(true);
// Make sure the sleep change flag is set (even if for some
// reasons the rigid-body was already awake) to make
// sure the code handling sleeping change adds the body to
// the active_dynamic_set.
rb.changes.set(RigidBodyChanges::SLEEP, true);
}
BodyStatus::Kinematic => {
// Remove from the active dynamic set if it was there.
if active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
active_dynamic_set.swap_remove(rb.active_set_id);
final_action =
Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id));
}
// Add to the active kinematic set.
if active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = active_kinematic_set.len();
active_kinematic_set.push(handle);
}
}
BodyStatus::Static => {}
}
}
// Update the positions of the colliders.
if rb.changes.contains(RigidBodyChanges::POSITION)
|| rb.changes.contains(RigidBodyChanges::COLLIDERS)
{
rb.update_colliders_positions(colliders);
if rb.is_static() {
modified_inactive_set.push(handle);
}
if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = active_kinematic_set.len();
active_kinematic_set.push(handle);
}
}
// Push the body to the active set if it is not
// sleeping and if it is not already inside of the active set.
if rb.changes.contains(RigidBodyChanges::SLEEP)
&& !rb.is_sleeping() // May happen if the body was put to sleep manually.
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& active_dynamic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
active_dynamic_set.push(handle);
}
rb.changes = RigidBodyChanges::empty();
// Adjust some ids, if needed.
if let Some((action, id)) = final_action {
let active_set = match action {
FinalAction::UpdateActiveKinematicSetId => active_kinematic_set,
FinalAction::UpdateActiveDynamicSetId => active_dynamic_set,
};
if id < active_set.len() {
if let Some(rb2) = bodies.get_mut(active_set[id].0) {
rb2.active_set_id = id;
}
}
}
}
}
pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
if self.modified_all_bodies {
// Unfortunately, we have to push all the bodies to `modified_bodies`
// instead of just calling `maintain_one` on each element i
// `self.bodies.iter_mut()` because otherwise it would be difficult to
// handle the final change of active_set_id in Self::maintain_one
// (because it has to modify another rigid-body because of the swap-remove.
// So this causes borrowing problems if we do this while iterating
// through self.bodies.iter_mut()).
for (handle, _) in self.bodies.iter_mut() {
self.modified_bodies.push(RigidBodyHandle(handle));
}
}
for handle in self.modified_bodies.drain(..) {
Self::maintain_one(
&mut self.bodies,
colliders,
handle,
&mut self.modified_inactive_set,
&mut self.active_kinematic_set,
&mut self.active_dynamic_set,
)
}
if self.modified_all_bodies {
self.modified_bodies.shrink_to_fit(); // save some memory.
self.modified_all_bodies = false;
}
}
pub(crate) fn update_active_set_with_contacts(
&mut self,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
min_island_size: usize,
) {
assert!(
min_island_size > 0,
"The minimum island size must be at least 1."
);
// Update the energy of every rigid body and
// keep only those that may not sleep.
// let t = instant::now();
self.active_set_timestamp += 1;
self.stack.clear();
self.can_sleep.clear();
// NOTE: the `.rev()` is here so that two successive timesteps preserve
// the order of the bodies in the `active_dynamic_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes
// debugging slightly nicer so we keep this rev.
for h in self.active_dynamic_set.drain(..).rev() {
let rb = &mut self.bodies[h.0];
rb.update_energy();
if rb.activation.energy <= rb.activation.threshold {
// 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;
self.can_sleep.push(h);
} else {
self.stack.push(h);
}
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)]
fn push_contacting_bodies(
rb: &RigidBody,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {
for collider_handle in &rb.colliders {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if !manifold.data.solver_contacts.is_empty() {
let other = crate::utils::select_other(
(inter.0, inter.1),
*collider_handle,
);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
}
}
}
}
}
}
// 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 rb = &self.bodies[h.0];
if !rb.is_moving() {
// If the kinematic body does not move, it does not have
// to wake up any dynamic body.
continue;
}
push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
}
// println!("Selection: {}", instant::now() - t);
// let t = instant::now();
// Propagation of awake state and awake island computation through the
// traversal of the interaction graph.
self.active_islands.clear();
self.active_islands.push(0);
// The max avoid underflow when the stack is empty.
let mut island_marker = self.stack.len().max(1) - 1;
while let Some(handle) = self.stack.pop() {
let rb = &mut self.bodies[handle.0];
if rb.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 static bodies.
continue;
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
// We are starting a new island.
self.active_islands.push(self.active_dynamic_set.len());
}
island_marker = self.stack.len();
}
rb.wake_up(false);
rb.active_island_id = self.active_islands.len() - 1;
rb.active_set_id = self.active_dynamic_set.len();
rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
rb.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::select_other((inter.0, inter.1), handle);
self.stack.push(other);
}
}
self.active_islands.push(self.active_dynamic_set.len());
// println!(
// "Extraction: {}, num islands: {}",
// instant::now() - t,
// self.active_islands.len() - 1
// );
// Actually put to sleep bodies which have not been detected as awake.
// let t = instant::now();
for h in &self.can_sleep {
let b = &mut self.bodies[h.0];
if b.activation.sleeping {
b.sleep();
}
}
// println!("Activation: {}", instant::now() - t);
let modified_bodies = &mut self.modified_bodies;
self.bodies.iter_mut().map(move |(h, b)| {
modified_bodies.push(RigidBodyHandle(h));
(RigidBodyHandle(h), b)
})
}
}
@@ -736,16 +261,19 @@ impl Index<RigidBodyHandle> for RigidBodySet {
}
}
impl Index<crate::data::Index> for RigidBodySet {
type Output = RigidBody;
fn index(&self, index: crate::data::Index) -> &RigidBody {
&self.bodies[index]
}
}
#[cfg(not(feature = "dev-remove-slow-accessors"))]
impl IndexMut<RigidBodyHandle> for RigidBodySet {
fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
let rb = &mut self.bodies[handle.0];
Self::mark_as_modified(
handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
rb
}
}

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

@@ -1,8 +1,9 @@
use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub(crate) fn categorize_contacts(
_bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
_bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
@@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts(
}
pub(crate) fn categorize_joints(
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBodyType>,
joints: &[JointGraphEdge],
joint_indices: &[JointIndex],
ground_joints: &mut Vec<JointIndex>,
@@ -28,10 +29,10 @@ pub(crate) fn categorize_joints(
) {
for joint_i in joint_indices {
let joint = &joints[*joint_i].weight;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let status1 = bodies.index(joint.body1.0);
let status2 = bodies.index(joint.body2.0);
if !rb1.is_dynamic() || !rb2.is_dynamic() {
if !status1.is_dynamic() || !status2.is_dynamic() {
ground_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);

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

@@ -1,4 +1,8 @@
use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
#[cfg(feature = "parallel")]
use crate::dynamics::BodyPair;
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyType};
use crate::dynamics::{JointGraphEdge, JointIndex};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -6,16 +10,19 @@ use {
vec_map::VecMap,
};
#[cfg(feature = "parallel")]
pub(crate) trait PairInteraction {
fn body_pair(&self) -> BodyPair;
}
#[cfg(feature = "parallel")]
impl<'a> PairInteraction for &'a mut ContactManifold {
fn body_pair(&self) -> BodyPair {
self.data.body_pair
}
}
#[cfg(feature = "parallel")]
impl<'a> PairInteraction for JointGraphEdge {
fn body_pair(&self) -> BodyPair {
BodyPair::new(self.weight.body1, self.weight.body2)
@@ -54,7 +61,7 @@ impl ParallelInteractionGroups {
pub fn group_interactions<Interaction: PairInteraction>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
interactions: &[Interaction],
interaction_indices: &[usize],
) {
@@ -78,8 +85,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].body_pair();
let rb1 = &bodies[body_pair.body1];
let rb2 = &bodies[body_pair.body2];
let rb1 = bodies.index(body_pair.body1);
let rb2 = bodies.index(body_pair.body2);
match (rb1.is_static(), rb2.is_static()) {
(false, false) => {
@@ -168,14 +175,15 @@ impl InteractionGroups {
self.nongrouped_interactions.clear();
}
// FIXME: there is a lot of duplicated code with group_manifolds here.
// TODO: there is a lot of duplicated code with group_manifolds here.
// But we don't refactor just now because we may end up with distinct
// grouping strategies in the future.
#[cfg(not(feature = "simd-is-enabled"))]
pub fn group_joints(
&mut self,
_island_id: usize,
_bodies: &RigidBodySet,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) {
@@ -184,13 +192,16 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_joints(
pub fn group_joints<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
// NOTE: in 3D we have up to 10 different joint types.
// In 2D we only have 5 joint types.
#[cfg(feature = "dim3")]
@@ -204,11 +215,11 @@ impl InteractionGroups {
// Note: each bit of a body mask indicates what bucket already contains
// a constraints involving this body.
// FIXME: currently, this is a bit overconservative because when a bucket
// TODO: currently, this is a bit overconservative because when a bucket
// is full, we don't clear the corresponding body mask bit. This may result
// in less grouped constraints.
self.body_masks
.resize(bodies.active_island(island_id).len(), 0u128);
.resize(islands.active_island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -216,10 +227,14 @@ impl InteractionGroups {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight;
let body1 = &bodies[interaction.body1];
let body2 = &bodies[interaction.body2];
let is_static1 = !body1.is_dynamic();
let is_static2 = !body2.is_dynamic();
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body1.0);
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let is_static1 = !status1.is_dynamic();
let is_static2 = !status2.is_dynamic();
if is_static1 && is_static2 {
continue;
@@ -232,8 +247,8 @@ impl InteractionGroups {
}
let ijoint = interaction.params.type_id();
let i1 = body1.active_set_offset;
let i2 = body2.active_set_offset;
let i1 = ids1.active_set_offset;
let i2 = ids2.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.
@@ -325,7 +340,8 @@ impl InteractionGroups {
pub fn group_manifolds(
&mut self,
_island_id: usize,
_bodies: &RigidBodySet,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) {
@@ -334,21 +350,24 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_manifolds(
pub fn group_manifolds<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
// Note: each bit of a body mask indicates what bucket already contains
// a constraints involving this body.
// FIXME: currently, this is a bit overconservative because when a bucket
// TODO: currently, this is a bit overconservative because when a bucket
// is full, we don't clear the corresponding body mask bit. This may result
// in less grouped contacts.
// NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
self.body_masks
.resize(bodies.active_island(island_id).len(), 0u128);
.resize(islands.active_island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -359,31 +378,44 @@ impl InteractionGroups {
.max()
.unwrap_or(1);
// FIXME: find a way to reduce the number of iteration.
// TODO: find a way to reduce the number of iteration.
// There must be a way to iterate just once on every interaction indices
// instead of MAX_MANIFOLD_POINTS times.
for k in 1..=max_interaction_points {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i];
// FIXME: how could we avoid iterating
// TODO: how could we avoid iterating
// on each interaction at every iteration on k?
if interaction.data.num_active_contacts() != k {
continue;
}
let body1 = &bodies[interaction.data.body_pair.body1];
let body2 = &bodies[interaction.data.body_pair.body2];
let is_static1 = !body1.is_dynamic();
let is_static2 = !body2.is_dynamic();
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)
} else {
(RigidBodyType::Static, 0)
};
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)
} else {
(RigidBodyType::Static, 0)
};
// FIXME: don't generate interactions between static bodies in the first place.
let is_static1 = !status1.is_dynamic();
let is_static2 = !status2.is_dynamic();
// TODO: don't generate interactions between static bodies in the first place.
if is_static1 && is_static2 {
continue;
}
let i1 = body1.active_set_offset;
let i2 = body2.active_set_offset;
let i1 = active_set_offset1;
let i2 = active_set_offset2;
let conflicts = self.body_masks[i1] | self.body_masks[i2];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;

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

@@ -1,10 +1,15 @@
use super::{PositionSolver, VelocitySolver};
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub struct IslandSolver {
@@ -24,17 +29,21 @@ impl IslandSolver {
}
}
pub fn solve_position_constraints(
pub fn solve_position_constraints<Bodies>(
&mut self,
island_id: usize,
islands: &IslandManager,
counters: &mut Counters,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
) {
bodies: &mut Bodies,
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
{
counters.solver.position_resolution_time.resume();
self.position_solver.solve(
island_id,
params,
islands,
bodies,
&self.contact_constraints.position_constraints,
&self.joint_constraints.position_constraints,
@@ -42,31 +51,47 @@ impl IslandSolver {
counters.solver.position_resolution_time.pause();
}
pub fn init_constraints_and_solve_velocity_constraints(
pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
&mut self,
island_id: usize,
counters: &mut Counters,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
manifolds: &mut [&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
joints: &mut [JointGraphEdge],
joint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
if has_constraints {
counters.solver.velocity_assembly_time.resume();
self.contact_constraints
.init(island_id, params, bodies, manifolds, manifold_indices);
self.contact_constraints.init(
island_id,
params,
islands,
bodies,
manifolds,
manifold_indices,
);
self.joint_constraints
.init(island_id, params, bodies, joints, joint_indices);
.init(island_id, params, islands, bodies, joints, joint_indices);
counters.solver.velocity_assembly_time.pause();
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve(
island_id,
params,
islands,
bodies,
manifolds,
joints,
@@ -76,21 +101,50 @@ impl IslandSolver {
counters.solver.velocity_resolution_time.pause();
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.apply_damping(params.dt);
rb.integrate_next_position(params.dt);
});
for handle in islands.active_island(island_id) {
let (poss, vels, damping, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_poss = *poss;
let new_vels = vels.apply_damping(params.dt, damping);
new_poss.next_position =
vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
bodies.set_internal(handle.0, new_vels);
bodies.set_internal(handle.0, new_poss);
}
counters.solver.velocity_update_time.pause();
} else {
self.contact_constraints.clear();
self.joint_constraints.clear();
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
for handle in islands.active_island(island_id) {
// Since we didn't run the velocity solver we need to integrate the accelerations here
rb.integrate_accelerations(params.dt);
rb.apply_damping(params.dt);
rb.integrate_next_position(params.dt);
});
let (poss, vels, forces, damping, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_poss = *poss;
let new_vels = forces
.integrate(params.dt, vels, mprops)
.apply_damping(params.dt, &damping);
new_poss.next_position =
vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
bodies.set_internal(handle.0, new_vels);
bodies.set_internal(handle.0, new_poss);
}
counters.solver.velocity_update_time.pause();
}
}

56
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
@@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint {
}
impl BallPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
Self {
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
}
}
@@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint {
impl BallPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
if flipped {
// Note the only thing that is flipped here
// are the local_anchors. The rb1 and rb2 have
// already been flipped by the caller.
Self {
anchor1: rb1.next_position * cparams.local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor2,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
} else {
Self {
anchor1: rb1.next_position * cparams.local_anchor1,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor1,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
}
}

83
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
@@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint {
impl WBallPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let (mprops1, ids1) = rbs1;
let (mprops2, ids2) = rbs2;
let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]);
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii1 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
])
.squared();
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let position1 = gather![|ii| ids1[ii].active_set_offset];
let position2 = gather![|ii| ids2[ii].active_set_offset];
Self {
local_com1,
@@ -61,8 +72,8 @@ impl WBallPositionConstraint {
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor1 = position1 * self.local_anchor1;
let anchor2 = position2 * self.local_anchor2;
@@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint {
impl WBallPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
let poss1 = rbs1;
let (mprops2, ids2) = rbs2;
let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
let anchor1 = position1
* Point::from(array![|ii| if flipped[ii] {
* Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
}]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}; SIMD_WIDTH]);
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
}]);
let position2 = gather![|ii| ids2[ii].active_set_offset];
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
Self {
anchor1,
@@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint {
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor2 = position2 * self.local_anchor2;
let com2 = position2 * self.local_com2;

114
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -36,19 +37,32 @@ impl BallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
) -> Self {
let anchor_world1 = rb1.position * joint.local_anchor1;
let anchor_world2 = rb2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let anchor_world1 = poss1.position * joint.local_anchor1;
let anchor_world2 = poss2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -59,12 +73,12 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2)
+ rb1
+ mprops1
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat1)
@@ -75,8 +89,8 @@ impl BallVelocityConstraint {
// it's just easier that way.
#[cfg(feature = "dim2")]
{
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -100,8 +114,8 @@ impl BallVelocityConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -113,15 +127,15 @@ impl BallVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / (ii1 + ii2))
} else {
Some(gamma)
@@ -132,8 +146,8 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some((ii1 + ii2).inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -151,8 +165,8 @@ impl BallVelocityConstraint {
BallVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
impulse: joint.impulse * params.warmstart_coeff,
@@ -164,8 +178,8 @@ impl BallVelocityConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}
@@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor_world1, anchor_world2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
poss1.position * joint.local_anchor2,
poss2.position * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
poss1.position * joint.local_anchor1,
poss2.position * joint.local_anchor2,
)
};
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let im2 = rb2.effective_inv_mass;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im2 = mprops2.effective_inv_mass;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
@@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim2")]
{
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im2 + cmat2.x * cmat2.x * ii2;
let m12 = cmat2.x * cmat2.y * ii2;
let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / ii2)
} else {
Some(gamma)
@@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(ii2.inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint {
BallVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
impulse: joint.impulse * params.warmstart_coeff,
r2: anchor2,
@@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}

182
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
@@ -34,33 +35,46 @@ impl WBallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -114,20 +128,16 @@ impl WBallVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda1.linear += self.impulse * self.im1;
@@ -147,20 +157,16 @@ impl WBallVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda2.linear -= self.impulse * self.im2;
@@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

48
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::WAngularInertia;
@@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint {
}
impl FixedPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
lin_inv_lhs,
ang_inv_lhs,
}
@@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint {
impl FixedPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
};
Self {
anchor1,
local_anchor2,
position2: rb2.active_set_offset,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
impulse: 0.0,
}
}

34
src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint {
impl WFixedPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint {
impl WFixedPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

86
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -45,18 +46,31 @@ impl FixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
) -> Self {
let anchor1 = rb1.position * cparams.local_anchor1;
let anchor2 = rb2.position * cparams.local_anchor2;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor1 = poss1.position * cparams.local_anchor1;
let anchor2 = poss2.position * cparams.local_anchor2;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat1 = r1.gcross_matrix();
let rmat2 = r2.gcross_matrix();
@@ -99,8 +113,9 @@ impl FixedVelocityConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
let ang_dvel = -rb1.angvel + rb2.angvel;
let lin_dvel =
-vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
let ang_dvel = -vels1.angvel + vels2.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -133,14 +148,14 @@ impl FixedVelocityConstraint {
FixedVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r1,
@@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor1, anchor2) = if flipped {
(
rb1.position * cparams.local_anchor2,
rb2.position * cparams.local_anchor1,
poss1.position * cparams.local_anchor2,
poss2.position * cparams.local_anchor1,
)
} else {
(
rb1.position * cparams.local_anchor1,
rb2.position * cparams.local_anchor2,
poss1.position * cparams.local_anchor1,
poss2.position * cparams.local_anchor2,
)
};
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat2 = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
let ang_dvel = rb2.angvel - rb1.angvel;
let lin_dvel =
vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
let ang_dvel = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint {
FixedVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r2,

188
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
@@ -53,33 +54,46 @@ impl WFixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -157,8 +171,7 @@ impl WFixedVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -185,20 +198,16 @@ impl WFixedVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -229,20 +238,16 @@ impl WFixedVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

2
src/dynamics/solver/joint_constraint/generic_position_constraint.rs
View File

@@ -1,6 +1,6 @@
use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{GenericJoint, IntegrationParameters};
use crate::math::{
AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
DIM,

13
src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
View File

@@ -15,7 +15,11 @@ impl WGenericPositionConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii]
)],
}
}
@@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint {
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionGroundConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii],
flipped[ii]
)],
}
}

16
src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
View File

@@ -50,8 +50,8 @@ impl GenericVelocityConstraint {
rb1: &RigidBody,
rb2: &RigidBody,
) -> SpatialVector<Real> {
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+ basis2.inverse_transform_vector(&rb2.angvel);
@@ -203,8 +203,8 @@ impl GenericVelocityConstraint {
rb2: &RigidBody,
joint: &GenericJoint,
) -> Self {
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = rb1.position() * joint.local_anchor1;
let anchor2 = rb2.position() * joint.local_anchor2;
let basis1 = anchor1.rotation;
let basis2 = anchor2.rotation;
let im1 = rb1.effective_inv_mass;
@@ -405,13 +405,13 @@ impl GenericVelocityGroundConstraint {
) -> Self {
let (anchor1, anchor2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
rb1.position() * joint.local_anchor2,
rb2.position() * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
rb1.position() * joint.local_anchor1,
rb2.position() * joint.local_anchor2,
)
};

146
src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
View File

@@ -57,29 +57,29 @@ impl WGenericVelocityConstraint {
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&GenericJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -160,20 +160,16 @@ impl WGenericVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -204,20 +200,16 @@ impl WGenericVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -306,28 +298,32 @@ impl WGenericVelocityGroundConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -395,12 +391,10 @@ impl WGenericVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -422,12 +416,10 @@ impl WGenericVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -16,9 +16,11 @@ use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
@@ -69,14 +71,30 @@ impl AnyJointVelocityConstraint {
1
}
pub fn from_joint(
pub fn from_joint<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rb1 = (
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
);
let rb2 = (
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
@@ -99,45 +117,59 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(
pub fn from_wide_joint<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericConstraint(
// WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticConstraint(
WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteConstraint(
WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
@@ -145,20 +177,31 @@ impl AnyJointVelocityConstraint {
}
}
pub fn from_joint_ground(
pub fn from_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index_bundle(handle1.0);
let rb2 = bodies.index_bundle(handle2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
@@ -186,26 +229,46 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
pub fn from_wide_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
);
let rbs2 = (
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)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallGroundConstraint(
WBallVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -213,7 +276,7 @@ impl AnyJointVelocityConstraint {
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedGroundConstraint(
WFixedVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -221,7 +284,7 @@ impl AnyJointVelocityConstraint {
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericGroundConstraint(
// WGenericVelocityGroundConstraint::from_params(
// params, joint_id, rbs1, rbs2, joints, flipped,
@@ -229,8 +292,7 @@ impl AnyJointVelocityConstraint {
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticGroundConstraint(
WPrismaticVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -239,8 +301,7 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteGroundConstraint(
WRevoluteVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,

99
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -13,7 +13,11 @@ use super::{
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{
IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType,
};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use crate::math::{Isometry, Real};
@@ -56,9 +60,12 @@ pub(crate) enum AnyJointPositionConstraint {
}
impl AnyJointPositionConstraint {
pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rb1 = bodies.index_bundle(joint.body1.0);
let rb2 = bodies.index_bundle(joint.body2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
@@ -81,40 +88,47 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
// rbs1, rbs2, joints,
// ))
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticJoint(
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteJoint(
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
)
@@ -122,15 +136,26 @@ impl AnyJointPositionConstraint {
}
}
pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index(handle1.0);
let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -154,48 +179,60 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
let rbs2 = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallGroundConstraint(
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedGroundConstraint(
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericGroundConstraint(
// WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticGroundConstraint(
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteGroundConstraint(
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)

42
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WAngularInertia;
use na::Unit;
@@ -27,11 +29,18 @@ pub(crate) struct PrismaticPositionConstraint {
}
impl PrismaticPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
@@ -46,8 +55,8 @@ impl PrismaticPositionConstraint {
local_frame2: cparams.local_frame2(),
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}
@@ -108,25 +117,28 @@ pub(crate) struct PrismaticPositionGroundConstraint {
impl PrismaticPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (_, ids2) = rb2;
let frame1;
let local_frame2;
let axis1;
let local_axis2;
if flipped {
frame1 = rb1.next_position * cparams.local_frame2();
frame1 = poss1.next_position * cparams.local_frame2();
local_frame2 = cparams.local_frame1();
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
} else {
frame1 = rb1.next_position * cparams.local_frame1();
frame1 = poss1.next_position * cparams.local_frame1();
local_frame2 = cparams.local_frame2();
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
};
@@ -135,7 +147,7 @@ impl PrismaticPositionGroundConstraint {
local_frame2,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}

34
src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WPrismaticPositionConstraint {
impl WPrismaticPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WPrismaticPositionGroundConstraint {
impl WPrismaticPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

138
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
@@ -74,32 +75,45 @@ impl PrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
#[cfg(feature = "dim2")]
let basis1 = rb1.position * joint.basis1[0];
let basis1 = poss1.position * joint.basis1[0];
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let im1 = rb1.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let im1 = mprops1.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -131,8 +145,8 @@ impl PrismaticVelocityConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -142,7 +156,7 @@ impl PrismaticVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -159,8 +173,8 @@ impl PrismaticVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
let frame1 = poss1.position * joint.local_frame1();
let frame2 = poss2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[cfg(feature = "dim2")]
@@ -195,9 +209,9 @@ impl PrismaticVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2)
- rb1.linvel.dot(&axis1)
- rb1.angvel.gdot(gcross1);
let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
- vels1.linvel.dot(&axis1)
- vels1.angvel.gdot(gcross1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -266,12 +280,12 @@ impl PrismaticVelocityConstraint {
PrismaticVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_impulse: limits_impulse * params.warmstart_coeff,
@@ -501,11 +515,19 @@ impl PrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis2;
@@ -513,35 +535,35 @@ impl PrismaticVelocityGroundConstraint {
let basis1;
if flipped {
anchor2 = rb2.position * joint.local_anchor1;
anchor1 = rb1.position * joint.local_anchor2;
axis2 = rb2.position * joint.local_axis1;
axis1 = rb1.position * joint.local_axis2;
anchor2 = poss2.position * joint.local_anchor1;
anchor1 = poss1.position * joint.local_anchor2;
axis2 = poss2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis2[0];
basis1 = poss1.position * joint.basis2[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
}
} else {
anchor2 = rb2.position * joint.local_anchor2;
anchor1 = rb1.position * joint.local_anchor1;
axis2 = rb2.position * joint.local_axis2;
axis1 = rb1.position * joint.local_axis1;
anchor2 = poss2.position * joint.local_anchor2;
anchor1 = poss1.position * joint.local_anchor1;
axis2 = poss2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis1[0];
basis1 = poss1.position * joint.basis1[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
}
};
@@ -560,10 +582,10 @@ impl PrismaticVelocityGroundConstraint {
// simplifications of the computation without introducing
// much instabilities.
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -592,8 +614,8 @@ impl PrismaticVelocityGroundConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -603,7 +625,7 @@ impl PrismaticVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -622,11 +644,11 @@ impl PrismaticVelocityGroundConstraint {
let (frame1, frame2);
if flipped {
frame1 = rb1.position * joint.local_frame2();
frame2 = rb2.position * joint.local_frame1();
frame1 = poss1.position * joint.local_frame2();
frame2 = poss2.position * joint.local_frame1();
} else {
frame1 = rb1.position * joint.local_frame1();
frame2 = rb2.position * joint.local_frame2();
frame1 = poss1.position * joint.local_frame1();
frame2 = poss2.position * joint.local_frame2();
}
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -660,7 +682,7 @@ impl PrismaticVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -714,9 +736,9 @@ impl PrismaticVelocityGroundConstraint {
PrismaticVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_forcedir2,

288
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
@@ -71,47 +72,60 @@ impl WPrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
#[cfg(feature = "dim2")]
let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
#[cfg(feature = "dim3")]
let local_basis1 = [
Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| cparams[ii].basis1[0]]),
Vector::from(gather![|ii| cparams[ii].basis1[1]]),
];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -207,8 +221,8 @@ impl WPrismaticVelocityConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
let frame1 = position1 * local_frame1;
let frame2 = position2 * local_frame2;
@@ -221,8 +235,7 @@ impl WPrismaticVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -237,15 +250,15 @@ impl WPrismaticVelocityConstraint {
let mut limits_inv_lhs = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -265,10 +278,9 @@ impl WPrismaticVelocityConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_inv_lhs = SimdReal::splat(1.0)
/ (im1
@@ -303,20 +315,16 @@ impl WPrismaticVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -428,20 +436,16 @@ impl WPrismaticVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
@@ -510,59 +514,85 @@ impl WPrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let local_axis1 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis2
} else {
*cparams[ii].local_axis1
}]);
let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis1
} else {
*cparams[ii].local_axis2
}]);
#[cfg(feature = "dim2")]
let basis1 = position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
);
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]);
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[1]
} else {
cparams[ii].basis1[1]
}]),
]);
let anchor1 = position1 * local_anchor1;
@@ -634,13 +664,17 @@ impl WPrismaticVelocityGroundConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = position1
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame2()
} else {
cparams[ii].local_frame1()
}]);
let frame2 = position2
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame1()
} else {
cparams[ii].local_frame2()
}]);
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -651,8 +685,7 @@ impl WPrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -666,14 +699,14 @@ impl WPrismaticVelocityGroundConstraint {
let mut limits_impulse = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -690,10 +723,9 @@ impl WPrismaticVelocityGroundConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
}
}
@@ -718,12 +750,10 @@ impl WPrismaticVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -791,12 +821,10 @@ impl WPrismaticVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda2);

60
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::Unit;
@@ -29,11 +31,18 @@ pub(crate) struct RevolutePositionConstraint {
}
impl RevolutePositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
@@ -42,14 +51,14 @@ impl RevolutePositionConstraint {
ii1,
ii2,
ang_inv_lhs,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
local_basis1: cparams.basis1,
local_basis2: cparams.basis2,
}
@@ -132,11 +141,14 @@ pub(crate) struct RevolutePositionGroundConstraint {
impl RevolutePositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
let axis1;
@@ -145,23 +157,23 @@ impl RevolutePositionGroundConstraint {
let local_basis2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
basis1 = [
rb1.next_position * cparams.basis2[0],
rb1.next_position * cparams.basis2[1],
poss1.next_position * cparams.basis2[0],
poss1.next_position * cparams.basis2[1],
];
local_basis2 = cparams.basis1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
basis1 = [
rb1.next_position * cparams.basis1[0],
rb1.next_position * cparams.basis1[1],
poss1.next_position * cparams.basis1[0],
poss1.next_position * cparams.basis1[1],
];
local_basis2 = cparams.basis2;
};
@@ -169,12 +181,12 @@ impl RevolutePositionGroundConstraint {
Self {
anchor1,
local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
basis1,
local_basis2,
}

34
src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WRevolutePositionConstraint {
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WRevolutePositionGroundConstraint {
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

140
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -43,34 +44,47 @@ impl RevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -90,8 +104,8 @@ impl RevoluteVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
@@ -105,8 +119,8 @@ impl RevoluteVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let lin_err = anchor2 - anchor1;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
let axis_error = axis1.cross(&axis2);
let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
@@ -118,8 +132,8 @@ impl RevoluteVelocityConstraint {
/*
* Motor.
*/
let motor_axis1 = rb1.position * *joint.local_axis1;
let motor_axis2 = rb2.position * *joint.local_axis2;
let motor_axis1 = poss1.position * *joint.local_axis1;
let motor_axis2 = poss2.position * *joint.local_axis2;
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let mut motor_angle = 0.0;
@@ -132,12 +146,12 @@ impl RevoluteVelocityConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -171,14 +185,14 @@ impl RevoluteVelocityConstraint {
RevoluteVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
basis1,
basis2,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse,
inv_lhs,
rhs,
@@ -330,11 +344,19 @@ impl RevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
flipped: bool,
) -> AnyJointVelocityConstraint {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis1;
@@ -343,39 +365,39 @@ impl RevoluteVelocityGroundConstraint {
let basis2;
if flipped {
axis1 = rb1.position * *joint.local_axis2;
axis2 = rb2.position * *joint.local_axis1;
anchor1 = rb1.position * joint.local_anchor2;
anchor2 = rb2.position * joint.local_anchor1;
axis1 = poss1.position * *joint.local_axis2;
axis2 = poss2.position * *joint.local_axis1;
anchor1 = poss1.position * joint.local_anchor2;
anchor2 = poss2.position * joint.local_anchor1;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis1[0],
rb2.position * joint.basis1[1],
poss2.position * joint.basis1[0],
poss2.position * joint.basis1[1],
]);
} else {
axis1 = rb1.position * *joint.local_axis1;
axis2 = rb2.position * *joint.local_axis2;
anchor1 = rb1.position * joint.local_anchor1;
anchor2 = rb2.position * joint.local_anchor2;
axis1 = poss1.position * *joint.local_axis1;
axis2 = poss2.position * *joint.local_axis2;
anchor1 = poss1.position * joint.local_anchor1;
anchor2 = poss2.position * joint.local_anchor2;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
};
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -393,8 +415,8 @@ impl RevoluteVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
linvel_err.y,
@@ -409,11 +431,11 @@ impl RevoluteVelocityGroundConstraint {
let (axis1, axis2);
if flipped {
axis1 = rb1.position * joint.local_axis2;
axis2 = rb2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
axis2 = poss2.position * joint.local_axis1;
} else {
axis1 = rb1.position * joint.local_axis1;
axis2 = rb2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
axis2 = poss2.position * joint.local_axis2;
}
let axis_error = axis1.cross(&axis2);
let ang_err = basis2.tr_mul(&axis_error);
@@ -437,12 +459,12 @@ impl RevoluteVelocityGroundConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -460,9 +482,9 @@ impl RevoluteVelocityGroundConstraint {
let result = RevoluteVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
basis2,
inv_lhs,

250
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
@@ -39,41 +40,54 @@ impl WRevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
let local_basis1 = [
Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis1[0]]),
Vector::from(gather![|ii| joints[ii].basis1[1]]),
];
let local_basis2 = [
Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis2[0]]),
Vector::from(gather![|ii| joints[ii].basis2[1]]),
];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -124,10 +138,8 @@ impl WRevoluteVelocityConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -150,12 +162,12 @@ impl WRevoluteVelocityConstraint {
let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
let mut impulse = impulse * warmstart_coeff;
let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
let rotated_impulse = Vector::from(array![|ii| {
let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
let rotated_impulse = Vector::from(gather![|ii| {
let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
.unwrap_or_else(Rotation::identity);
axis_rot * joints[ii].world_ang_impulse
}; SIMD_WIDTH]);
}]);
let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
@@ -182,20 +194,16 @@ impl WRevoluteVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -225,20 +233,16 @@ impl WRevoluteVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -314,52 +318,76 @@ impl WRevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor2
} else {
joints[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor1
} else {
joints[ii].local_anchor2
}]);
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[0]
} else {
joints[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[1]
} else {
joints[ii].basis1[1]
}]),
]);
let basis2 = Matrix3x2::from_columns(&[
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[0]
} else {
joints[ii].basis2[0]
}]),
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[1]
} else {
joints[ii].basis2[1]
}]),
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
@@ -403,12 +431,16 @@ impl WRevoluteVelocityGroundConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis2
} else {
joints[ii].local_axis1
}]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis1
} else {
joints[ii].local_axis2
}]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -434,12 +466,10 @@ impl WRevoluteVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -458,12 +488,10 @@ impl WRevoluteVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -1,9 +1,10 @@
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use crate::data::ComponentSet;
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real};
use crate::utils::WAngularInertia;
@@ -150,13 +151,15 @@ impl ParallelIslandSolver {
}
}
pub fn solve_position_constraints<'s>(
pub fn solve_position_constraints<'s, Bodies>(
&'s mut self,
scope: &Scope<'s>,
island_id: usize,
params: &'s IntegrationParameters,
bodies: &'s mut RigidBodySet,
) {
bodies: &'s mut Bodies,
) where
Bodies: ComponentSet<RigidBody>,
{
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -179,7 +182,7 @@ impl ParallelIslandSolver {
// Transmute *mut -> &mut
let positions: &mut Vec<Isometry<Real>> =
unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
let bodies: &mut Bodies =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
@@ -197,8 +200,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
positions[rb.active_set_offset] = rb.next_position;
}
}
@@ -216,8 +219,8 @@ impl ParallelIslandSolver {
// Write results back to rigid bodies.
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.position_writeback_index) {
let rb = &mut bodies.index(handle.0);
rb.set_next_position(positions[rb.active_set_offset]);
}
}
@@ -225,17 +228,19 @@ impl ParallelIslandSolver {
}
}
pub fn init_constraints_and_solve_velocity_constraints<'s>(
pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>(
&'s mut self,
scope: &Scope<'s>,
island_id: usize,
params: &'s IntegrationParameters,
bodies: &'s mut RigidBodySet,
bodies: &'s mut Bodies,
manifolds: &'s mut Vec<&'s mut ContactManifold>,
manifold_indices: &'s [ContactManifoldIndex],
joints: &'s mut Vec<JointGraphEdge>,
joint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBody>,
{
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -280,7 +285,7 @@ impl ParallelIslandSolver {
// Transmute *mut -> &mut
let mj_lambdas: &mut Vec<DeltaVel<Real>> =
unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
let bodies: &mut Bodies =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let manifolds: &mut Vec<&mut ContactManifold> =
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -303,8 +308,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
let dvel = &mut mj_lambdas[rb.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
@@ -348,8 +353,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
let dvel = mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);

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

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::dynamics::{IntegrationParameters, JointGraphEdge};
use crate::geometry::ContactManifold;
#[cfg(feature = "simd-is-enabled")]
use crate::{
@@ -20,7 +20,7 @@ use std::sync::atomic::Ordering;
// pub fn init_constraint_groups(
// &mut self,
// island_id: usize,
// bodies: &RigidBodySet,
// bodies: &impl ComponentSet<RigidBody>,
// manifolds: &mut [&mut ContactManifold],
// manifold_groups: &ParallelInteractionGroups,
// joints: &mut [JointGraphEdge],
@@ -36,9 +36,9 @@ pub(crate) enum ConstraintDesc {
NongroundNongrouped(usize),
GroundNongrouped(usize),
#[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize; SIMD_WIDTH]),
NongroundGrouped([usize]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
GroundGrouped([usize]),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
@@ -78,7 +78,7 @@ macro_rules! impl_init_constraints_group {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
) {
@@ -144,7 +144,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
@@ -172,7 +172,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
@@ -223,7 +223,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
manifolds_all: &[&mut ContactManifold],
) {
let descs = &self.constraint_descs;
@@ -244,13 +244,13 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
@@ -265,7 +265,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
joints_all: &[JointGraphEdge],
) {
let descs = &self.constraint_descs;
@@ -290,7 +290,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
@@ -298,7 +298,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;

44
src/dynamics/solver/position_constraint.rs
View File

@@ -1,7 +1,8 @@
use crate::data::ComponentSet;
use crate::dynamics::solver::PositionGroundConstraint;
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -51,15 +52,26 @@ pub(crate) struct PositionConstraint {
}
impl PositionConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let ids1: &RigidBodyIds = bodies.index(handle1.0);
let ids2: &RigidBodyIds = bodies.index(handle2.0);
let poss1: &RigidBodyPosition = bodies.index(handle1.0);
let poss2: &RigidBodyPosition = bodies.index(handle2.0);
let mprops1: &RigidBodyMassProps = bodies.index(handle1.0);
let mprops2: &RigidBodyMassProps = bodies.index(handle2.0);
for (l, manifold_points) in manifold
.data
@@ -72,26 +84,28 @@ impl PositionConstraint {
let mut dists = [0.0; MAX_MANIFOLD_POINTS];
for l in 0..manifold_points.len() {
local_p1[l] = rb1
local_p1[l] = poss1
.position
.inverse_transform_point(&manifold_points[l].point);
local_p2[l] = rb2
local_p2[l] = poss2
.position
.inverse_transform_point(&manifold_points[l].point);
dists[l] = manifold_points[l].dist;
}
let constraint = PositionConstraint {
rb1: rb1.active_set_offset,
rb2: rb2.active_set_offset,
rb1: ids1.active_set_offset,
rb2: ids2.active_set_offset,
local_p1,
local_p2,
local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
local_n1: poss1
.position
.inverse_transform_vector(&manifold.data.normal),
dists,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
num_contacts: manifold_points.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

61
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -1,5 +1,5 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
};
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::data::ComponentSet;
use num::Zero;
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
@@ -28,39 +29,47 @@ pub(crate) struct WPositionConstraint {
}
impl WPositionConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let poss1: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let poss2: [&RigidBodyPosition; 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 pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let sqrt_ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let local_n1 = pos1.inverse_transform_vector(&Vector::from(
array![|ii| manifolds[ii].data.normal; SIMD_WIDTH],
));
let pos1 = Isometry::from(gather![|ii| poss1[ii].position]);
let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_n1 =
pos1.inverse_transform_vector(&Vector::from(gather![|ii| manifolds[ii].data.normal]));
let rb1 = gather![|ii| ids1[ii].active_set_offset];
let rb2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionConstraint {
@@ -80,8 +89,8 @@ impl WPositionConstraint {
};
for i in 0..num_points {
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
constraint.local_p1[i] = pos1.inverse_transform_point(&point);
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
@@ -99,8 +108,8 @@ impl WPositionConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let mut pos1 = Isometry::from(gather![|ii| positions[self.rb1[ii]]]);
let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
for k in 0..self.num_contacts as usize {

33
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -1,5 +1,6 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -21,24 +22,28 @@ pub(crate) struct PositionGroundConstraint {
}
impl PositionGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let flip = manifold.data.relative_dominance < 0;
let n1 = if flip {
std::mem::swap(&mut rb1, &mut rb2);
-manifold.data.normal
let (handle2, n1) = if flip {
(manifold.data.rigid_body1.unwrap(), -manifold.data.normal)
} else {
manifold.data.normal
(manifold.data.rigid_body2.unwrap(), manifold.data.normal)
};
let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) =
bodies.index_bundle(handle2.0);
for (l, manifold_contacts) in manifold
.data
.solver_contacts
@@ -51,20 +56,20 @@ impl PositionGroundConstraint {
for k in 0..manifold_contacts.len() {
p1[k] = manifold_contacts[k].point;
local_p2[k] = rb2
local_p2[k] = poss2
.position
.inverse_transform_point(&manifold_contacts[k].point);
dists[k] = manifold_contacts[k].dist;
}
let constraint = PositionGroundConstraint {
rb2: rb2.active_set_offset,
rb2: ids2.active_set_offset,
p1,
local_p2,
n1,
dists,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
num_contacts: manifold_contacts.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

54
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -1,5 +1,5 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
};
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::data::ComponentSet;
use num::Zero;
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
@@ -25,42 +26,51 @@ pub(crate) struct WPositionGroundConstraint {
}
impl WPositionGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rbs1 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let mut rbs2 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>,
{
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if manifolds[ii].data.relative_dominance < 0 {
flipped[ii] = true;
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
}
}
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let poss2: [&RigidBodyPosition; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let n1 = Vector::from(
array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],
);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let n1 = Vector::from(gather![|ii| if flipped[ii] {
-manifolds[ii].data.normal
} else {
manifolds[ii].data.normal
}]);
let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
let rb2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionGroundConstraint {
@@ -77,8 +87,8 @@ impl WPositionGroundConstraint {
};
for i in 0..num_points {
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
constraint.p1[i] = point;
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
@@ -96,7 +106,7 @@ impl WPositionGroundConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
for k in 0..self.num_contacts as usize {

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

@@ -1,5 +1,7 @@
use super::AnyJointPositionConstraint;
use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters};
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition};
use crate::math::{Isometry, Real};
pub(crate) struct PositionSolver {
@@ -13,25 +15,28 @@ impl PositionSolver {
}
}
pub fn solve(
pub fn solve<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
contact_constraints: &[AnyPositionConstraint],
joint_constraints: &[AnyJointPositionConstraint],
) {
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
{
if contact_constraints.is_empty() && joint_constraints.is_empty() {
// Nothing to do.
return;
}
self.positions.clear();
self.positions.extend(
bodies
.iter_active_island(island_id)
.map(|(_, b)| b.next_position),
);
self.positions
.extend(islands.active_island(island_id).iter().map(|h| {
let poss: &RigidBodyPosition = bodies.index(h.0);
poss.next_position
}));
for _ in 0..params.max_position_iterations {
for constraint in joint_constraints {
@@ -43,8 +48,10 @@ impl PositionSolver {
}
}
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.set_next_position(self.positions[rb.active_set_offset])
});
for handle in islands.active_island(island_id) {
let ids: &RigidBodyIds = bodies.index(handle.0);
let next_pos = &self.positions[ids.active_set_offset];
bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos);
}
}
}

152
src/dynamics/solver/solver_constraints.rs
View File

@@ -5,13 +5,16 @@ use super::{
use super::{
WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
};
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
@@ -50,13 +53,16 @@ impl<VelocityConstraint, PositionConstraint>
}
impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
pub fn init_constraint_groups(
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_contacts(
@@ -70,6 +76,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.not_ground_interactions,
@@ -78,6 +85,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.ground_interactions,
@@ -92,18 +100,25 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init(
pub fn init<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
self.velocity_constraints.clear();
self.position_constraints.clear();
self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices);
#[cfg(feature = "simd-is-enabled")]
{
@@ -118,19 +133,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
fn compute_grouped_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
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];
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityConstraint::generate(
params,
manifold_id,
@@ -149,12 +169,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
}
fn compute_nongrouped_constraints(
fn compute_nongrouped_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityConstraint::generate(
@@ -176,19 +201,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
fn compute_grouped_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
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];
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityGroundConstraint::generate(
params,
manifold_id,
@@ -207,12 +237,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
}
fn compute_nongrouped_ground_constraints(
fn compute_nongrouped_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityGroundConstraint::generate(
@@ -235,14 +270,21 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
pub fn init(
pub fn init<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>,
{
// Generate constraints for joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
@@ -260,6 +302,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
islands,
bodies,
joints,
&self.not_ground_interactions,
@@ -268,6 +311,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_joints(
island_id,
islands,
bodies,
joints,
&self.ground_interactions,
@@ -292,12 +336,18 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
}
fn compute_nongrouped_joint_ground_constraints(
fn compute_nongrouped_joint_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyIds>,
{
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
@@ -309,19 +359,25 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
fn compute_grouped_joint_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
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 joints_id = gather![|ii| joints_i[ii]];
let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
params, joints_id, joints, bodies,
);
@@ -332,12 +388,17 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
}
fn compute_nongrouped_joint_constraints(
fn compute_nongrouped_joint_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
@@ -349,19 +410,24 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
fn compute_grouped_joint_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
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 joints_id = gather![|ii| joints_i[ii]];
let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
let vel_constraint =
AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
self.velocity_constraints.push(vel_constraint);

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

@@ -1,7 +1,8 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -102,23 +103,32 @@ impl VelocityConstraint {
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
}
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
assert_eq!(manifold.data.relative_dominance, 0);
let inv_dt = params.inv_dt();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
let mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset;
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 force_dir1 = -manifold.data.normal;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
@@ -126,7 +136,7 @@ impl VelocityConstraint {
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let (tangents1, tangent_rot1) =
super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
for (_l, manifold_points) in manifold
.data
@@ -142,8 +152,8 @@ impl VelocityConstraint {
#[cfg(feature = "dim3")]
tangent_rot1,
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda1,
mj_lambda2,
@@ -190,8 +200,8 @@ impl VelocityConstraint {
constraint.tangent1 = tangents1[0];
constraint.tangent_rot1 = tangent_rot1;
}
constraint.im1 = rb1.effective_inv_mass;
constraint.im2 = rb2.effective_inv_mass;
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.limit = 0.0;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
@@ -202,11 +212,11 @@ impl VelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - rb1.world_com;
let dp2 = manifold_point.point - rb2.world_com;
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
let warmstart_correction;
@@ -215,16 +225,16 @@ impl VelocityConstraint {
// Normal part.
{
let gcross1 = rb1
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
/ (mprops1.effective_inv_mass
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -261,15 +271,15 @@ impl VelocityConstraint {
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
let gcross1 = rb1
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
/ (mprops1.effective_inv_mass
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let rhs =

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

@@ -1,7 +1,8 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -32,14 +33,18 @@ pub(crate) struct WVelocityConstraint {
}
impl WVelocityConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
for ii in 0..SIMD_WIDTH {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}
@@ -49,36 +54,39 @@ impl WVelocityConstraint {
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
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 linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -89,9 +97,8 @@ impl WVelocityConstraint {
super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii|
&manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH
];
let manifold_points =
gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityConstraint {
@@ -112,24 +119,20 @@ impl WVelocityConstraint {
};
for k in 0..num_points {
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let is_bouncy = SimdReal::from(
array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
);
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let is_resting = SimdReal::splat(1.0) - is_bouncy;
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
);
let prev_rhs =
SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -140,8 +143,7 @@ impl WVelocityConstraint {
let warmstart_correction;
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
{
@@ -172,15 +174,15 @@ impl WVelocityConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
) * warmstart_correction];
let impulse = [SimdReal::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
]) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
)
* na::Vector2::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
])
* warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
@@ -210,21 +212,17 @@ impl WVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityConstraintElement::warmstart_group(
@@ -250,21 +248,17 @@ impl WVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityConstraintElement::solve_group(

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

@@ -2,12 +2,13 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -28,35 +29,50 @@ pub(crate) struct VelocityGroundConstraint {
}
impl VelocityGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
let inv_dt = params.inv_dt();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let mut handle1 = manifold.data.rigid_body1;
let mut handle2 = manifold.data.rigid_body2;
let flipped = manifold.data.relative_dominance < 0;
let (force_dir1, flipped_multiplier) = if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let (tangents1, tangent_rot1) =
super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = rb2.active_set_offset;
let mj_lambda2 = ids2.active_set_offset;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (_l, manifold_points) in manifold
@@ -73,7 +89,7 @@ impl VelocityGroundConstraint {
#[cfg(feature = "dim3")]
tangent_rot1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -119,7 +135,7 @@ impl VelocityGroundConstraint {
constraint.tangent1 = tangents1[0];
constraint.tangent_rot1 = tangent_rot1;
}
constraint.im2 = rb2.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.limit = 0.0;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
@@ -129,10 +145,10 @@ impl VelocityGroundConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp2 = manifold_point.point - rb2.world_com;
let dp1 = manifold_point.point - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
let dp2 = manifold_point.point - mprops2.world_com;
let dp1 = manifold_point.point - world_com1;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
let warmstart_correction;
constraint.limit = manifold_point.friction;
@@ -140,11 +156,11 @@ impl VelocityGroundConstraint {
// Normal part.
{
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
@@ -178,10 +194,10 @@ impl VelocityGroundConstraint {
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);

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

@@ -2,7 +2,8 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -31,52 +32,71 @@ pub(crate) struct WVelocityGroundConstraint {
}
impl WVelocityGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
let mut flipped = [1.0; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if manifolds[ii].data.relative_dominance < 0 {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = -1.0;
}
}
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| *bodies.index(h.0))
.unwrap_or_else(RigidBodyVelocity::zero)
}];
let world_com1 = Point::from(gather![|ii| {
handles1[ii]
.map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).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)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let flipped_sign = SimdReal::from(flipped);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = normal1 * -flipped_sign;
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -88,7 +108,7 @@ impl WVelocityGroundConstraint {
super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityGroundConstraint {
@@ -107,24 +127,20 @@ impl WVelocityGroundConstraint {
};
for k in 0..num_points {
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let is_bouncy = SimdReal::from(
array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
);
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let is_resting = SimdReal::splat(1.0) - is_bouncy;
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
);
let prev_rhs =
SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -133,8 +149,7 @@ impl WVelocityGroundConstraint {
let warmstart_correction;
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
{
@@ -162,14 +177,14 @@ impl WVelocityGroundConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
) * warmstart_correction];
let impulse = [SimdReal::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
]) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
)
* na::Vector2::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
])
* warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
@@ -195,12 +210,10 @@ impl WVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityGroundConstraintElement::warmstart_group(
@@ -220,12 +233,10 @@ impl WVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityGroundConstraintElement::solve_group(

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

@@ -1,8 +1,10 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, RigidBodySet,
IntegrationParameters, JointGraphEdge, RigidBodyForces, RigidBodyVelocity,
};
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::WAngularInertia;
@@ -18,31 +20,38 @@ impl VelocitySolver {
}
}
pub fn solve(
pub fn solve<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
joint_constraints: &mut [AnyJointVelocityConstraint],
) {
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
self.mj_lambdas.clear();
self.mj_lambdas
.resize(bodies.active_island(island_id).len(), DeltaVel::zero());
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
// Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
let dvel = &mut self.mj_lambdas[rb.active_set_offset];
for handle in islands.active_island(island_id) {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
rb.force = na::zero();
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
// dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
rb.torque = na::zero();
});
// 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 * (mprops.effective_inv_mass * params.dt);
}
/*
* Warmstart constraints.
@@ -69,13 +78,19 @@ impl VelocitySolver {
}
// Update velocities.
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
let dvel = self.mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb
for handle in islands.active_island(island_id) {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
});
bodies.map_mut_internal(handle.0, |vels| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
});
}
// Write impulses back into the manifold structures.
for constraint in &*joint_constraints {