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

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());
}