From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Jan 2022 14:47:40 +0100 Subject: Implement multibody joints and the new solver --- examples3d/Cargo.toml | 6 +- examples3d/all_examples3.rs | 11 +- examples3d/articulations3.rs | 678 ++++++++++++++++++++++++++++++ examples3d/ccd3.rs | 5 +- examples3d/collision_groups3.rs | 5 +- examples3d/compound3.rs | 5 +- examples3d/convex_decomposition3.rs | 6 +- examples3d/convex_polyhedron3.rs | 5 +- examples3d/damping3.rs | 12 +- examples3d/debug_add_remove_collider3.rs | 6 +- examples3d/debug_articulations3.rs | 98 +++++ examples3d/debug_big_colliders3.rs | 5 +- examples3d/debug_boxes3.rs | 9 +- examples3d/debug_cylinder3.rs | 5 +- examples3d/debug_dynamic_collider_add3.rs | 6 +- examples3d/debug_friction3.rs | 5 +- examples3d/debug_infinite_fall3.rs | 5 +- examples3d/debug_prismatic3.rs | 24 +- examples3d/debug_rollback3.rs | 6 +- examples3d/debug_shape_modification3.rs | 6 +- examples3d/debug_triangle3.rs | 5 +- examples3d/debug_trimesh3.rs | 5 +- examples3d/domino3.rs | 5 +- examples3d/fountain3.rs | 11 +- examples3d/harness_capsules3.rs | 5 +- examples3d/heightfield3.rs | 5 +- examples3d/joints3.rs | 248 +++++------ examples3d/keva3.rs | 5 +- examples3d/locked_rotations3.rs | 5 +- examples3d/one_way_platforms3.rs | 6 +- examples3d/platform3.rs | 5 +- examples3d/primitives3.rs | 19 +- examples3d/rapier.data | Bin 0 -> 2017666 bytes examples3d/restitution3.rs | 5 +- examples3d/sensor3.rs | 5 +- examples3d/trimesh3.rs | 5 +- 36 files changed, 1015 insertions(+), 232 deletions(-) create mode 100644 examples3d/articulations3.rs create mode 100644 examples3d/debug_articulations3.rs create mode 100644 examples3d/rapier.data (limited to 'examples3d') diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 897c92b..bbab27e 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -2,7 +2,7 @@ name = "rapier-examples-3d" version = "0.1.0" authors = [ "Sébastien Crozet " ] -edition = "2018" +edition = "2021" default-run = "all_examples3" [features] @@ -20,10 +20,10 @@ wasm-bindgen = "0.2" obj-rs = { version = "0.6", default-features = false } [dependencies.rapier_testbed3d] -path = "../build/rapier_testbed3d" +path = "../crates/rapier_testbed3d" [dependencies.rapier3d] -path = "../build/rapier3d" +path = "../crates/rapier3d" [[bin]] name = "all_examples3" diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index a9e1456..0aad4c2 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -8,6 +8,7 @@ use inflector::Inflector; use rapier_testbed3d::{Testbed, TestbedApp}; use std::cmp::Ordering; +mod articulations3; mod ccd3; mod collision_groups3; mod compound3; @@ -15,6 +16,7 @@ mod convex_decomposition3; mod convex_polyhedron3; mod damping3; mod debug_add_remove_collider3; +mod debug_articulations3; mod debug_big_colliders3; mod debug_boxes3; mod debug_cylinder3; @@ -29,7 +31,7 @@ mod debug_trimesh3; mod domino3; mod fountain3; mod heightfield3; -mod joints3; +// mod joints3; mod keva3; mod locked_rotations3; mod one_way_platforms3; @@ -78,6 +80,10 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Fountain", fountain3::init_world), ("Primitives", primitives3::init_world), + ( + "Multibody joints", + articulations3::init_world_with_articulations, + ), ("CCD", ccd3::init_world), ("Collision groups", collision_groups3::init_world), ("Compound", compound3::init_world), @@ -86,7 +92,7 @@ pub fn main() { ("Damping", damping3::init_world), ("Domino", domino3::init_world), ("Heightfield", heightfield3::init_world), - ("Joints", joints3::init_world), + ("Impulse Joints", articulations3::init_world_with_joints), ("Locked rotations", locked_rotations3::init_world), ("One-way platforms", one_way_platforms3::init_world), ("Platform", platform3::init_world), @@ -94,6 +100,7 @@ pub fn main() { ("Sensor", sensor3::init_world), ("TriMesh", trimesh3::init_world), ("Keva tower", keva3::init_world), + ("(Debug) multibody_joints", debug_articulations3::init_world), ( "(Debug) add/rm collider", debug_add_remove_collider3::init_world, diff --git a/examples3d/articulations3.rs b/examples3d/articulations3.rs new file mode 100644 index 0000000..0c87c41 --- /dev/null +++ b/examples3d/articulations3.rs @@ -0,0 +1,678 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +fn create_prismatic_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(vector![origin.x, origin.y, origin.z]) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, curr_parent, bodies); + + for i in 0..num { + let z = origin.z + (i + 1) as f32 * shift; + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(vector![origin.x, origin.y, z]) + .build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, curr_child, bodies); + + let axis = if i % 2 == 0 { + UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0]) + } else { + UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) + }; + + let prism = PrismaticJoint::new(axis) + .local_anchor1(point![0.0, 0.0, 0.0]) + .local_anchor2(point![0.0, 0.0, -shift]) + .limit_axis([-2.0, 2.0]); + + if use_articulations { + multibody_joints.insert(curr_parent, curr_child, prism); + } else { + impulse_joints.insert(curr_parent, curr_child, prism); + } + curr_parent = curr_child; + } +} + +fn create_actuated_prismatic_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(vector![origin.x, origin.y, origin.z]) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, curr_parent, bodies); + + for i in 0..num { + let z = origin.z + (i + 1) as f32 * shift; + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(vector![origin.x, origin.y, z]) + .build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, curr_child, bodies); + + let axis = if i % 2 == 0 { + UnitVector::new_normalize(vector![1.0, 1.0, 0.0]) + } else { + UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) + }; + + let mut prism = PrismaticJoint::new(axis) + .local_anchor1(point![0.0, 0.0, 0.0]) + .local_anchor2(point![0.0, 0.0, -shift]); + + if i == 1 { + prism = prism + .limit_axis([-Real::MAX, 5.0]) + .motor_velocity(1.0, 1.0) + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + .motor_max_impulse(1.0); + } else if i > 1 { + prism = prism.motor_position(2.0, 0.01, 1.0); + } else { + prism = prism + .motor_velocity(1.0, 1.0) + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + .motor_max_impulse(0.7) + .limit_axis([-2.0, 5.0]); + } + + if use_articulations { + multibody_joints.insert(curr_parent, curr_child, prism); + } else { + impulse_joints.insert(curr_parent, curr_child, prism); + } + + curr_parent = curr_child; + } +} + +fn create_revolute_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(vector![origin.x, origin.y, 0.0]) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, curr_parent, bodies); + + for i in 0..num { + // Create four bodies. + let z = origin.z + i as f32 * shift * 2.0 + shift; + let positions = [ + Isometry::translation(origin.x, origin.y, z), + Isometry::translation(origin.x + shift, origin.y, z), + Isometry::translation(origin.x + shift, origin.y, z + shift), + Isometry::translation(origin.x, origin.y, z + shift), + ]; + + let mut handles = [curr_parent; 4]; + for k in 0..4 { + let rigid_body = RigidBodyBuilder::new_dynamic() + .position(positions[k]) + .build(); + handles[k] = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert_with_parent(collider, handles[k], bodies); + } + + // Setup four impulse_joints. + let x = Vector::x_axis(); + let z = Vector::z_axis(); + let revs = [ + RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]), + RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]), + ]; + + if use_articulations { + multibody_joints.insert(curr_parent, handles[0], revs[0]); + multibody_joints.insert(handles[0], handles[1], revs[1]); + multibody_joints.insert(handles[1], handles[2], revs[2]); + multibody_joints.insert(handles[2], handles[3], revs[3]); + } else { + impulse_joints.insert(curr_parent, handles[0], revs[0]); + impulse_joints.insert(handles[0], handles[1], revs[1]); + impulse_joints.insert(handles[1], handles[2], revs[2]); + impulse_joints.insert(handles[2], handles[3], revs[3]); + } + + curr_parent = handles[3]; + } +} + +fn create_revolute_joints_with_limits( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + use_articulations: bool, +) { + let ground = bodies.insert( + RigidBodyBuilder::new_static() + .translation(origin.coords) + .build(), + ); + + let platform1 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), + platform1, + bodies, + ); + + let shift = vector![0.0, 0.0, 6.0]; + let platform2 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), + platform2, + bodies, + ); + + let z = Vector::z_axis(); + let joint1 = RevoluteJoint::new(z).limit_axis([-0.2, 0.2]); + + if use_articulations { + multibody_joints.insert(ground, platform1, joint1); + } else { + impulse_joints.insert(ground, platform1, joint1); + } + + let joint2 = RevoluteJoint::new(z) + .local_anchor2(-Point::from(shift)) + .limit_axis([-0.2, 0.2]); + + if use_articulations { + multibody_joints.insert(platform1, platform2, joint2); + } else { + impulse_joints.insert(platform1, platform2, joint2); + } + + // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. + let cuboid_body1 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + vector![-2.0, 4.0, 0.0]) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + cuboid_body1, + bodies, + ); + + let cuboid_body2 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + cuboid_body2, + bodies, + ); +} + +fn create_fixed_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for i in 0..num { + for k in 0..num { + let fk = k as f32; + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { + RigidBodyType::Static + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![ + origin.x + fk * shift, + origin.y, + origin.z + fi * shift + ]) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).build(); + colliders.insert_with_parent(collider, child_handle, bodies); + + // Vertical joint. + if i > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]); + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint); + } else { + impulse_joints.insert(parent_handle, child_handle, joint); + } + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - 1; + let parent_handle = body_handles[parent_index]; + let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + impulse_joints.insert(parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } +} + +fn create_spherical_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + let status = if i == 0 && (k % 4 == 0 || k == num - 1) { + RigidBodyType::Static + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + colliders.insert_with_parent(collider, child_handle, bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint); + } else { + impulse_joints.insert(parent_handle, child_handle, joint); + } + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + impulse_joints.insert(parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } +} + +fn create_spherical_joints_with_limits( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + use_articulations: bool, +) { + let shift = vector![0.0, 0.0, 3.0]; + + let ground = bodies.insert( + RigidBodyBuilder::new_static() + .translation(origin.coords) + .build(), + ); + + let ball1 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift) + .linvel(vector![20.0, 20.0, 0.0]) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), + ball1, + bodies, + ); + + let ball2 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift * 2.0) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), + ball2, + bodies, + ); + + let joint1 = SphericalJoint::new() + .local_anchor2(Point::from(-shift)) + .limit_axis(JointAxis::X, [-0.2, 0.2]) + .limit_axis(JointAxis::Y, [-0.2, 0.2]); + + let joint2 = SphericalJoint::new() + .local_anchor2(Point::from(-shift)) + .limit_axis(JointAxis::X, [-0.3, 0.3]) + .limit_axis(JointAxis::Y, [-0.3, 0.3]); + + if use_articulations { + multibody_joints.insert(ground, ball1, joint1); + multibody_joints.insert(ball1, ball2, joint2); + } else { + impulse_joints.insert(ground, ball1, joint1); + impulse_joints.insert(ball1, ball2, joint2); + } +} + +fn create_actuated_revolute_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the impulse_joints here. + let z = Vector::z_axis(); + let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + RigidBodyType::Static + } else { + RigidBodyType::Dynamic + }; + + let shifty = (i >= 1) as u32 as f32 * -2.0; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + colliders.insert_with_parent(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i % 3 == 1 { + joint = joint.motor_velocity(-20.0, 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint = joint.motor_position(3.14 / 2.0, stiffness, damping); + } + + if i == 1 { + joint = joint + .local_anchor2(point![0.0, 2.0, -shift]) + .motor_velocity(-2.0, 0.1); + } + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint); + } else { + impulse_joints.insert(parent_handle, child_handle, joint); + } + } + + parent_handle = child_handle; + } +} + +fn create_actuated_spherical_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + origin: Point, + num: usize, + use_articulations: bool, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the impulse_joints here. + let joint_template = SphericalJoint::new().local_anchor1(point![0.0, 0.0, shift]); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + RigidBodyType::Static + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![origin.x, origin.y, origin.z + fi * shift]) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + colliders.insert_with_parent(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i == 1 { + joint = joint + .motor_velocity(JointAxis::AngX, 0.0, 0.1) + .motor_velocity(JointAxis::AngY, 0.5, 0.1) + .motor_velocity(JointAxis::AngZ, -2.0, 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint = joint + .motor_position(JointAxis::AngX, 0.0, stiffness, damping) + .motor_position(JointAxis::AngY, 1.0, stiffness, damping) + .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping); + } + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint); + } else { + impulse_joints.insert(parent_handle, child_handle, joint); + } + } + + parent_handle = child_handle; + } +} + +fn do_init_world(testbed: &mut Testbed, use_articulations: bool) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + create_prismatic_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![20.0, 5.0, 0.0], + 4, + use_articulations, + ); + create_actuated_prismatic_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![25.0, 5.0, 0.0], + 4, + use_articulations, + ); + create_revolute_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![20.0, 0.0, 0.0], + 3, + use_articulations, + ); + create_revolute_joints_with_limits( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![34.0, 0.0, 0.0], + use_articulations, + ); + create_fixed_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![0.0, 10.0, 0.0], + 10, + use_articulations, + ); + create_actuated_revolute_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![20.0, 10.0, 0.0], + 6, + use_articulations, + ); + create_actuated_spherical_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![13.0, 10.0, 0.0], + 3, + use_articulations, + ); + create_spherical_joints( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + 15, + use_articulations, + ); + create_spherical_joints_with_limits( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + point![-5.0, 0.0, 0.0], + use_articulations, + ); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]); +} + +pub fn init_world_with_joints(testbed: &mut Testbed) { + do_init_world(testbed, false) +} + +pub fn init_world_with_articulations(testbed: &mut Testbed) { + do_init_world(testbed, true) +} diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs index 8d676e3..2e00b86 100644 --- a/examples3d/ccd3.rs +++ b/examples3d/ccd3.rs @@ -44,7 +44,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -153,6 +154,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/collision_groups3.rs b/examples3d/collision_groups3.rs index 8db86fa..d43a7fc 100644 --- a/examples3d/collision_groups3.rs +++ b/examples3d/collision_groups3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -93,6 +94,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin()); } diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs index f5f1de1..433210d 100644 --- a/examples3d/compound3.rs +++ b/examples3d/compound3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -90,6 +91,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/convex_decomposition3.rs b/examples3d/convex_decomposition3.rs index 64c8802..98cf4a9 100644 --- a/examples3d/convex_decomposition3.rs +++ b/examples3d/convex_decomposition3.rs @@ -15,7 +15,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -52,7 +53,6 @@ pub fn init_world(testbed: &mut Testbed) { .iter() .map(|v| point![v.0, v.1, v.2]) .collect(); - use std::iter::FromIterator; let indices: Vec<_> = model .polygons .into_iter() @@ -104,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/convex_polyhedron3.rs b/examples3d/convex_polyhedron3.rs index 896eb03..7abcefb 100644 --- a/examples3d/convex_polyhedron3.rs +++ b/examples3d/convex_polyhedron3.rs @@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -68,6 +69,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![30.0, 30.0, 30.0], Point::origin()); } diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs index c634a0a..6f2edb9 100644 --- a/examples3d/damping3.rs +++ b/examples3d/damping3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Create the cubes @@ -38,6 +39,13 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world_with_params(bodies, colliders, joints, Vector::zeros(), ()); + testbed.set_world_with_params( + bodies, + colliders, + impulse_joints, + multibody_joints, + Vector::zeros(), + (), + ); testbed.look_at(point![2.0, 2.5, 20.0], point![2.0, 2.5, 0.0]); } diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs index 12d58ec..dd680d5 100644 --- a/examples3d/debug_add_remove_collider3.rs +++ b/examples3d/debug_add_remove_collider3.rs @@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + /* * Ground. */ @@ -54,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_articulations3.rs b/examples3d/debug_articulations3.rs new file mode 100644 index 0000000..26831ef --- /dev/null +++ b/examples3d/debug_articulations3.rs @@ -0,0 +1,98 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +fn create_ball_articulations( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + num: usize, +) { + let rad = 0.4; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + let status = if i == 0 { + // && (k % 4 == 0 || k == num - 1) { + RigidBodyType::Static + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + colliders.insert_with_parent(collider, child_handle, bodies); + + // Vertical multibody_joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + multibody_joints.insert(parent_handle, child_handle, joint); + } + + // Horizontal multibody_joint. + if k > 0 && i > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + // let joint = + // PrismaticJoint::new(Vector::y_axis()).local_anchor2(point![-shift, 0.0, 0.0]); + // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + // let joint = + // RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]); + impulse_joints.insert(parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } +} + +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 rigid_body = RigidBodyBuilder::new_dynamic().build(); + let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis()) + .translation(vector![0.0, -3.0, 0.0]) + .rotation(vector![0.1, 0.0, 0.1]) + .build(); + let handle = bodies.insert(rigid_body); + colliders.insert_with_parent(collider, handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::new_static().build(); + let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis()) + .translation(vector![0.0, -3.02, 0.0]) + .rotation(vector![0.1, 0.0, 0.1]) + .build(); + let handle = bodies.insert(rigid_body); + colliders.insert_with_parent(collider, handle, &mut bodies); + + create_ball_articulations( + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + 15, + ); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]); +} diff --git a/examples3d/debug_big_colliders3.rs b/examples3d/debug_big_colliders3.rs index 864782f..33460dc 100644 --- a/examples3d/debug_big_colliders3.rs +++ b/examples3d/debug_big_colliders3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -38,6 +39,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs index ea39a8a..ddf86db 100644 --- a/examples3d/debug_boxes3.rs +++ b/examples3d/debug_boxes3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -25,10 +26,10 @@ pub fn init_world(testbed: &mut Testbed) { } // Build the dynamic box rigid body. - for _ in 0..6 { + for _ in 0..2 { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![1.1, 0.0, 0.0]) - .rotation(vector![0.8, 0.2, 0.1]) + // .rotation(vector![0.8, 0.2, 0.1]) .can_sleep(false) .build(); let handle = bodies.insert(rigid_body); @@ -39,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs index 88908c1..cb087be 100644 --- a/examples3d/debug_cylinder3.rs +++ b/examples3d/debug_cylinder3.rs @@ -10,7 +10,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -55,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index f66d8ce..27e1b0b 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + /* * Ground. */ @@ -114,6 +116,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_friction3.rs b/examples3d/debug_friction3.rs index 7d01986..44c9fae 100644 --- a/examples3d/debug_friction3.rs +++ b/examples3d/debug_friction3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -38,6 +39,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/debug_infinite_fall3.rs b/examples3d/debug_infinite_fall3.rs index dcf4f12..4d950cf 100644 --- a/examples3d/debug_infinite_fall3.rs +++ b/examples3d/debug_infinite_fall3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -44,5 +45,5 @@ pub fn init_world(testbed: &mut Testbed) { * Set up the testbed. */ testbed.look_at(point![100.0, -10.0, 100.0], Point::origin()); - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); } diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs index 43c6cc0..15176f3 100644 --- a/examples3d/debug_prismatic3.rs +++ b/examples3d/debug_prismatic3.rs @@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed; fn prismatic_repro( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, box_center: Point, ) { let box_rb = bodies.insert( @@ -39,19 +39,12 @@ fn prismatic_repro( ); colliders.insert_with_parent(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies); - let mut prismatic = rapier3d::dynamics::PrismaticJoint::new( - point![pos.x, pos.y, pos.z], - Vector::y_axis(), - Vector::zeros(), - Point::origin(), - Vector::y_axis(), - Vector::default(), - ); - prismatic.configure_motor_model(rapier3d::dynamics::SpringModel::VelocityBased); let (stiffness, damping) = (0.05, 0.2); - prismatic.configure_motor_position(0.0, stiffness, damping); - joints.insert(box_rb, wheel_rb, prismatic); + let prismatic = PrismaticJoint::new(Vector::y_axis()) + .local_anchor1(point![pos.x, pos.y, pos.z]) + .motor_position(0.0, stiffness, damping); + impulse_joints.insert(box_rb, wheel_rb, prismatic); } // put a small box under one of the wheels @@ -73,7 +66,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let mut joints = JointSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -91,13 +85,13 @@ pub fn init_world(testbed: &mut Testbed) { prismatic_repro( &mut bodies, &mut colliders, - &mut joints, + &mut impulse_joints, point![0.0, 5.0, 0.0], ); /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_rollback3.rs b/examples3d/debug_rollback3.rs index c5e5bde..b795434 100644 --- a/examples3d/debug_rollback3.rs +++ b/examples3d/debug_rollback3.rs @@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + /* * Ground. */ @@ -65,6 +67,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_shape_modification3.rs b/examples3d/debug_shape_modification3.rs index 6c27288..eb2966e 100644 --- a/examples3d/debug_shape_modification3.rs +++ b/examples3d/debug_shape_modification3.rs @@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + /* * Ground. */ @@ -113,6 +115,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_triangle3.rs b/examples3d/debug_triangle3.rs index 0c40882..8151cf9 100644 --- a/examples3d/debug_triangle3.rs +++ b/examples3d/debug_triangle3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); // Triangle ground. let vtx = [ @@ -36,6 +37,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/debug_trimesh3.rs b/examples3d/debug_trimesh3.rs index d21d0d3..8e38719 100644 --- a/examples3d/debug_trimesh3.rs +++ b/examples3d/debug_trimesh3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); // Triangle ground. let width = 0.5; @@ -57,6 +58,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); } diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs index 7e9143f..067a86d 100644 --- a/examples3d/domino3.rs +++ b/examples3d/domino3.rs @@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 788d05d..4e47878 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -6,7 +6,9 @@ const MAX_NUMBER_OF_BODIES: usize = 400; pub fn init_world(testbed: &mut Testbed) { let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + let rad = 0.5; /* @@ -63,7 +65,8 @@ pub fn init_world(testbed: &mut Testbed) { *handle, &mut physics.islands, &mut physics.colliders, - &mut physics.joints, + &mut physics.impulse_joints, + &mut physics.multibody_joints, ); if let Some(graphics) = &mut graphics { @@ -76,10 +79,10 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); // testbed // .physics_state_mut() // .integration_parameters - // .velocity_based_erp = 0.2; + // .erp = 0.2; testbed.look_at(point![-30.0, 4.0, -30.0], point![0.0, 1.0, 0.0]); } diff --git a/examples3d/harness_capsules3.rs b/examples3d/harness_capsules3.rs index e2f19d5..dae364a 100644 --- a/examples3d/harness_capsules3.rs +++ b/examples3d/harness_capsules3.rs @@ -7,7 +7,8 @@ pub fn init_world(harness: &mut Harness) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -59,7 +60,7 @@ pub fn init_world(harness: &mut Harness) { /* * Set up the harness. */ - harness.set_world(bodies, colliders, joints); + harness.set_world(bodies, colliders, impulse_joints, multibody_joints); } fn main() { diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index bb93667..f5a3ba6 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let joints = JointSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); /* * Ground @@ -95,6 +96,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world(bodies, colliders, joints); + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); } diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 11cd533..02aa5f9 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed; fn create_prismatic_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, num: usize, ) { @@ -28,25 +28,17 @@ fn create_prismatic_joints( colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { - UnitVector::new_normalize(vector![1.0, 1.0, 0.0]) + UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0]) } else { - UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) + UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) }; - let z = Vector::z(); - let mut prism = PrismaticJoint::new( - point![0.0, 0.0, 0.0], - axis, - z, - point![0.0, 0.0, -shift], - axis, - z, - ); - prism.limits_enabled = true; - prism.limits[0] = -2.0; - prism.limits[1] = 2.0; - - joints.insert(curr_parent, curr_child, prism); + let mut prism = JointData::prismatic(axis) + .local_anchor1(point![0.0, 0.0, shift]) + .local_anchor2(point![0.0, 0.0, 0.0]) + .limit_axis(JointAxis::X, [-2.0, 2.0]); + + impulse_joints.insert(curr_parent, curr_child, prism); curr_parent = curr_child; } @@ -55,7 +47,7 @@ fn create_prismatic_joints( fn create_actuated_prismatic_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, num: usize, ) { @@ -84,36 +76,29 @@ fn create_actuated_prismatic_joints( UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) }; - let z = Vector::z(); - let mut prism = PrismaticJoint::new( - point![0.0, 0.0, 0.0], - axis, - z, - point![0.0, 0.0, -shift], - axis, - z, - ); + let mut prism = JointData::prismatic(axis) + .local_anchor1(point![0.0, 0.0, 0.0]) + .local_anchor2(point![0.0, 0.0, -shift]); if i == 1 { - prism.configure_motor_velocity(1.0, 1.0); - prism.limits_enabled = true; - prism.limits[1] = 5.0; - // We set a max impulse so that the motor doesn't fight - // the limits with large forces. - prism.motor_max_impulse = 1.0; + prism = prism + .limit_axis(JointAxis::X, [-Real::MAX, 5.0]) + .motor_velocity(JointAxis::X, 1.0, 1.0) + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + .motor_max_impulse(JointAxis::X, 1.0); } else if i > 1 { - prism.configure_motor_position(2.0, 0.01, 1.0); + prism = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0); } else { - prism.configure_motor_velocity(1.0, 1.0); - // We set a max impulse so that the motor doesn't fight - // the limits with large forces. - prism.motor_max_impulse = 0.7; - prism.limits_enabled = true; - prism.limits[0] = -2.0; - prism.limits[1] = 5.0; + prism = prism + .motor_velocity(JointAxis::X, 1.0, 1.0) + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + .motor_max_impulse(JointAxis::X, 0.7) + .limit_axis(JointAxis::X, [-2.0, 5.0]); } - joints.insert(curr_parent, curr_child, prism); + impulse_joints.insert(curr_parent, curr_child, prism); curr_parent = curr_child; } @@ -122,7 +107,7 @@ fn create_actuated_prismatic_joints( fn create_revolute_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, num: usize, ) { @@ -156,22 +141,20 @@ fn create_revolute_joints( colliders.insert_with_parent(collider, handles[k], bodies); } - // Setup four joints. - let o = Point::origin(); + // Setup four impulse_joints. let x = Vector::x_axis(); let z = Vector::z_axis(); - let revs = [ - RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z), - RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x), - RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z), - RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x), + RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJoint::new(z).local_anchor2(point![-shift, 0.0, 0.0]), + RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJoint::new(z).local_anchor2(point![shift, 0.0, 0.0]), ]; - joints.insert(curr_parent, handles[0], revs[0]); - joints.insert(handles[0], handles[1], revs[1]); - joints.insert(handles[1], handles[2], revs[2]); - joints.insert(handles[2], handles[3], revs[3]); + impulse_joints.insert(curr_parent, handles[0], revs[0]); + impulse_joints.insert(handles[0], handles[1], revs[1]); + impulse_joints.insert(handles[1], handles[2], revs[2]); + impulse_joints.insert(handles[2], handles[3], revs[3]); curr_parent = handles[3]; } @@ -180,7 +163,7 @@ fn create_revolute_joints( fn create_revolute_joints_with_limits( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, ) { let ground = bodies.insert( @@ -212,25 +195,14 @@ fn create_revolute_joints_with_limits( bodies, ); - let mut joint1 = RevoluteJoint::new( - Point::origin(), - Vector::z_axis(), - Point::origin(), - Vector::z_axis(), - ); - joint1.limits_enabled = true; - joint1.limits = [-0.2, 0.2]; - joints.insert(ground, platform1, joint1); - - let mut joint2 = RevoluteJoint::new( - Point::origin(), - Vector::z_axis(), - Point::from(-shift), - Vector::z_axis(), - ); - joint2.limits_enabled = true; - joint2.limits = [-0.3, 0.3]; - joints.insert(platform1, platform2, joint2); + let z = Vector::z_axis(); + let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]); + impulse_joints.insert(ground, platform1, joint1); + + let mut joint2 = RevoluteJoint::new(z) + .local_anchor2(shift.into()) + .limit_axis(JointAxis::Z, [-0.2, 0.2]); + impulse_joints.insert(platform1, platform2, joint2); // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. let cuboid_body1 = bodies.insert( @@ -259,7 +231,7 @@ fn create_revolute_joints_with_limits( fn create_fixed_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, num: usize, ) { @@ -268,8 +240,8 @@ fn create_fixed_joints( let mut body_handles = Vec::new(); - for k in 0..num { - for i in 0..num { + for i in 0..num { + for k in 0..num { let fk = k as f32; let fi = i as f32; @@ -295,23 +267,18 @@ fn create_fixed_joints( // Vertical joint. if i > 0 { - let parent_handle = *body_handles.last().unwrap(); - let joint = FixedJoint::new( - Isometry::identity(), - Isometry::translation(0.0, 0.0, -shift), - ); - joints.insert(parent_handle, child_handle, joint); + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = JointData::fixed().local_anchor2(point![0.0, 0.0, -shift]); + impulse_joints.insert(parent_handle, child_handle, joint); } // Horizontal joint. if k > 0 { - let parent_index = body_handles.len() - num; + let parent_index = body_handles.len() - 1; let parent_handle = body_handles[parent_index]; - let joint = FixedJoint::new( - Isometry::identity(), - Isometry::translation(-shift, 0.0, 0.0), - ); - joints.insert(parent_handle, child_handle, joint); + let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]); + impulse_joints.insert(parent_handle, child_handle, joint); } body_handles.push(child_handle); @@ -322,7 +289,7 @@ fn create_fixed_joints( fn create_ball_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, num: usize, ) { let rad = 0.4; @@ -351,16 +318,16 @@ fn create_ball_joints( // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift * 2.0]); - joints.insert(parent_handle, child_handle, joint); + let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + impulse_joints.insert(parent_handle, child_handle, joint); } // Horizontal joint. if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]); - joints.insert(parent_handle, child_handle, joint); + let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]); + impulse_joints.insert(parent_handle, child_handle, joint); } body_handles.push(child_handle); @@ -371,7 +338,7 @@ fn create_ball_joints( fn create_ball_joints_with_limits( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, ) { let shift = vector![0.0, 0.0, 3.0]; @@ -405,38 +372,32 @@ fn create_ball_joints_with_limits( bodies, ); - let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift)); - joint1.limits_enabled = true; - joint1.limits_local_axis1 = Vector::z_axis(); - joint1.limits_local_axis2 = Vector::z_axis(); - joint1.limits_angle = 0.2; - joints.insert(ground, ball1, joint1); - - let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift)); - joint2.limits_enabled = true; - joint2.limits_local_axis1 = Vector::z_axis(); - joint2.limits_local_axis2 = Vector::z_axis(); - joint2.limits_angle = 0.3; - joints.insert(ball1, ball2, joint2); + let mut joint1 = JointData::ball() + .local_anchor2(Point::from(-shift)) + .limit_axis(JointAxis::X, [-0.2, 0.2]) + .limit_axis(JointAxis::Y, [-0.2, 0.2]); + impulse_joints.insert(ground, ball1, joint1); + + let mut joint2 = JointData::ball() + .local_anchor2(Point::from(-shift)) + .limit_axis(JointAxis::X, [-0.3, 0.3]) + .limit_axis(JointAxis::Y, [-0.3, 0.3]); + impulse_joints.insert(ball1, ball2, joint2); } fn create_actuated_revolute_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, + impulse_joints: &mut ImpulseJointSet, origin: Point, num: usize, ) { let rad = 0.4; let shift = 2.0; - // We will reuse this base configuration for all the joints here. - let joint_template = RevoluteJoint::new( - Point::origin(), - Vector::z_axis(), - point![0.0, 0.0, -shift], - Vector::z_axis(), - ); + // We will reuse this base configuration for all the impulse_joints here. + let z = Vector::z_axis(); + let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]); let mut parent_handle = RigidBodyHandle::invalid(); @@ -467,19 +428,19 @@ fn create_actuated_revolute_joints( let mut joint = joint_template.clone(); if i % 3 == 1 { - joint.configure_motor_velocity(-20.0, 0.1); + joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1); } else if i == num - 1 { let stiffness = 0.2; let damping = 1.0; - joint.configure_motor_position(3.14 / 2.0, stiffness, damping); + joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); } if i == 1 { - joint.local_anchor2.y = 2.0; - joint.configure_motor_ve