diff options
Diffstat (limited to 'examples3d')
| -rw-r--r-- | examples3d/all_examples3.rs | 2 | ||||
| -rw-r--r-- | examples3d/inverse_kinematics3.rs | 103 |
2 files changed, 105 insertions, 0 deletions
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 4c4d962..3e6398f 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -42,6 +42,7 @@ mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; mod debug_long_chain3; mod debug_multibody_ang_motor_pos3; +mod inverse_kinematics3; mod joint_motor_position3; mod keva3; mod locked_rotations3; @@ -108,6 +109,7 @@ pub fn main() { ("Dynamic trimeshes", dynamic_trimesh3::init_world), ("Heightfield", heightfield3::init_world), ("Impulse Joints", joints3::init_world_with_joints), + ("Inverse kinematics", inverse_kinematics3::init_world), ("Joint Motor Position", joint_motor_position3::init_world), ("Locked rotations", locked_rotations3::init_world), ("One-way platforms", one_way_platforms3::init_world), diff --git a/examples3d/inverse_kinematics3.rs b/examples3d/inverse_kinematics3.rs new file mode 100644 index 0000000..7905ae0 --- /dev/null +++ b/examples3d/inverse_kinematics3.rs @@ -0,0 +1,103 @@ +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 mut multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 0.2; + let ground_height = 0.01; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, floor_handle, &mut bodies); + + /* + * Setup groups + */ + let num_segments = 10; + let body = RigidBodyBuilder::fixed(); + let mut last_body = bodies.insert(body); + let mut last_link = MultibodyJointHandle::invalid(); + + for i in 0..num_segments { + let size = 1.0 / num_segments as f32; + let body = RigidBodyBuilder::dynamic().can_sleep(false); + let new_body = bodies.insert(body); + // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make + // the demo look nicer. IK could be used without cuboid. + let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0, size / 8.0) + .density(0.0) + .sensor(true); + colliders.insert_with_parent(collider, new_body, &mut bodies); + + let link_ab = SphericalJointBuilder::new() + .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32, 0.0]) + .local_anchor2(point![0.0, -size / 2.0, 0.0]) + .build() + .data; + + last_link = multibody_joints + .insert(last_body, new_body, link_ab, true) + .unwrap(); + + last_body = new_body; + } + + let mut displacements = DVector::zeros(0); + + testbed.add_callback(move |graphics, physics, _, _| { + let Some(graphics) = graphics else { return }; + if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) { + // Ensure our displacement vector has the right number of elements. + if displacements.nrows() < multibody.ndofs() { + displacements = DVector::zeros(multibody.ndofs()); + } else { + displacements.fill(0.0); + } + + let Some(mouse_ray) = graphics.mouse().ray else { + return; + }; + + // Cast a ray on a plane aligned with the camera passing through the origin. + let halfspace = HalfSpace { + normal: -UnitVector::new_normalize(graphics.camera_fwd_dir()), + }; + let mouse_ray = Ray::new(mouse_ray.0, mouse_ray.1); + let Some(hit) = halfspace.cast_local_ray(&mouse_ray, f32::MAX, false) else { + return; + }; + let target_point = mouse_ray.point_at(hit); + + let options = InverseKinematicsOption { + constrained_axes: JointAxesMask::LIN_AXES, + ..Default::default() + }; + + multibody.inverse_kinematics( + &physics.bodies, + link_id, + &options, + &Isometry::from(target_point), + &mut displacements, + ); + multibody.apply_displacements(displacements.as_slice()); + } + }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 0.5, 2.5], point![0.0, 0.5, 0.0]); +} |