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

Outsource the contact manifold, SAT, and some shapes.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-08 17:31:49 +01:00
parent fd3b4801b6
commit 9bf1321f8f
62 changed files with 552 additions and 2904 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dynamics/rigid_body_set.rs
View File

@@ -479,7 +479,7 @@ impl RigidBodySet {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
if manifold.num_active_contacts > 0 {
let other = crate::utils::other_handle(
(inter.0, inter.1),
*collider_handle,

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

@@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts(
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
match manifold.kinematics.category {
@@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts(
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
out_ground.push(*manifold_i)

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

@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
impl<'a> PairInteraction for &'a mut ContactManifold {
fn body_pair(&self) -> BodyPair {
self.body_pair
self.data.body_pair
}
}
@@ -77,7 +77,7 @@ impl ParallelInteractionGroups {
.iter()
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].body_pair();
let body_pair = interactions[*interaction_id].data.body_pair();
let rb1 = &bodies[body_pair.body1];
let rb2 = &bodies[body_pair.body2];
@@ -338,7 +338,7 @@ impl InteractionGroups {
let mut occupied_mask = 0u128;
let max_interaction_points = interaction_indices
.iter()
.map(|i| interactions[*i].num_active_contacts())
.map(|i| interactions[*i].num_active_contacts)
.max()
.unwrap_or(1);
@@ -351,12 +351,12 @@ impl InteractionGroups {
// FIXME: how could we avoid iterating
// on each interaction at every iteration on k?
if interaction.num_active_contacts() != k {
if interaction.num_active_contacts != k {
continue;
}
let body1 = &bodies[interaction.body_pair.body1];
let body2 = &bodies[interaction.body_pair.body2];
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();

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

@@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
VelocityConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
self.constraints[joints[0].constraint_index] = constraint;
self.constraints[joints[0].data.constraint_index] = constraint;
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
self.constraints[joints[0].constraint_index] = constraint;
self.constraints[joints[0].data.constraint_index] = constraint;
}
}
}

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

@@ -88,8 +88,8 @@ impl PositionConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
let radius =
@@ -104,8 +104,8 @@ impl PositionConstraint {
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
for l in 0..manifold_points.len() {
local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
}
let constraint = PositionConstraint {
@@ -132,10 +132,10 @@ impl PositionConstraint {
}
} else {
if manifold.kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPointPoint(constraint);
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPlanePoint(constraint);
}
}

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

@@ -35,8 +35,8 @@ impl WPositionConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
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];
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdFloat> =
@@ -51,8 +51,8 @@ impl WPositionConstraint {
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -100,10 +100,10 @@ impl WPositionConstraint {
}
} else {
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPointPoint(constraint);
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPlanePoint(constraint);
}
}

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

@@ -28,8 +28,8 @@ impl PositionGroundConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.body_pair.body1];
let mut rb2 = &bodies[manifold.body_pair.body2];
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flip = !rb2.is_dynamic();
let local_n1;
@@ -41,13 +41,13 @@ impl PositionGroundConstraint {
std::mem::swap(&mut rb1, &mut rb2);
local_n1 = manifold.local_n2;
local_n2 = manifold.local_n1;
delta1 = &manifold.delta2;
delta2 = &manifold.delta1;
delta1 = &manifold.data.delta2;
delta2 = &manifold.data.delta1;
} else {
local_n1 = manifold.local_n1;
local_n2 = manifold.local_n2;
delta1 = &manifold.delta1;
delta2 = &manifold.delta2;
delta1 = &manifold.data.delta1;
delta2 = &manifold.data.delta2;
};
let coll_pos1 = rb1.position * delta1;
@@ -105,10 +105,10 @@ impl PositionGroundConstraint {
}
} else {
if manifold.kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPointPointGround(constraint);
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPlanePointGround(constraint);
}
}

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

@@ -32,8 +32,10 @@ impl WPositionGroundConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
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];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -55,10 +57,10 @@ impl WPositionGroundConstraint {
);
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
@@ -112,10 +114,10 @@ impl WPositionGroundConstraint {
}
} else {
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPointPointGround(constraint);
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPlanePointGround(constraint);
}
}

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

@@ -144,14 +144,14 @@ impl VelocityConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
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 pos_coll1 = rb1.position * manifold.delta1;
let pos_coll2 = rb2.position * manifold.delta2;
let pos_coll1 = rb1.position * manifold.data.delta1;
let pos_coll2 = rb2.position * manifold.data.delta2;
let force_dir1 = pos_coll1 * (-manifold.local_n1);
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
.active_contacts()
@@ -164,7 +164,7 @@ impl VelocityConstraint {
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
limit: manifold.friction,
limit: manifold.data.friction,
mj_lambda1,
mj_lambda2,
manifold_id,
@@ -207,7 +207,7 @@ impl VelocityConstraint {
constraint.dir1 = force_dir1;
constraint.im1 = rb1.mass_properties.inv_mass;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.limit = manifold.friction;
constraint.limit = manifold.data.friction;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
@@ -241,12 +241,12 @@ impl VelocityConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.restitution * rhs
rhs += manifold.data.restitution * rhs
}
rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].impulse * warmstart_coeff;
let impulse = manifold_points[k].data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityConstraintElementPart {
gcross1,
@@ -275,9 +275,9 @@ impl VelocityConstraint {
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
gcross1,
@@ -294,7 +294,7 @@ impl VelocityConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyVelocityConstraint::Nongrouped(constraint);
}
}
@@ -388,15 +388,15 @@ impl VelocityConstraint {
for k in 0..self.num_contacts as usize {
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse = [
active_contacts[k_base + k].data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

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

@@ -69,11 +69,11 @@ impl WVelocityConstraint {
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
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 delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdFloat> =
@@ -103,13 +103,13 @@ impl WVelocityConstraint {
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 friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
@@ -140,7 +140,7 @@ impl WVelocityConstraint {
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -176,11 +176,11 @@ impl WVelocityConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::Grouped(constraint);
}
}
@@ -342,15 +342,15 @@ impl WVelocityConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = impulses[ii];
active_contacts[k_base + k].data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}

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

@@ -63,26 +63,26 @@ impl VelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.body_pair.body1];
let mut rb2 = &bodies[manifold.body_pair.body2];
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flipped = !rb2.is_dynamic();
let force_dir1;
let coll_pos1;
let coll_pos2;
if flipped {
coll_pos1 = rb2.position * manifold.delta2;
coll_pos2 = rb1.position * manifold.delta1;
coll_pos1 = rb2.position * manifold.data.delta2;
coll_pos2 = rb1.position * manifold.data.delta1;
force_dir1 = coll_pos1 * (-manifold.local_n2);
std::mem::swap(&mut rb1, &mut rb2);
} else {
coll_pos1 = rb1.position * manifold.delta1;
coll_pos2 = rb2.position * manifold.delta2;
coll_pos1 = rb1.position * manifold.data.delta1;
coll_pos2 = rb2.position * manifold.data.delta2;
force_dir1 = coll_pos1 * (-manifold.local_n1);
}
let mj_lambda2 = rb2.active_set_offset;
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
.active_contacts()
@@ -94,7 +94,7 @@ impl VelocityGroundConstraint {
dir1: force_dir1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.mass_properties.inv_mass,
limit: manifold.friction,
limit: manifold.data.friction,
mj_lambda2,
manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
@@ -135,7 +135,7 @@ impl VelocityGroundConstraint {
{
constraint.dir1 = force_dir1;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.limit = manifold.friction;
constraint.limit = manifold.data.friction;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
@@ -173,12 +173,12 @@ impl VelocityGroundConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.restitution * rhs
rhs += manifold.data.restitution * rhs
}
rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].impulse * warmstart_coeff;
let impulse = manifold_points[k].data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
gcross2,
@@ -199,9 +199,9 @@ impl VelocityGroundConstraint {
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
constraint.elements[k].tangent_part[j] =
VelocityGroundConstraintElementPart {
@@ -218,7 +218,7 @@ impl VelocityGroundConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyVelocityConstraint::NongroupedGround(constraint);
}
}
@@ -290,15 +290,15 @@ impl VelocityGroundConstraint {
for k in 0..self.num_contacts as usize {
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse = [
active_contacts[k_base + k].data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

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

@@ -65,8 +65,8 @@ impl WVelocityGroundConstraint {
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
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 flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -90,10 +90,10 @@ impl WVelocityGroundConstraint {
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let coll_pos1 = pos1 * delta1;
@@ -109,13 +109,13 @@ impl WVelocityGroundConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
@@ -146,7 +146,7 @@ impl WVelocityGroundConstraint {
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -178,11 +178,11 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityGroundConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::GroupedGround(constraint);
}
}
@@ -302,15 +302,15 @@ impl WVelocityGroundConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = impulses[ii];
active_contacts[k_base + k].data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}