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

feat: implement new "small-steps" solver + joint improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2024-01-21 21:02:23 +01:00
parent 9ac3503b87
commit 9b87f06a85
76 changed files with 6672 additions and 4305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples2d/one_way_platforms2.rs
View File

@@ -90,7 +90,7 @@ pub fn init_world(testbed: &mut Testbed) {
* depending on their position.
*/
testbed.add_callback(move |graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
if run_state.timestep_id % 200 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.5, 2.0);
let body = RigidBodyBuilder::dynamic().translation(vector![20.0, 10.0]);

4
examples2d/rope_joints2.rs
View File

@@ -51,9 +51,7 @@ pub fn init_world(testbed: &mut Testbed) {
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, child_handle, &mut bodies);
let joint = RopeJointBuilder::new()
.local_anchor2(point![0.0, 0.0])
.limits([2.0, 2.0]);
let joint = RopeJointBuilder::new(2.0).local_anchor2(point![0.0, 0.0]);
impulse_joints.insert(character_handle, child_handle, joint, true);
/*

16
examples3d/all_examples3.rs
View File

@@ -34,7 +34,10 @@ mod heightfield3;
mod joints3;
// mod joints3;
mod character_controller3;
mod debug_chain_high_mass_ratio3;
mod debug_cube_high_mass_ratio3;
mod debug_internal_edges3;
mod debug_long_chain3;
mod joint_motor_position3;
mod keva3;
mod locked_rotations3;
@@ -45,8 +48,10 @@ mod primitives3;
mod restitution3;
mod rope_joints3;
mod sensor3;
mod spring_joints3;
mod trimesh3;
mod vehicle_controller3;
mod vehicle_joints3;
fn demo_name_from_command_line() -> Option<String> {
let mut args = std::env::args();
@@ -105,8 +110,10 @@ pub fn main() {
("Restitution", restitution3::init_world),
("Rope Joints", rope_joints3::init_world),
("Sensor", sensor3::init_world),
("Spring Joints", spring_joints3::init_world),
("TriMesh", trimesh3::init_world),
("Vehicle controller", vehicle_controller3::init_world),
("Vehicle joints", vehicle_joints3::init_world),
("Keva tower", keva3::init_world),
("Newton cradle", newton_cradle3::init_world),
("(Debug) multibody_joints", debug_articulations3::init_world),
@@ -122,6 +129,15 @@ pub fn main() {
),
("(Debug) friction", debug_friction3::init_world),
("(Debug) internal edges", debug_internal_edges3::init_world),
("(Debug) long chain", debug_long_chain3::init_world),
(
"(Debug) high mass ratio: chain",
debug_chain_high_mass_ratio3::init_world,
),
(
"(Debug) high mass ratio: cube",
debug_cube_high_mass_ratio3::init_world,
),
("(Debug) triangle", debug_triangle3::init_world),
("(Debug) trimesh", debug_trimesh3::init_world),
("(Debug) cylinder", debug_cylinder3::init_world),

73
examples3d/debug_chain_high_mass_ratio3.rs Normal file
View File

@@ -0,0 +1,73 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let use_articulations = false;
/*
* Create a chain with a very heavy ball at the end.
*/
let num = 17;
let rad = 0.2;
let mut body_handles = Vec::new();
for i in 0..num {
let fi = i as f32;
let status = if i == 0 {
RigidBodyType::Fixed
} else {
RigidBodyType::Dynamic
};
let ball_rad = if i == num - 1 { rad * 10.0 } else { rad };
let shift1 = rad * 1.1;
let shift2 = ball_rad + rad * 0.1;
let z = if i == 0 {
0.0
} else {
(fi - 1.0) * 2.0 * shift1 + shift1 + shift2
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![0.0, 0.0, z])
.additional_solver_iterations(16);
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(ball_rad);
colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = if i == 1 {
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift1 * 2.0])
} else {
SphericalJointBuilder::new()
.local_anchor1(point![0.0, 0.0, shift1])
.local_anchor2(point![0.0, 0.0, -shift2])
};
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint, true);
} else {
impulse_joints.insert(parent_handle, child_handle, joint, true);
}
}
body_handles.push(child_handle);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
}

96
examples3d/debug_cube_high_mass_ratio3.rs Normal file
View File

@@ -0,0 +1,96 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let num_levels = 4;
let stick_len = 2.0;
let stick_rad = 0.2;
/*
* Floor.
*/
let floor_body =
RigidBodyBuilder::fixed().translation(vector![0.0, -stick_len - stick_rad, 0.0]);
let floor_handle = bodies.insert(floor_body);
let floor_cube = ColliderBuilder::cuboid(stick_len, stick_len, stick_len);
colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies);
/*
* Create a stack of capsule with a very heavy cube on top.
*/
for i in 0..num_levels {
let fi = i as f32;
let body = RigidBodyBuilder::dynamic().translation(vector![
0.0,
fi * stick_rad * 4.0,
-(stick_len / 2.0 - stick_rad)
]);
let handle = bodies.insert(body);
let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad);
colliders.insert_with_parent(capsule, handle, &mut bodies);
let body = RigidBodyBuilder::dynamic().translation(vector![
0.0,
fi * stick_rad * 4.0,
(stick_len / 2.0 - stick_rad)
]);
let handle = bodies.insert(body);
let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad);
colliders.insert_with_parent(capsule, handle, &mut bodies);
let body = RigidBodyBuilder::dynamic().translation(vector![
-(stick_len / 2.0 - stick_rad),
(fi + 0.5) * stick_rad * 4.0,
0.0
]);
let handle = bodies.insert(body);
let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0);
colliders.insert_with_parent(capsule, handle, &mut bodies);
let body = RigidBodyBuilder::dynamic().translation(vector![
(stick_len / 2.0 - stick_rad),
(fi + 0.5) * stick_rad * 4.0,
0.0
]);
let handle = bodies.insert(body);
let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0);
colliders.insert_with_parent(capsule, handle, &mut bodies);
}
/*
* Big cube on top.
*/
let cube_len = stick_len * 2.0;
let floor_body = RigidBodyBuilder::dynamic()
.translation(vector![
0.0,
cube_len / 2.0 + (num_levels as f32 - 0.25) * stick_rad * 4.0,
0.0
])
.additional_solver_iterations(36);
let floor_handle = bodies.insert(floor_body);
let floor_cube = ColliderBuilder::cuboid(cube_len / 2.0, cube_len / 2.0, cube_len / 2.0);
colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies);
let small_mass =
MassProperties::from_cuboid(1.0, vector![stick_rad, stick_rad, stick_len / 2.0]).mass();
let big_mass =
MassProperties::from_cuboid(1.0, vector![cube_len / 2.0, cube_len / 2.0, cube_len / 2.0])
.mass();
println!("debug_cube_high_mass_ratio3: small stick mass: {small_mass}, big cube mass: {big_mass}, mass_ratio: {}", big_mass / small_mass);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
}

63
examples3d/debug_long_chain3.rs Normal file
View File

@@ -0,0 +1,63 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let use_articulations = false;
/*
* Create the long chain.
*/
let num = 100;
let rad = 0.2;
let shift = rad * 2.2;
let mut body_handles = Vec::new();
for i in 0..num {
let fi = i as f32;
let status = if i == 0 {
RigidBodyType::Fixed
} else {
RigidBodyType::Dynamic
};
let rigid_body = RigidBodyBuilder::new(status).translation(vector![0.0, 0.0, fi * shift]);
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = if i == 1 {
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift])
} else {
SphericalJointBuilder::new()
.local_anchor1(point![0.0, 0.0, shift / 2.0])
.local_anchor2(point![0.0, 0.0, -shift / 2.0])
};
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint, true);
} else {
impulse_joints.insert(parent_handle, child_handle, joint, true);
}
}
body_handles.push(child_handle);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
}

1
examples3d/joints3.rs
View File

@@ -20,7 +20,6 @@ fn create_coupled_joints(
let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
.limits(JointAxis::X, [-3.0, 3.0])
.limits(JointAxis::Y, [0.0, 3.0])
.limits(JointAxis::Z, [0.0, 3.0])
.coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
if use_articulations {

2
examples3d/one_way_platforms3.rs
View File

@@ -90,7 +90,7 @@ pub fn init_world(testbed: &mut Testbed) {
* depending on their position.
*/
testbed.add_callback(move |graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
if run_state.timestep_id % 200 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5);
let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 6.0, 20.0]);

24
examples3d/platform3.rs
View File

@@ -29,12 +29,12 @@ pub fn init_world(testbed: &mut Testbed) {
let shift = rad * 2.0;
let centerx = shift * num as f32 / 2.0;
let centery = shift / 2.0 + 3.04;
let centerz = shift * num as f32 / 2.0;
for i in 0usize..num {
for j in 0usize..num {
for k in 0usize..num {
let centery = if j >= num / 2 { 5.0 } else { 3.0 };
let x = i as f32 * shift - centerx;
let y = j as f32 * shift + centery;
let z = k as f32 * shift - centerz;
@@ -51,11 +51,8 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Setup a velocity-based kinematic rigid body.
*/
let platform_body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![
0.0,
1.5 + 0.8,
-10.0 * rad
]);
let platform_body =
RigidBodyBuilder::kinematic_velocity_based().translation(vector![0.0, 1.5 + 0.8, 0.0]);
let velocity_based_platform_handle = bodies.insert(platform_body);
let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0);
colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
@@ -65,8 +62,8 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let platform_body = RigidBodyBuilder::kinematic_position_based().translation(vector![
0.0,
2.0 + 1.5 + 0.8,
-10.0 * rad
3.0 + 1.5 + 0.8,
0.0
]);
let position_based_platform_handle = bodies.insert(platform_body);
let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0);
@@ -75,22 +72,17 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Setup a callback to control the platform.
*/
let mut count = 0;
testbed.add_callback(move |_, physics, _, run_state| {
count += 1;
if count % 100 > 50 {
return;
}
let velocity = vector![
0.0,
(run_state.time * 5.0).sin(),
run_state.time.sin() * 5.0
(run_state.time * 2.0).sin(),
run_state.time.sin() * 2.0
];
// Update the velocity-based kinematic body by setting its velocity.
if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) {
platform.set_linvel(velocity, true);
platform.set_angvel(vector![0.0, 0.2, 0.0], true);
}
// Update the position-based kinematic body by setting its next position.

4
examples3d/rope_joints3.rs
View File

@@ -80,9 +80,7 @@ pub fn init_world(testbed: &mut Testbed) {
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, child_handle, &mut bodies);
let joint = RopeJointBuilder::new()
.local_anchor2(point![0.0, 0.0, 0.0])
.limits([2.0, 2.0]);
let joint = RopeJointBuilder::new(2.0);
impulse_joints.insert(character_handle, child_handle, joint, true);
/*

64
examples3d/spring_joints3.rs Normal file
View File

@@ -0,0 +1,64 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Fixed ground to attach one end of the joints.
*/
let rigid_body = RigidBodyBuilder::fixed();
let ground_handle = bodies.insert(rigid_body);
/*
* Spring joints with a variety of spring parameters.
* The middle one has uses critical damping.
*/
let num = 30;
let radius = 0.5;
let mass = Ball::new(radius).mass_properties(1.0).mass();
let stiffness = 1.0e3;
let critical_damping = 2.0 * (stiffness * mass).sqrt();
for i in 0..=num {
let x_pos = -6.0 + 1.5 * i as f32;
let ball_pos = point![x_pos, 4.5, 0.0];
let rigid_body = RigidBodyBuilder::dynamic()
.translation(ball_pos.coords)
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(radius);
colliders.insert_with_parent(collider, handle, &mut bodies);
let damping_ratio = i as f32 / (num as f32 / 2.0);
let damping = damping_ratio * critical_damping;
let joint = SpringJointBuilder::new(0.0, stiffness, damping)
.local_anchor1(ball_pos - Vector::y() * 3.0);
impulse_joints.insert(ground_handle, handle, joint, true);
// Box that will fall on to of the springed balls, makes the simulation funier to watch.
let rigid_body =
RigidBodyBuilder::dynamic().translation(ball_pos.coords + Vector::y() * 5.0);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(radius, radius, radius).density(100.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world_with_params(
bodies,
colliders,
impulse_joints,
multibody_joints,
vector![0.0, -9.81, 0.0],
(),
);
testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
}

227
examples3d/vehicle_joints3.rs Normal file
View File

@@ -0,0 +1,227 @@
use rapier3d::prelude::*;
use rapier_testbed3d::{KeyCode, Testbed};
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground.
*/
let ground_size = Vector::new(60.0, 0.4, 60.0);
let nsubdivs = 100;
let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
-(i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos()
- (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos()
});
let collider = ColliderBuilder::heightfield(heights, ground_size)
.translation(vector![-7.0, 0.0, 0.0])
.friction(1.0);
colliders.insert(collider);
/*
* Vehicle we will control manually, simulated using joints.
* Strongly inspired from https://github.com/h3r2tic/cornell-mcray/blob/main/src/car.rs (MIT/Apache license).
*/
const CAR_GROUP: Group = Group::GROUP_1;
let wheel_params = [
vector![0.6874, 0.2783, -0.7802],
vector![-0.6874, 0.2783, -0.7802],
vector![0.64, 0.2783, 1.0254],
vector![-0.64, 0.2783, 1.0254],
];
// TODO: lower center of mass?
// let mut center_of_mass = wheel_params.iter().sum().unwrap() / 4.0;
// center_of_mass.y = 0.0;
let suspension_height = 0.12;
let max_steering_angle = 35.0f32.to_radians();
let drive_strength = 1.0;
let wheel_radius = 0.28;
let car_position = point![0.0, wheel_radius + suspension_height, 0.0];
let body_position_in_car_space = vector![0.0, 0.4739, 0.0];
let body_position = car_position + body_position_in_car_space;
let body_co = ColliderBuilder::cuboid(0.65, 0.3, 0.9)
.density(100.0)
.collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP));
let body_rb = RigidBodyBuilder::dynamic()
.position(body_position.into())
.build();
let body_handle = bodies.insert(body_rb);
colliders.insert_with_parent(body_co, body_handle, &mut bodies);
let mut steering_joints = vec![];
let mut motor_joints = vec![];
for (wheel_id, wheel_pos_in_car_space) in wheel_params.into_iter().enumerate() {
let is_front = wheel_id >= 2;
let wheel_center = car_position + wheel_pos_in_car_space;
let axle_mass_props = MassProperties::from_ball(100.0, wheel_radius);
let axle_rb = RigidBodyBuilder::dynamic()
.position(wheel_center.into())
.additional_mass_properties(axle_mass_props);
let axle_handle = bodies.insert(axle_rb);
// This is a fake cylinder collider that we add only because our testbed can
// only render colliders. Setting it as sensor makes it show up as wireframe.
let wheel_fake_co = ColliderBuilder::cylinder(wheel_radius / 2.0, wheel_radius)
.rotation(Vector::z() * std::f32::consts::FRAC_PI_2)
.sensor(true)
.density(0.0)
.collision_groups(InteractionGroups::none());
// The actual wheel collider. Simulating the wheel as a ball is interesting as it
// is mathematically simpler than a cylinder and cheaper to compute for collision-detection.
let wheel_co = ColliderBuilder::ball(wheel_radius)
.density(100.0)
.collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP))
.friction(1.0);
let wheel_rb = RigidBodyBuilder::dynamic().position(wheel_center.into());
let wheel_handle = bodies.insert(wheel_rb);
colliders.insert_with_parent(wheel_co, wheel_handle, &mut bodies);
colliders.insert_with_parent(wheel_fake_co, wheel_handle, &mut bodies);
let suspension_attachment_in_body_space =
wheel_pos_in_car_space - body_position_in_car_space;
// Suspension between the body and the axle.
let mut locked_axes =
JointAxesMask::X | JointAxesMask::Z | JointAxesMask::ANG_X | JointAxesMask::ANG_Z;
if !is_front {
locked_axes |= JointAxesMask::ANG_Y;
}
let mut suspension_joint = GenericJointBuilder::new(locked_axes)
.limits(JointAxis::Y, [0.0, suspension_height])
.motor_position(JointAxis::Y, 0.0, 1.0e4, 1.0e3)
.local_anchor1(suspension_attachment_in_body_space.into());
if is_front {
suspension_joint =
suspension_joint.limits(JointAxis::AngY, [-max_steering_angle, max_steering_angle]);
}
let body_axle_joint_handle =
impulse_joints.insert(body_handle, axle_handle, suspension_joint, true);
// Joint between the axle and the wheel.
let wheel_joint = RevoluteJointBuilder::new(Vector::x_axis());
let wheel_joint_handle =
impulse_joints.insert(axle_handle, wheel_handle, wheel_joint, true);
if is_front {
steering_joints.push(body_axle_joint_handle);
motor_joints.push(wheel_joint_handle);
}
}
/*
* Callback to control the wheels motors.
*/
testbed.add_callback(move |gfx, physics, _, _| {
let Some(gfx) = gfx else { return };
let mut thrust = 0.0;
let mut steering = 0.0;
let mut boost = 1.0;
for key in gfx.keys().get_pressed() {
match *key {
KeyCode::Right => {
steering = -1.0;
}
KeyCode::Left => {
steering = 1.0;
}
KeyCode::Up => {
thrust = -drive_strength;
}
KeyCode::Down => {
thrust = drive_strength;
}
KeyCode::ShiftRight => {
boost = 1.5;
}
_ => {}
}
}
if thrust != 0.0 || steering != 0.0 {
physics.bodies.get_mut(body_handle).unwrap().wake_up(true);
}
// Apply steering to the axles.
for steering_handle in &steering_joints {
let steering_joint = physics.impulse_joints.get_mut(*steering_handle).unwrap();
steering_joint.data.set_motor_position(
JointAxis::AngY,
max_steering_angle * steering,
1.0e4,
1.0e3,
);
}
// Apply thrust.
// Pseudo-differential adjusting speed of engines depending on steering arc
// Higher values result in more drifty behavior.
let differential_strength = 0.5;
let sideways_shift = (max_steering_angle * steering).sin() * differential_strength;
let speed_diff = if sideways_shift > 0.0 {
f32::hypot(1.0, sideways_shift)
} else {
1.0 / f32::hypot(1.0, sideways_shift)
};
let ms = [1.0 / speed_diff, speed_diff];
for (motor_handle, &ms) in motor_joints.iter().copied().zip(ms.iter()) {
let motor_joint = physics.impulse_joints.get_mut(motor_handle).unwrap();
motor_joint.data.set_motor_velocity(
JointAxis::AngX,
-30.0 * thrust * ms * boost,
1.0e2,
);
}
});
/*
* Create some cubes on the ground.
*/
// let num = 8;
// let rad = 0.1;
//
// let shift = rad * 2.0;
// let centerx = shift * (num / 2) as f32;
// let centery = rad;
//
// for j in 0usize..1 {
// for k in 0usize..4 {
// for i in 0..num {
// let x = i as f32 * shift - centerx;
// let y = j as f32 * shift + centery;
// let z = k as f32 * shift + centerx;
//
// let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
// let handle = bodies.insert(rigid_body);
// let collider = ColliderBuilder::cuboid(rad, rad, rad);
// colliders.insert_with_parent(collider, handle, &mut bodies);
// }
// }
// }
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin());
}

2
src/control/ray_cast_vehicle_controller.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, Ray};
use crate::math::{Point, Real, Rotation, Vector};
use crate::pipeline::{QueryFilter, QueryPipeline};
use crate::utils::{WCross, WDot};
use crate::utils::{SimdCross, SimdDot};
/// A character controller to simulate vehicles using ray-casting for the wheels.
pub struct DynamicRayCastVehicleController {

3
src/dynamics/ccd/mod.rs
View File

@@ -1,3 +1,6 @@
// TODO: not sure why it complains about PredictedImpacts being unused,
// making it private or pub(crate) triggers a different error.
#[allow(unused_imports)]
pub use self::ccd_solver::{CCDSolver, PredictedImpacts};
pub use self::toi_entry::TOIEntry;

26
src/dynamics/integration_parameters.rs
View File

@@ -1,4 +1,5 @@
use crate::math::Real;
use std::num::NonZeroUsize;
/// Parameters for a time-step of the physics engine.
#[derive(Copy, Clone, Debug)]
@@ -43,15 +44,10 @@ pub struct IntegrationParameters {
pub max_penetration_correction: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
pub prediction_distance: Real,
/// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`).
pub max_velocity_iterations: usize,
/// Maximum number of iterations performed to solve friction constraints (default: `8`).
pub max_velocity_friction_iterations: usize,
/// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`).
pub max_stabilization_iterations: usize,
/// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise,
/// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`).
pub interleave_restitution_and_friction_resolution: bool,
/// Number of iterations performed to solve friction constraints at solver iteration (default: `2`).
pub num_friction_iteration_per_solver_iteration: usize,
/// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
pub num_solver_iterations: NonZeroUsize,
/// Minimum number of dynamic bodies in each active island (default: `128`).
pub min_island_size: usize,
/// Maximum number of substeps performed by the solver (default: `1`).
@@ -151,17 +147,15 @@ impl Default for IntegrationParameters {
Self {
dt: 1.0 / 60.0,
min_ccd_dt: 1.0 / 60.0 / 100.0,
erp: 0.8,
damping_ratio: 0.25,
erp: 0.6,
damping_ratio: 1.0,
joint_erp: 1.0,
joint_damping_ratio: 1.0,
allowed_linear_error: 0.001, // 0.005
allowed_linear_error: 0.001,
max_penetration_correction: Real::MAX,
prediction_distance: 0.002,
max_velocity_iterations: 4,
max_velocity_friction_iterations: 8,
max_stabilization_iterations: 1,
interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
num_friction_iteration_per_solver_iteration: 2,
num_solver_iterations: NonZeroUsize::new(4).unwrap(),
// TODO: what is the optimal value for min_island_size?
// It should not be too big so that we don't end up with
// huge islands that don't fit in cache.

28
src/dynamics/island_manager.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::{
};
use crate::geometry::{ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::utils::WDot;
use crate::utils::SimdDot;
/// Structure responsible for maintaining the set of active rigid-bodies, and
/// putting non-moving rigid-bodies to sleep to save computation times.
@@ -14,6 +14,7 @@ pub struct IslandManager {
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
active_set_timestamp: u32,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>, // Workspace.
@@ -28,6 +29,7 @@ impl IslandManager {
active_dynamic_set: vec![],
active_kinematic_set: vec![],
active_islands: vec![],
active_islands_additional_solver_iterations: vec![],
active_set_timestamp: 0,
can_sleep: vec![],
stack: vec![],
@@ -127,6 +129,10 @@ impl IslandManager {
&self.active_dynamic_set[island_range]
}
pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
self.active_islands_additional_solver_iterations[island_id]
}
#[inline(always)]
pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
self.active_dynamic_set
@@ -232,12 +238,21 @@ impl IslandManager {
// let t = instant::now();
// Propagation of awake state and awake island computation through the
// traversal of the interaction graph.
self.active_islands_additional_solver_iterations.clear();
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;
// NOTE: islands containing a body with non-standard number of iterations won’t
// be merged with another island, unless another island with standard
// iterations number already started before and got continued due to the
// `min_island_size`. That could be avoided by pushing bodies with non-standard
// iterations on top of the stack (and other bodies on the back). Not sure it’s
// worth it though.
let mut additional_solver_iterations = 0;
while let Some(handle) = self.stack.pop() {
let rb = bodies.index_mut_internal(handle);
@@ -248,16 +263,23 @@ impl IslandManager {
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
if additional_solver_iterations != rb.additional_solver_iterations
|| self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
// We are starting a new island.
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_dynamic_set.len());
additional_solver_iterations = 0;
}
island_marker = self.stack.len();
}
additional_solver_iterations =
additional_solver_iterations.max(rb.additional_solver_iterations);
// 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);
@@ -281,6 +303,8 @@ impl IslandManager {
self.active_dynamic_set.push(handle);
}
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_dynamic_set.len());
// println!(
// "Extraction: {}, num islands: {}",

25
src/dynamics/joint/generic_joint.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
use crate::utils::{WBasis, WReal};
use crate::utils::{SimdBasis, SimdRealCopy};
#[cfg(feature = "dim3")]
use crate::dynamics::SphericalJoint;
@@ -121,7 +121,7 @@ pub struct JointLimits<N> {
pub impulse: N,
}
impl<N: WReal> Default for JointLimits<N> {
impl<N: SimdRealCopy> Default for JointLimits<N> {
fn default() -> Self {
Self {
min: -N::splat(Real::MAX),
@@ -131,6 +131,16 @@ impl<N: WReal> Default for JointLimits<N> {
}
}
impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
fn from(value: [N; 2]) -> Self {
Self {
min: value[0],
max: value[1],
impulse: N::splat(0.0),
}
}
}
/// A joint’s motor along one of its degrees of freedom.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -210,14 +220,23 @@ pub struct GenericJoint {
/// The degrees-of-freedoms motorised by this joint.
pub motor_axes: JointAxesMask,
/// The coupled degrees of freedom of this joint.
///
/// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors.
/// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first
/// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
/// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
pub coupled_axes: JointAxesMask,
/// The limits, along each degrees of freedoms of this joint.
///
/// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
/// bitmask is applied to the coupled linear (resp. angular) axes.
pub limits: [JointLimits<Real>; SPATIAL_DIM],
/// The motors, along each degrees of freedoms of this joint.
///
/// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
/// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
/// bitmask is applied to the coupled linear (resp. angular) axes.
pub motors: [JointMotor; SPATIAL_DIM],
/// Are contacts between the attached rigid-bodies enabled?
pub contacts_enabled: bool,

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

@@ -214,7 +214,7 @@ impl ImpulseJointSet {
// // .map(|e| &mut e.weight)
// }
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
&mut self.joint_graph.graph.edges[..]
}

2
src/dynamics/joint/mod.rs
View File

@@ -6,6 +6,7 @@ pub use self::multibody_joint::*;
pub use self::prismatic_joint::*;
pub use self::revolute_joint::*;
pub use self::rope_joint::*;
pub use self::spring_joint::*;
#[cfg(feature = "dim3")]
pub use self::spherical_joint::*;
@@ -21,3 +22,4 @@ mod rope_joint;
#[cfg(feature = "dim3")]
mod spherical_joint;
mod spring_joint;

4
src/dynamics/joint/multibody_joint/mod.rs
View File

@@ -2,7 +2,9 @@
pub use self::multibody::Multibody;
pub use self::multibody_joint::MultibodyJoint;
pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
pub use self::multibody_joint_set::{
MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
};
pub use self::multibody_link::MultibodyLink;
pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};

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

@@ -1,16 +1,13 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
#[repr(C)]
@@ -372,6 +369,7 @@ impl Multibody {
self.accelerations.fill(0.0);
// Eqn 42 to 45
for i in 0..self.links.len() {
let link = &self.links[i];
let rb = &bodies[link.rigid_body];
@@ -400,7 +398,7 @@ impl Multibody {
}
acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23));
acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
acc.linvel += acc.angvel.gcross(link.shift23);
self.workspace.accs[i] = acc;
@@ -728,7 +726,7 @@ impl Multibody {
/// The generalized velocity at the multibody_joint of the given link.
#[inline]
pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
let ndofs = link.joint().ndofs();
DVectorView::from_slice(
&self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
@@ -829,8 +827,10 @@ impl Multibody {
}
}
// TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
// (i.e. just something used by the velocity solver’s small steps).
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
@@ -839,7 +839,7 @@ impl Multibody {
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_mass_props {
if update_rb_mass_props {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
@@ -873,7 +873,7 @@ impl Multibody {
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_mass_props {
if update_rb_mass_props {
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
@@ -951,40 +951,4 @@ impl Multibody {
.sum();
(num_constraints, num_constraints)
}
#[inline]
pub(crate) fn generate_internal_constraints(
&self,
params: &IntegrationParameters,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<AnyJointVelocityConstraint>,
mut insert_at: Option<usize>,
) {
if !cfg!(feature = "parallel") {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
if jacobians.nrows() < required_jacobian_len {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
}
for link in self.links.iter() {
link.joint().velocity_constraints(
params,
self,
link,
0,
j_id,
jacobians,
out,
&mut insert_at,
);
}
}
}

30
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::solver::JointGenericOneBodyConstraint;
use crate::dynamics::{
joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity,
@@ -254,15 +254,15 @@ impl MultibodyJoint {
params: &IntegrationParameters,
multibody: &Multibody,
link: &MultibodyLink,
dof_id: usize,
j_id: &mut usize,
mut j_id: usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
constraints: &mut [JointGenericOneBodyConstraint],
) -> usize {
let j_id = &mut j_id;
let locked_bits = self.data.locked_axes.bits();
let limit_bits = self.data.limit_axes.bits();
let motor_bits = self.data.motor_axes.bits();
let mut num_constraints = 0;
let mut curr_free_dof = 0;
for i in 0..DIM {
@@ -281,11 +281,11 @@ impl MultibodyJoint {
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
@@ -296,11 +296,11 @@ impl MultibodyJoint {
link,
[self.data.limits[i].min, self.data.limits[i].max],
self.coords[i],
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
curr_free_dof += 1;
@@ -331,11 +331,11 @@ impl MultibodyJoint {
link,
limits,
self.coords[i],
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
Some(limits)
} else {
@@ -350,15 +350,17 @@ impl MultibodyJoint {
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
curr_free_dof += 1;
}
}
num_constraints
}
}

46
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct MultibodyJointLink {
pub graph_id: RigidBodyGraphIndex,
/// Indexes usable to get a multibody link from a `MultibodyJointSet`.
///
/// ```rust
/// // With:
/// // multibody_joint_set: MultibodyJointSet
/// // multibody_link_id: MultibodyLinkId
/// let multibody = &multibody_joint_set[multibody_link_id.multibody];
/// let link = multibody.link(multibody_link_id.id).expect("Link not found.");
pub struct MultibodyLinkId {
pub(crate) graph_id: RigidBodyGraphIndex,
/// The multibody index to be used as `&multibody_joint_set[multibody]` to
/// retrieve the multibody reference.
pub multibody: MultibodyIndex,
/// The multibody link index to be given to [`Multibody::link`].
pub id: usize,
}
impl Default for MultibodyJointLink {
impl Default for MultibodyLinkId {
fn default() -> Self {
Self {
graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
@@ -78,7 +89,7 @@ impl Default for MultibodyJointLink {
#[derive(Clone)]
pub struct MultibodyJointSet {
pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
pub(crate) rb2mb: Coarena<MultibodyJointLink>,
pub(crate) rb2mb: Coarena<MultibodyLinkId>,
// NOTE: this is mostly for the island extraction. So perhaps we won’t need
// that any more in the future when we improve our island builder.
pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
@@ -97,13 +108,22 @@ impl MultibodyJointSet {
}
/// Iterates through all the multibody joints from this set.
pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
pub fn iter(
&self,
) -> impl Iterator<
Item = (
MultibodyJointHandle,
&MultibodyLinkId,
&Multibody,
&MultibodyLink,
),
> {
self.rb2mb
.iter()
.filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
.map(|(h, link)| {
let mb = &self.multibodies[link.multibody.0];
(MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
(MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap())
})
}
@@ -118,7 +138,7 @@ impl MultibodyJointSet {
let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
MultibodyJointLink {
MultibodyLinkId {
graph_id: self.connectivity_graph.graph.add_node(body1),
multibody: MultibodyIndex(mb_handle),
id: 0,
@@ -127,7 +147,7 @@ impl MultibodyJointSet {
let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
MultibodyJointLink {
MultibodyLinkId {
graph_id: self.connectivity_graph.graph.add_node(body2),
multibody: MultibodyIndex(mb_handle),
id: 0,
@@ -257,7 +277,7 @@ impl MultibodyJointSet {
/// Returns the link of this multibody attached to the given rigid-body.
///
/// Returns `None` if `rb` isn’t part of any rigid-body.
pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> {
self.rb2mb.get(rb.0)
}
@@ -340,15 +360,15 @@ impl MultibodyJointSet {
// NOTE: if there is a joint between these two bodies, then
// one of the bodies must be the parent of the other.
let link1 = mb.link(id1.id)?;
let parent1 = link1.parent_id()?;
let parent1 = link1.parent_id();
if parent1 == id2.id {
if parent1 == Some(id2.id) {
Some((MultibodyJointHandle(rb1.0), mb, &link1))
} else {
let link2 = mb.link(id2.id)?;
let parent2 = link2.parent_id()?;
let parent2 = link2.parent_id();
if parent2 == id1.id {
if parent2 == Some(id1.id) {
Some((MultibodyJointHandle(rb2.0), mb, &link2))
} else {
None

51
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -1,9 +1,7 @@
#![allow(missing_docs)] // For downcast.
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
};
use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
use crate::math::Real;
use na::DVector;
@@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
constraints: &mut [JointGenericOneBodyConstraint],
insert_at: &mut usize,
) {
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let min_enabled = curr_pos < limits[0];
let max_enabled = limits[1] < curr_pos;
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
let rhs_wo_bias = joint_velocity[dof_id];
let rhs_wo_bias = 0.0;
let dof_j_id = *j_id + dof_id + link.assembly_id;
jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
@@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint(
max_enabled as u32 as Real * Real::MAX,
];
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
let constraint = JointGenericOneBodyConstraint {
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
@@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
constraints[*insert_at] = constraint;
*insert_at += 1;
*j_id += 2 * ndofs;
}
@@ -84,13 +75,11 @@ pub fn unit_joint_motor_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
constraints: &mut [JointGenericOneBodyConstraint],
insert_at: &mut usize,
) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let motor_params = motor.motor_params(params.dt);
let dof_j_id = *j_id + dof_id + link.assembly_id;
@@ -117,11 +106,10 @@ pub fn unit_joint_motor_constraint(
);
};
let dvel = joint_velocity[dof_id];
rhs_wo_bias += dvel - target_vel;
rhs_wo_bias += -target_vel;
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
let constraint = JointGenericOneBodyConstraint {
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
@@ -135,13 +123,8 @@ pub fn unit_joint_motor_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
constraints[*insert_at] = constraint;
*insert_at += 1;
*j_id += 2 * ndofs;
}

104
src/dynamics/joint/rope_joint.rs
View File

@@ -1,8 +1,8 @@
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
use crate::math::{Point, Real};
use super::{JointLimits, JointMotor};
use super::JointMotor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -14,12 +14,16 @@ pub struct RopeJoint {
}
impl RopeJoint {
/// Creates a new rope joint limiting the max distance between to bodies
pub fn new() -> Self {
let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES)
/// Creates a new rope joint limiting the max distance between two bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn new(max_dist: Real) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::empty())
.coupled_axes(JointAxesMask::LIN_AXES)
.build();
Self { data }
let mut result = Self { data };
result.set_max_distance(max_dist);
result
}
/// The underlying generic joint.
@@ -62,30 +66,6 @@ impl RopeJoint {
self
}
/// The principal axis of the joint, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.data.local_axis1()
}
/// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis1(axis1);
self
}
/// The principal axis of the joint, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.data.local_axis2()
}
/// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis2(axis2);
self
}
/// The motor affecting the joint’s translational degree of freedom.
#[must_use]
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
@@ -95,9 +75,6 @@ impl RopeJoint {
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::X, model);
self.data.set_motor_model(JointAxis::Y, model);
#[cfg(feature = "dim3")]
self.data.set_motor_model(JointAxis::Z, model);
self
}
@@ -105,11 +82,6 @@ impl RopeJoint {
pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
self.data
.set_motor_velocity(JointAxis::X, target_vel, factor);
self.data
.set_motor_velocity(JointAxis::Y, target_vel, factor);
#[cfg(feature = "dim3")]
self.data
.set_motor_velocity(JointAxis::Z, target_vel, factor);
self
}
@@ -122,11 +94,6 @@ impl RopeJoint {
) -> &mut Self {
self.data
.set_motor_position(JointAxis::X, target_pos, stiffness, damping);
self.data
.set_motor_position(JointAxis::Y, target_pos, stiffness, damping);
#[cfg(feature = "dim3")]
self.data
.set_motor_position(JointAxis::Z, target_pos, stiffness, damping);
self
}
@@ -140,35 +107,26 @@ impl RopeJoint {
) -> &mut Self {
self.data
.set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
self.data
.set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
#[cfg(feature = "dim3")]
self.data
.set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
self
}
/// Sets the maximum force the motor can deliver.
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(JointAxis::X, max_force);
self.data.set_motor_max_force(JointAxis::Y, max_force);
#[cfg(feature = "dim3")]
self.data.set_motor_max_force(JointAxis::Z, max_force);
self
}
/// The limit maximum distance attached bodies can translate.
/// The maximum distance allowed between the attached objects.
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
self.data.limits(axis)
pub fn max_distance(&self) -> Option<Real> {
self.data.limits(JointAxis::X).map(|l| l.max)
}
/// Sets the `[min,max]` limit distances attached bodies can translate.
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::X, limits);
self.data.set_limits(JointAxis::Y, limits);
#[cfg(feature = "dim3")]
self.data.set_limits(JointAxis::Z, limits);
/// Sets the maximum allowed distance between the attached objects.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn set_max_distance(&mut self, max_dist: Real) -> &mut Self {
self.data.set_limits(JointAxis::X, [0.0, max_dist]);
self
}
}
@@ -190,8 +148,8 @@ impl RopeJointBuilder {
/// Creates a new builder for rope joints.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new() -> Self {
Self(RopeJoint::new())
pub fn new(max_dist: Real) -> Self {
Self(RopeJoint::new(max_dist))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
@@ -215,20 +173,6 @@ impl RopeJointBuilder {
self
}
/// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
self.0.set_local_axis1(axis1);
self
}
/// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
self.0.set_local_axis2(axis2);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, model: MotorModel) -> Self {
@@ -270,10 +214,12 @@ impl RopeJointBuilder {
self
}
/// Sets the `[min,max]` limit distances attached bodies can translate.
/// Sets the maximum allowed distance between the attached bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
pub fn max_distance(mut self, max_dist: Real) -> Self {
self.0.set_max_distance(max_dist);
self
}

172
src/dynamics/joint/spring_joint.rs Normal file
View File

@@ -0,0 +1,172 @@
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A spring-damper joint, applies a force proportional to the distance between two objects.
///
/// The spring is integrated implicitly, implying that an even undamped spring will still be subject to some
/// amount of numerical damping (so it will eventually come to a rest). More solver iterations, or smaller
/// timesteps, will lower the effect of numerical damping, providing a more realistic result.
pub struct SpringJoint {
/// The underlying joint data.
pub data: GenericJoint,
}
impl SpringJoint {
/// Creates a new spring joint limiting the max distance between two bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::empty())
.coupled_axes(JointAxesMask::LIN_AXES)
.motor_position(JointAxis::X, rest_length, stiffness, damping)
.motor_model(JointAxis::X, MotorModel::ForceBased)
.build();
Self { data }
}
/// The underlying generic joint.
pub fn data(&self) -> &GenericJoint {
&self.data
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
self
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
/// Set the spring model used by this joint to reach the desired target velocity and position.
///
/// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
/// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
/// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
/// making the spring more mass-independent.
pub fn set_spring_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::X, model);
self
}
// /// The maximum distance allowed between the attached objects.
// #[must_use]
// pub fn rest_length(&self) -> Option<Real> {
// self.data.limits(JointAxis::X).map(|l| l.max)
// }
//
// /// Sets the maximum allowed distance between the attached objects.
// ///
// /// The `max_dist` must be strictly greater than 0.0.
// pub fn set_rest_length(&mut self, max_dist: Real) -> &mut Self {
// self.data.set_limits(JointAxis::X, [0.0, max_dist]);
// self
// }
}
impl Into<GenericJoint> for SpringJoint {
fn into(self) -> GenericJoint {
self.data
}
}
/// A [SpringJoint] joint using the builder pattern.
///
/// This builds a spring-damper joint which applies a force proportional to the distance between two objects.
/// See the documentation of [SpringJoint] for more information on its behavior.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SpringJointBuilder(pub SpringJoint);
impl SpringJointBuilder {
/// Creates a new builder for spring joints.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
Self(SpringJoint::new(rest_length, stiffness, damping))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
/// Set the spring used by this joint to reach the desired target velocity and position.
///
/// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
/// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
/// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
/// making the spring more mass-independent.
#[must_use]
pub fn spring_model(mut self, model: MotorModel) -> Self {
self.0.set_spring_model(model);
self
}
// /// Sets the maximum allowed distance between the attached bodies.
// ///
// /// The `max_dist` must be strictly greater than 0.0.
// #[must_use]
// pub fn max_distance(mut self, max_dist: Real) -> Self {
// self.0.set_max_distance(max_dist);
// self
// }
/// Builds the spring joint.
#[must_use]
pub fn build(self) -> SpringJoint {
self.0
}
}
impl Into<GenericJoint> for SpringJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}

6
src/dynamics/mod.rs
View File

@@ -8,10 +8,10 @@ pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::joint::JointIndex;
pub use self::joint::*;
pub use self::rigid_body_components::*;
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
pub(crate) use self::solver::IslandSolver;
#[cfg(feature = "parallel")]
pub(crate) use self::solver::ParallelIslandSolver;
// #[cfg(feature = "parallel")]
// pub(crate) use self::solver::ParallelIslandSolver;
pub use parry::mass_properties::MassProperties;
pub use self::rigid_body::{RigidBody, RigidBodyBuilder};

41
src/dynamics/rigid_body.rs
View File

@@ -7,7 +7,7 @@ use crate::geometry::{
ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet, ColliderShape,
};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WCross;
use crate::utils::SimdCross;
use num::Zero;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -36,6 +36,7 @@ pub struct RigidBody {
/// The dominance group this rigid-body is part of.
pub(crate) dominance: RigidBodyDominance,
pub(crate) enabled: bool,
pub(crate) additional_solver_iterations: usize,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
}
@@ -64,6 +65,7 @@ impl RigidBody {
dominance: RigidBodyDominance::default(),
enabled: true,
user_data: 0,
additional_solver_iterations: 0,
}
}
@@ -72,6 +74,27 @@ impl RigidBody {
self.ids = Default::default();
}
/// Set the additional number of solver iterations run for this rigid-body and
/// everything interacting with it.
///
/// See [`Self::set_additional_solver_iterations`] for additional information.
pub fn additional_solver_iterations(&self) -> usize {
self.additional_solver_iterations
}
/// Set the additional number of solver iterations run for this rigid-body and
/// everything interacting with it.
///
/// Increasing this number will help improve simulation accuracy on this rigid-body
/// and every rigid-body interacting directly or indirectly with it (through joints
/// or contacts). This implies a performance hit.
///
/// The default value is 0, meaning [`IntegrationParameters::num_solver_iterations`] will
/// be used as number of solver iterations for this body.
pub fn set_additional_solver_iterations(&mut self, additional_iterations: usize) {
self.additional_solver_iterations = additional_iterations;
}
/// The activation status of this rigid-body.
pub fn activation(&self) -> &RigidBodyActivation {
&self.activation
@@ -1030,6 +1053,11 @@ pub struct RigidBodyBuilder {
pub enabled: bool,
/// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
pub user_data: u128,
/// The additional number of solver iterations run for this rigid-body and
/// everything interacting with it.
///
/// See [`RigidBody::set_additional_solver_iterations`] for additional information.
pub additional_solver_iterations: usize,
}
impl RigidBodyBuilder {
@@ -1051,6 +1079,7 @@ impl RigidBodyBuilder {
dominance_group: 0,
enabled: true,
user_data: 0,
additional_solver_iterations: 0,
}
}
@@ -1090,6 +1119,15 @@ impl RigidBodyBuilder {
Self::new(RigidBodyType::Dynamic)
}
/// Sets the additional number of solver iterations run for this rigid-body and
/// everything interacting with it.
///
/// See [`RigidBody::set_additional_solver_iterations`] for additional information.
pub fn additional_solver_iterations(mut self, additional_iterations: usize) -> Self {
self.additional_solver_iterations = additional_iterations;
self
}
/// Sets the scale applied to the gravity force affecting the rigid-body to be created.
pub fn gravity_scale(mut self, scale_factor: Real) -> Self {
self.gravity_scale = scale_factor;
@@ -1311,6 +1349,7 @@ impl RigidBodyBuilder {
rb.vels.angvel = self.angvel;
rb.body_type = self.body_type;
rb.user_data = self.user_data;
rb.additional_solver_iterations = self.additional_solver_iterations;
if self.additional_mass_properties
!= RigidBodyAdditionalMassProps::MassProps(MassProperties::zero())

45
src/dynamics/rigid_body_components.rs
View File

@@ -7,7 +7,7 @@ use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
use crate::parry::partitioning::IndexedData;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
/// The unique handle of a rigid body added to a `RigidBodySet`.
@@ -307,11 +307,52 @@ impl RigidBodyMassProps {
self.effective_inv_mass.map(crate::utils::inv)
}
/// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia<Real> {
#[allow(unused_mut)] // mut needed in 3D.
let mut ang_inertia = self.effective_world_inv_inertia_sqrt;
// Make the matrix invertible.
#[cfg(feature = "dim3")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
ang_inertia.m11 = 1.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
ang_inertia.m22 = 1.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
ang_inertia.m33 = 1.0;
}
}
#[allow(unused_mut)] // mut needed in 3D.
let mut result = ang_inertia.inverse();
// Remove the locked axes again.
#[cfg(feature = "dim3")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
result.m11 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
result.m22 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
result.m33 = 0.0;
}
}
result
}
/// The effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
self.effective_world_inv_inertia_sqrt.squared().inverse()
self.effective_angular_inertia_sqrt().squared()
}
/// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.

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

@@ -6,10 +6,10 @@ pub(crate) fn categorize_contacts(
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
out_not_ground: &mut Vec<ContactManifoldIndex>,
out_generic_ground: &mut Vec<ContactManifoldIndex>,
out_generic_not_ground: &mut Vec<ContactManifoldIndex>,
out_one_body: &mut Vec<ContactManifoldIndex>,
out_two_body: &mut Vec<ContactManifoldIndex>,
out_generic_one_body: &mut Vec<ContactManifoldIndex>,
out_generic_two_body: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
@@ -26,14 +26,14 @@ pub(crate) fn categorize_contacts(
.is_some()
{
if manifold.data.relative_dominance != 0 {
out_generic_ground.push(*manifold_i);
out_generic_one_body.push(*manifold_i);
} else {
out_generic_not_ground.push(*manifold_i);
out_generic_two_body.push(*manifold_i);
}
} else if manifold.data.relative_dominance != 0 {
out_ground.push(*manifold_i)
out_one_body.push(*manifold_i)
} else {
out_not_ground.push(*manifold_i)
out_two_body.push(*manifold_i)
}
}
}
@@ -43,10 +43,10 @@ pub(crate) fn categorize_joints(
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_indices: &[JointIndex],
ground_joints: &mut Vec<JointIndex>,
nonground_joints: &mut Vec<JointIndex>,
generic_ground_joints: &mut Vec<JointIndex>,
generic_nonground_joints: &mut Vec<JointIndex>,
one_body_joints: &mut Vec<JointIndex>,
two_body_joints: &mut Vec<JointIndex>,
generic_one_body_joints: &mut Vec<JointIndex>,
generic_two_body_joints: &mut Vec<JointIndex>,
) {
for joint_i in joint_indices {
let joint = &impulse_joints[*joint_i].weight;
@@ -57,14 +57,14 @@ pub(crate) fn categorize_joints(
|| multibody_joints.rigid_body_link(joint.body2).is_some()
{
if !rb1.is_dynamic() || !rb2.is_dynamic() {
generic_ground_joints.push(*joint_i);
generic_one_body_joints.push(*joint_i);
} else {
generic_nonground_joints.push(*joint_i);
generic_two_body_joints.push(*joint_i);
}
} else if !rb1.is_dynamic() || !rb2.is_dynamic() {
ground_joints.push(*joint_i);
one_body_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);
two_body_joints.push(*joint_i);
}
}
}

528
src/dynamics/solver/contact_constraint/contact_constraints_set.rs Normal file
View File

@@ -0,0 +1,528 @@
use crate::dynamics::solver::categorization::categorize_contacts;
use crate::dynamics::solver::contact_constraint::{
GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint,
GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder,
TwoBodyConstraint, TwoBodyConstraintBuilder,
};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, MAX_MANIFOLD_POINTS};
use na::DVector;
use parry::math::DIM;
#[cfg(feature = "simd-is-enabled")]
use {
crate::dynamics::solver::contact_constraint::{
OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
TwoBodyConstraintSimd,
},
crate::math::SIMD_WIDTH,
};
pub struct ConstraintsCounts {
pub num_constraints: usize,
pub num_jacobian_lines: usize,
}
impl ConstraintsCounts {
pub fn from_contacts(manifold: &ContactManifold) -> Self {
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
Self {
num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS
+ rest as usize,
num_jacobian_lines: manifold.data.solver_contacts.len() * DIM,
}
}
pub fn from_joint(joint: &ImpulseJoint) -> Self {
let joint = &joint.data;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
+ locked_axes.count_ones() as usize
+ (limit_axes & !coupled_axes).count_ones() as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
Self {
num_constraints,
num_jacobian_lines: num_constraints,
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct ContactConstraintTypes;
impl ConstraintTypes for ContactConstraintTypes {
type OneBody = OneBodyConstraint;
type TwoBodies = TwoBodyConstraint;
type GenericOneBody = GenericOneBodyConstraint;
type GenericTwoBodies = GenericTwoBodyConstraint;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody = OneBodyConstraintSimd;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies = TwoBodyConstraintSimd;
type BuilderOneBody = OneBodyConstraintBuilder;
type BuilderTwoBodies = TwoBodyConstraintBuilder;
type GenericBuilderOneBody = GenericOneBodyConstraintBuilder;
type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody = SimdOneBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd;
}
pub type ContactConstraintsSet = SolverConstraintsSet<ContactConstraintTypes>;
impl ContactConstraintsSet {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.two_body_interactions.clear();
self.one_body_interactions.clear();
self.generic_two_body_interactions.clear();
self.generic_one_body_interactions.clear();
categorize_contacts(
bodies,
multibody_joints,
manifolds,
manifold_indices,
&mut self.one_body_interactions,
&mut self.two_body_interactions,
&mut self.generic_one_body_interactions,
&mut self.generic_two_body_interactions,
);
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.two_body_interactions,
);
self.one_body_interaction_groups.clear_groups();
self.one_body_interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.one_body_interactions,
);
// NOTE: uncomment this do disable SIMD contact resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.simd_interactions);
// self.one_body_interaction_groups
// .nongrouped_interactions
// .append(&mut self.one_body_interaction_groups.simd_interactions);
}
pub fn init(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.clear_constraints();
self.clear_builders();
self.init_constraint_groups(
island_id,
islands,
bodies,
multibody_joints,
manifolds,
manifold_indices,
);
let mut jacobian_id = 0;
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_constraints(bodies, manifolds);
}
self.compute_constraints(bodies, manifolds);
self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id);
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_one_body_constraints(bodies, manifolds);
}
self.compute_one_body_constraints(bodies, manifolds);
self.compute_generic_one_body_constraints(
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.simd_velocity_constraints_builder,
total_num_constraints,
);
reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints);
}
let mut curr_start = 0;
for manifolds_i in self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
TwoBodyConstraintBuilderSimd::generate(
manifold_id,
manifolds,
bodies,
&mut self.simd_velocity_constraints_builder[curr_start..],
&mut self.simd_velocity_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
fn compute_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.velocity_constraints_builder,
total_num_constraints,
);
reset_buffer(&mut self.velocity_constraints, total_num_constraints);
}
let mut curr_start = 0;
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
TwoBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
&mut self.velocity_constraints_builder[curr_start..],
&mut self.velocity_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
fn compute_generic_constraints(
&mut self,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
let total_num_constraints = self
.generic_two_body_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
.sum();
self.generic_velocity_constraints_builder.resize(
total_num_constraints,
GenericTwoBodyConstraintBuilder::invalid(),
);
self.generic_velocity_constraints
.resize(total_num_constraints, GenericTwoBodyConstraint::invalid());
let mut curr_start = 0;
for manifold_i in &self.generic_two_body_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
GenericTwoBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints_builder[curr_start..],
&mut self.generic_velocity_constraints[curr_start..],
&mut self.generic_jacobians,
jacobian_id,
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
fn compute_generic_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
let total_num_constraints = self
.generic_one_body_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
.sum();
self.generic_velocity_one_body_constraints_builder.resize(
total_num_constraints,
GenericOneBodyConstraintBuilder::invalid(),
);
self.generic_velocity_one_body_constraints
.resize(total_num_constraints, GenericOneBodyConstraint::invalid());
let mut curr_start = 0;
for manifold_i in &self.generic_one_body_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
GenericOneBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_one_body_constraints_builder[curr_start..],
&mut self.generic_velocity_one_body_constraints[curr_start..],
&mut self.generic_jacobians,
jacobian_id,
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.simd_velocity_one_body_constraints,
total_num_constraints,
);
}
let mut curr_start = 0;
for manifolds_i in self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
SimdOneBodyConstraintBuilder::generate(
manifold_id,
manifolds,
bodies,
&mut self.simd_velocity_one_body_constraints_builder[curr_start..],
&mut self.simd_velocity_one_body_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
fn compute_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.one_body_interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.velocity_one_body_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.velocity_one_body_constraints,
total_num_constraints,
);
}
let mut curr_start = 0;
for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
OneBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
&mut self.velocity_one_body_constraints_builder[curr_start..],
&mut self.velocity_one_body_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
pub fn solve_restitution(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.solve_restitution(jac, solver_vels, generic_solver_vels);
}
}
pub fn solve_restitution_wo_bias(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.remove_bias();
c.solve_restitution(jac, solver_vels, generic_solver_vels);
}
}
pub fn solve_friction(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.solve_friction(jac, solver_vels, generic_solver_vels);
}
}
pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
let (_, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.writeback_impulses(manifolds_all);
}
}
pub fn update(
&mut self,
params: &IntegrationParameters,
small_step_id: usize,
multibodies: &MultibodyJointSet,
solver_bodies: &[SolverBody],
) {
macro_rules! update_contacts(
($builders: ident, $constraints: ident) => {
for (builder, constraint) in self.$builders.iter().zip(self.$constraints.iter_mut()) {
builder.update(
&params,
small_step_id as Real * params.dt,
solver_bodies,
multibodies,
constraint,
);
}
}
);
update_contacts!(
generic_velocity_constraints_builder,
generic_velocity_constraints
);
update_contacts!(velocity_constraints_builder, velocity_constraints);
#[cfg(feature = "simd-is-enabled")]
update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints);
update_contacts!(
generic_velocity_one_body_constraints_builder,
generic_velocity_one_body_constraints
);
update_contacts!(
velocity_one_body_constraints_builder,
velocity_one_body_constraints
);
#[cfg(feature = "simd-is-enabled")]
update_contacts!(
simd_velocity_one_body_constraints_builder,
simd_velocity_one_body_constraints
);
}
}

292
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs Normal file
View File

@@ -0,0 +1,292 @@
use crate::dynamics::solver::OneBodyConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::SimdCross;
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use na::DVector;
#[derive(Copy, Clone)]
pub(crate) struct GenericOneBodyConstraintBuilder {
link2: MultibodyLinkId,
ccd_thickness: Real,
inner: OneBodyConstraintBuilder,
}
impl GenericOneBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
link2: MultibodyLinkId::default(),
ccd_thickness: 0.0,
inner: OneBodyConstraintBuilder::invalid(),
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builders: &mut [GenericOneBodyConstraintBuilder],
out_constraints: &mut [GenericOneBodyConstraint],
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
) {
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb1 = handle1
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_else(SolverBody::default);
let rb2 = &bodies[handle2.unwrap()];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap();
let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id);
let solver_vel2 = mb2.solver_id;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
// the multibodies jacobian and weighted jacobians
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
builder.inner.rb1 = rb1;
builder.inner.vels1 = vels1;
constraint.inner.dir1 = force_dir1;
constraint.inner.im2 = mprops2.effective_inv_mass;
constraint.inner.solver_vel2 = solver_vel2;
constraint.inner.manifold_id = manifold_id;
constraint.inner.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.inner.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let point = manifold_point.point;
let dp1 = point - world_com1;
let dp2 = point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.inner.limit = manifold_point.friction;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let torque_dir2 = dp2.gcross(-force_dir1);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-force_dir1,
#[cfg(feature = "dim2")]
na::vector!(torque_dir2),
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let proj_vel1 = vel1.dot(&force_dir1);
let proj_vel2 = vel2.dot(&force_dir1);
let dvel = proj_vel1 - proj_vel2;
// NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
normal_rhs_wo_bias =
proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r,
};
}
// Tangent parts.
{
constraint.inner.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let torque_dir2 = dp2.gcross(-tangents1[j]);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-tangents1[j],
#[cfg(feature = "dim2")]
na::vector![torque_dir2],
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let rhs_wo_bias = (vel1
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.inner.elements[k].tangent_part.r[j] = r;
}
}
// Builder.
let infos = ContactPointInfos {
local_p1: rb1.position.inverse_transform_point(&manifold_point.point),
local_p2: rb2
.pos
.position
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.link2 = link2;
builder.ccd_thickness = rb2.ccd.ccd_thickness;
builder.inner.infos[k] = infos;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
}
constraint.j_id = chunk_j_id;
constraint.ndofs2 = mb2.ndofs();
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
_solver_bodies: &[SolverBody],
multibodies: &MultibodyJointSet,
constraint: &mut GenericOneBodyConstraint,
) {
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pos2 = &multibodies[self.link2.multibody]
.link(self.link2.id)
.unwrap()
.local_to_world;
self.inner.update_with_positions(
params,
solved_dt,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericOneBodyConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub inner: OneBodyConstraint,
pub j_id: usize,
pub ndofs2: usize,
}
impl GenericOneBodyConstraint {
pub fn invalid() -> Self {
Self {
inner: OneBodyConstraint::invalid(),
j_id: usize::MAX,
ndofs2: usize::MAX,
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
generic_solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let solver_vel2 = self.inner.solver_vel2 as usize;
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
OneBodyConstraintElement::generic_solve_group(
self.inner.cfm_factor,
elements,
jacobians,
self.inner.limit,
self.ndofs2,
self.j_id,
solver_vel2,
generic_solver_vels,
solve_restitution,
solve_friction,
);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.inner.writeback_impulses(manifolds_all);
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.inner.remove_cfm_and_bias_from_rhs();
}
}

46
src/dynamics/solver/generic_velocity_ground_constraint_element.rs → src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
View File

@@ -1,13 +1,12 @@
use crate::dynamics::solver::{
VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
VelocityGroundConstraintTangentPart,
OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
};
use crate::math::{Real, DIM};
use na::DVector;
#[cfg(feature = "dim2")]
use na::SimdPartialOrd;
impl VelocityGroundConstraintTangentPart<Real> {
impl OneBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
@@ -15,21 +14,21 @@ impl VelocityGroundConstraintTangentPart<Real> {
jacobians: &DVector<Real>,
ndofs2: usize,
limit: Real,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
@@ -41,11 +40,11 @@ impl VelocityGroundConstraintTangentPart<Real> {
let j_step = ndofs2 * 2;
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[0];
let dvel_1 = jacobians
.rows(j_id2 + j_step, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[1];
let new_impulse = na::Vector2::new(
@@ -57,12 +56,12 @@ impl VelocityGroundConstraintTangentPart<Real> {
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
@@ -71,7 +70,7 @@ impl VelocityGroundConstraintTangentPart<Real> {
}
}
impl VelocityGroundConstraintNormalPart<Real> {
impl OneBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
@@ -79,19 +78,19 @@ impl VelocityGroundConstraintNormalPart<Real> {
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
let dvel = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
@@ -99,7 +98,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
}
}
impl VelocityGroundConstraintElement<Real> {
impl OneBodyConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
@@ -109,8 +108,8 @@ impl VelocityGroundConstraintElement<Real> {
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
@@ -122,7 +121,12 @@ impl VelocityGroundConstraintElement<Real> {
for element in elements.iter_mut() {
element.normal_part.generic_solve(
cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas,
cfm_factor,
nrm_j_id,
jacobians,
ndofs2,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
@@ -135,7 +139,7 @@ impl VelocityGroundConstraintElement<Real> {
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas);
part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels);
tng_j_id += j_step;
}
}

273
src/dynamics/solver/generic_velocity_constraint.rs → src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
View File

@@ -1,43 +1,45 @@
use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::prelude::RigidBodyHandle;
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::SimdBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub velocity_constraint: VelocityConstraint,
pub j_id: usize,
pub ndofs1: usize,
pub ndofs2: usize,
pub generic_constraint_mask: u8,
#[derive(Copy, Clone)]
pub(crate) struct GenericTwoBodyConstraintBuilder {
handle1: RigidBodyHandle,
handle2: RigidBodyHandle,
ccd_thickness: Real,
inner: TwoBodyConstraintBuilder,
}
impl GenericTwoBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
handle1: RigidBodyHandle::invalid(),
handle2: RigidBodyHandle::invalid(),
ccd_thickness: Real::MAX,
inner: TwoBodyConstraintBuilder::invalid(),
}
}
impl GenericVelocityConstraint {
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
out_builders: &mut [GenericTwoBodyConstraintBuilder],
out_constraints: &mut [GenericTwoBodyConstraint],
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
@@ -46,7 +48,6 @@ impl GenericVelocityConstraint {
let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
let multibody1 = multibodies
.rigid_body_link(handle1)
@@ -54,14 +55,14 @@ impl GenericVelocityConstraint {
let multibody2 = multibodies
.rigid_body_link(handle2)
.map(|m| (&multibodies[m.multibody], m.id));
let mj_lambda1 = multibody1
let solver_vel1 = multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if type1.is_dynamic() {
rb1.ids.active_set_offset
} else {
0
});
let mj_lambda2 = multibody2
let solver_vel2 = multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if type2.is_dynamic() {
rb2.ids.active_set_offset
@@ -87,50 +88,50 @@ impl GenericVelocityConstraint {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (_l, manifold_points) in manifold
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let mut is_fast_contact = false;
let mut constraint = VelocityConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: if type1.is_dynamic() {
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
constraint.inner.dir1 = force_dir1;
constraint.inner.im1 = if type1.is_dynamic() {
mprops1.effective_inv_mass
} else {
na::zero()
},
im2: if type2.is_dynamic() {
};
constraint.inner.im2 = if type2.is_dynamic() {
mprops2.effective_inv_mass
} else {
na::zero()
},
cfm_factor,
limit: 0.0,
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
constraint.inner.solver_vel1 = solver_vel1;
constraint.inner.solver_vel2 = solver_vel2;
constraint.inner.manifold_id = manifold_id;
constraint.inner.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.inner.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let point = manifold_point.point;
let dp1 = point - mprops1.world_com;
let dp2 = point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
constraint.inner.limit = manifold_point.friction;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
@@ -191,23 +192,16 @@ impl GenericVelocityConstraint {
let r = crate::utils::inv(inv_r1 + inv_r2);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
normal_rhs_wo_bias =
(is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
rhs,
rhs_wo_bias,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
total_impulse: na::zero(),
impulse: na::zero(),
r,
};
@@ -215,7 +209,7 @@ impl GenericVelocityConstraint {
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.inner.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -226,7 +220,7 @@ impl GenericVelocityConstraint {
} else {
na::zero()
};
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1;
let torque_dir2 = dp2.gcross(-tangents1[j]);
let gcross2 = if type2.is_dynamic() {
@@ -236,7 +230,7 @@ impl GenericVelocityConstraint {
} else {
na::zero()
};
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2;
let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
mb1.fill_jacobians(
@@ -277,23 +271,43 @@ impl GenericVelocityConstraint {
};
let r = crate::utils::inv(inv_r1 + inv_r2);
let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
let rhs =
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// TODO: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.elements[k].tangent_part.r[j] = r;
}
constraint.inner.elements[k].tangent_part.r[j] = r;
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
// Builder.
let infos = ContactPointInfos {
local_p1: rb1
.pos
.position
.inverse_transform_point(&manifold_point.point),
local_p2: rb2
.pos
.position
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.handle1 = handle1;
builder.handle2 = handle2;
builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
builder.inner.infos[k] = infos;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
}
let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0);
// NOTE: we use the generic constraint for non-dynamic bodies because this will
// reduce all ops to nothing because its ndofs will be zero.
let generic_constraint_mask = (multibody1.is_some() as u8)
@@ -301,78 +315,119 @@ impl GenericVelocityConstraint {
| (!type1.is_dynamic() as u8)
| ((!type2.is_dynamic() as u8) << 1);
let constraint = GenericVelocityConstraint {
velocity_constraint: constraint,
j_id: chunk_j_id,
ndofs1,
ndofs2,
generic_constraint_mask,
};
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
constraint.j_id = chunk_j_id;
constraint.ndofs1 = ndofs1;
constraint.ndofs2 = ndofs2;
constraint.generic_constraint_mask = generic_constraint_mask;
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
multibodies: &MultibodyJointSet,
constraint: &mut GenericTwoBodyConstraint,
) {
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pos1 = multibodies
.rigid_body_link(self.handle1)
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position);
let pos2 = multibodies
.rigid_body_link(self.handle2)
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
self.inner.update_with_positions(
params,
solved_dt,
pos1,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericTwoBodyConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub inner: TwoBodyConstraint,
pub j_id: usize,
pub ndofs1: usize,
pub ndofs2: usize,
pub generic_constraint_mask: u8,
}
impl GenericTwoBodyConstraint {
pub fn invalid() -> Self {
Self {
inner: TwoBodyConstraint::invalid(),
j_id: usize::MAX,
ndofs1: usize::MAX,
ndofs2: usize::MAX,
generic_constraint_mask: u8::MAX,
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let mut mj_lambda1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda1 as usize])
let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1 as usize])
} else {
GenericRhs::GenericId(self.velocity_constraint.mj_lambda1 as usize)
GenericRhs::GenericId(self.inner.solver_vel1 as usize)
};
let mut mj_lambda2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda2 as usize])
let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2 as usize])
} else {
GenericRhs::GenericId(self.velocity_constraint.mj_lambda2 as usize)
GenericRhs::GenericId(self.inner.solver_vel2 as usize)
};
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityConstraintElement::generic_solve_group(
self.velocity_constraint.cfm_factor,
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
TwoBodyConstraintElement::generic_solve_group(
self.inner.cfm_factor,
elements,
jacobians,
&self.velocity_constraint.dir1,
&self.inner.dir1,
#[cfg(feature = "dim3")]
&self.velocity_constraint.tangent1,
&self.velocity_constraint.im1,
&self.velocity_constraint.im2,
self.velocity_constraint.limit,
&self.inner.tangent1,
&self.inner.im1,
&self.inner.im2,
self.inner.limit,
self.ndofs1,
self.ndofs2,
self.j_id,
&mut mj_lambda1,
&mut mj_lambda2,
generic_mj_lambdas,
&mut solver_vel1,
&mut solver_vel2,
generic_solver_vels,
solve_restitution,
solve_friction,
);
if let GenericRhs::DeltaVel(mj_lambda1) = mj_lambda1 {
mj_lambdas[self.velocity_constraint.mj_lambda1 as usize] = mj_lambda1;
if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
solver_vels[self.inner.solver_vel1 as usize] = solver_vel1;
}
if let GenericRhs::DeltaVel(mj_lambda2) = mj_lambda2 {
mj_lambdas[self.velocity_constraint.mj_lambda2 as usize] = mj_lambda2;
if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
solver_vels[self.inner.solver_vel2 as usize] = solver_vel2;
}
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.velocity_constraint.writeback_impulses(manifolds_all);
self.inner.writeback_impulses(manifolds_all);
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_cfm_and_bias_from_rhs();
self.inner.remove_cfm_and_bias_from_rhs();
}
}

136
src/dynamics/solver/generic_velocity_constraint_element.rs → src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
View File

@@ -1,15 +1,15 @@
use super::DeltaVel;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{
VelocityConstraintElement, VelocityConstraintNormalPart, VelocityConstraintTangentPart,
TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
};
use crate::math::{AngVector, Real, Vector, DIM};
use crate::utils::WDot;
use crate::utils::SimdDot;
use na::DVector;
#[cfg(feature = "dim2")]
use {crate::utils::WBasis, na::SimdPartialOrd};
use {crate::utils::SimdBasis, na::SimdPartialOrd};
pub(crate) enum GenericRhs {
DeltaVel(DeltaVel<Real>),
SolverVel(SolverVel<Real>),
GenericId(usize),
}
@@ -48,13 +48,13 @@ impl GenericRhs {
jacobians: &DVector<Real>,
dir: &Vector<Real>,
gcross: &AngVector<Real>,
mj_lambdas: &DVector<Real>,
solver_vels: &DVector<Real>,
) -> Real {
match self {
GenericRhs::DeltaVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
GenericRhs::GenericId(mj_lambda) => {
GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
GenericRhs::GenericId(solver_vel) => {
let j = jacobians.rows(j_id, ndofs);
let rhs = mj_lambdas.rows(*mj_lambda, ndofs);
let rhs = solver_vels.rows(*solver_vel, ndofs);
j.dot(&rhs)
}
}
@@ -69,25 +69,25 @@ impl GenericRhs {
jacobians: &DVector<Real>,
dir: &Vector<Real>,
gcross: &AngVector<Real>,
mj_lambdas: &mut DVector<Real>,
solver_vels: &mut DVector<Real>,
inv_mass: &Vector<Real>,
) {
match self {
GenericRhs::DeltaVel(rhs) => {
GenericRhs::SolverVel(rhs) => {
rhs.linear += dir.component_mul(inv_mass) * impulse;
rhs.angular += gcross * impulse;
}
GenericRhs::GenericId(mj_lambda) => {
GenericRhs::GenericId(solver_vel) => {
let wj_id = j_id + ndofs;
let wj = jacobians.rows(wj_id, ndofs);
let mut rhs = mj_lambdas.rows_mut(*mj_lambda, ndofs);
let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs);
rhs.axpy(impulse, &wj, 1.0);
}
}
}
}
impl VelocityConstraintTangentPart<Real> {
impl TwoBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
@@ -99,9 +99,9 @@ impl VelocityConstraintTangentPart<Real> {
ndofs1: usize,
ndofs2: usize,
limit: Real,
mj_lambda1: &mut GenericRhs,
mj_lambda2: &mut GenericRhs,
mj_lambdas: &mut DVector<Real>,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
@@ -110,79 +110,79 @@ impl VelocityConstraintTangentPart<Real> {
#[cfg(feature = "dim2")]
{
let dvel_0 = mj_lambda1.dvel(
let dvel_0 = solver_vel1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dvel(
solver_vels,
) + solver_vel2.dvel(
j_id2,
ndofs2,
jacobians,
&-tangents1[0],
&self.gcross2[0],
mj_lambdas,
solver_vels,
) + self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
mj_lambda1.apply_impulse(
solver_vel1.apply_impulse(
j_id1,
ndofs1,
dlambda,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
solver_vels,
im1,
);
mj_lambda2.apply_impulse(
solver_vel2.apply_impulse(
j_id2,
ndofs2,
dlambda,
jacobians,
&-tangents1[0],
&self.gcross2[0],
mj_lambdas,
solver_vels,
im2,
);
}
#[cfg(feature = "dim3")]
{
let dvel_0 = mj_lambda1.dvel(
let dvel_0 = solver_vel1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dvel(
solver_vels,
) + solver_vel2.dvel(
j_id2,
ndofs2,
jacobians,
&-tangents1[0],
&self.gcross2[0],
mj_lambdas,
solver_vels,
) + self.rhs[0];
let dvel_1 = mj_lambda1.dvel(
let dvel_1 = solver_vel1.dvel(
j_id1 + j_step,
ndofs1,
jacobians,
&tangents1[1],
&self.gcross1[1],
mj_lambdas,
) + mj_lambda2.dvel(
solver_vels,
) + solver_vel2.dvel(
j_id2 + j_step,
ndofs2,
jacobians,
&-tangents1[1],
&self.gcross2[1],
mj_lambdas,
solver_vels,
) + self.rhs[1];
let new_impulse = na::Vector2::new(
@@ -194,52 +194,52 @@ impl VelocityConstraintTangentPart<Real> {
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda1.apply_impulse(
solver_vel1.apply_impulse(
j_id1,
ndofs1,
dlambda[0],
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
solver_vels,
im1,
);
mj_lambda1.apply_impulse(
solver_vel1.apply_impulse(
j_id1 + j_step,
ndofs1,
dlambda[1],
jacobians,
&tangents1[1],
&self.gcross1[1],
mj_lambdas,
solver_vels,
im1,
);
mj_lambda2.apply_impulse(
solver_vel2.apply_impulse(
j_id2,
ndofs2,
dlambda[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
mj_lambdas,
solver_vels,
im2,
);
mj_lambda2.apply_impulse(
solver_vel2.apply_impulse(
j_id2 + j_step,
ndofs2,
dlambda[1],
jacobians,
&-tangents1[1],
&self.gcross2[1],
mj_lambdas,
solver_vels,
im2,
);
}
}
}
impl VelocityConstraintNormalPart<Real> {
impl TwoBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
@@ -251,45 +251,45 @@ impl VelocityConstraintNormalPart<Real> {
im2: &Vector<Real>,
ndofs1: usize,
ndofs2: usize,
mj_lambda1: &mut GenericRhs,
mj_lambda2: &mut GenericRhs,
mj_lambdas: &mut DVector<Real>,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+ mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, solver_vels)
+ solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels)
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda1.apply_impulse(
solver_vel1.apply_impulse(
j_id1,
ndofs1,
dlambda,
jacobians,
&dir1,
&self.gcross1,
mj_lambdas,
solver_vels,
im1,
);
mj_lambda2.apply_impulse(
solver_vel2.apply_impulse(
j_id2,
ndofs2,
dlambda,
jacobians,
&-dir1,
&self.gcross2,
mj_lambdas,
solver_vels,
im2,
);
}
}
impl VelocityConstraintElement<Real> {
impl TwoBodyConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
@@ -306,9 +306,9 @@ impl VelocityConstraintElement<Real> {
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
mj_lambda1: &mut GenericRhs,
mj_lambda2: &mut GenericRhs,
mj_lambdas: &mut DVector<Real>,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
@@ -320,8 +320,17 @@ impl VelocityConstraintElement<Real> {
for element in elements.iter_mut() {
element.normal_part.generic_solve(
cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1,
mj_lambda2, mj_lambdas,
cfm_factor,
nrm_j_id,
jacobians,
&dir1,
im1,
im2,
ndofs1,
ndofs2,
solver_vel1,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
@@ -339,8 +348,17 @@ impl VelocityConstraintElement<Real> {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.generic_solve(
tng_j_id, jacobians, tangents1, im1, im2, ndofs1, ndofs2, limit, mj_lambda1,
mj_lambda2, mj_lambdas,
tng_j_id,
jacobians,
tangents1,
im1,
im2,
ndofs1,
ndofs2,
limit,
solver_vel1,
solver_vel2,
solver_vels,
);
tng_j_id += j_step;
}

29
src/dynamics/solver/contact_constraint/mod.rs Normal file
View File

@@ -0,0 +1,29 @@
pub(crate) use generic_one_body_constraint::*;
// pub(crate) use generic_one_body_constraint_element::*;
pub(crate) use contact_constraints_set::{
ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet,
};
pub(crate) use generic_two_body_constraint::*;
pub(crate) use generic_two_body_constraint_element::*;
pub(crate) use one_body_constraint::*;
pub(crate) use one_body_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use one_body_constraint_simd::*;
pub(crate) use two_body_constraint::*;
pub(crate) use two_body_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use two_body_constraint_simd::*;
mod contact_constraints_set;
mod generic_one_body_constraint;
mod generic_one_body_constraint_element;
mod generic_two_body_constraint;
mod generic_two_body_constraint_element;
mod one_body_constraint;
mod one_body_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod one_body_constraint_simd;
mod two_body_constraint;
mod two_body_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod two_body_constraint_simd;

382
src/dynamics/solver/contact_constraint/one_body_constraint.rs Normal file
View File

@@ -0,0 +1,382 @@
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use parry::math::Isometry;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
// TODO: move this struct somewhere else.
#[derive(Copy, Clone, Debug)]
pub struct ContactPointInfos<N: SimdRealCopy> {
pub tangent_vel: Vector<N>,
pub local_p1: Point<N>,
pub local_p2: Point<N>,
pub dist: N,
pub normal_rhs_wo_bias: N,
}
impl<N: SimdRealCopy> Default for ContactPointInfos<N> {
fn default() -> Self {
Self {
tangent_vel: Vector::zeros(),
local_p1: Point::origin(),
local_p2: Point::origin(),
dist: N::zero(),
normal_rhs_wo_bias: N::zero(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintBuilder {
// PERF: only store what’s necessary for the bias updates instead of the complete solver body.
pub rb1: SolverBody,
pub vels1: RigidBodyVelocity,
pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
}
impl OneBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
rb1: SolverBody::default(),
vels1: RigidBodyVelocity::zero(),
infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_builders: &mut [OneBodyConstraintBuilder],
out_constraints: &mut [OneBodyConstraint],
) {
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb1 = handle1
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_else(SolverBody::default);
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let solver_vel2 = rb2.ids.active_set_offset;
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
builder.rb1 = rb1;
builder.vels1 = vels1;
constraint.dir1 = force_dir1;
constraint.im2 = mprops2.effective_inv_mass;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
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);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = utils::inv(
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let proj_vel1 = vel1.dot(&force_dir1);
let proj_vel2 = vel2.dot(&force_dir1);
let dvel = proj_vel1 - proj_vel2;
// NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
normal_rhs_wo_bias =
proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
constraint.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r: projected_mass,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2);
let rhs_wo_bias = (vel1
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
}
// Builder.
{
let local_p1 = rb1.position.inverse_transform_point(&manifold_point.point);
let local_p2 = rb2
.pos
.position
.inverse_transform_point(&manifold_point.point);
let infos = ContactPointInfos {
local_p1,
local_p2,
tangent_vel: flipped_multiplier * manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut OneBodyConstraint,
) {
let rb2 = &bodies[constraint.solver_vel2];
self.update_with_positions(
params,
solved_dt,
&rb2.position,
rb2.ccd_thickness,
constraint,
)
}
// TODO: this code is SOOOO similar to TwoBodyConstraint::update.
// In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
pub fn update_with_positions(
&self,
params: &IntegrationParameters,
solved_dt: Real,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut OneBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
let rb1 = &self.rb1;
// Integrate the velocity of the static rigid-body, if it’s kinematic.
let new_pos1 = self
.vels1
.integrate(solved_dt, &rb1.position, &rb1.local_com);
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = rb2_pos * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error)
.clamp(-params.max_penetration_correction, 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
}
// Tangent part.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraint {
pub solver_vel2: usize,
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub elements: [OneBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
}
impl OneBodyConstraint {
pub fn invalid() -> Self {
Self {
solver_vel2: usize::MAX,
dir1: Vector::zeros(),
#[cfg(feature = "dim3")]
tangent1: Vector::zeros(),
im2: Vector::zeros(),
cfm_factor: 0.0,
limit: 0.0,
elements: [OneBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
manifold_id: ContactManifoldIndex::MAX,
manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
num_contacts: u8::MAX,
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel2 = solver_vels[self.solver_vel2 as usize];
OneBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
self.limit,
&mut solver_vel2,
solve_normal,
solve_friction,
);
solver_vels[self.solver_vel2 as usize] = solver_vel2;
}
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

232
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs Normal file
View File

@@ -0,0 +1,232 @@
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub total_impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub total_impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
total_impulse: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
#[inline]
pub fn apply_limit(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
#[inline]
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let dvel = -tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = -tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let dvel_1 = -tangents1[1].dot(&solver_vel2.linear)
+ self.gcross2[1].gdot(solver_vel2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub total_impulse: N,
pub r: N,
}
impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r: na::zero(),
}
}
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel =
-dir1.dot(&solver_vel2.linear) + self.gcross2.gdot(solver_vel2.angular) + self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintElement<N: SimdRealCopy> {
pub normal_part: OneBodyConstraintNormalPart<N>,
pub tangent_part: OneBodyConstraintTangentPart<N>,
}
impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: OneBodyConstraintNormalPart::zero(),
tangent_part: OneBodyConstraintTangentPart::zero(),
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
solve_normal: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(&tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, &dir1, im2, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im2, limit, solver_vel2);
}
}
// Solve friction.
if solve_friction {
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im2, limit, solver_vel2);
}
}
}
}

269
src/dynamics/solver/velocity_ground_constraint_wide.rs → src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -1,52 +1,38 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityGroundConstraint {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
pub(crate) struct SimdOneBodyConstraintBuilder {
// PERF: only store what’s needed, and store it in simd form.
rb1: [SolverBody; SIMD_WIDTH],
vels1: [RigidBodyVelocity; SIMD_WIDTH],
infos: [ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
}
impl WVelocityGroundConstraint {
impl SimdOneBodyConstraintBuilder {
pub fn generate(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
out_builders: &mut [SimdOneBodyConstraintBuilder],
out_constraints: &mut [OneBodyConstraintSimd],
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
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];
@@ -58,23 +44,26 @@ impl WVelocityGroundConstraint {
}
}
let rb1: [SolverBody; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_else(SolverBody::default)
}];
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::zero)
.unwrap_or_else(RigidBodyVelocity::default)
}];
let world_com1 = Point::from(gather![|ii| {
handles1[ii]
.map(|h| bodies[h].mprops.world_com)
.unwrap_or_else(Point::origin)
}]);
let world_com1 = Point::from(gather![|ii| { rb1[ii].world_com }]);
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]);
let flipped_sign = SimdReal::from(flipped);
@@ -88,12 +77,13 @@ impl WVelocityGroundConstraint {
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let poss2 = Isometry::from(gather![|ii| bodies2[ii].pos.position]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = normal1 * -flipped_sign;
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -106,20 +96,21 @@ impl WVelocityGroundConstraint {
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut is_fast_contact = SimdBool::splat(false);
let mut constraint = WVelocityGroundConstraint {
dir1: force_dir1,
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
builder.rb1 = rb1;
builder.vels1 = vels1;
constraint.dir1 = force_dir1;
constraint.im2 = im2;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2,
cfm_factor,
limit: SimdReal::splat(0.0),
mj_lambda2,
manifold_id,
manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8,
};
{
constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
@@ -127,9 +118,10 @@ impl WVelocityGroundConstraint {
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(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let tangent_velocity =
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
@@ -143,6 +135,7 @@ impl WVelocityGroundConstraint {
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
let normal_rhs_wo_bias;
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
@@ -150,24 +143,18 @@ impl WVelocityGroundConstraint {
force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
let projected_vel1 = vel1.dot(&force_dir1);
let projected_vel2 = vel2.dot(&force_dir1);
let projected_velocity = projected_vel1 - projected_vel2;
normal_rhs_wo_bias =
(is_bouncy * restitution) * projected_velocity + projected_vel1; // Add projected_vel1 since it’s not accessible through solver_vel.
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
constraint.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2,
rhs,
rhs_wo_bias,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r: projected_mass,
};
}
@@ -179,10 +166,11 @@ impl WVelocityGroundConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r =
tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
let rhs_wo_bias = (vel1 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
@@ -196,33 +184,139 @@ impl WVelocityGroundConstraint {
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
// Builder.
{
let local_p1 = poss1.inverse_transform_point(&point);
let local_p2 = poss2.inverse_transform_point(&point);
let infos = ContactPointInfos {
local_p1,
local_p2,
tangent_vel: tangent_velocity * flipped_sign,
dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
}
}
}
// TODO: this code is SOOOO similar to TwoBodyConstraintSimd::update.
// In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut OneBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
// Integrate the velocity of the static rigid-body, if it’s kinematic.
let new_pos1 = Isometry::from(gather![|ii| self.vels1[ii].integrate(
solved_dt,
&self.rb1[ii].position,
&self.rb1[ii].local_com
)]);
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = poss2 * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
}
// tangent parts.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::GroupedGround(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintSimd {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [OneBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub solver_vel2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl OneBodyConstraintSimd {
pub fn solve(
&mut self,
mj_lambdas: &mut [DeltaVel<Real>],
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel2[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|ii| solver_vels[self.solver_vel2[ii] as usize].angular
]),
};
VelocityGroundConstraintElement::solve_group(
OneBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
@@ -230,18 +324,18 @@ impl WVelocityGroundConstraint {
&self.tangent1,
&self.im2,
self.limit,
&mut mj_lambda2,
&mut solver_vel2,
solve_normal,
solve_friction,
);
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
}
}
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
@@ -272,6 +366,7 @@ impl WVelocityGroundConstraint {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

470
src/dynamics/solver/contact_constraint/two_body_constraint.rs Normal file
View File

@@ -0,0 +1,470 @@
use super::{ContactConstraintTypes, ContactPointInfos};
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
use na::DVector;
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
pub fn remove_bias(&mut self) {
match self {
Self::OneBody(c) => c.remove_cfm_and_bias_from_rhs(),
Self::TwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
Self::GenericOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
Self::GenericTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
}
}
pub fn solve_restitution(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels, true, false),
Self::TwoBodies(c) => c.solve(solver_vels, true, false),
Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, true, false),
Self::GenericTwoBodies(c) => c.solve(
generic_jacobians,
solver_vels,
generic_solver_vels,
true,
false,
),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels, true, false),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels, true, false),
}
}
pub fn solve_friction(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels, false, true),
Self::TwoBodies(c) => c.solve(solver_vels, false, true),
Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, false, true),
Self::GenericTwoBodies(c) => c.solve(
generic_jacobians,
solver_vels,
generic_solver_vels,
false,
true,
),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels, false, true),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels, false, true),
}
}
pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
match self {
Self::OneBody(c) => c.writeback_impulses(manifolds_all),
Self::TwoBodies(c) => c.writeback_impulses(manifolds_all),
Self::GenericOneBody(c) => c.writeback_impulses(manifolds_all),
Self::GenericTwoBodies(c) => c.writeback_impulses(manifolds_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.writeback_impulses(manifolds_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.writeback_impulses(manifolds_all),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraint {
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub solver_vel1: usize,
pub solver_vel2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub elements: [TwoBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraint {
pub fn invalid() -> Self {
Self {
dir1: Vector::zeros(),
#[cfg(feature = "dim3")]
tangent1: Vector::zeros(),
im1: Vector::zeros(),
im2: Vector::zeros(),
cfm_factor: 0.0,
limit: 0.0,
solver_vel1: usize::MAX,
solver_vel2: usize::MAX,
manifold_id: ContactManifoldIndex::MAX,
manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
num_contacts: u8::MAX,
elements: [TwoBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintBuilder {
pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_builders: &mut [TwoBodyConstraintBuilder],
out_constraints: &mut [TwoBodyConstraint],
) {
assert_eq!(manifold.data.relative_dominance, 0);
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let rb1 = &bodies[handle1];
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let solver_vel1 = rb1.ids.active_set_offset;
let solver_vel2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
constraint.dir1 = force_dir1;
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let point = manifold_point.point;
let dp1 = point - mprops1.world_com;
let dp2 = point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let projected_mass = utils::inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
normal_rhs_wo_bias =
(is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r: projected_mass,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
}
// Builder.
let infos = ContactPointInfos {
local_p1: rb1
.pos
.position
.inverse_transform_point(&manifold_point.point),
local_p2: rb2
.pos
.position
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut TwoBodyConstraint,
) {
let rb1 = &bodies[constraint.solver_vel1];
let rb2 = &bodies[constraint.solver_vel2];
let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
self.update_with_positions(
params,
solved_dt,
&rb1.position,
&rb2.position,
ccd_thickness,
constraint,
)
}
// Used by both generic and non-generic builders..
pub fn update_with_positions(
&self,
params: &IntegrationParameters,
solved_dt: Real,
rb1_pos: &Isometry<Real>,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut TwoBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// Tangent velocity is equivalent to the first body’s surface moving artificially.
let p1 = rb1_pos * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = rb2_pos * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error)
.clamp(-params.max_penetration_correction, 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
}
// Tangent part.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
}
}
impl TwoBodyConstraint {
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel1 = solver_vels[self.solver_vel1 as usize];
let mut solver_vel2 = solver_vels[self.solver_vel2 as usize];
TwoBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
self.limit,
&mut solver_vel1,
&mut solver_vel2,
solve_normal,
solve_friction,
);
solver_vels[self.solver_vel1 as usize] = solver_vel1;
solver_vels[self.solver_vel2 as usize] = solver_vel2;
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
// elt.normal_part.impulse = elt.normal_part.total_impulse;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
// elt.tangent_part.impulse = elt.tangent_part.total_impulse;
}
}
}
#[inline(always)]
#[cfg(feature = "dim3")]
pub(crate) fn compute_tangent_contact_directions<N>(
force_dir1: &Vector<N>,
linvel1: &Vector<N>,
linvel2: &Vector<N>,
) -> [Vector<N>; DIM - 1]
where
N: utils::SimdRealCopy,
Vector<N>: SimdBasis,
{
use na::SimdValue;
// Compute the tangent direction. Pick the direction of
// the linear relative velocity, if it is not too small.
// Otherwise use a fallback direction.
let relative_linvel = linvel1 - linvel2;
let mut tangent_relative_linvel =
relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
let tangent_linvel_norm = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
tangent_relative_linvel.normalize_mut()
};
const THRESHOLD: Real = 1.0e-4;
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
let tangent_fallback = force_dir1.orthonormal_vector();
let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
let bitangent1 = force_dir1.cross(&tangent1);
[tangent1, bitangent1]
}

271
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs Normal file
View File

@@ -0,0 +1,271 @@
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross1: [AngVector<N>; DIM - 1],
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub total_impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub total_impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross1: [na::zero(); DIM - 1],
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
total_impulse: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
#[inline]
pub fn apply_limit(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
#[inline]
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let dvel = tangents1[0].dot(&solver_vel1.linear)
+ self.gcross1[0].gdot(solver_vel1.angular)
- tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = tangents1[0].dot(&solver_vel1.linear)
+ self.gcross1[0].gdot(solver_vel1.angular)
- tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let dvel_1 = tangents1[1].dot(&solver_vel1.linear)
+ self.gcross1[1].gdot(solver_vel1.angular)
- tangents1[1].dot(&solver_vel2.linear)
+ self.gcross2[1].gdot(solver_vel2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
pub gcross1: AngVector<N>,
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub total_impulse: N,
pub r: N,
}
impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross1: na::zero(),
gcross2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
r: na::zero(),
}
}
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel = dir1.dot(&solver_vel1.linear) + self.gcross1.gdot(solver_vel1.angular)
- dir1.dot(&solver_vel2.linear)
+ self.gcross2.gdot(solver_vel2.angular)
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += dir1.component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1 * dlambda;
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintElement<N: SimdRealCopy> {
pub normal_part: TwoBodyConstraintNormalPart<N>,
pub tangent_part: TwoBodyConstraintTangentPart<N>,
}
impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: TwoBodyConstraintNormalPart::zero(),
tangent_part: TwoBodyConstraintTangentPart::zero(),
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
solve_normal: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(&tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, &dir1, im1, im2, solver_vel1, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
}
// Solve friction.
if solve_friction {
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
}
}
}

258
src/dynamics/solver/velocity_constraint_wide.rs → src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -1,57 +1,39 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityConstraint {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
pub(crate) struct TwoBodyConstraintBuilderSimd {
infos: [super::ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
}
impl WVelocityConstraint {
impl TwoBodyConstraintBuilderSimd {
pub fn generate(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
out_builders: &mut [TwoBodyConstraintBuilderSimd],
out_constraints: &mut [TwoBodyConstraintSimd],
) {
for ii in 0..SIMD_WIDTH {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
@@ -62,9 +44,8 @@ impl WVelocityConstraint {
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]);
let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]);
let ccd_thickness = ccd_thickness1 + ccd_thickness2;
let poss1 = Isometry::from(gather![|ii| bodies[handles1[ii]].pos.position]);
let poss2 = Isometry::from(gather![|ii| bodies[handles2[ii]].pos.position]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
@@ -84,8 +65,8 @@ impl WVelocityConstraint {
let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let solver_vel1 = gather![|ii| ids1[ii].active_set_offset];
let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -99,22 +80,20 @@ impl WVelocityConstraint {
gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut is_fast_contact = SimdBool::splat(false);
let mut constraint = WVelocityConstraint {
dir1: force_dir1,
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = im1;
constraint.im2 = im2;
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1,
im2,
cfm_factor,
limit: SimdReal::splat(0.0),
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8,
};
{
constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
@@ -122,9 +101,10 @@ impl WVelocityConstraint {
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(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let tangent_velocity =
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let dp1 = point - world_com1;
@@ -137,6 +117,7 @@ impl WVelocityConstraint {
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
let normal_rhs_wo_bias;
{
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
@@ -149,24 +130,15 @@ impl WVelocityConstraint {
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
normal_rhs_wo_bias = is_bouncy * restitution * projected_velocity;
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: SimdReal::splat(0.0),
total_impulse: SimdReal::splat(0.0),
r: projected_mass,
};
}
@@ -181,11 +153,12 @@ impl WVelocityConstraint {
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
let rhs_wo_bias = tangent_velocity.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
@@ -201,40 +174,142 @@ impl WVelocityConstraint {
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
// Builder.
let infos = ContactPointInfos {
local_p1: poss1.inverse_transform_point(&point),
local_p2: poss2.inverse_transform_point(&point),
tangent_vel: tangent_velocity,
dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut TwoBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
+ SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = poss1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = poss2 * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
}
// tangent parts.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::Grouped(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintSimd {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [TwoBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub solver_vel1: [usize; SIMD_WIDTH],
pub solver_vel2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraintSimd {
pub fn solve(
&mut self,
mj_lambdas: &mut [DeltaVel<Real>],
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel1[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|ii| solver_vels[self.solver_vel1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel2[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|ii| solver_vels[self.solver_vel2[ii] as usize].angular
]),
};
VelocityConstraintElement::solve_group(
TwoBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
@@ -243,19 +318,19 @@ impl WVelocityConstraint {
&self.im1,
&self.im2,
self.limit,
&mut mj_lambda1,
&mut mj_lambda2,
&mut solver_vel1,
&mut solver_vel2,
solve_normal,
solve_friction,
);
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.angular.extract(ii);
}
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
}
}
@@ -289,6 +364,7 @@ impl WVelocityConstraint {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

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

@@ -1,240 +0,0 @@
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross;
use super::{
AnyVelocityConstraint, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityGroundConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub velocity_constraint: VelocityGroundConstraint,
pub j_id: usize,
pub ndofs2: usize,
}
impl GenericVelocityGroundConstraint {
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb2 = &bodies[handle2.unwrap()];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h))
.map(|m| (&multibodies[m.multibody], m.id))
.unwrap();
let mj_lambda2 = mb2.solver_id;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
// the multibodies jacobian and weighted jacobians
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let mut is_fast_contact = false;
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
let torque_dir2 = dp2.gcross(-force_dir1);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-force_dir1,
#[cfg(feature = "dim2")]
na::vector!(torque_dir2),
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let dvel = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs,
rhs_wo_bias,
impulse: na::zero(),
r,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let torque_dir2 = dp2.gcross(-tangents1[j]);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-tangents1[j],
#[cfg(feature = "dim2")]
na::vector![torque_dir2],
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.rhs[j] = rhs;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.elements[k].tangent_part.r[j] = r;
}
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
let constraint = GenericVelocityGroundConstraint {
velocity_constraint: constraint,
j_id: chunk_j_id,
ndofs2: mb2.ndofs(),
};
if let Some(at) = insert_at {
out_constraints[at + _l] =
AnyVelocityConstraint::NongroupedGenericGround(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
}
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize;
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityGroundConstraintElement::generic_solve_group(
self.velocity_constraint.cfm_factor,
elements,
jacobians,
self.velocity_constraint.limit,
self.ndofs2,
self.j_id,
mj_lambda2,
generic_mj_lambdas,
solve_restitution,
solve_friction,
);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.velocity_constraint.writeback_impulses(manifolds_all);
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_cfm_and_bias_from_rhs();
}
}

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

@@ -171,7 +171,7 @@ pub(crate) struct InteractionGroups {
#[cfg(feature = "simd-is-enabled")]
body_masks: Vec<u128>,
#[cfg(feature = "simd-is-enabled")]
pub grouped_interactions: Vec<usize>,
pub simd_interactions: Vec<usize>,
pub nongrouped_interactions: Vec<usize>,
}
@@ -183,7 +183,7 @@ impl InteractionGroups {
#[cfg(feature = "simd-is-enabled")]
body_masks: Vec::new(),
#[cfg(feature = "simd-is-enabled")]
grouped_interactions: Vec::new(),
simd_interactions: Vec::new(),
nongrouped_interactions: Vec::new(),
}
}
@@ -194,7 +194,7 @@ impl InteractionGroups {
// {
// self.buckets.clear();
// self.body_masks.clear();
// self.grouped_interactions.clear();
// self.simd_interactions.clear();
// }
// self.nongrouped_interactions.clear();
// }
@@ -300,7 +300,7 @@ impl InteractionGroups {
if bucket.1 == SIMD_LAST_INDEX {
// We completed our group.
(bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
self.grouped_interactions.extend_from_slice(&bucket.0);
self.simd_interactions.extend_from_slice(&bucket.0);
bucket.1 = 0;
occupied_mask &= !target_mask_bit;
@@ -340,20 +340,20 @@ impl InteractionGroups {
self.body_masks.iter_mut().for_each(|e| *e = 0);
assert!(
self.grouped_interactions.len() % SIMD_WIDTH == 0,
self.simd_interactions.len() % SIMD_WIDTH == 0,
"Invalid SIMD contact grouping."
);
// println!(
// "Num grouped interactions: {}, nongrouped: {}",
// self.grouped_interactions.len(),
// self.simd_interactions.len(),
// self.nongrouped_interactions.len()
// );
}
pub fn clear_groups(&mut self) {
#[cfg(feature = "simd-is-enabled")]
self.grouped_interactions.clear();
self.simd_interactions.clear();
self.nongrouped_interactions.clear();
}
@@ -466,7 +466,7 @@ impl InteractionGroups {
if bucket.1 == SIMD_LAST_INDEX {
// We completed our group.
(bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
self.grouped_interactions.extend_from_slice(&bucket.0);
self.simd_interactions.extend_from_slice(&bucket.0);
bucket.1 = 0;
occupied_mask = occupied_mask & (!target_mask_bit);
} else {
@@ -497,7 +497,7 @@ impl InteractionGroups {
}
assert!(
self.grouped_interactions.len() % SIMD_WIDTH == 0,
self.simd_interactions.len() % SIMD_WIDTH == 0,
"Invalid SIMD contact grouping."
);
}

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

@@ -1,16 +1,15 @@
use super::VelocitySolver;
use super::{JointConstraintsSet, VelocitySolver};
use crate::counters::Counters;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
use crate::dynamics::IslandManager;
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::MultibodyJointSet;
use parry::math::Real;
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint>,
contact_constraints: ContactConstraintsSet,
joint_constraints: JointConstraintsSet,
velocity_solver: VelocitySolver,
}
@@ -23,8 +22,8 @@ impl Default for IslandSolver {
impl IslandSolver {
pub fn new() -> Self {
Self {
contact_constraints: SolverConstraints::new(),
joint_constraints: SolverConstraints::new(),
contact_constraints: ContactConstraintsSet::new(),
joint_constraints: JointConstraintsSet::new(),
velocity_solver: VelocitySolver::new(),
}
}
@@ -33,57 +32,71 @@ impl IslandSolver {
&mut self,
island_id: usize,
counters: &mut Counters,
params: &IntegrationParameters,
base_params: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut RigidBodySet,
manifolds: &mut [&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
impulse_joints: &mut [JointGraphEdge],
joint_indices: &[JointIndex],
multibody_joints: &mut MultibodyJointSet,
multibodies: &mut MultibodyJointSet,
) {
// Init the solver id for multibody_joints.
// We need that for building the constraints.
let mut solver_id = 0;
for (_, multibody) in multibody_joints.multibodies.iter_mut() {
multibody.solver_id = solver_id;
solver_id += multibody.ndofs();
}
counters.solver.velocity_resolution_time.resume();
let num_solver_iterations = base_params.num_solver_iterations.get()
+ islands.active_island_additional_solver_iterations(island_id);
counters.solver.velocity_assembly_time.resume();
self.contact_constraints.init(
let mut params = *base_params;
params.dt /= num_solver_iterations as Real;
params.damping_ratio /= num_solver_iterations as Real;
// params.joint_damping_ratio /= num_solver_iterations as Real;
/*
*
* Bellow this point, the `params` is using the "small step" settings.
*
*/
// INIT
self.velocity_solver
.init_solver_velocities_and_solver_bodies(
&params,
island_id,
params,
islands,
bodies,
multibody_joints,
multibodies,
);
self.velocity_solver.init_constraints(
island_id,
islands,
bodies,
multibodies,
manifolds,
manifold_indices,
);
self.joint_constraints.init(
island_id,
params,
islands,
bodies,
multibody_joints,
impulse_joints,
joint_indices,
&mut self.contact_constraints,
&mut self.joint_constraints,
);
counters.solver.velocity_assembly_time.pause();
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve(
island_id,
params,
islands,
// SOLVE
self.velocity_solver.solve_constraints(
&params,
num_solver_iterations,
bodies,
multibody_joints,
manifolds,
impulse_joints,
&mut self.contact_constraints.velocity_constraints,
&self.contact_constraints.generic_jacobians,
&mut self.joint_constraints.velocity_constraints,
&self.joint_constraints.generic_jacobians,
multibodies,
&mut self.contact_constraints,
&mut self.joint_constraints,
);
// WRITEBACK
self.joint_constraints.writeback_impulses(impulse_joints);
self.contact_constraints.writeback_impulses(manifolds);
self.velocity_solver.writeback_bodies(
base_params,
num_solver_iterations,
islands,
island_id,
bodies,
multibodies,
);
counters.solver.velocity_resolution_time.pause();
}

97
src/dynamics/solver/joint_constraint/any_joint_constraint.rs Normal file
View File

@@ -0,0 +1,97 @@
use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointOneBodyConstraint, JointTwoBodyConstraint,
};
use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
use crate::dynamics::JointGraphEdge;
use crate::math::Real;
use na::DVector;
use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
};
use crate::dynamics::solver::solver_vel::SolverVel;
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
},
math::{SimdReal, SIMD_WIDTH},
};
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
};
pub struct JointConstraintTypes;
impl<'a> AnyConstraintMut<'a, JointConstraintTypes> {
pub fn remove_bias(&mut self) {
match self {
Self::OneBody(c) => c.remove_bias_from_rhs(),
Self::TwoBodies(c) => c.remove_bias_from_rhs(),
Self::GenericOneBody(c) => c.remove_bias_from_rhs(),
Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(),
}
}
pub fn solve(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels),
Self::TwoBodies(c) => c.solve(solver_vels),
Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels),
Self::GenericTwoBodies(c) => {
c.solve(generic_jacobians, solver_vels, generic_solver_vels)
}
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels),
}
}
pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
match self {
Self::OneBody(c) => c.writeback_impulses(joints_all),
Self::TwoBodies(c) => c.writeback_impulses(joints_all),
Self::GenericOneBody(c) => c.writeback_impulses(joints_all),
Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all),
}
}
}
impl ConstraintTypes for JointConstraintTypes {
type OneBody = JointOneBodyConstraint<Real, 1>;
type TwoBodies = JointTwoBodyConstraint<Real, 1>;
type GenericOneBody = JointGenericOneBodyConstraint;
type GenericTwoBodies = JointGenericTwoBodyConstraint;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody = JointOneBodyConstraint<SimdReal, SIMD_WIDTH>;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies = JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>;
type BuilderOneBody = JointOneBodyConstraintBuilder;
type BuilderTwoBodies = JointTwoBodyConstraintBuilder;
type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder;
type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd;
}

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

@@ -1,540 +0,0 @@
use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointVelocityConstraint, JointVelocityGroundConstraint, SolverBody,
};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
};
use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
use na::DVector;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
#[cfg(feature = "parallel")]
use crate::dynamics::JointAxesMask;
pub enum AnyJointVelocityConstraint {
JointConstraint(JointVelocityConstraint<Real, 1>),
JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
JointGenericConstraint(JointGenericVelocityConstraint),
JointGenericGroundConstraint(JointGenericVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
JointConstraintSimd(JointVelocityConstraint<SimdReal, SIMD_WIDTH>),
#[cfg(feature = "simd-is-enabled")]
JointGroundConstraintSimd(JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH>),
Empty,
}
impl AnyJointVelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, usize) {
let joint = &joint.data;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
+ locked_axes.count_ones() as usize
+ (limit_axes & !coupled_axes).count_ones() as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
(num_constraints, num_constraints)
}
pub fn from_joint(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [rb1.ids.active_set_offset],
};
let body2 = SolverBody {
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
let mb1 = multibodies
.rigid_body_link(joint.body1)
.map(|link| (&multibodies[link.multibody], link.id));
let mb2 = multibodies
.rigid_body_link(joint.body2)
.map(|link| (&multibodies[link.multibody], link.id));
if mb1.is_some() || mb2.is_some() {
let multibodies_ndof = mb1.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM)
+ mb2.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM);
if multibodies_ndof == 0 {
// Both multibodies are fixed, don’t generate any constraint.
return;
}
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
// constraints appends the multibodies jacobian and weighted jacobians.
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
// to the generic DVector.
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointGenericVelocityConstraint::invalid(); 12];
let out_tmp_len = JointGenericVelocityConstraint::lock_axes(
params,
joint_id,
&body1,
&body2,
mb1,
mb2,
&frame1,
&frame2,
&joint.data,
jacobians,
j_id,
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
}
}
} else {
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
let out_tmp_len = JointVelocityConstraint::<Real, 1>::lock_axes(
params,
joint_id,
&body1,
&body2,
&frame1,
&frame2,
&joint.data,
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointConstraint(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraint(c));
}
}
}
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &RigidBodySet,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| &bodies[impulse_joints[ii].body1].pos],
gather![|ii| &bodies[impulse_joints[ii].body1].vels],
gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
gather![|ii| &bodies[impulse_joints[ii].body1].ids],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| &bodies[impulse_joints[ii].body2].pos],
gather![|ii| &bodies[impulse_joints[ii].body2].vels],
gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
gather![|ii| &bodies[impulse_joints[ii].body2].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
let local_frame1: Isometry<SimdReal> =
gather![|ii| impulse_joints[ii].data.local_frame1].into();
let local_frame2: Isometry<SimdReal> =
gather![|ii| impulse_joints[ii].data.local_frame2].into();
let frame1 = pos1 * local_frame1;
let frame2 = pos2 * local_frame2;
let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
linvel: gather![|ii| rb_vel1[ii].linvel].into(),
angvel: gather![|ii| rb_vel1[ii].angvel].into(),
im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
mj_lambda: gather![|ii| rb_ids1[ii].active_set_offset],
};
let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
linvel: gather![|ii| rb_vel2[ii].linvel].into(),
angvel: gather![|ii| rb_vel2[ii].angvel].into(),
im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
};
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
let out_tmp_len = JointVelocityConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
params,
joint_id,
&body1,
&body2,
&frame1,
&frame2,
impulse_joints[0].data.locked_axes.bits(),
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
}
}
}
pub fn from_joint_ground(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let flipped = !bodies[handle2].is_dynamic();
let (local_frame1, local_frame2) = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
(joint.data.local_frame2, joint.data.local_frame1)
} else {
(joint.data.local_frame1, joint.data.local_frame2)
};
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [crate::INVALID_USIZE],
};
let body2 = SolverBody {
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
if let Some(mb2) = multibodies
.rigid_body_link(handle2)
.map(|link| (&multibodies[link.multibody], link.id))
{
let multibodies_ndof = mb2.0.ndofs();
if multibodies_ndof == 0 {
// The multibody is fixed, don’t generate any constraint.
return;
}
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
// constraints appends the multibodies jacobian and weighted jacobians.
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
// to the generic DVector.
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointGenericVelocityGroundConstraint::invalid(); 12];
let out_tmp_len = JointGenericVelocityGroundConstraint::lock_axes(
params,
joint_id,
&body1,
&body2,
mb2,
&frame1,
&frame2,
&joint.data,
jacobians,
j_id,
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
}
}
} else {
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
let out_tmp_len = JointVelocityGroundConstraint::<Real, 1>::lock_axes(
params,
joint_id,
&body1,
&body2,
&frame1,
&frame2,
&joint.data,
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
}
}
}
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &RigidBodySet,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
let mut handles2 = gather![|ii| impulse_joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let local_frame1: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
impulse_joints[ii].data.local_frame2
} else {
impulse_joints[ii].data.local_frame1
}]
.into();
let local_frame2: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
impulse_joints[ii].data.local_frame1
} else {
impulse_joints[ii].data.local_frame2
}]
.into();
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
) = (
gather![|ii| &bodies[handles1[ii]].pos],
gather![|ii| &bodies[handles1[ii]].vels],
gather![|ii| &bodies[handles1[ii]].mprops],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| &bodies[handles2[ii]].pos],
gather![|ii| &bodies[handles2[ii]].vels],
gather![|ii| &bodies[handles2[ii]].mprops],
gather![|ii| &bodies[handles2[ii]].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
let frame1 = pos1 * local_frame1;
let frame2 = pos2 * local_frame2;
let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
linvel: gather![|ii| rb_vel1[ii].linvel].into(),
angvel: gather![|ii| rb_vel1[ii].angvel].into(),
im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
mj_lambda: [crate::INVALID_USIZE; SIMD_WIDTH],
};
let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
linvel: gather![|ii| rb_vel2[ii].linvel].into(),
angvel: gather![|ii| rb_vel2[ii].angvel].into(),
im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
};
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
let out_tmp_len = JointVelocityGroundConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
params,
joint_id,
&body1,
&body2,
&frame1,
&frame2,
impulse_joints[0].data.locked_axes.bits(),
&mut out_tmp,
);
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
}
} else {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
}
}
}
pub fn remove_bias_from_rhs(&mut self) {
match self {
AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(),
AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.remove_bias_from_rhs(),
AnyJointVelocityConstraint::JointGenericConstraint(c) => c.remove_bias_from_rhs(),
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => c.remove_bias_from_rhs(),
AnyJointVelocityConstraint::Empty => unreachable!(),
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
) {
match self {
AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::JointGenericConstraint(c) => {
c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
}
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
}
AnyJointVelocityConstraint::Empty => unreachable!(),
}
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
match self {
AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::JointGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::JointGenericConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::Empty => unreachable!(),
}
}
}

844
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs → src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

File diff suppressed because it is too large Load Diff

446
src/dynamics/solver/joint_constraint/joint_constraints_set.rs Normal file
View File

@@ -0,0 +1,446 @@
use crate::dynamics::solver::categorization::categorize_joints;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{
reset_buffer, JointConstraintTypes, JointGenericOneBodyConstraint,
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraint,
JointGenericTwoBodyConstraintBuilder, JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet,
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
RigidBodySet,
};
use na::DVector;
use parry::math::Real;
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
};
#[cfg(feature = "simd-is-enabled")]
use {
crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
},
crate::math::SIMD_WIDTH,
};
pub type JointConstraintsSet = SolverConstraintsSet<JointConstraintTypes>;
impl JointConstraintsSet {
pub fn init(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
// Generate constraints for impulse_joints.
self.two_body_interactions.clear();
self.one_body_interactions.clear();
self.generic_two_body_interactions.clear();
self.generic_one_body_interactions.clear();
categorize_joints(
bodies,
multibody_joints,
impulse_joints,
joint_constraint_indices,
&mut self.one_body_interactions,
&mut self.two_body_interactions,
&mut self.generic_one_body_interactions,
&mut self.generic_two_body_interactions,
);
self.clear_constraints();
self.clear_builders();
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
islands,
bodies,
impulse_joints,
&self.two_body_interactions,
);
self.one_body_interaction_groups.clear_groups();
self.one_body_interaction_groups.group_joints(
island_id,
islands,
bodies,
impulse_joints,
&self.one_body_interactions,
);
// NOTE: uncomment this do disable SIMD joint resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.simd_interactions);
// self.one_body_interaction_groups
// .nongrouped_interactions
// .append(&mut self.one_body_interaction_groups.simd_interactions);
let mut j_id = 0;
self.compute_joint_constraints(bodies, impulse_joints);
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_joint_constraints(bodies, impulse_joints);
}
self.compute_generic_joint_constraints(bodies, multibody_joints, impulse_joints, &mut j_id);
self.compute_joint_one_body_constraints(bodies, impulse_joints);
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_joint_one_body_constraints(bodies, impulse_joints);
}
self.compute_generic_one_body_joint_constraints(
island_id,
islands,
bodies,
multibody_joints,
impulse_joints,
&mut j_id,
);
}
fn compute_joint_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let total_num_builders = self
.one_body_interaction_groups
.nongrouped_interactions
.len();
unsafe {
reset_buffer(
&mut self.velocity_one_body_constraints_builder,
total_num_builders,
);
}
let mut num_constraints = 0;
for (joint_i, builder) in self
.one_body_interaction_groups
.nongrouped_interactions
.iter()
.zip(self.velocity_one_body_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointOneBodyConstraintBuilder::generate(
joint,
bodies,
*joint_i,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(&mut self.velocity_one_body_constraints, num_constraints);
}
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_joint_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let total_num_builders =
self.one_body_interaction_groups.simd_interactions.len() / SIMD_WIDTH;
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints_builder,
total_num_builders,
);
}
let mut num_constraints = 0;
for (joints_i, builder) in self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.zip(self.simd_velocity_one_body_constraints_builder.iter_mut())
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
JointOneBodyConstraintBuilderSimd::generate(
impulse_joints,
bodies,
joints_id,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints,
num_constraints,
);
}
}
fn compute_joint_constraints(&mut self, bodies: &RigidBodySet, joints_all: &[JointGraphEdge]) {
let total_num_builders = self.interaction_groups.nongrouped_interactions.len();
unsafe {
reset_buffer(&mut self.velocity_constraints_builder, total_num_builders);
}
let mut num_constraints = 0;
for (joint_i, builder) in self
.interaction_groups
.nongrouped_interactions
.iter()
.zip(self.velocity_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointTwoBodyConstraintBuilder::generate(
joint,
bodies,
*joint_i,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(&mut self.velocity_constraints, num_constraints);
}
}
fn compute_generic_joint_constraints(
&mut self,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
let total_num_builders = self.generic_two_body_interactions.len();
self.generic_velocity_constraints_builder.resize(
total_num_builders,
JointGenericTwoBodyConstraintBuilder::invalid(),
);
let mut num_constraints = 0;
for (joint_i, builder) in self
.generic_two_body_interactions
.iter()
.zip(self.generic_velocity_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointGenericTwoBodyConstraintBuilder::generate(
*joint_i,
joint,
bodies,
multibodies,
builder,
j_id,
&mut self.generic_jacobians,
&mut num_constraints,
);
}
self.generic_velocity_constraints
.resize(num_constraints, JointGenericTwoBodyConstraint::invalid());
}
fn compute_generic_one_body_joint_constraints(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
let mut total_num_builders = self.generic_one_body_interactions.len();
for handle in islands.active_island(island_id) {
if let Some(link_id) = multibodies.rigid_body_link(*handle) {
if JointGenericVelocityOneBodyInternalConstraintBuilder::num_constraints(
multibodies,
link_id,
) > 0
{
total_num_builders += 1;
}
}
}
self.generic_velocity_one_body_constraints_builder.resize(
total_num_builders,
JointGenericOneBodyConstraintBuilder::invalid(),
);
let mut num_constraints = 0;
for (joint_i, builder) in self.generic_one_body_interactions.iter().zip(
self.generic_velocity_one_body_constraints_builder
.iter_mut(),
) {
let joint = &joints_all[*joint_i].weight;
JointGenericVelocityOneBodyExternalConstraintBuilder::generate(
*joint_i,
joint,
bodies,
multibodies,
builder,
j_id,
&mut self.generic_jacobians,
&mut num_constraints,
);
}
let mut curr_builder = self.generic_one_body_interactions.len();
for handle in islands.active_island(island_id) {
if curr_builder >= self.generic_velocity_one_body_constraints_builder.len() {
break; // No more builder need to be generated.
}
if let Some(link_id) = multibodies.rigid_body_link(*handle) {
let prev_num_constraints = num_constraints;
JointGenericVelocityOneBodyInternalConstraintBuilder::generate(
multibodies,
link_id,
&mut self.generic_velocity_one_body_constraints_builder[curr_builder],
j_id,
&mut self.generic_jacobians,
&mut num_constraints,
);
if num_constraints != prev_num_constraints {
curr_builder += 1;
}
}
}
self.generic_velocity_one_body_constraints
.resize(num_constraints, JointGenericOneBodyConstraint::invalid());
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_joint_constraints(
&mut self,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let total_num_builders = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH;
unsafe {
reset_buffer(
&mut self.simd_velocity_constraints_builder,
total_num_builders,
);
}
let mut num_constraints = 0;
for (joints_i, builder) in self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.zip(self.simd_velocity_constraints_builder.iter_mut())
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
JointTwoBodyConstraintBuilderSimd::generate(
impulse_joints,
bodies,
joints_id,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(&mut self.simd_velocity_constraints, num_constraints);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.solve(jac, solver_vels, generic_solver_vels);
}
}
pub fn solve_wo_bias(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.remove_bias();
c.solve(jac, solver_vels, generic_solver_vels);
}
}
pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
let (_, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.writeback_impulses(joints_all);
}
}
pub fn update(
&mut self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
solver_bodies: &[SolverBody],
) {
for builder in &mut self.generic_velocity_constraints_builder {
builder.update(
&params,
multibodies,
solver_bodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_constraints,
);
}
for builder in &mut self.generic_velocity_one_body_constraints_builder {
builder.update(
&params,
multibodies,
solver_bodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_one_body_constraints,
);
}
for builder in &mut self.velocity_constraints_builder {
builder.update(&params, solver_bodies, &mut self.velocity_constraints);
}
#[cfg(feature = "simd-is-enabled")]
for builder in &mut self.simd_velocity_constraints_builder {
builder.update(&params, solver_bodies, &mut self.simd_velocity_constraints);
}
for builder in &mut self.velocity_one_body_constraints_builder {
builder.update(
&params,
solver_bodies,
&mut self.velocity_one_body_constraints,
);
}
#[cfg(feature = "simd-is-enabled")]
for builder in &mut self.simd_velocity_one_body_constraints_builder {
builder.update(
&params,
solver_bodies,
&mut self.simd_velocity_one_body_constraints,
);
}
}
}

188
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs → src/dynamics/solver/joint_constraint/joint_generic_constraint.rs
View File

@@ -1,17 +1,19 @@
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::solver::SolverVel;
use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
use crate::math::{Isometry, Real, DIM};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorView, DVectorViewMut};
#[derive(Debug, Copy, Clone)]
pub struct JointGenericVelocityConstraint {
pub struct JointGenericTwoBodyConstraint {
pub is_rigid_body1: bool,
pub is_rigid_body2: bool,
pub mj_lambda1: usize,
pub mj_lambda2: usize,
pub solver_vel1: usize,
pub solver_vel2: usize,
pub ndofs1: usize,
pub j_id1: usize,
@@ -31,31 +33,31 @@ pub struct JointGenericVelocityConstraint {
pub writeback_id: WritebackId,
}
impl Default for JointGenericVelocityConstraint {
impl Default for JointGenericTwoBodyConstraint {
fn default() -> Self {
JointGenericVelocityConstraint::invalid()
JointGenericTwoBodyConstraint::invalid()
}
}
impl JointGenericVelocityConstraint {
impl JointGenericTwoBodyConstraint {
pub fn invalid() -> Self {
Self {
is_rigid_body1: false,
is_rigid_body2: false,
mj_lambda1: 0,
mj_lambda2: 0,
ndofs1: 0,
j_id1: 0,
ndofs2: 0,
j_id2: 0,
joint_id: 0,
solver_vel1: usize::MAX,
solver_vel2: usize::MAX,
ndofs1: usize::MAX,
j_id1: usize::MAX,
ndofs2: usize::MAX,
j_id2: usize::MAX,
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds: [-Real::MAX, Real::MAX],
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
inv_lhs: Real::MAX,
rhs: Real::MAX,
rhs_wo_bias: Real::MAX,
cfm_coeff: Real::MAX,
cfm_gain: Real::MAX,
writeback_id: WritebackId::Dof(0),
}
}
@@ -63,8 +65,8 @@ impl JointGenericVelocityConstraint {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
frame1: &Isometry<Real>,
@@ -79,7 +81,7 @@ impl JointGenericVelocityConstraint {
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -91,7 +93,6 @@ impl JointGenericVelocityConstraint {
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
params,
jacobians,
j_id,
joint_id,
@@ -109,7 +110,6 @@ impl JointGenericVelocityConstraint {
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic(
params,
jacobians,
j_id,
joint_id,
@@ -125,10 +125,7 @@ impl JointGenericVelocityConstraint {
len += 1;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
@@ -203,10 +200,7 @@ impl JointGenericVelocityConstraint {
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
len
}
@@ -218,69 +212,69 @@ impl JointGenericVelocityConstraint {
self.j_id2 + self.ndofs2
}
fn mj_lambda1<'a>(
fn solver_vel1<'a>(
&self,
mj_lambdas: &'a [DeltaVel<Real>],
generic_mj_lambdas: &'a DVector<Real>,
solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.is_rigid_body1 {
mj_lambdas[self.mj_lambda1].as_vector_slice()
solver_vels[self.solver_vel1].as_vector_slice()
} else {
generic_mj_lambdas.rows(self.mj_lambda1, self.ndofs1)
generic_solver_vels.rows(self.solver_vel1, self.ndofs1)
}
}
fn mj_lambda1_mut<'a>(
fn solver_vel1_mut<'a>(
&self,
mj_lambdas: &'a mut [DeltaVel<Real>],
generic_mj_lambdas: &'a mut DVector<Real>,
solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.is_rigid_body1 {
mj_lambdas[self.mj_lambda1].as_vector_slice_mut()
solver_vels[self.solver_vel1].as_vector_slice_mut()
} else {
generic_mj_lambdas.rows_mut(self.mj_lambda1, self.ndofs1)
generic_solver_vels.rows_mut(self.solver_vel1, self.ndofs1)
}
}
fn mj_lambda2<'a>(
fn solver_vel2<'a>(
&self,
mj_lambdas: &'a [DeltaVel<Real>],
generic_mj_lambdas: &'a DVector<Real>,
solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.is_rigid_body2 {
mj_lambdas[self.mj_lambda2].as_vector_slice()
solver_vels[self.solver_vel2].as_vector_slice()
} else {
generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
}
}
fn mj_lambda2_mut<'a>(
fn solver_vel2_mut<'a>(
&self,
mj_lambdas: &'a mut [DeltaVel<Real>],
generic_mj_lambdas: &'a mut DVector<Real>,
solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.is_rigid_body2 {
mj_lambdas[self.mj_lambda2].as_vector_slice_mut()
solver_vels[self.solver_vel2].as_vector_slice_mut()
} else {
generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let jacobians = jacobians.as_slice();
let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas);
let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels);
let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1);
let vel1 = j1.dot(&mj_lambda1);
let vel1 = j1.dot(&solver_vel1);
let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
let vel2 = j2.dot(&mj_lambda2);
let vel2 = j2.dot(&solver_vel2);
let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp(
@@ -291,13 +285,13 @@ impl JointGenericVelocityConstraint {
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let mut mj_lambda1 = self.mj_lambda1_mut(mj_lambdas, generic_mj_lambdas);
let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels);
let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
mj_lambda1.axpy(delta_impulse, &wj1, 1.0);
solver_vel1.axpy(delta_impulse, &wj1, 1.0);
let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -315,8 +309,8 @@ impl JointGenericVelocityConstraint {
}
#[derive(Debug, Copy, Clone)]
pub struct JointGenericVelocityGroundConstraint {
pub mj_lambda2: usize,
pub struct JointGenericOneBodyConstraint {
pub solver_vel2: usize,
pub ndofs2: usize,
pub j_id2: usize,
@@ -333,16 +327,16 @@ pub struct JointGenericVelocityGroundConstraint {
pub writeback_id: WritebackId,
}
impl Default for JointGenericVelocityGroundConstraint {
impl Default for JointGenericOneBodyConstraint {
fn default() -> Self {
JointGenericVelocityGroundConstraint::invalid()
JointGenericOneBodyConstraint::invalid()
}
}
impl JointGenericVelocityGroundConstraint {
impl JointGenericOneBodyConstraint {
pub fn invalid() -> Self {
Self {
mj_lambda2: crate::INVALID_USIZE,
solver_vel2: crate::INVALID_USIZE,
ndofs2: 0,
j_id2: crate::INVALID_USIZE,
joint_id: 0,
@@ -360,8 +354,8 @@ impl JointGenericVelocityGroundConstraint {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
body2: &JointSolverBody<Real, 1>,
mb2: (&Multibody, usize),
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
@@ -375,7 +369,7 @@ impl JointGenericVelocityGroundConstraint {
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -386,13 +380,11 @@ impl JointGenericVelocityGroundConstraint {
let start = len;
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_ground(
params,
out[len] = builder.motor_angular_generic_one_body(
jacobians,
j_id,
joint_id,
body1,
body2,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
@@ -404,13 +396,11 @@ impl JointGenericVelocityGroundConstraint {
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic_ground(
params,
out[len] = builder.motor_linear_generic_one_body(
jacobians,
j_id,
joint_id,
body1,
body2,
mb2,
// locked_ang_axes,
i,
@@ -421,7 +411,7 @@ impl JointGenericVelocityGroundConstraint {
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
jacobians,
&mut out[start..len],
);
@@ -429,7 +419,7 @@ impl JointGenericVelocityGroundConstraint {
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground(
out[len] = builder.lock_angular_generic_one_body(
params,
jacobians,
j_id,
@@ -444,7 +434,7 @@ impl JointGenericVelocityGroundConstraint {
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic_ground(
out[len] = builder.lock_linear_generic_one_body(
params,
jacobians,
j_id,
@@ -460,7 +450,7 @@ impl JointGenericVelocityGroundConstraint {
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_angular_generic_ground(
out[len] = builder.limit_angular_generic_one_body(
params,
jacobians,
j_id,
@@ -476,7 +466,7 @@ impl JointGenericVelocityGroundConstraint {
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_ground(
out[len] = builder.limit_linear_generic_one_body(
params,
jacobians,
j_id,
@@ -491,7 +481,7 @@ impl JointGenericVelocityGroundConstraint {
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
jacobians,
&mut out[start..len],
);
@@ -502,33 +492,33 @@ impl JointGenericVelocityGroundConstraint {
self.j_id2 + self.ndofs2
}
fn mj_lambda2<'a>(
fn solver_vel2<'a>(
&self,
_mj_lambdas: &'a [DeltaVel<Real>],
generic_mj_lambdas: &'a DVector<Real>,
_solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
}
fn mj_lambda2_mut<'a>(
fn solver_vel2_mut<'a>(
&self,
_mj_lambdas: &'a mut [DeltaVel<Real>],
generic_mj_lambdas: &'a mut DVector<Real>,
_solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let jacobians = jacobians.as_slice();
let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
let vel2 = j2.dot(&mj_lambda2);
let vel2 = j2.dot(&solver_vel2);
let dvel = self.rhs + vel2;
let total_impulse = na::clamp(
@@ -539,9 +529,9 @@ impl JointGenericVelocityGroundConstraint {
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {

632
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs → src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
View File

@@ -1,29 +1,459 @@
use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{IntegrationParameters, JointIndex, Multibody};
use crate::dynamics::{
GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
};
use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
use crate::utils;
use crate::utils::IndexMut2;
use crate::utils::WDot;
use crate::utils::SimdDot;
use na::{DVector, SVector};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::ConstraintsCounts;
#[cfg(feature = "dim3")]
use crate::utils::WAngularInertia;
use crate::utils::SimdAngularInertia;
#[cfg(feature = "dim2")]
use na::Vector1;
use parry::math::Isometry;
impl SolverBody<Real, 1> {
#[derive(Copy, Clone)]
enum LinkOrBody {
Link(MultibodyLinkId),
Body(usize),
}
#[derive(Copy, Clone)]
pub struct JointGenericTwoBodyConstraintBuilder {
link1: LinkOrBody,
link2: LinkOrBody,
joint_id: JointIndex,
joint: GenericJoint,
j_id: usize,
// These are solver body for both joints, except that
// the world_com is actually in local-space.
local_body1: JointSolverBody<Real, 1>,
local_body2: JointSolverBody<Real, 1>,
multibodies_ndof: usize,
constraint_id: usize,
}
impl JointGenericTwoBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
link1: LinkOrBody::Body(usize::MAX),
link2: LinkOrBody::Body(usize::MAX),
joint_id: JointIndex::MAX,
joint: GenericJoint::default(),
j_id: usize::MAX,
local_body1: JointSolverBody::invalid(),
local_body2: JointSolverBody::invalid(),
multibodies_ndof: usize::MAX,
constraint_id: usize::MAX,
}
}
pub fn generate(
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builder: &mut Self,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let starting_j_id = *j_id;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let local_body1 = JointSolverBody {
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.local_mprops.local_com,
solver_vel: [rb1.ids.active_set_offset],
};
let local_body2 = JointSolverBody {
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.local_mprops.local_com,
solver_vel: [rb2.ids.active_set_offset],
};
let mut multibodies_ndof = 0;
let link1 = match multibodies.rigid_body_link(joint.body1) {
Some(link) => {
multibodies_ndof += multibodies[link.multibody].ndofs();
LinkOrBody::Link(*link)
}
None => {
multibodies_ndof += SPATIAL_DIM;
LinkOrBody::Body(rb2.ids.active_set_offset)
}
};
let link2 = match multibodies.rigid_body_link(joint.body2) {
Some(link) => {
multibodies_ndof += multibodies[link.multibody].ndofs();
LinkOrBody::Link(*link)
}
None => {
multibodies_ndof += SPATIAL_DIM;
LinkOrBody::Body(rb2.ids.active_set_offset)
}
};
if multibodies_ndof == 0 {
// Both multibodies are fixed, don’t generate any constraint.
out_builder.multibodies_ndof = multibodies_ndof;
return;
}
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
// constraints appends the multibodies jacobian and weighted jacobians.
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
// to the generic DVector.
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
// TODO: use a more precise increment.
*j_id += multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
*out_builder = Self {
link1,
link2,
joint_id,
joint: joint.data,
j_id: starting_j_id,
local_body1,
local_body2,
multibodies_ndof,
constraint_id: *out_constraint_id,
};
*out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints;
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
bodies: &[SolverBody],
jacobians: &mut DVector<Real>,
out: &mut [JointGenericTwoBodyConstraint],
) {
if self.multibodies_ndof == 0 {
// The joint is between two static bodies, no constraint needed.
return;
}
// NOTE: right now, the "update", is basically reconstructing all the
// constraints. Could we make this more incremental?
let pos1;
let pos2;
let mb1;
let mb2;
match self.link1 {
LinkOrBody::Link(link) => {
let mb = &multibodies[link.multibody];
pos1 = &mb.link(link.id).unwrap().local_to_world;
mb1 = Some((mb, link.id));
}
LinkOrBody::Body(body1) => {
pos1 = &bodies[body1].position;
mb1 = None;
}
};
match self.link2 {
LinkOrBody::Link(link) => {
let mb = &multibodies[link.multibody];
pos2 = &mb.link(link.id).unwrap().local_to_world;
mb2 = Some((mb, link.id));
}
LinkOrBody::Body(body2) => {
pos2 = &bodies[body2].position;
mb2 = None;
}
};
let frame1 = pos1 * self.joint.local_frame1;
let frame2 = pos2 * self.joint.local_frame2;
let joint_body1 = JointSolverBody {
world_com: pos1 * self.local_body1.world_com, // the world_com was stored in local-space.
..self.local_body1
};
let joint_body2 = JointSolverBody {
world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
..self.local_body2
};
let mut j_id = self.j_id;
JointGenericTwoBodyConstraint::lock_axes(
params,
self.joint_id,
&joint_body1,
&joint_body2,
mb1,
mb2,
&frame1,
&frame2,
&self.joint,
jacobians,
&mut j_id,
&mut out[self.constraint_id..],
);
}
}
#[derive(Copy, Clone)]
pub enum JointGenericOneBodyConstraintBuilder {
Internal(JointGenericVelocityOneBodyInternalConstraintBuilder),
External(JointGenericVelocityOneBodyExternalConstraintBuilder),
Empty,
}
#[derive(Copy, Clone)]
pub struct JointGenericVelocityOneBodyInternalConstraintBuilder {
link: MultibodyLinkId,
j_id: usize,
constraint_id: usize,
}
impl JointGenericOneBodyConstraintBuilder {
pub fn invalid() -> Self {
Self::Empty
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
bodies: &[SolverBody],
jacobians: &mut DVector<Real>,
out: &mut [JointGenericOneBodyConstraint],
) {
match self {
Self::Empty => {}
Self::Internal(builder) => builder.update(params, multibodies, jacobians, out),
Self::External(builder) => builder.update(params, multibodies, bodies, jacobians, out),
}
}
}
impl JointGenericVelocityOneBodyInternalConstraintBuilder {
pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize {
let multibody = &multibodies[link_id.multibody];
let link = multibody.link(link_id.id).unwrap();
link.joint().num_velocity_constraints()
}
pub fn generate(
multibodies: &MultibodyJointSet,
link_id: &MultibodyLinkId,
out_builder: &mut JointGenericOneBodyConstraintBuilder,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let multibody = &multibodies[link_id.multibody];
let link = multibody.link(link_id.id).unwrap();
let num_constraints = link.joint().num_velocity_constraints();
if num_constraints == 0 {
return;
}
*out_builder = JointGenericOneBodyConstraintBuilder::Internal(Self {
link: *link_id,
j_id: *j_id,
constraint_id: *out_constraint_id,
});
*j_id += num_constraints * multibody.ndofs() * 2;
if jacobians.nrows() < *j_id {
jacobians.resize_vertically_mut(*j_id, 0.0);
}
*out_constraint_id += num_constraints;
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
jacobians: &mut DVector<Real>,
out: &mut [JointGenericOneBodyConstraint],
) {
let mb = &multibodies[self.link.multibody];
let link = mb.link(self.link.id).unwrap();
link.joint().velocity_constraints(
params,
mb,
link,
self.j_id,
jacobians,
&mut out[self.constraint_id..],
);
}
}
#[derive(Copy, Clone)]
pub struct JointGenericVelocityOneBodyExternalConstraintBuilder {
body1: JointFixedSolverBody<Real>,
frame1: Isometry<Real>,
link2: MultibodyLinkId,
joint_id: JointIndex,
joint: GenericJoint,
j_id: usize,
flipped: bool,
constraint_id: usize,
// These are solver body for both joints, except that
// the world_com is actually in local-space.
local_body2: JointSolverBody<Real, 1>,
}
impl JointGenericVelocityOneBodyExternalConstraintBuilder {
pub fn generate(
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builder: &mut JointGenericOneBodyConstraintBuilder,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let flipped = !bodies[handle2].is_dynamic();
let local_frame1 = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
joint.data.local_frame2
} else {
joint.data.local_frame1
};
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let frame1 = rb1.pos.position * local_frame1;
let starting_j_id = *j_id;
let body1 = JointFixedSolverBody {
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
world_com: rb1.mprops.world_com,
};
let local_body2 = JointSolverBody {
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.local_mprops.local_com,
solver_vel: [rb2.ids.active_set_offset],
};
let link2 = *multibodies.rigid_body_link(handle2).unwrap();
let mb2 = &multibodies[link2.multibody];
let multibodies_ndof = mb2.ndofs();
if multibodies_ndof == 0 {
// The multibody is fixed, don’t generate any constraint.
*out_builder = JointGenericOneBodyConstraintBuilder::Empty;
return;
}
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
// constraints appends the multibodies jacobian and weighted jacobians.
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
// to the generic DVector.
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
// TODO: use a more precise increment.
*j_id += multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
*out_builder = JointGenericOneBodyConstraintBuilder::External(Self {
body1,
link2,
joint_id,
joint: joint.data,
j_id: starting_j_id,
frame1,
local_body2,
flipped,
constraint_id: *out_constraint_id,
});
*out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints;
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
_bodies: &[SolverBody],
jacobians: &mut DVector<Real>,
out: &mut [JointGenericOneBodyConstraint],
) {
// NOTE: right now, the "update", is basically reconstructing all the
// constraints. Could we make this more incremental?
let mb2 = &multibodies[self.link2.multibody];
let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world;
let frame2 = pos2
* if self.flipped {
self.joint.local_frame1
} else {
self.joint.local_frame2
};
let joint_body2 = JointSolverBody {
world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
..self.local_body2
};
let mut j_id = self.j_id;
JointGenericOneBodyConstraint::lock_axes(
params,
self.joint_id,
&self.body1,
&joint_body2,
(mb2, self.link2.id),
&self.frame1,
&frame2,
&self.joint,
jacobians,
&mut j_id,
&mut out[self.constraint_id..],
);
}
}
impl JointSolverBody<Real, 1> {
pub fn fill_jacobians(
&self,
unit_force: Vector<Real>,
unit_torque: SVector<Real, ANG_DIM>,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
) -> Real {
) {
let wj_id = *j_id + SPATIAL_DIM;
jacobians
.fixed_rows_mut::<DIM>(*j_id)
@@ -54,26 +484,24 @@ impl SolverBody<Real, 1> {
}
*j_id += SPATIAL_DIM * 2;
unit_force.dot(&self.linvel) + unit_torque.gdot(self.angvel)
}
}
impl JointVelocityConstraintBuilder<Real> {
impl JointTwoBodyConstraintHelper<Real> {
pub fn lock_jacobians_generic(
&self,
_params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
writeback_id: WritebackId,
lin_jac: Vector<Real>,
ang_jac1: SVector<Real, ANG_DIM>,
ang_jac2: SVector<Real, ANG_DIM>,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
let is_rigid_body1 = mb1.is_none();
let is_rigid_body2 = mb2.is_none();
@@ -81,19 +509,17 @@ impl JointVelocityConstraintBuilder<Real> {
let ndofs2 = mb2.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM);
let j_id1 = *j_id;
let vel1 = if let Some((mb1, link_id1)) = mb1 {
mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians)
.1
if let Some((mb1, link_id1)) = mb1 {
mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians);
} else {
body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians)
body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians);
};
let j_id2 = *j_id;
let vel2 = if let Some((mb2, link_id2)) = mb2 {
mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
.1
if let Some((mb2, link_id2)) = mb2 {
mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians);
} else {
body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians)
body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians);
};
if is_rigid_body1 {
@@ -116,16 +542,16 @@ impl JointVelocityConstraintBuilder<Real> {
j.copy_from(&wj);
}
let rhs_wo_bias = vel2 - vel1;
let rhs_wo_bias = 0.0;
let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
let solver_vel1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.solver_vel[0]);
let solver_vel2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.solver_vel[0]);
JointGenericVelocityConstraint {
JointGenericTwoBodyConstraint {
is_rigid_body1,
is_rigid_body2,
mj_lambda1,
mj_lambda2,
solver_vel1,
solver_vel2,
ndofs1,
j_id1,
ndofs2,
@@ -148,19 +574,18 @@ impl JointVelocityConstraintBuilder<Real> {
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
let lin_jac = self.basis.column(locked_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
let mut c = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -186,20 +611,19 @@ impl JointVelocityConstraintBuilder<Real> {
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
let lin_jac = self.basis.column(limited_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -230,18 +654,17 @@ impl JointVelocityConstraintBuilder<Real> {
pub fn motor_linear_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
@@ -255,7 +678,6 @@ impl JointVelocityConstraintBuilder<Real> {
// }
let mut constraint = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -275,9 +697,7 @@ impl JointVelocityConstraintBuilder<Real> {
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - motor_params.target_vel;
rhs_wo_bias += -motor_params.target_vel;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
@@ -293,20 +713,19 @@ impl JointVelocityConstraintBuilder<Real> {
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
_locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -335,21 +754,20 @@ impl JointVelocityConstraintBuilder<Real> {
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
_limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -386,25 +804,23 @@ impl JointVelocityConstraintBuilder<Real> {
pub fn motor_angular_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
_motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
) -> JointGenericTwoBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
jacobians,
j_id,
joint_id,
@@ -429,8 +845,7 @@ impl JointVelocityConstraintBuilder<Real> {
* motor_params.erp_inv_dt;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
rhs_wo_bias += -motor_params.target_vel;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.rhs = rhs_wo_bias;
@@ -442,7 +857,7 @@ impl JointVelocityConstraintBuilder<Real> {
pub fn finalize_generic_constraints(
jacobians: &mut DVector<Real>,
constraints: &mut [JointGenericVelocityConstraint],
constraints: &mut [JointGenericTwoBodyConstraint],
) {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
@@ -506,34 +921,30 @@ impl JointVelocityConstraintBuilder<Real> {
}
}
impl JointVelocityConstraintBuilder<Real> {
pub fn lock_jacobians_generic_ground(
impl JointTwoBodyConstraintHelper<Real> {
pub fn lock_jacobians_generic_one_body(
&self,
_params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
(mb2, link_id2): (&Multibody, usize),
writeback_id: WritebackId,
lin_jac: Vector<Real>,
ang_jac1: SVector<Real, ANG_DIM>,
ang_jac2: SVector<Real, ANG_DIM>,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
let ndofs2 = mb2.ndofs();
let vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel);
let proj_vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel);
let j_id2 = *j_id;
let vel2 = mb2
.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
.1;
let rhs_wo_bias = vel2 - vel1;
mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians);
let rhs_wo_bias = -proj_vel1;
let mj_lambda2 = mb2.solver_id;
let solver_vel2 = mb2.solver_id;
JointGenericVelocityGroundConstraint {
mj_lambda2,
JointGenericOneBodyConstraint {
solver_vel2,
ndofs2,
j_id2,
joint_id,
@@ -548,23 +959,22 @@ impl JointVelocityConstraintBuilder<Real> {
}
}
pub fn lock_linear_generic_ground(
pub fn lock_linear_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
let lin_jac = self.basis.column(locked_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
let mut c = self.lock_jacobians_generic_ground(
params,
let mut c = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -582,24 +992,23 @@ impl JointVelocityConstraintBuilder<Real> {
c
}
pub fn limit_linear_generic_ground(
pub fn limit_linear_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
let lin_jac = self.basis.column(limited_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
let mut constraint = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -626,19 +1035,17 @@ impl JointVelocityConstraintBuilder<Real> {
constraint
}
pub fn motor_linear_generic_ground(
pub fn motor_linear_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
@@ -651,8 +1058,7 @@ impl JointVelocityConstraintBuilder<Real> {
// constraint.ang_jac2.fill(0.0);
// }
let mut constraint = self.lock_jacobians_generic_ground(
params,
let mut constraint = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -670,9 +1076,8 @@ impl JointVelocityConstraintBuilder<Real> {
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - motor_params.target_vel;
let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel);
rhs_wo_bias += proj_vel1 - motor_params.target_vel;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
@@ -682,24 +1087,23 @@ impl JointVelocityConstraintBuilder<Real> {
constraint
}
pub fn lock_angular_generic_ground(
pub fn lock_angular_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
_locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
let mut constraint = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -720,25 +1124,24 @@ impl JointVelocityConstraintBuilder<Real> {
constraint
}
pub fn limit_angular_generic_ground(
pub fn limit_angular_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
_limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
let mut constraint = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -771,26 +1174,23 @@ impl JointVelocityConstraintBuilder<Real> {
constraint
}
pub fn motor_angular_generic_ground(
pub fn motor_angular_generic_one_body(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
body1: &JointFixedSolverBody<Real>,
mb2: (&Multibody, usize),
_motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
) -> JointGenericOneBodyConstraint {
#[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
let mut constraint = self.lock_jacobians_generic_one_body(
jacobians,
j_id,
joint_id,
@@ -813,8 +1213,8 @@ impl JointVelocityConstraintBuilder<Real> {
* motor_params.erp_inv_dt;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs += dvel - motor_params.target_vel;
let proj_vel1 = -ang_jac.gdot(body1.angvel);
rhs += proj_vel1 - motor_params.target_vel;
constraint.rhs_wo_bias = rhs;
constraint.rhs = rhs;
@@ -824,9 +1224,9 @@ impl JointVelocityConstraintBuilder<Real> {
constraint
}
pub fn finalize_generic_constraints_ground(
pub fn finalize_generic_constraints_one_body(
jacobians: &mut DVector<Real>,
constraints: &mut [JointGenericVelocityGroundConstraint],
constraints: &mut [JointGenericOneBodyConstraint],
) {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;

371
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,10 +1,11 @@
use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::utils::{WDot, WReal};
use crate::num::Zero;
use crate::utils::{SimdDot, SimdRealCopy};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -13,7 +14,7 @@ use {
};
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: WReal> {
pub struct MotorParameters<N: SimdRealCopy> {
pub erp_inv_dt: N,
pub cfm_coeff: N,
pub cfm_gain: N,
@@ -22,7 +23,7 @@ pub struct MotorParameters<N: WReal> {
pub max_impulse: N,
}
impl<N: WReal> Default for MotorParameters<N> {
impl<N: SimdRealCopy> Default for MotorParameters<N> {
fn default() -> Self {
Self {
erp_inv_dt: N::zero(),
@@ -47,19 +48,46 @@ pub enum WritebackId {
// the solver, to avoid fetching data from the rigid-body set
// every time.
#[derive(Copy, Clone)]
pub struct SolverBody<N: WReal, const LANES: usize> {
pub linvel: Vector<N>,
pub angvel: AngVector<N>,
pub struct JointSolverBody<N: SimdRealCopy, const LANES: usize> {
pub im: Vector<N>,
pub sqrt_ii: AngularInertia<N>,
pub world_com: Point<N>,
pub mj_lambda: [usize; LANES],
pub solver_vel: [usize; LANES],
}
impl<N: SimdRealCopy, const LANES: usize> JointSolverBody<N, LANES> {
pub fn invalid() -> Self {
Self {
im: Vector::zeros(),
sqrt_ii: AngularInertia::zero(),
world_com: Point::origin(),
solver_vel: [usize::MAX; LANES],
}
}
}
#[derive(Copy, Clone)]
pub struct JointFixedSolverBody<N: SimdRealCopy> {
pub linvel: Vector<N>,
pub angvel: AngVector<N>,
// TODO: is this really needed?
pub world_com: Point<N>,
}
impl<N: SimdRealCopy> JointFixedSolverBody<N> {
pub fn invalid() -> Self {
Self {
linvel: Vector::zeros(),
angvel: AngVector::zero(),
world_com: Point::origin(),
}
}
}
#[derive(Debug, Copy, Clone)]
pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub mj_lambda1: [usize; LANES],
pub mj_lambda2: [usize; LANES],
pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub solver_vel1: [usize; LANES],
pub solver_vel2: [usize; LANES],
pub joint_id: [JointIndex; LANES],
@@ -81,32 +109,15 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub writeback_id: WritebackId,
}
impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
pub fn invalid() -> Self {
Self {
mj_lambda1: [crate::INVALID_USIZE; LANES],
mj_lambda2: [crate::INVALID_USIZE; LANES],
joint_id: [crate::INVALID_USIZE; LANES],
impulse: N::zero(),
impulse_bounds: [N::zero(), N::zero()],
lin_jac: Vector::zeros(),
ang_jac1: na::zero(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_gain: N::zero(),
cfm_coeff: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im1: na::zero(),
im2: na::zero(),
writeback_id: WritebackId::Dof(0),
}
}
pub fn solve_generic(&mut self, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>) {
let dlinvel = self.lin_jac.dot(&(mj_lambda2.linear - mj_lambda1.linear));
impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
pub fn solve_generic(
&mut self,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
let dlinvel = self.lin_jac.dot(&(solver_vel2.linear - solver_vel1.linear));
let dangvel =
self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
self.ang_jac2.gdot(solver_vel2.angular) - self.ang_jac1.gdot(solver_vel1.angular);
let rhs = dlinvel + dangvel + self.rhs;
let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
@@ -118,10 +129,10 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
let ang_impulse1 = self.ang_jac1 * delta_impulse;
let ang_impulse2 = self.ang_jac2 * delta_impulse;
mj_lambda1.linear += lin_impulse.component_mul(&self.im1);
mj_lambda1.angular += ang_impulse1;
mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
mj_lambda2.angular -= ang_impulse2;
solver_vel1.linear += lin_impulse.component_mul(&self.im1);
solver_vel1.angular += ang_impulse1;
solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
solver_vel2.angular -= ang_impulse2;
}
pub fn remove_bias_from_rhs(&mut self) {
@@ -129,12 +140,12 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
}
}
impl JointVelocityConstraint<Real, 1> {
impl JointTwoBodyConstraint<Real, 1> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
@@ -146,7 +157,18 @@ impl JointVelocityConstraint<Real, 1> {
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
// The has_lin/ang_coupling test is needed to avoid shl overflow later.
let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
let first_coupled_lin_axis_id =
(coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
#[cfg(feature = "dim3")]
let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
#[cfg(feature = "dim3")]
let first_coupled_ang_axis_id =
(coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -195,20 +217,31 @@ impl JointVelocityConstraint<Real, 1> {
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear motor constraint.
// out[len] = builder.motor_linear_coupled(
// if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 {
// let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 {
// Some([
// joint.limits[first_coupled_lin_axis_id].min,
// joint.limits[first_coupled_lin_axis_id].max,
// ])
// } else {
// None
// };
//
// out[len] = builder.motor_linear_coupled
// params,
// [joint_id],
// body1,
// body2,
// limit_axes & coupled_axes,
// &joint.limits,
// WritebackId::Limit(0), // TODO: writeback
// coupled_axes,
// &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
// limits,
// WritebackId::Motor(first_coupled_lin_axis_id),
// );
// len += 1;
// }
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
@@ -262,44 +295,50 @@ impl JointVelocityConstraint<Real, 1> {
}
#[cfg(feature = "dim3")]
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
out[len] = builder.limit_angular_coupled(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
coupled_axes,
[
joint.limits[first_coupled_ang_axis_id].min,
joint.limits[first_coupled_ang_axis_id].max,
],
WritebackId::Limit(first_coupled_ang_axis_id),
);
len += 1;
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
out[len] = builder.limit_linear_coupled(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
coupled_axes,
[
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
],
WritebackId::Limit(first_coupled_lin_axis_id),
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
len
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1[0] as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = solver_vels[self.solver_vel1[0] as usize];
let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize];
self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
self.solve_generic(&mut solver_vel1, &mut solver_vel2);
mj_lambdas[self.mj_lambda1[0] as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
solver_vels[self.solver_vel1[0] as usize] = solver_vel1;
solver_vels[self.solver_vel2[0] as usize] = solver_vel2;
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -312,18 +351,18 @@ impl JointVelocityConstraint<Real, 1> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
body1: &SolverBody<SimdReal, SIMD_WIDTH>,
body2: &SolverBody<SimdReal, SIMD_WIDTH>,
body1: &JointSolverBody<SimdReal, SIMD_WIDTH>,
body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
frame1: &Isometry<SimdReal>,
frame2: &Isometry<SimdReal>,
locked_axes: u8,
out: &mut [Self],
) -> usize {
let builder = JointVelocityConstraintBuilder::new(
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -354,31 +393,35 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
}
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]);
len
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel1[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|ii| solver_vels[self.solver_vel1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel2[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|ii| solver_vels[self.solver_vel2[ii] as usize].angular
]),
};
self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
self.solve_generic(&mut solver_vel1, &mut solver_vel2);
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.angular.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
}
}
@@ -398,8 +441,8 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
}
#[derive(Debug, Copy, Clone)]
pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub mj_lambda2: [usize; LANES],
pub struct JointOneBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub solver_vel2: [usize; LANES],
pub joint_id: [JointIndex; LANES],
@@ -419,28 +462,10 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub writeback_id: WritebackId,
}
impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
pub fn invalid() -> Self {
Self {
mj_lambda2: [crate::INVALID_USIZE; LANES],
joint_id: [crate::INVALID_USIZE; LANES],
impulse: N::zero(),
impulse_bounds: [N::zero(), N::zero()],
lin_jac: Vector::zeros(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im2: na::zero(),
writeback_id: WritebackId::Dof(0),
}
}
pub fn solve_generic(&mut self, mj_lambda2: &mut DeltaVel<N>) {
let dlinvel = mj_lambda2.linear;
let dangvel = mj_lambda2.angular;
impl<N: SimdRealCopy, const LANES: usize> JointOneBodyConstraint<N, LANES> {
pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel<N>) {
let dlinvel = solver_vel2.linear;
let dangvel = solver_vel2.angular;
let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
@@ -451,8 +476,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
let lin_impulse = self.lin_jac * delta_impulse;
let ang_impulse = self.ang_jac2 * delta_impulse;
mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
mj_lambda2.angular -= ang_impulse;
solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
solver_vel2.angular -= ang_impulse;
}
pub fn remove_bias_from_rhs(&mut self) {
@@ -460,12 +485,12 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
}
}
impl JointVelocityGroundConstraint<Real, 1> {
impl JointOneBodyConstraint<Real, 1> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
body1: &JointFixedSolverBody<Real>,
body2: &JointSolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
@@ -477,7 +502,18 @@ impl JointVelocityGroundConstraint<Real, 1> {
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
// The has_lin/ang_coupling test is needed to avoid shl overflow later.
let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
let first_coupled_lin_axis_id =
(coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
#[cfg(feature = "dim3")]
let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
#[cfg(feature = "dim3")]
let first_coupled_ang_axis_id =
(coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -488,7 +524,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.motor_angular_ground(
out[len] = builder.motor_angular_one_body(
[joint_id],
body1,
body2,
@@ -507,7 +543,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
None
};
out[len] = builder.motor_linear_ground(
out[len] = builder.motor_linear_one_body(
params,
[joint_id],
body1,
@@ -521,35 +557,40 @@ impl JointVelocityGroundConstraint<Real, 1> {
}
}
if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
#[cfg(feature = "dim3")]
if has_ang_coupling && (motor_axes & (1 << first_coupled_ang_axis_id)) != 0 {
// TODO: coupled angular motor constraint.
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
/*
// TODO: coupled linear motor constraint.
out[len] = builder.motor_linear_coupled_ground(
if has_lin_coupling && (motor_axes & (1 << first_coupled_lin_axis_id)) != 0 {
let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
Some([
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
])
} else {
None
};
out[len] = builder.motor_linear_coupled_one_body(
params,
[joint_id],
body1,
body2,
motor_axes & coupled_axes,
&joint.motors,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
coupled_axes,
&joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
limits,
WritebackId::Motor(first_coupled_lin_axis_id),
);
len += 1;
*/
todo!()
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_ground(
out[len] = builder.lock_angular_one_body(
params,
[joint_id],
body1,
@@ -562,7 +603,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
out[len] = builder.lock_linear_one_body(
params,
[joint_id],
body1,
@@ -576,7 +617,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
for i in DIM..SPATIAL_DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular_ground(
out[len] = builder.limit_angular_one_body(
params,
[joint_id],
body1,
@@ -590,7 +631,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
}
for i in 0..DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_linear_ground(
out[len] = builder.limit_linear_one_body(
params,
[joint_id],
body1,
@@ -604,40 +645,46 @@ impl JointVelocityGroundConstraint<Real, 1> {
}
#[cfg(feature = "dim3")]
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
out[len] = builder.limit_angular_coupled_ground(
if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
out[len] = builder.limit_angular_coupled_one_body(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
coupled_axes,
[
joint.limits[first_coupled_ang_axis_id].min,
joint.limits[first_coupled_ang_axis_id].max,
],
WritebackId::Limit(first_coupled_ang_axis_id),
);
len += 1;
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
out[len] = builder.limit_linear_coupled_ground(
if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
out[len] = builder.limit_linear_coupled_one_body(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
coupled_axes,
[
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
],
WritebackId::Limit(first_coupled_lin_axis_id),
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
len
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
self.solve_generic(&mut mj_lambda2);
mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize];
self.solve_generic(&mut solver_vel2);
solver_vels[self.solver_vel2[0] as usize] = solver_vel2;
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -651,19 +698,19 @@ impl JointVelocityGroundConstraint<Real, 1> {
}
#[cfg(feature = "simd-is-enabled")]
impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
impl JointOneBodyConstraint<SimdReal, SIMD_WIDTH> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
body1: &SolverBody<SimdReal, SIMD_WIDTH>,
body2: &SolverBody<SimdReal, SIMD_WIDTH>,
body1: &JointFixedSolverBody<SimdReal>,
body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
frame1: &Isometry<SimdReal>,
frame2: &Isometry<SimdReal>,
locked_axes: u8,
out: &mut [Self],
) -> usize {
let mut len = 0;
let builder = JointVelocityConstraintBuilder::new(
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -673,7 +720,7 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
out[len] = builder.lock_linear_one_body(
params,
joint_id,
body1,
@@ -686,7 +733,7 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
}
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_ground(
out[len] = builder.lock_angular_one_body(
params,
joint_id,
body1,
@@ -698,23 +745,25 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
}
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[..len]);
len
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![
|ii| solver_vels[self.solver_vel2[ii] as usize].linear
]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|ii| solver_vels[self.solver_vel2[ii] as usize].angular
]),
};
self.solve_generic(&mut mj_lambda2);
self.solve_generic(&mut solver_vel2);
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
}
}

23
src/dynamics/solver/joint_constraint/mod.rs
View File

@@ -1,13 +1,18 @@
pub use joint_velocity_constraint::{MotorParameters, SolverBody, WritebackId};
pub use joint_velocity_constraint::{JointSolverBody, MotorParameters, WritebackId};
pub use joint_constraint::AnyJointVelocityConstraint;
pub use joint_generic_velocity_constraint::{
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
pub use any_joint_constraint::JointConstraintTypes;
pub use joint_constraint_builder::JointTwoBodyConstraintHelper;
pub use joint_constraints_set::JointConstraintsSet;
pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint};
pub use joint_generic_constraint_builder::{
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder,
};
pub use joint_velocity_constraint_builder::JointVelocityConstraintBuilder;
mod joint_constraint;
mod joint_generic_velocity_constraint;
mod joint_generic_velocity_constraint_builder;
mod any_joint_constraint;
mod joint_constraint_builder;
mod joint_constraints_set;
mod joint_generic_constraint;
mod joint_generic_constraint_builder;
mod joint_velocity_constraint;
mod joint_velocity_constraint_builder;

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

@@ -1,56 +1,47 @@
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
pub(crate) use self::island_solver::IslandSolver;
#[cfg(feature = "parallel")]
pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
#[cfg(feature = "parallel")]
pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
#[cfg(feature = "parallel")]
pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
#[cfg(not(feature = "parallel"))]
pub(self) use self::solver_constraints::SolverConstraints;
#[cfg(not(feature = "parallel"))]
// #[cfg(feature = "parallel")]
// pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
// #[cfg(feature = "parallel")]
// pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
// #[cfg(feature = "parallel")]
// pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
// #[cfg(not(feature = "parallel"))]
pub(self) use self::solver_constraints_set::SolverConstraintsSet;
// #[cfg(not(feature = "parallel"))]
pub(self) use self::velocity_solver::VelocitySolver;
pub(self) use delta_vel::DeltaVel;
pub(self) use generic_velocity_constraint::*;
pub(self) use generic_velocity_constraint_element::*;
pub(self) use generic_velocity_ground_constraint::*;
pub(self) use contact_constraint::*;
pub(self) use interaction_groups::*;
pub(crate) use joint_constraint::MotorParameters;
pub use joint_constraint::*;
pub(self) use velocity_constraint::*;
pub(self) use velocity_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(self) use velocity_constraint_wide::*;
pub(self) use velocity_ground_constraint::*;
pub(self) use velocity_ground_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(self) use velocity_ground_constraint_wide::*;
pub(self) use solver_body::SolverBody;
pub(self) use solver_constraints_set::{AnyConstraintMut, ConstraintTypes};
pub(self) use solver_vel::SolverVel;
mod categorization;
mod delta_vel;
mod generic_velocity_constraint;
mod generic_velocity_constraint_element;
mod generic_velocity_ground_constraint;
mod generic_velocity_ground_constraint_element;
mod contact_constraint;
mod interaction_groups;
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
mod island_solver;
mod joint_constraint;
#[cfg(feature = "parallel")]
mod parallel_island_solver;
#[cfg(feature = "parallel")]
mod parallel_solver_constraints;
#[cfg(feature = "parallel")]
mod parallel_velocity_solver;
#[cfg(not(feature = "parallel"))]
mod solver_constraints;
mod velocity_constraint;
mod velocity_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod velocity_constraint_wide;
mod velocity_ground_constraint;
mod velocity_ground_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod velocity_ground_constraint_wide;
#[cfg(not(feature = "parallel"))]
// #[cfg(feature = "parallel")]
// mod parallel_island_solver;
// #[cfg(feature = "parallel")]
// mod parallel_solver_constraints;
// #[cfg(feature = "parallel")]
// mod parallel_velocity_solver;
mod solver_body;
// #[cfg(not(feature = "parallel"))]
mod solver_constraints_set;
mod solver_vel;
// #[cfg(not(feature = "parallel"))]
mod velocity_solver;
// TODO: SAFETY: restrict with bytemuck::AnyBitPattern to make this safe.
pub unsafe fn reset_buffer<T>(buffer: &mut Vec<T>, len: usize) {
buffer.clear();
buffer.reserve(len);
buffer.as_mut_ptr().write_bytes(u8::MAX, len);
buffer.set_len(len);
}

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

@@ -3,7 +3,7 @@ use std::sync::atomic::{AtomicUsize, Ordering};
use rayon::Scope;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints,
ContactConstraintTypes, JointConstraintTypes, ParallelSolverConstraints,
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
@@ -12,7 +12,7 @@ use crate::dynamics::{
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use na::DVector;
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use super::{ParallelInteractionGroups, ParallelVelocitySolver, SolverVel};
#[macro_export]
#[doc(hidden)]
@@ -137,8 +137,8 @@ pub struct ParallelIslandSolver {
velocity_solver: ParallelVelocitySolver,
parallel_groups: ParallelInteractionGroups,
parallel_joint_groups: ParallelInteractionGroups,
parallel_contact_constraints: ParallelSolverConstraints<AnyVelocityConstraint>,
parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint>,
parallel_contact_constraints: ParallelSolverConstraints<ContactConstraintTypes>,
parallel_joint_constraints: ParallelSolverConstraints<JointConstraintTypes>,
thread: ThreadContext,
}
@@ -247,16 +247,16 @@ impl ParallelIslandSolver {
}
}
if self.velocity_solver.generic_mj_lambdas.len() < solver_id {
self.velocity_solver.generic_mj_lambdas = DVector::zeros(solver_id);
if self.velocity_solver.generic_solver_vels.len() < solver_id {
self.velocity_solver.generic_solver_vels = DVector::zeros(solver_id);
} else {
self.velocity_solver.generic_mj_lambdas.fill(0.0);
self.velocity_solver.generic_solver_vels.fill(0.0);
}
self.velocity_solver.mj_lambdas.clear();
self.velocity_solver.solver_vels.clear();
self.velocity_solver
.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
.solver_vels
.resize(islands.active_island(island_id).len(), SolverVel::zero());
}
for _ in 0..num_task_per_island {
@@ -286,16 +286,16 @@ impl ParallelIslandSolver {
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
let impulse_joints: &mut Vec<JointGraphEdge> =
unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint> = unsafe {
let parallel_contact_constraints: &mut ParallelSolverConstraints<ContactConstraintTypes> = unsafe {
std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
};
let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint> = unsafe {
let parallel_joint_constraints: &mut ParallelSolverConstraints<JointConstraintTypes> = unsafe {
std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
};
enable_flush_to_zero!(); // Ensure this is enabled on each thread.
// Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc):
// Initialize `solver_vels` (per-body velocity deltas) with external accelerations (gravity etc):
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
@@ -309,14 +309,14 @@ impl ParallelIslandSolver {
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mut mj_lambdas = velocity_solver
.generic_mj_lambdas
let mut solver_vels = velocity_solver
.generic_solver_vels
.rows_mut(multibody.solver_id, multibody.ndofs());
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
solver_vels.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let rb = &bodies[*handle];
let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
let dvel = &mut velocity_solver.solver_vels[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:

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

@@ -1,10 +1,9 @@
use super::ParallelInteractionGroups;
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
use super::{ContactConstraintTypes, JointConstraintTypes, ThreadContext};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
use crate::dynamics::solver::generic_two_body_constraint::GenericTwoBodyConstraint;
use crate::dynamics::solver::{
GenericVelocityGroundConstraint, InteractionGroups, VelocityConstraint,
VelocityGroundConstraint,
GenericOneBodyConstraint, InteractionGroups, OneBodyConstraint, TwoBodyConstraint,
};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
@@ -14,7 +13,7 @@ use crate::geometry::ContactManifold;
use crate::math::{Real, SPATIAL_DIM};
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
dynamics::solver::{OneBodyConstraintSimd, TwoBodyConstraintSimd},
math::SIMD_WIDTH,
};
use na::DVector;
@@ -36,40 +35,40 @@ use std::sync::atomic::Ordering;
// }
pub(crate) enum ConstraintDesc {
NongroundNongrouped(usize),
GroundNongrouped(usize),
TwoBodyNongrouped(usize),
OneBodyNongrouped(usize),
#[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize; SIMD_WIDTH]),
TwoBodyGrouped([usize; SIMD_WIDTH]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
GenericNongroundNongrouped(usize, usize),
GenericGroundNongrouped(usize, usize),
OneBodyGrouped([usize; SIMD_WIDTH]),
GenericTwoBodyNongrouped(usize, usize),
GenericOneBodyNongrouped(usize, usize),
GenericMultibodyInternal(MultibodyIndex, usize),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
pub(crate) struct ParallelSolverConstraints<TwoBodyConstraint> {
pub generic_jacobians: DVector<Real>,
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub generic_not_ground_interactions: Vec<usize>,
pub generic_ground_interactions: Vec<usize>,
pub two_body_interactions: Vec<usize>,
pub one_body_interactions: Vec<usize>,
pub generic_two_body_interactions: Vec<usize>,
pub generic_one_body_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
pub one_body_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<TwoBodyConstraint>,
pub constraint_descs: Vec<(usize, ConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
impl<TwoBodyConstraint> ParallelSolverConstraints<TwoBodyConstraint> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
not_ground_interactions: vec![],
ground_interactions: vec![],
generic_not_ground_interactions: vec![],
generic_ground_interactions: vec![],
two_body_interactions: vec![],
one_body_interactions: vec![],
generic_two_body_interactions: vec![],
generic_one_body_interactions: vec![],
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
one_body_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
constraint_descs: vec![],
parallel_desc_groups: vec![],
@@ -78,14 +77,14 @@ impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
}
macro_rules! impl_init_constraints_group {
($VelocityConstraint: ty, $Interaction: ty,
($TwoBodyConstraint: ty, $Interaction: ty,
$categorize: ident, $group: ident,
$body1: ident,
$body2: ident,
$generate_internal_constraints: expr,
$num_active_constraints_and_jacobian_lines: path,
$empty_velocity_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint> {
impl ParallelSolverConstraints<$TwoBodyConstraint> {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
@@ -100,7 +99,7 @@ macro_rules! impl_init_constraints_group {
let num_groups = interaction_groups.num_groups();
self.interaction_groups.clear_groups();
self.ground_interaction_groups.clear_groups();
self.one_body_interaction_groups.clear_groups();
self.parallel_desc_groups.clear();
self.constraint_descs.clear();
self.parallel_desc_groups.push(0);
@@ -108,43 +107,43 @@ macro_rules! impl_init_constraints_group {
for i in 0..num_groups {
let group = interaction_groups.group(i);
self.not_ground_interactions.clear();
self.ground_interactions.clear();
self.generic_not_ground_interactions.clear();
self.generic_ground_interactions.clear();
self.two_body_interactions.clear();
self.one_body_interactions.clear();
self.generic_two_body_interactions.clear();
self.generic_one_body_interactions.clear();
$categorize(
bodies,
multibodies,
interactions,
group,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
&mut self.generic_ground_interactions,
&mut self.generic_not_ground_interactions,
&mut self.one_body_interactions,
&mut self.two_body_interactions,
&mut self.generic_one_body_interactions,
&mut self.generic_two_body_interactions,
);
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_grouped = self.interaction_groups.simd_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
let start_grouped_one_body = self.one_body_interaction_groups.simd_interactions.len();
let start_nongrouped_one_body = self.one_body_interaction_groups.nongrouped_interactions.len();
self.interaction_groups.$group(
island_id,
islands,
bodies,
interactions,
&self.not_ground_interactions,
&self.two_body_interactions,
);
self.ground_interaction_groups.$group(
self.one_body_interaction_groups.$group(
island_id,
islands,
bodies,
interactions,
&self.ground_interactions,
&self.one_body_interactions,
);
// Compute constraint indices.
@@ -152,19 +151,19 @@ macro_rules! impl_init_constraints_group {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundNongrouped(*interaction_i),
ConstraintDesc::TwoBodyNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
self.interaction_groups.simd_interactions[start_grouped..].chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundGrouped(
ConstraintDesc::TwoBodyGrouped(
gather![|ii| interaction_i[ii]],
),
));
@@ -172,25 +171,25 @@ macro_rules! impl_init_constraints_group {
}
for interaction_i in
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
&self.one_body_interaction_groups.nongrouped_interactions[start_nongrouped_one_body..]
{
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundNongrouped(*interaction_i),
ConstraintDesc::OneBodyNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in self.ground_interaction_groups.grouped_interactions
[start_grouped_ground..]
for interaction_i in self.one_body_interaction_groups.simd_interactions
[start_grouped_one_body..]
.chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundGrouped(
ConstraintDesc::OneBodyGrouped(
gather![|ii| interaction_i[ii]],
),
));
@@ -208,11 +207,11 @@ macro_rules! impl_init_constraints_group {
}
};
for interaction_i in &self.generic_not_ground_interactions[..] {
for interaction_i in &self.generic_two_body_interactions[..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id),
ConstraintDesc::GenericTwoBodyNongrouped(*interaction_i, *j_id),
));
let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
@@ -222,11 +221,11 @@ macro_rules! impl_init_constraints_group {
total_num_constraints += num_constraints;
}
for interaction_i in &self.generic_ground_interactions[..] {
for interaction_i in &self.generic_one_body_interactions[..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id),
ConstraintDesc::GenericOneBodyNongrouped(*interaction_i, *j_id),
));
let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
@@ -289,31 +288,31 @@ fn manifold_body2(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
}
impl_init_constraints_group!(
AnyVelocityConstraint,
ContactConstraintTypes,
&mut ContactManifold,
categorize_contacts,
group_manifolds,
manifold_body1,
manifold_body2,
false,
VelocityConstraint::num_active_constraints_and_jacobian_lines,
AnyVelocityConstraint::Empty
TwoBodyConstraint::num_active_constraints_and_jacobian_lines,
ContactConstraintTypes::Empty
);
impl_init_constraints_group!(
AnyJointVelocityConstraint,
JointConstraintTypes,
JointGraphEdge,
categorize_joints,
group_joints,
joint_body1,
joint_body2,
true,
AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines,
AnyJointVelocityConstraint::Empty,
JointConstraintTypes::num_active_constraints_and_jacobian_lines,
JointConstraintTypes::Empty,
weight
);
impl ParallelSolverConstraints<AnyVelocityConstraint> {
impl ParallelSolverConstraints<ContactConstraintTypes> {
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
@@ -328,33 +327,33 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
let batch_size = thread.batch_size;
for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
match &desc.1 {
ConstraintDesc::NongroundNongrouped(manifold_id) => {
ConstraintDesc::TwoBodyNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
TwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GroundNongrouped(manifold_id) => {
ConstraintDesc::OneBodyNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
OneBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(manifold_id) => {
ConstraintDesc::TwoBodyGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
TwoBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(manifold_id) => {
ConstraintDesc::OneBodyGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
OneBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => {
ConstraintDesc::GenericTwoBodyNongrouped(manifold_id, j_id) => {
let mut j_id = *j_id;
let manifold = &*manifolds_all[*manifold_id];
GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
GenericTwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
}
ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => {
ConstraintDesc::GenericOneBodyNongrouped(manifold_id, j_id) => {
let mut j_id = *j_id;
let manifold = &*manifolds_all[*manifold_id];
GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
GenericOneBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
}
ConstraintDesc::GenericMultibodyInternal(..) => unreachable!()
}
@@ -363,7 +362,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
}
}
impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
impl ParallelSolverConstraints<JointConstraintTypes> {
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
@@ -378,33 +377,33 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
let batch_size = thread.batch_size;
for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
match &desc.1 {
ConstraintDesc::NongroundNongrouped(joint_id) => {
ConstraintDesc::TwoBodyNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GroundNongrouped(joint_id) => {
ConstraintDesc::OneBodyNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
ConstraintDesc::TwoBodyGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
ConstraintDesc::OneBodyGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_wide_joint_one_body(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => {
ConstraintDesc::GenericTwoBodyNongrouped(joint_id, j_id) => {
let mut j_id = *j_id;
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => {
ConstraintDesc::GenericOneBodyNongrouped(joint_id, j_id) => {
let mut j_id = *j_id;
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => {
let mut j_id = *j_id;

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

@@ -1,4 +1,4 @@
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
use super::{ContactConstraintTypes, JointConstraintTypes, SolverVel, ThreadContext};
use crate::concurrent_loop;
use crate::dynamics::{
solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
@@ -6,21 +6,21 @@ use crate::dynamics::{
};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::WAngularInertia;
use crate::utils::SimdAngularInertia;
use na::DVector;
use std::sync::atomic::Ordering;
pub(crate) struct ParallelVelocitySolver {
pub mj_lambdas: Vec<DeltaVel<Real>>,
pub generic_mj_lambdas: DVector<Real>,
pub solver_vels: Vec<SolverVel<Real>>,
pub generic_solver_vels: DVector<Real>,
}
impl ParallelVelocitySolver {
pub fn new() -> Self {
Self {
mj_lambdas: Vec::new(),
generic_mj_lambdas: DVector::zeros(0),
solver_vels: Vec::new(),
generic_solver_vels: DVector::zeros(0),
}
}
@@ -34,8 +34,8 @@ impl ParallelVelocitySolver {
multibodies: &mut MultibodyJointSet,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint>,
joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint>,
contact_constraints: &mut ParallelSolverConstraints<ContactConstraintTypes>,
joint_constraints: &mut ParallelSolverConstraints<JointConstraintTypes>,
) {
let mut start_index = thread
.solve_interaction_index
@@ -104,16 +104,15 @@ impl ParallelVelocitySolver {
* Solve constraints.
*/
{
for i in 0..params.max_velocity_iterations {
let solve_friction = params.interleave_restitution_and_friction_resolution
&& params.max_velocity_friction_iterations + i
>= params.max_velocity_iterations;
for i in 0..params.num_velocity_iterations_per_small_step {
let solve_friction = params.num_friction_iteration_per_solver_iteration + i
>= params.num_velocity_iterations_per_small_step;
// Solve joints.
solve!(
joint_constraints,
&joint_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas
&mut self.solver_vels,
&mut self.generic_solver_vels
);
shift += joint_descs.len();
start_index -= joint_descs.len();
@@ -122,8 +121,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
true,
false
);
@@ -134,8 +133,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
true,
false
);
@@ -146,8 +145,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
false,
true
);
@@ -157,11 +156,12 @@ impl ParallelVelocitySolver {
}
// Solve the remaining friction iterations.
let remaining_friction_iterations =
if !params.interleave_restitution_and_friction_resolution {
params.max_velocity_friction_iterations
} else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
params.max_velocity_friction_iterations - params.max_velocity_iterations
let remaining_friction_iterations = if params
.num_friction_iteration_per_solver_iteration
> params.num_velocity_iterations_per_small_step
{
params.num_friction_iteration_per_solver_iteration
- params.num_velocity_iterations_per_small_step
} else {
0
};
@@ -170,8 +170,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
false,
true
);
@@ -194,18 +194,18 @@ impl ParallelVelocitySolver {
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
multibody.velocities += mj_lambdas;
multibody.velocities += solver_vels;
multibody.integrate(params.dt);
multibody.forward_kinematics(bodies, false);
multibody.velocities = prev_vels;
}
} else {
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dvel = self.solver_vels[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
@@ -252,8 +252,8 @@ impl ParallelVelocitySolver {
solve!(
joint_constraints,
&joint_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas
&mut self.solver_vels,
&mut self.generic_solver_vels
);
shift += joint_descs.len();
start_index -= joint_descs.len();
@@ -261,8 +261,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
true,
false
);
@@ -272,8 +272,8 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
&mut self.solver_vels,
&mut self.generic_solver_vels,
false,
true
);
@@ -296,14 +296,14 @@ impl ParallelVelocitySolver {
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities += mj_lambdas;
multibody.velocities += solver_vels;
}
} else {
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dvel = self.solver_vels[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);

59
src/dynamics/solver/solver_body.rs Normal file
View File

@@ -0,0 +1,59 @@
use crate::dynamics::{RigidBody, RigidBodyVelocity};
use crate::math::{AngularInertia, Isometry, Point, Real, Vector};
use crate::prelude::RigidBodyDamping;
#[cfg(feature = "dim2")]
use crate::num::Zero;
#[derive(Copy, Clone, Debug)]
pub(crate) struct SolverBody {
pub position: Isometry<Real>,
pub integrated_vels: RigidBodyVelocity,
pub im: Vector<Real>,
pub sqrt_ii: AngularInertia<Real>,
pub world_com: Point<Real>,
pub ccd_thickness: Real,
pub damping: RigidBodyDamping,
pub local_com: Point<Real>,
}
impl Default for SolverBody {
fn default() -> Self {
Self {
position: Isometry::identity(),
integrated_vels: RigidBodyVelocity::zero(),
im: na::zero(),
sqrt_ii: AngularInertia::zero(),
world_com: Point::origin(),
ccd_thickness: 0.0,
damping: RigidBodyDamping::default(),
local_com: Point::origin(),
}
}
}
impl SolverBody {
pub fn from(rb: &RigidBody) -> Self {
Self {
position: rb.pos.position,
integrated_vels: RigidBodyVelocity::zero(),
im: rb.mprops.effective_inv_mass,
sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt,
world_com: rb.mprops.world_com,
ccd_thickness: rb.ccd.ccd_thickness,
damping: rb.damping,
local_com: rb.mprops.local_mprops.local_com,
}
}
pub fn copy_from(&mut self, rb: &RigidBody) {
self.position = rb.pos.position;
self.integrated_vels = RigidBodyVelocity::zero();
self.im = rb.mprops.effective_inv_mass;
self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt;
self.world_com = rb.mprops.world_com;
self.ccd_thickness = rb.ccd.ccd_thickness;
self.damping = rb.damping;
self.local_com = rb.mprops.local_mprops.local_com;
}
}

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

@@ -1,564 +0,0 @@
use super::{
AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
use crate::dynamics::solver::GenericVelocityConstraint;
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge,
JointIndex, MultibodyJointSet, RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use na::DVector;
pub(crate) struct SolverConstraints<VelocityConstraint> {
pub generic_jacobians: DVector<Real>,
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub generic_not_ground_interactions: Vec<usize>,
pub generic_ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
}
impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
not_ground_interactions: vec![],
ground_interactions: vec![],
generic_not_ground_interactions: vec![],
generic_ground_interactions: vec![],
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
}
}
// pub fn clear(&mut self) {
// self.not_ground_interactions.clear();
// self.ground_interactions.clear();
// self.generic_not_ground_interactions.clear();
// self.generic_ground_interactions.clear();
// self.interaction_groups.clear();
// self.ground_interaction_groups.clear();
// self.velocity_constraints.clear();
// }
}
impl SolverConstraints<AnyVelocityConstraint> {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.not_ground_interactions.clear();
self.ground_interactions.clear();
self.generic_not_ground_interactions.clear();
self.generic_ground_interactions.clear();
categorize_contacts(
bodies,
multibody_joints,
manifolds,
manifold_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
&mut self.generic_ground_interactions,
&mut self.generic_not_ground_interactions,
);
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD contact resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
self.velocity_constraints.clear();
self.init_constraint_groups(
island_id,
islands,
bodies,
multibody_joints,
manifolds,
manifold_indices,
);
let mut jacobian_id = 0;
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_constraints(params, bodies, manifolds);
self.compute_generic_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_ground_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
self.compute_generic_ground_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifolds_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
fn compute_nongrouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
fn compute_generic_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
for manifold_i in &self.generic_not_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
None,
);
}
}
fn compute_generic_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
for manifold_i in &self.generic_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
None,
);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifolds_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityGroundConstraint::generate(
params,
manifold_id,
manifolds,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
fn compute_nongrouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
}
impl SolverConstraints<AnyJointVelocityConstraint> {
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
// Generate constraints for impulse_joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
self.generic_not_ground_interactions.clear();
self.generic_ground_interactions.clear();
categorize_joints(
bodies,
multibody_joints,
impulse_joints,
joint_constraint_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
&mut self.generic_ground_interactions,
&mut self.generic_not_ground_interactions,
);
self.velocity_constraints.clear();
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
islands,
bodies,
impulse_joints,
&self.not_ground_interactions,
);
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_joints(
island_id,
islands,
bodies,
impulse_joints,
&self.ground_interactions,
);
// NOTE: uncomment this do disable SIMD joint resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.grouped_interactions);
// self.ground_interaction_groups
// .nongrouped_interactions
// .append(&mut self.ground_interaction_groups.grouped_interactions);
let mut j_id = 0;
self.compute_nongrouped_joint_constraints(
params,
bodies,
multibody_joints,
impulse_joints,
&mut j_id,
);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_constraints(params, bodies, impulse_joints);
}
self.compute_generic_joint_constraints(
params,
bodies,
multibody_joints,
impulse_joints,
&mut j_id,
);
self.compute_nongrouped_joint_ground_constraints(
params,
bodies,
multibody_joints,
impulse_joints,
);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_joint_ground_constraints(params, bodies, impulse_joints);
}
self.compute_generic_ground_joint_constraints(
params,
bodies,
multibody_joints,
impulse_joints,
&mut j_id,
);
self.compute_articulation_constraints(
params,
island_id,
islands,
multibody_joints,
&mut j_id,
);
}
fn compute_articulation_constraints(
&mut self,
params: &IntegrationParameters,
island_id: usize,
islands: &IslandManager,
multibody_joints: &MultibodyJointSet,
j_id: &mut usize,
) {
for handle in islands.active_island(island_id) {
if let Some(link) = multibody_joints.rigid_body_link(*handle) {
let multibody = multibody_joints.get_multibody(link.multibody).unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
multibody.generate_internal_constraints(
params,
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
)
}
}
}
}
fn compute_nongrouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
) {
let mut j_id = 0;
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint_ground(
params,
*joint_i,
joint,
bodies,
multibody_joints,
&mut j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
);
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint_ground(
params,
joints_id,
impulse_joints,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
fn compute_nongrouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint(
params,
*joint_i,
joint,
bodies,
multibody_joints,
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
);
}
}
fn compute_generic_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
for joint_i in &self.generic_not_ground_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint(
params,
*joint_i,
joint,
bodies,
multibody_joints,
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
)
}
}
fn compute_generic_ground_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
for joint_i in &self.generic_ground_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint_ground(
params,
*joint_i,
joint,
bodies,
multibody_joints,
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
)
}
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
for joints_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint(
params,
joints_id,
impulse_joints,
bodies,
&mut self.velocity_constraints,
None,
);
}
}
}

241
src/dynamics/solver/solver_constraints_set.rs Normal file
View File

@@ -0,0 +1,241 @@
use super::InteractionGroups;
use crate::math::Real;
use na::DVector;
pub(crate) trait ConstraintTypes {
type OneBody;
type TwoBodies;
type GenericOneBody;
type GenericTwoBodies;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies;
type BuilderOneBody;
type BuilderTwoBodies;
type GenericBuilderOneBody;
type GenericBuilderTwoBodies;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies;
}
pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> {
OneBody(&'a mut Constraints::OneBody),
TwoBodies(&'a mut Constraints::TwoBodies),
GenericOneBody(&'a mut Constraints::GenericOneBody),
GenericTwoBodies(&'a mut Constraints::GenericTwoBodies),
#[cfg(feature = "simd-is-enabled")]
SimdOneBody(&'a mut Constraints::SimdOneBody),
#[cfg(feature = "simd-is-enabled")]
SimdTwoBodies(&'a mut Constraints::SimdTwoBodies),
}
pub(crate) struct SolverConstraintsSet<Constraints: ConstraintTypes> {
pub generic_jacobians: DVector<Real>,
pub two_body_interactions: Vec<usize>,
pub one_body_interactions: Vec<usize>,
pub generic_two_body_interactions: Vec<usize>,
pub generic_one_body_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub one_body_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<Constraints::TwoBodies>,
pub generic_velocity_constraints: Vec<Constraints::GenericTwoBodies>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints: Vec<Constraints::SimdTwoBodies>,
pub velocity_one_body_constraints: Vec<Constraints::OneBody>,
pub generic_velocity_one_body_constraints: Vec<Constraints::GenericOneBody>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_one_body_constraints: Vec<Constraints::SimdOneBody>,
pub velocity_constraints_builder: Vec<Constraints::BuilderTwoBodies>,
pub generic_velocity_constraints_builder: Vec<Constraints::GenericBuilderTwoBodies>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints_builder: Vec<Constraints::SimdBuilderTwoBodies>,
pub velocity_one_body_constraints_builder: Vec<Constraints::BuilderOneBody>,
pub generic_velocity_one_body_constraints_builder: Vec<Constraints::GenericBuilderOneBody>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_one_body_constraints_builder: Vec<Constraints::SimdBuilderOneBody>,
}
impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
two_body_interactions: vec![],
one_body_interactions: vec![],
generic_two_body_interactions: vec![],
generic_one_body_interactions: vec![],
interaction_groups: InteractionGroups::new(),
one_body_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
generic_velocity_constraints: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints: vec![],
velocity_one_body_constraints: vec![],
generic_velocity_one_body_constraints: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_one_body_constraints: vec![],
velocity_constraints_builder: vec![],
generic_velocity_constraints_builder: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints_builder: vec![],
velocity_one_body_constraints_builder: vec![],
generic_velocity_one_body_constraints_builder: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_one_body_constraints_builder: vec![],
}
}
#[allow(dead_code)] // Useful for debuging.
pub fn print_counts(&self) {
println!("Solver constraints:");
println!(
"|__ two_body_interactions: {}",
self.two_body_interactions.len()
);
println!(
"|__ one_body_interactions: {}",
self.one_body_interactions.len()
);
println!(
"|__ generic_two_body_interactions: {}",
self.generic_two_body_interactions.len()
);
println!(
"|__ generic_one_body_interactions: {}",
self.generic_one_body_interactions.len()
);
println!(
"|__ velocity_constraints: {}",
self.velocity_constraints.len()
);
println!(
"|__ generic_velocity_constraints: {}",
self.generic_velocity_constraints.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_constraints: {}",
self.simd_velocity_constraints.len()
);
println!(
"|__ velocity_one_body_constraints: {}",
self.velocity_one_body_constraints.len()
);
println!(
"|__ generic_velocity_one_body_constraints: {}",
self.generic_velocity_one_body_constraints.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_one_body_constraints: {}",
self.simd_velocity_one_body_constraints.len()
);
println!(
"|__ velocity_constraints_builder: {}",
self.velocity_constraints_builder.len()
);
println!(
"|__ generic_velocity_constraints_builder: {}",
self.generic_velocity_constraints_builder.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_constraints_builder: {}",
self.simd_velocity_constraints_builder.len()
);
println!(
"|__ velocity_one_body_constraints_builder: {}",
self.velocity_one_body_constraints_builder.len()
);
println!(
"|__ generic_velocity_one_body_constraints_builder: {}",
self.generic_velocity_one_body_constraints_builder.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_one_body_constraints_builder: {}",
self.simd_velocity_one_body_constraints_builder.len()
);
}
pub fn clear_constraints(&mut self) {
self.generic_jacobians.fill(0.0);
self.velocity_constraints.clear();
self.velocity_one_body_constraints.clear();
self.generic_velocity_constraints.clear();
self.generic_velocity_one_body_constraints.clear();
#[cfg(feature = "simd-is-enabled")]
{
self.simd_velocity_constraints.clear();
self.simd_velocity_one_body_constraints.clear();
}
}
pub fn clear_builders(&mut self) {
self.velocity_constraints_builder.clear();
self.velocity_one_body_constraints_builder.clear();
self.generic_velocity_constraints_builder.clear();
self.generic_velocity_one_body_constraints_builder.clear();
#[cfg(feature = "simd-is-enabled")]
{
self.simd_velocity_constraints_builder.clear();
self.simd_velocity_one_body_constraints_builder.clear();
}
}
// Returns the generic jacobians and a mutable iterator through all the constraints.
pub fn iter_constraints_mut(
&mut self,
) -> (
&DVector<Real>,
impl Iterator<Item = AnyConstraintMut<Constraints>>,
) {
let jac = &self.generic_jacobians;
let a = self
.velocity_constraints
.iter_mut()
.map(AnyConstraintMut::TwoBodies);
let b = self
.generic_velocity_constraints
.iter_mut()
.map(AnyConstraintMut::GenericTwoBodies);
#[cfg(feature = "simd-is-enabled")]
let c = self
.simd_velocity_constraints
.iter_mut()
.map(AnyConstraintMut::SimdTwoBodies);
let d = self
.velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::OneBody);
let e = self
.generic_velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::GenericOneBody);
#[cfg(feature = "simd-is-enabled")]
let f = self
.simd_velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::SimdOneBody);
#[cfg(feature = "simd-is-enabled")]
return (jac, a.chain(b).chain(c).chain(d).chain(e).chain(f));
#[cfg(not(feature = "simd-is-enabled"))]
return (jac, a.chain(b).chain(d).chain(e));
}
}

16
src/dynamics/solver/delta_vel.rs → src/dynamics/solver/solver_vel.rs
View File

@@ -1,17 +1,19 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use crate::utils::WReal;
use crate::utils::SimdRealCopy;
use na::{DVectorView, DVectorViewMut, Scalar};
use std::ops::{AddAssign, Sub};
#[derive(Copy, Clone, Debug, Default)]
#[repr(C)]
//#[repr(align(64))]
pub struct DeltaVel<N: Scalar + Copy> {
pub struct SolverVel<N: Scalar + Copy> {
// The linear velocity of a solver body.
pub linear: Vector<N>,
// The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body.
pub angular: AngVector<N>,
}
impl<N: Scalar + Copy> DeltaVel<N> {
impl<N: Scalar + Copy> SolverVel<N> {
pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
unsafe { std::mem::transmute(self) }
}
@@ -29,7 +31,7 @@ impl<N: Scalar + Copy> DeltaVel<N> {
}
}
impl<N: WReal> DeltaVel<N> {
impl<N: SimdRealCopy> SolverVel<N> {
pub fn zero() -> Self {
Self {
linear: na::zero(),
@@ -38,18 +40,18 @@ impl<N: WReal> DeltaVel<N> {
}
}
impl<N: WReal> AddAssign for DeltaVel<N> {
impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
fn add_assign(&mut self, rhs: Self) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: WReal> Sub for DeltaVel<N> {
impl<N: SimdRealCopy> Sub for SolverVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
DeltaVel {
SolverVel {
linear: self.linear - rhs.linear,
angular: self.angular - rhs.angular,
}

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

@@ -1,441 +0,0 @@
use crate::dynamics::solver::{
GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
use na::DVector;
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
//#[repr(align(64))]
#[derive(Copy, Clone, Debug)]
pub(crate) enum AnyVelocityConstraint {
NongroupedGround(VelocityGroundConstraint),
Nongrouped(VelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
GroupedGround(WVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
Grouped(WVelocityConstraint),
NongroupedGenericGround(GenericVelocityGroundConstraint),
NongroupedGeneric(GenericVelocityConstraint),
#[allow(dead_code)] // The Empty variant is only used with parallel code.
Empty,
}
impl AnyVelocityConstraint {
#[cfg(target_arch = "wasm32")]
pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> {
if let AnyVelocityConstraint::Nongrouped(c) = self {
Some(c)
} else {
None
}
}
#[cfg(target_arch = "wasm32")]
pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> {
if let AnyVelocityConstraint::NongroupedGround(c) = self {
Some(c)
} else {
None
}
}
pub fn remove_bias_from_rhs(&mut self) {
match self {
AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => {
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::Nongrouped(c) => {
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => {
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => {
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
jacobians,
mj_lambdas,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
jacobians,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::NongroupedGeneric(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraint {
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub mj_lambda1: usize,
pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub elements: [VelocityConstraintElement<Real>; MAX_MANIFOLD_POINTS],
}
impl VelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) {
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
(
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize,
manifold.data.solver_contacts.len() * DIM,
)
}
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
assert_eq!(manifold.data.relative_dominance, 0);
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let rb1 = &bodies[handle1];
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
let mj_lambda1 = rb1.ids.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let mut is_fast_contact = false;
#[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
// TODO: this is a WIP optimization for WASM platforms.
// For some reasons, the compiler does not inline the `Vec::push` method
// in this method. This generates two memset and one memcpy which are both very
// expansive.
// This would likely be solved by some kind of "placement-push" (like emplace in C++).
// In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
// avoid spurious copying.
// Is this optimization beneficial when targeting non-WASM platforms?
//
// NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
// for the moment.
#[cfg(target_arch = "wasm32")]
let constraint = if insert_at.is_none() {
let new_len = out_constraints.len() + 1;
unsafe {
#[allow(invalid_value)]
out_constraints.resize_with(new_len, || {
AnyVelocityConstraint::Nongrouped(
std::mem::MaybeUninit::uninit().assume_init(),
)
});
}
out_constraints
.last_mut()
.unwrap()
.as_nongrouped_mut()
.unwrap()
} else {
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
};
#[cfg(target_arch = "wasm32")]
{
constraint.dir1 = force_dir1;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.cfm_factor = cfm_factor;
constraint.limit = 0.0;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
constraint.num_contacts = manifold_points.len() as u8;
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let projected_mass = utils::inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs =
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
#[cfg(not(target_arch = "wasm32"))]
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
}
}
}
pub fn solve(
&mut self,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
self.limit,
&mut mj_lambda1,
&mut mj_lambda2,
solve_normal,
solve_friction,
);
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}
}
}
#[inline(always)]
#[cfg(feature = "dim3")]
pub(crate) fn compute_tangent_contact_directions<N>(
force_dir1: &Vector<N>,
linvel1: &Vector<N>,
linvel2: &Vector<N>,
) -> [Vector<N>; DIM - 1]
where
N: utils::WReal,
Vector<N>: WBasis,
{
use na::SimdValue;
// Compute the tangent direction. Pick the direction of
// the linear relative velocity, if it is not too small.
// Otherwise use a fallback direction.
let relative_linvel = linvel1 - linvel2;
let mut tangent_relative_linvel =
relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
let tangent_linvel_norm = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
tangent_relative_linvel.normalize_mut()
};
const THRESHOLD: Real = 1.0e-4;
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
let tangent_fallback = force_dir1.orthonormal_vector();
let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
let bitangent1 = force_dir1.cross(&tangent1);
[tangent1, bitangent1]
}

211
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -1,211 +0,0 @@
use super::DeltaVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{WBasis, WDot, WReal};
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintTangentPart<N: WReal> {
pub gcross1: [AngVector<N>; DIM - 1],
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: WReal> VelocityConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross1: [na::zero(); DIM - 1],
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
#[inline]
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let dvel = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda;
mj_lambda1.angular += self.gcross1[0] * dlambda;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
mj_lambda2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dvel_1 = tangents1[1].dot(&mj_lambda1.linear)
+ self.gcross1[1].gdot(mj_lambda1.angular)
- tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
pub gcross1: AngVector<N>,
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub r: N,
}
impl<N: WReal> VelocityConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross1: na::zero(),
gcross2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
r: na::zero(),
}
}
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
- dir1.dot(&mj_lambda2.linear)
+ self.gcross2.gdot(mj_lambda2.angular)
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda1.linear += dir1.component_mul(im1) * dlambda;
mj_lambda1.angular += self.gcross1 * dlambda;
mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
mj_lambda2.angular += self.gcross2 * dlambda;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintElement<N: WReal> {
pub normal_part: VelocityConstraintNormalPart<N>,
pub tangent_part: VelocityConstraintTangentPart<N>,
}
impl<N: WReal> VelocityConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: VelocityConstraintNormalPart::zero(),
tangent_part: VelocityConstraintTangentPart::zero(),
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &mut DeltaVel<N>,
solve_normal: bool,
solve_friction: bool,
) where
Vector<N>: WBasis,
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2);
}
}
// Solve friction.
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(&tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2);
}
}
}
}

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

@@ -1,281 +0,0 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraint {
pub mj_lambda2: usize,
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
}
impl VelocityGroundConstraint {
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = rb2.ids.active_set_offset;
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
#[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
// TODO: this is a WIP optimization for WASM platforms.
// For some reasons, the compiler does not inline the `Vec::push` method
// in this method. This generates two memset and one memcpy which are both very
// expansive.
// This would likely be solved by some kind of "placement-push" (like emplace in C++).
// In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
// avoid spurious copying.
// Is this optimization beneficial when targeting non-WASM platforms?
//
// NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
// for the moment.
#[cfg(target_arch = "wasm32")]
let constraint = if insert_at.is_none() {
let new_len = out_constraints.len() + 1;
unsafe {
#[allow(invalid_value)]
out_constraints.resize_with(new_len, || {
AnyVelocityConstraint::NongroupedGround(
std::mem::MaybeUninit::uninit().assume_init(),
)
});
}
out_constraints
.last_mut()
.unwrap()
.as_nongrouped_ground_mut()
.unwrap()
} else {
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
};
#[cfg(target_arch = "wasm32")]
{
constraint.dir1 = force_dir1;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
constraint.im2 = mprops2.effective_inv_mass;
constraint.cfm_factor = cfm_factor;
constraint.limit = 0.0;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
constraint.num_contacts = manifold_points.len() as u8;
}
let mut is_fast_contact = false;
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
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);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = utils::inv(
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let dvel = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
#[cfg(not(target_arch = "wasm32"))]
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
}
}
}
pub fn solve(
&mut self,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityGroundConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
self.limit,
&mut mj_lambda2,
solve_normal,
solve_friction,
);
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}
}
}

181
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -1,181 +0,0 @@
use super::DeltaVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{WBasis, WDot, WReal};
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintTangentPart<N: WReal> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: WReal> VelocityGroundConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
#[inline]
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let dvel = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
mj_lambda2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub r: N,
}
impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
r: na::zero(),
}
}
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
mj_lambda2.angular += self.gcross2 * dlambda;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintElement<N: WReal> {
pub normal_part: VelocityGroundConstraintNormalPart<N>,
pub tangent_part: VelocityGroundConstraintTangentPart<N>,
}
impl<N: WReal> VelocityGroundConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: VelocityGroundConstraintNormalPart::zero(),
tangent_part: VelocityGroundConstraintTangentPart::zero(),
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
limit: N,
mj_lambda2: &mut DeltaVel<N>,
solve_normal: bool,
solve_friction: bool,
) where
Vector<N>: WBasis,
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, &dir1, im2, mj_lambda2);
}
}
// Solve friction.
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(&tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im2, limit, mj_lambda2);
}
}
}
}

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

@@ -1,48 +1,99 @@
use super::AnyJointVelocityConstraint;
use super::{JointConstraintTypes, SolverConstraintsSet};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
solver::{ContactConstraintTypes, SolverVel},
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;
use crate::utils::WAngularInertia;
use crate::prelude::RigidBodyVelocity;
use crate::utils::SimdAngularInertia;
use na::DVector;
pub(crate) struct VelocitySolver {
pub mj_lambdas: Vec<DeltaVel<Real>>,
pub generic_mj_lambdas: DVector<Real>,
pub solver_bodies: Vec<SolverBody>,
pub solver_vels: Vec<SolverVel<Real>>,
pub solver_vels_increment: Vec<SolverVel<Real>>,
pub generic_solver_vels: DVector<Real>,
pub generic_solver_vels_increment: DVector<Real>,
pub multibody_roots: Vec<MultibodyLinkId>,
}
impl VelocitySolver {
pub fn new() -> Self {
Self {
mj_lambdas: Vec::new(),
generic_mj_lambdas: DVector::zeros(0),
solver_bodies: Vec::new(),
solver_vels: Vec::new(),
solver_vels_increment: Vec::new(),
generic_solver_vels: DVector::zeros(0),
generic_solver_vels_increment: DVector::zeros(0),
multibody_roots: Vec::new(),
}
}
pub fn solve(
&mut self,
pub fn init_constraints(
&self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
manifolds_all: &mut [&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
generic_contact_jacobians: &DVector<Real>,
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,
joint_indices: &[JointIndex],
contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
) {
self.mj_lambdas.clear();
self.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
contact_constraints.init(
island_id,
islands,
bodies,
multibodies,
manifolds_all,
manifold_indices,
);
let total_multibodies_ndofs = multibodies.multibodies.iter().map(|m| m.1.ndofs()).sum();
self.generic_mj_lambdas = DVector::zeros(total_multibodies_ndofs);
joint_constraints.init(
island_id,
islands,
bodies,
multibodies,
joints_all,
joint_indices,
);
}
// Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
pub fn init_solver_velocities_and_solver_bodies(
&mut self,
params: &IntegrationParameters,
island_id: usize,
islands: &IslandManager,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
) {
self.multibody_roots.clear();
self.solver_bodies.clear();
self.solver_bodies.resize(
islands.active_island(island_id).len(),
SolverBody::default(),
);
self.solver_vels_increment.clear();
self.solver_vels_increment
.resize(islands.active_island(island_id).len(), SolverVel::zero());
self.solver_vels.clear();
self.solver_vels
.resize(islands.active_island(island_id).len(), SolverVel::zero());
/*
* Initialize solver bodies and delta-velocities (`solver_vels_increment`) with external forces (gravity etc):
* NOTE: we compute this only once by neglecting changes of mass matrices.
*/
// Assign solver ids to multibodies, and collect the relevant roots.
// And init solver_vels for rigidb-bodies.
let mut multibody_solver_id = 0;
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
@@ -50,196 +101,213 @@ impl VelocitySolver {
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mut mj_lambdas = self
.generic_mj_lambdas
.rows_mut(multibody.solver_id, multibody.ndofs());
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
multibody.solver_id = multibody_solver_id;
multibody_solver_id += multibody.ndofs();
self.multibody_roots.push(link);
}
} else {
let rb = &bodies[*handle];
let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset];
let solver_vel = &mut self.solver_vels[rb.ids.active_set_offset];
let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset];
let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset];
solver_body.copy_from(rb);
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular +=
solver_vel_incr.angular =
rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear +=
solver_vel_incr.linear =
rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
solver_vel.linear = rb.vels.linvel;
// PERF: can we avoid the call to effective_angular_inertia_sqrt?
solver_vel.angular = rb.mprops.effective_angular_inertia_sqrt() * rb.vels.angvel;
}
}
// PERF: don’t reallocate at each iteration.
self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id);
self.generic_solver_vels = DVector::zeros(multibody_solver_id);
// init solver_vels for multibodies.
for link in &self.multibody_roots {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
multibody.update_dynamics(params.dt, bodies);
multibody.update_acceleration(bodies);
let mut solver_vels_incr = self
.generic_solver_vels_increment
.rows_mut(multibody.solver_id, multibody.ndofs());
let mut solver_vels = self
.generic_solver_vels
.rows_mut(multibody.solver_id, multibody.ndofs());
solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
solver_vels.copy_from(&multibody.velocities);
}
}
pub fn solve_constraints(
&mut self,
params: &IntegrationParameters,
num_solver_iterations: usize,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
) {
for small_step_id in 0..num_solver_iterations {
let is_last_small_step = small_step_id == num_solver_iterations - 1;
for (solver_vels, incr) in self
.solver_vels
.iter_mut()
.zip(self.solver_vels_increment.iter())
{
solver_vels.linear += incr.linear;
solver_vels.angular += incr.angular;
}
self.generic_solver_vels += &self.generic_solver_vels_increment;
/*
* Update & solve constraints.
*/
joint_constraints.update(&params, multibodies, &self.solver_bodies);
contact_constraints.update(&params, small_step_id, multibodies, &self.solver_bodies);
joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
.solve_restitution(&mut self.solver_vels, &mut self.generic_solver_vels);
let num_friction_iterations = if is_last_small_step {
params.num_friction_iteration_per_solver_iteration * num_solver_iterations
- (num_solver_iterations - 1)
} else {
1
};
for _ in 0..num_friction_iterations {
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
/*
* Solve constraints.
* Integrate positions.
*/
for i in 0..params.max_velocity_iterations {
let solve_friction = params.interleave_restitution_and_friction_resolution
&& params.max_velocity_friction_iterations + i >= params.max_velocity_iterations;
self.integrate_positions(&params, is_last_small_step, bodies, multibodies);
for constraint in &mut *joint_constraints {
constraint.solve(
generic_joint_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
);
/*
* Resolution without bias.
*/
joint_constraints.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
.solve_restitution_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
}
}
for constraint in &mut *contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
true,
false,
);
pub fn integrate_positions(
&mut self,
params: &IntegrationParameters,
is_last_small_step: bool,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
) {
// Integrate positions.
for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut())
{
let linvel = solver_vels.linear;
let angvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
let mut new_vels = RigidBodyVelocity { linvel, angvel };
new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
let new_pos =
new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com);
solver_body.integrated_vels += new_vels;
solver_body.position = new_pos;
solver_body.world_com = new_pos * solver_body.local_com;
}
if solve_friction {
for constraint in &mut *contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
false,
true,
);
// Integrate multibody positions.
for link in &self.multibody_roots {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
multibody.integrate(params.dt);
// PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
multibody.forward_kinematics(bodies, !is_last_small_step);
if !is_last_small_step {
// These are very expensive and not needed if we don’t
// have to run another step.
multibody.update_dynamics(params.dt, bodies);
multibody.update_acceleration(bodies);
let mut solver_vels_incr = self
.generic_solver_vels_increment
.rows_mut(multibody.solver_id, multibody.ndofs());
solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
}
}
}
let remaining_friction_iterations =
if !params.interleave_restitution_and_friction_resolution {
params.max_velocity_friction_iterations
} else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
params.max_velocity_friction_iterations - params.max_velocity_iterations
pub fn writeback_bodies(
&mut self,
params: &IntegrationParameters,
num_solver_iterations: usize,
islands: &IslandManager,
island_id: usize,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
) {
for handle in islands.active_island(island_id) {
let link = if self.multibody_roots.is_empty() {
None
} else {
0
multibodies.rigid_body_link(*handle).copied()
};
for _ in 0..remaining_friction_iterations {
for constraint in &mut *contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
false,
true,
);
}
}
// Integrate positions.
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
if let Some(link) = link {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
multibody.velocities += mj_lambdas;
multibody.integrate(params.dt);
multibody.forward_kinematics(bodies, false);
multibody.velocities = prev_vels;
multibody.velocities.copy_from(&solver_vels);
}
} else {
let rb = bodies.index_mut_internal(*handle);
let solver_body = &self.solver_bodies[rb.ids.active_set_offset];
let solver_vels = &self.solver_vels[rb.ids.active_set_offset];
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb
.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let dangvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
// Update positions.
let mut new_pos = rb.pos;
let mut new_vels = rb.vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, &rb.damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb.pos.position,
&rb.mprops.local_mprops.local_com,
);
rb.integrated_vels = new_vels;
rb.pos = new_pos;
}
}
let mut new_vels = RigidBodyVelocity {
linvel: solver_vels.linear,
angvel: dangvel,
};
new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
for joint in &mut *joint_constraints {
joint.remove_bias_from_rhs();
}
for constraint in &mut *contact_constraints {
constraint.remove_bias_from_rhs();
}
for _ in 0..params.max_stabilization_iterations {
for constraint in &mut *joint_constraints {
constraint.solve(
generic_joint_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
);
}
for constraint in &mut *contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
true,
false,
);
}
for constraint in &mut *contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
false,
true,
);
}
}
// Update velocities.
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities += mj_lambdas;
}
} else {
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb
.mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
rb.vels.linvel += dvel.linear;
rb.vels.angvel += dangvel;
rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
}
}
// Write impulses back into the manifold structures.
for constraint in &*joint_constraints {
constraint.writeback_impulses(joints_all);
}
for constraint in &*contact_constraints {
constraint.writeback_impulses(manifolds_all);
// NOTE: using integrated_vels instead of interpolation is interesting for
// high angular velocities. However, it is a bit inexact due to the
// solver integrating at intermediate sub-steps. Should we just switch
// to interpolation?
rb.integrated_vels.linvel =
solver_body.integrated_vels.linvel / num_solver_iterations as Real;
rb.integrated_vels.angvel =
solver_body.integrated_vels.angvel / num_solver_iterations as Real;
rb.vels = new_vels;
rb.pos.next_position = solver_body.position;
}
}
}
}

2
src/geometry/contact_pair.rs
View File

@@ -283,7 +283,7 @@ pub struct ContactManifoldData {
pub struct SolverContact {
/// The index of the manifold contact used to generate this solver contact.
pub(crate) contact_id: u8,
/// The world-space contact point.
/// The contact point in world-space.
pub point: Point<Real>,
/// The distance between the two original contacts points along the contact normal.
/// If negative, this is measures the penetration depth.

8
src/geometry/narrow_phase.rs
View File

@@ -927,6 +927,7 @@ impl NarrowPhase {
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos);
let world_pos2 = manifold.subshape_pos2.prepend_to(&co2.pos);
manifold.data.solver_contacts.clear();
manifold.data.rigid_body1 = co1.parent.map(|p| p.handle);
manifold.data.rigid_body2 = co2.parent.map(|p| p.handle);
@@ -944,10 +945,13 @@ impl NarrowPhase {
if contact.dist < prediction_distance {
// Generate the solver contact.
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
let effective_point = na::center(&world_pt1, &world_pt2);
let solver_contact = SolverContact {
contact_id: contact_id as u8,
point: world_pos1 * contact.local_p1
+ manifold.data.normal * contact.dist / 2.0,
point: effective_point,
dist: contact.dist,
friction,
restitution,

4
src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
View File

@@ -8,7 +8,7 @@ use crate::geometry::{Cone, Cylinder};
use crate::math::{Isometry, Point, Real, Vector, DIM};
use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
use crate::utils::WBasis;
use crate::utils::SimdBasis;
use std::any::TypeId;
use std::collections::HashMap;
@@ -221,7 +221,7 @@ impl DebugRenderPipeline {
}
if self.mode.contains(DebugRenderMode::MULTIBODY_JOINTS) {
for (handle, multibody, link) in multibody_joints.iter() {
for (handle, _, multibody, link) in multibody_joints.iter() {
let anc_color = self.style.multibody_joint_anchor_color;
let sep_color = self.style.multibody_joint_separation_color;
let parent = multibody.link(link.parent_id().unwrap()).unwrap();

31
src/pipeline/physics_pipeline.rs
View File

@@ -1,14 +1,14 @@
//! Physics pipeline structures.
use crate::counters::Counters;
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
#[cfg(feature = "parallel")]
use crate::dynamics::JointGraphEdge;
use crate::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType,
};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair,
ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
@@ -206,19 +206,14 @@ impl PhysicsPipeline {
self.counters.stages.update_time.resume();
for handle in islands.active_dynamic_bodies() {
// TODO: should that be moved to the solver (just like we moved
// the multibody dynamics update) since it depends on dt?
let rb = bodies.index_mut_internal(*handle);
rb.mprops.update_world_mass_properties(&rb.pos.position);
let effective_mass = rb.mprops.effective_mass();
rb.forces
.compute_effective_force_and_torque(&gravity, &effective_mass);
}
for multibody in &mut multibody_joints.multibodies {
multibody
.1
.update_dynamics(integration_parameters.dt, bodies);
multibody.1.update_acceleration(bodies);
}
self.counters.stages.update_time.pause();
self.counters.stages.solver_time.resume();
@@ -263,6 +258,10 @@ impl PhysicsPipeline {
let manifold_indices = &self.manifold_indices[..];
let joint_constraint_indices = &self.joint_constraint_indices[..];
// PERF: right now, we are only doing islands-based parallelism.
// Intra-island parallelism (that hasn’t been ported to the new
// solver yet) will be supported in the future.
self.counters.solver.velocity_resolution_time.resume();
rayon::scope(|scope| {
enable_flush_to_zero!();
@@ -280,20 +279,22 @@ impl PhysicsPipeline {
std::mem::transmute(multibody_joints.load(Ordering::Relaxed))
};
let mut counters = Counters::new(false);
solver.init_and_solve(
scope,
island_id,
islands,
&mut counters,
integration_parameters,
islands,
bodies,
manifolds,
&manifold_indices[island_id],
&mut manifolds[..],
&self.manifold_indices[island_id],
impulse_joints,
&joint_constraint_indices[island_id],
&self.joint_constraint_indices[island_id],
multibody_joints,
)
});
});
self.counters.solver.velocity_resolution_time.pause();
}
// Generate contact force events if needed.

108
src/utils.rs
View File

@@ -18,9 +18,9 @@ use {
/// The trait for real numbers used by Rapier.
///
/// This includes `f32`, `f64` and their related SIMD types.
pub trait WReal: SimdRealField<Element = Real> + Copy {}
impl WReal for Real {}
impl WReal for SimdReal {}
pub trait SimdRealCopy: SimdRealField<Element = Real> + Copy {}
impl SimdRealCopy for Real {}
impl SimdRealCopy for SimdReal {}
const INV_EPSILON: Real = 1.0e-20;
@@ -32,19 +32,19 @@ pub(crate) fn inv(val: Real) -> Real {
}
}
pub(crate) fn simd_inv<N: WReal>(val: N) -> N {
pub(crate) fn simd_inv<N: SimdRealCopy>(val: N) -> N {
let eps = N::splat(INV_EPSILON);
N::zero().select(val.simd_gt(-eps) & val.simd_lt(eps), N::one() / val)
}
/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
pub trait WSign<Rhs>: Sized {
pub trait SimdSign<Rhs>: Sized {
// See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
/// Copy the sign of each component of `self` to the corresponding component of `to`.
fn copy_sign_to(self, to: Rhs) -> Rhs;
}
impl WSign<Real> for Real {
impl SimdSign<Real> for Real {
fn copy_sign_to(self, to: Self) -> Self {
const MINUS_ZERO: Real = -0.0;
let signbit = MINUS_ZERO.to_bits();
@@ -52,13 +52,13 @@ impl WSign<Real> for Real {
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for N {
impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector2<N>> for N {
fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for N {
impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector3<N>> for N {
fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
Vector3::new(
self.copy_sign_to(to.x),
@@ -68,13 +68,13 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for N {
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for Vector2<N> {
impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector2<N>> for Vector2<N> {
fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector3<N>> for Vector3<N> {
fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
Vector3::new(
self.x.copy_sign_to(to.x),
@@ -84,20 +84,20 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
}
}
impl WSign<SimdReal> for SimdReal {
impl SimdSign<SimdReal> for SimdReal {
fn copy_sign_to(self, to: SimdReal) -> SimdReal {
to.simd_copysign(self)
}
}
pub(crate) trait WComponent: Sized {
pub(crate) trait SimdComponent: Sized {
type Element;
fn min_component(self) -> Self::Element;
fn max_component(self) -> Self::Element;
}
impl WComponent for Real {
impl SimdComponent for Real {
type Element = Real;
fn min_component(self) -> Self::Element {
@@ -108,7 +108,7 @@ impl WComponent for Real {
}
}
impl WComponent for SimdReal {
impl SimdComponent for SimdReal {
type Element = Real;
fn min_component(self) -> Self::Element {
@@ -120,7 +120,7 @@ impl WComponent for SimdReal {
}
/// Trait to compute the orthonormal basis of a vector.
pub trait WBasis: Sized {
pub trait SimdBasis: Sized {
/// The type of the array of orthonormal vectors.
type Basis;
/// Computes the vectors which, when combined with `self`, form an orthonormal basis.
@@ -129,7 +129,7 @@ pub trait WBasis: Sized {
fn orthonormal_vector(self) -> Self;
}
impl<N: WReal> WBasis for Vector2<N> {
impl<N: SimdRealCopy> SimdBasis for Vector2<N> {
type Basis = [Vector2<N>; 1];
fn orthonormal_basis(self) -> [Vector2<N>; 1] {
[Vector2::new(-self.y, self.x)]
@@ -139,7 +139,7 @@ impl<N: WReal> WBasis for Vector2<N> {
}
}
impl<N: WReal + WSign<N>> WBasis for Vector3<N> {
impl<N: SimdRealCopy + SimdSign<N>> SimdBasis for Vector3<N> {
type Basis = [Vector3<N>; 2];
// Robust and branchless implementation from Pixar:
// https://graphics.pixar.com/library/OrthonormalB/paper.pdf
@@ -166,14 +166,14 @@ impl<N: WReal + WSign<N>> WBasis for Vector3<N> {
}
}
pub(crate) trait WVec: Sized {
pub(crate) trait SimdVec: Sized {
type Element;
fn horizontal_inf(&self) -> Self::Element;
fn horizontal_sup(&self) -> Self::Element;
}
impl<N: Scalar + Copy + WComponent> WVec for Vector2<N>
impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector2<N>
where
N::Element: Scalar,
{
@@ -188,7 +188,7 @@ where
}
}
impl<N: Scalar + Copy + WComponent> WVec for Point2<N>
impl<N: Scalar + Copy + SimdComponent> SimdVec for Point2<N>
where
N::Element: Scalar,
{
@@ -203,7 +203,7 @@ where
}
}
impl<N: Scalar + Copy + WComponent> WVec for Vector3<N>
impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector3<N>
where
N::Element: Scalar,
{
@@ -226,7 +226,7 @@ where
}
}
impl<N: Scalar + Copy + WComponent> WVec for Point3<N>
impl<N: Scalar + Copy + SimdComponent> SimdVec for Point3<N>
where
N::Element: Scalar,
{
@@ -249,7 +249,7 @@ where
}
}
pub(crate) trait WCrossMatrix: Sized {
pub(crate) trait SimdCrossMatrix: Sized {
type CrossMat;
type CrossMatTr;
@@ -257,7 +257,7 @@ pub(crate) trait WCrossMatrix: Sized {
fn gcross_matrix_tr(self) -> Self::CrossMatTr;
}
impl<N: WReal> WCrossMatrix for Vector3<N> {
impl<N: SimdRealCopy> SimdCrossMatrix for Vector3<N> {
type CrossMat = Matrix3<N>;
type CrossMatTr = Matrix3<N>;
@@ -282,7 +282,7 @@ impl<N: WReal> WCrossMatrix for Vector3<N> {
}
}
impl<N: WReal> WCrossMatrix for Vector2<N> {
impl<N: SimdRealCopy> SimdCrossMatrix for Vector2<N> {
type CrossMat = RowVector2<N>;
type CrossMatTr = Vector2<N>;
@@ -295,7 +295,7 @@ impl<N: WReal> WCrossMatrix for Vector2<N> {
Vector2::new(-self.y, self.x)
}
}
impl WCrossMatrix for Real {
impl SimdCrossMatrix for Real {
type CrossMat = Matrix2<Real>;
type CrossMatTr = Matrix2<Real>;
@@ -310,7 +310,7 @@ impl WCrossMatrix for Real {
}
}
impl WCrossMatrix for SimdReal {
impl SimdCrossMatrix for SimdReal {
type CrossMat = Matrix2<SimdReal>;
type CrossMatTr = Matrix2<SimdReal>;
@@ -325,12 +325,12 @@ impl WCrossMatrix for SimdReal {
}
}
pub(crate) trait WCross<Rhs>: Sized {
pub(crate) trait SimdCross<Rhs>: Sized {
type Result;
fn gcross(&self, rhs: Rhs) -> Self::Result;
}
impl WCross<Vector3<Real>> for Vector3<Real> {
impl SimdCross<Vector3<Real>> for Vector3<Real> {
type Result = Self;
fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
@@ -338,7 +338,7 @@ impl WCross<Vector3<Real>> for Vector3<Real> {
}
}
impl WCross<Vector2<Real>> for Vector2<Real> {
impl SimdCross<Vector2<Real>> for Vector2<Real> {
type Result = Real;
fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
@@ -346,7 +346,7 @@ impl WCross<Vector2<Real>> for Vector2<Real> {
}
}
impl WCross<Vector2<Real>> for Real {
impl SimdCross<Vector2<Real>> for Real {
type Result = Vector2<Real>;
fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
@@ -354,12 +354,12 @@ impl WCross<Vector2<Real>> for Real {
}
}
pub(crate) trait WDot<Rhs>: Sized {
pub(crate) trait SimdDot<Rhs>: Sized {
type Result;
fn gdot(&self, rhs: Rhs) -> Self::Result;
}
impl<N: WReal> WDot<Vector3<N>> for Vector3<N> {
impl<N: SimdRealCopy> SimdDot<Vector3<N>> for Vector3<N> {
type Result = N;
fn gdot(&self, rhs: Vector3<N>) -> Self::Result {
@@ -367,7 +367,7 @@ impl<N: WReal> WDot<Vector3<N>> for Vector3<N> {
}
}
impl<N: WReal> WDot<Vector2<N>> for Vector2<N> {
impl<N: SimdRealCopy> SimdDot<Vector2<N>> for Vector2<N> {
type Result = N;
fn gdot(&self, rhs: Vector2<N>) -> Self::Result {
@@ -375,7 +375,7 @@ impl<N: WReal> WDot<Vector2<N>> for Vector2<N> {
}
}
impl<N: WReal> WDot<Vector1<N>> for N {
impl<N: SimdRealCopy> SimdDot<Vector1<N>> for N {
type Result = N;
fn gdot(&self, rhs: Vector1<N>) -> Self::Result {
@@ -383,7 +383,7 @@ impl<N: WReal> WDot<Vector1<N>> for N {
}
}
impl<N: WReal> WDot<N> for N {
impl<N: SimdRealCopy> SimdDot<N> for N {
type Result = N;
fn gdot(&self, rhs: N) -> Self::Result {
@@ -391,7 +391,7 @@ impl<N: WReal> WDot<N> for N {
}
}
impl<N: WReal> WDot<N> for Vector1<N> {
impl<N: SimdRealCopy> SimdDot<N> for Vector1<N> {
type Result = N;
fn gdot(&self, rhs: N) -> Self::Result {
@@ -399,7 +399,7 @@ impl<N: WReal> WDot<N> for Vector1<N> {
}
}
impl WCross<Vector3<SimdReal>> for Vector3<SimdReal> {
impl SimdCross<Vector3<SimdReal>> for Vector3<SimdReal> {
type Result = Vector3<SimdReal>;
fn gcross(&self, rhs: Self) -> Self::Result {
@@ -407,7 +407,7 @@ impl WCross<Vector3<SimdReal>> for Vector3<SimdReal> {
}
}
impl WCross<Vector2<SimdReal>> for SimdReal {
impl SimdCross<Vector2<SimdReal>> for SimdReal {
type Result = Vector2<SimdReal>;
fn gcross(&self, rhs: Vector2<SimdReal>) -> Self::Result {
@@ -415,7 +415,7 @@ impl WCross<Vector2<SimdReal>> for SimdReal {
}
}
impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
impl SimdCross<Vector2<SimdReal>> for Vector2<SimdReal> {
type Result = SimdReal;
fn gcross(&self, rhs: Self) -> Self::Result {
@@ -426,7 +426,7 @@ impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
}
/// Trait implemented by quaternions.
pub trait WQuat<N> {
pub trait SimdQuat<N> {
/// The result of quaternion differentiation.
type Result;
@@ -434,7 +434,7 @@ pub trait WQuat<N> {
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
}
impl<N: WReal> WQuat<N> for UnitComplex<N> {
impl<N: SimdRealCopy> SimdQuat<N> for UnitComplex<N> {
type Result = Matrix1<N>;
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
@@ -443,7 +443,7 @@ impl<N: WReal> WQuat<N> for UnitComplex<N> {
}
}
impl<N: WReal> WQuat<N> for UnitQuaternion<N> {
impl<N: SimdRealCopy> SimdQuat<N> for UnitQuaternion<N> {
type Result = Matrix3<N>;
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
@@ -461,7 +461,7 @@ impl<N: WReal> WQuat<N> for UnitQuaternion<N> {
}
}
pub(crate) trait WAngularInertia<N> {
pub(crate) trait SimdAngularInertia<N> {
type AngVector;
type LinVector;
type AngMatrix;
@@ -473,7 +473,7 @@ pub(crate) trait WAngularInertia<N> {
fn into_matrix(self) -> Self::AngMatrix;
}
impl<N: WReal> WAngularInertia<N> for N {
impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
type AngVector = N;
type LinVector = Vector2<N>;
type AngMatrix = N;
@@ -502,7 +502,7 @@ impl<N: WReal> WAngularInertia<N> for N {
}
}
impl WAngularInertia<Real> for SdpMatrix3<Real> {
impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
type AngVector = Vector3<Real>;
type LinVector = Vector3<Real>;
type AngMatrix = Matrix3<Real>;
@@ -566,7 +566,7 @@ impl WAngularInertia<Real> for SdpMatrix3<Real> {
}
}
impl WAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
type AngVector = Vector3<SimdReal>;
type LinVector = Vector3<SimdReal>;
type AngMatrix = Matrix3<SimdReal>;
@@ -668,6 +668,7 @@ impl FlushToZeroDenormalsAreZeroFlags {
any(target_arch = "x86", target_arch = "x86_64"),
target_feature = "sse"
))]
#[allow(deprecated)] // will address that later.
pub fn flush_denormal_to_zero() -> Self {
unsafe {
#[cfg(target_arch = "x86")]
@@ -691,6 +692,7 @@ impl FlushToZeroDenormalsAreZeroFlags {
target_feature = "sse"
))]
impl Drop for FlushToZeroDenormalsAreZeroFlags {
#[allow(deprecated)] // will address that later.
fn drop(&mut self) {
#[cfg(target_arch = "x86")]
unsafe {
@@ -806,7 +808,7 @@ impl<T> IndexMut2<usize> for [T] {
}
/// Calculate the difference with smallest absolute value between the two given values.
pub fn smallest_abs_diff_between_sin_angles<N: WReal>(a: N, b: N) -> N {
pub fn smallest_abs_diff_between_sin_angles<N: SimdRealCopy>(a: N, b: N) -> N {
// Select the smallest path among the two angles to reach the target.
let s_err = a - b;
let sgn = s_err.simd_signum();
@@ -814,3 +816,13 @@ pub fn smallest_abs_diff_between_sin_angles<N: WReal>(a: N, b: N) -> N {
let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
s_err.select(s_err_is_smallest, s_err_complement)
}
/// Calculate the difference with smallest absolute value between the two given angles.
pub fn smallest_abs_diff_between_angles<N: SimdRealCopy>(a: N, b: N) -> N {
// Select the smallest path among the two angles to reach the target.
let s_err = a - b;
let sgn = s_err.simd_signum();
let s_err_complement = s_err - sgn * N::simd_two_pi();
let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
s_err.select(s_err_is_smallest, s_err_complement)
}

7
src_testbed/box2d_backend.rs
View File

@@ -223,11 +223,8 @@ impl Box2dWorld {
pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
counters.step_started();
self.world.step(
params.dt,
params.max_velocity_iterations as i32,
params.max_stabilization_iterations as i32,
);
self.world
.step(params.dt, params.num_solver_iterations.get() as i32, 1);
counters.step_completed();
}

1
src_testbed/lib.rs
View File

@@ -12,6 +12,7 @@ pub use crate::harness::plugin::HarnessPlugin;
pub use crate::physics::PhysicsState;
pub use crate::plugin::TestbedPlugin;
pub use crate::testbed::{Testbed, TestbedApp, TestbedGraphics, TestbedState};
pub use bevy::prelude::KeyCode;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
mod box2d_backend;

7
src_testbed/physx_backend.rs
View File

@@ -209,10 +209,9 @@ impl PhysxWorld {
actor.set_linear_velocity(&linvel, true);
actor.set_angular_velocity(&angvel, true);
actor.set_solver_iteration_counts(
// Use our number of velocity iterations as their number of position iterations.
integration_parameters.max_velocity_iterations.max(1) as u32,
// Use our number of velocity stabilization iterations as their number of velocity iterations.
integration_parameters.max_stabilization_iterations.max(1) as u32,
// Use our number of solver iterations as their number of position iterations.
integration_parameters.num_solver_iterations.get() as u32,
1,
);
rapier2dynamic.insert(rapier_handle, actor);

17
src_testbed/testbed.rs
View File

@@ -1,5 +1,6 @@
use std::env;
use std::mem;
use std::num::NonZeroUsize;
use bevy::prelude::*;
@@ -147,6 +148,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
#[allow(dead_code)] // Dead in 2D but not in 3D.
camera_transform: GlobalTransform,
camera: &'a mut OrbitCamera,
keys: &'a Input<KeyCode>,
}
pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -284,11 +286,7 @@ impl TestbedApp {
self.harness
.physics
.integration_parameters
.max_velocity_iterations = 4;
self.harness
.physics
.integration_parameters
.max_stabilization_iterations = 1;
.num_solver_iterations = NonZeroUsize::new(4).unwrap();
// Init world.
let mut testbed = Testbed {
@@ -458,6 +456,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
colliders,
)
}
pub fn keys(&self) -> &Input<KeyCode> {
&*self.keys
}
}
impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -910,7 +912,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
.multibody_joints
.iter()
.next()
.map(|a| a.2.rigid_body_handle());
.map(|(_, _, _, link)| link.rigid_body_handle());
if let Some(to_delete) = to_delete {
self.harness
.physics
@@ -1107,6 +1109,7 @@ fn update_testbed(
components: &mut gfx_components,
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &*keys,
};
let mut testbed = Testbed {
@@ -1200,6 +1203,7 @@ fn update_testbed(
components: &mut gfx_components,
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &*keys,
};
let mut testbed = Testbed {
@@ -1351,6 +1355,7 @@ fn update_testbed(
components: &mut gfx_components,
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &*keys,
};
harness.step_with_graphics(Some(&mut testbed_graphics));

43
src_testbed/ui.rs
View File

@@ -1,5 +1,6 @@
use rapier::counters::Counters;
use rapier::math::Real;
use std::num::NonZeroUsize;
use crate::debug_render::DebugRenderPipelineResource;
use crate::harness::Harness;
@@ -98,43 +99,25 @@ pub fn update_ui(
let integration_parameters = &mut harness.physics.integration_parameters;
ui.checkbox(
&mut integration_parameters.interleave_restitution_and_friction_resolution,
"interleave friction resolution",
);
if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
|| state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR
{
ui.add(
Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200)
.text("pos. iters."),
);
ui.add(
Slider::new(
&mut integration_parameters.max_stabilization_iterations,
1..=200,
)
.text("vel. iters."),
);
let mut num_iterations = integration_parameters.num_solver_iterations.get();
ui.add(Slider::new(&mut num_iterations, 1..=40).text("pos. iters."));
integration_parameters.num_solver_iterations =
NonZeroUsize::new(num_iterations).unwrap();
} else {
ui.add(
Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200)
.text("vel. rest. iters."),
);
let mut num_iterations = integration_parameters.num_solver_iterations.get();
ui.add(Slider::new(&mut num_iterations, 1..=40).text("num solver iters."));
integration_parameters.num_solver_iterations =
NonZeroUsize::new(num_iterations).unwrap();
ui.add(
Slider::new(
&mut integration_parameters.max_velocity_friction_iterations,
1..=200,
&mut integration_parameters.num_friction_iteration_per_solver_iteration,
1..=40,
)
.text("vel. frict. iters."),
);
ui.add(
Slider::new(
&mut integration_parameters.max_stabilization_iterations,
1..=200,
)
.text("vel. stab. iters."),
.text("frict. iters. per solver iters."),
);
}