From 62379de9ecc81fb42b7c2a0d2b8e3e1b02d63f38 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 25 May 2024 10:36:34 +0200 Subject: feat: add simple inverse-kinematics solver for multibodies (#632) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: add a simple jacobian-based inverse-kinematics implementation for multibodies * feat: add 2d inverse kinematics example * feat: make forward_kinematics auto-fix the root’s degrees of freedom * feat: add 3d inverse kinematics example * chore: update changelog * chore: clippy fixes * chore: more clippy fixes * fix tests --- CHANGELOG.md | 23 ++ crates/rapier_testbed2d-f64/Cargo.toml | 2 +- crates/rapier_testbed2d/Cargo.toml | 2 +- crates/rapier_testbed3d-f64/Cargo.toml | 2 +- crates/rapier_testbed3d/Cargo.toml | 2 +- examples2d/all_examples2.rs | 2 + examples2d/inverse_kinematics2.rs | 96 +++++++++ examples3d/all_examples3.rs | 2 + examples3d/inverse_kinematics3.rs | 103 +++++++++ src/dynamics/joint/multibody_joint/mod.rs | 2 + src/dynamics/joint/multibody_joint/multibody.rs | 217 ++++++++++++++++--- src/dynamics/joint/multibody_joint/multibody_ik.rs | 238 +++++++++++++++++++++ .../joint/multibody_joint/multibody_joint_set.rs | 11 +- .../joint/multibody_joint/multibody_link.rs | 2 +- src/dynamics/rigid_body.rs | 6 +- src/dynamics/solver/velocity_solver.rs | 5 +- src/geometry/contact_pair.rs | 6 - src/pipeline/physics_pipeline.rs | 6 +- src/utils.rs | 163 +------------- src_testbed/lib.rs | 1 + src_testbed/mouse.rs | 46 ++++ src_testbed/testbed.rs | 30 ++- 22 files changed, 755 insertions(+), 212 deletions(-) create mode 100644 examples2d/inverse_kinematics2.rs create mode 100644 examples3d/inverse_kinematics3.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody_ik.rs create mode 100644 src_testbed/mouse.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 74f0497..a7c1efe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,26 @@ +## Unreleased + +### Added + +- Add `Multibody::inverse_kinematics`, `Multibody::inverse_kinematics_delta`, + and `::inverse_kinematics_delta_with_jacobian` + for running inverse kinematics on a multibody to align one its links pose to the given prescribed pose. +- Add `InverseKinematicsOption` to customize some behaviors of the inverse-kinematics solver. +- Add `Multibody::body_jacobian` to get the jacobian of a specific link. +- Add `Multibody::update_rigid_bodies` to update rigid-bodies based on the multibody links poses. +- Add `Multibody::forward_kinematics_single_link` to run forward-kinematics to compute the new pose and jacobian of a + single link without mutating the multibody. This can take an optional displacement on generalized coordinates that are + taken into account during transform propagation. + +### Modified + +- The `Multibody::forward_kinematics` method will no longer automatically update the poses of the `RigidBody` associated + to each joint. Instead `Multibody::update_rigid_bodies` has to be called explicitly. +- The `Multibody::forward_kinematics` method will automatically adjust the multibody’s degrees of freedom if the root + rigid-body changed type (between dynamic and non-dynamic). It can also optionally apply the root’s rigid-body pose + instead of the root link’s pose (useful for example if you modified the root rigid-body pose externally and wanted + to propagate it to the multibody). + ## v0.19.0 (05 May 2024) ### Fix diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml index 2a090bf..800ac49 100644 --- a/crates/rapier_testbed2d-f64/Cargo.toml +++ b/crates/rapier_testbed2d-f64/Cargo.toml @@ -28,7 +28,7 @@ other-backends = ["wrapped2d"] features = ["parallel", "other-backends"] [dependencies] -nalgebra = { version = "0.32", features = ["rand"] } +nalgebra = { version = "0.32", features = ["rand", "glam025"] } rand = "0.8" rand_pcg = "0.3" instant = { version = "0.1", features = ["web-sys", "now"] } diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml index 6165e23..3935fcb 100644 --- a/crates/rapier_testbed2d/Cargo.toml +++ b/crates/rapier_testbed2d/Cargo.toml @@ -28,7 +28,7 @@ other-backends = ["wrapped2d"] features = ["parallel", "other-backends"] [dependencies] -nalgebra = { version = "0.32", features = ["rand"] } +nalgebra = { version = "0.32", features = ["rand", "glam025"] } rand = "0.8" rand_pcg = "0.3" instant = { version = "0.1", features = ["web-sys", "now"] } diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml index 99d6e85..8fd5628 100644 --- a/crates/rapier_testbed3d-f64/Cargo.toml +++ b/crates/rapier_testbed3d-f64/Cargo.toml @@ -27,7 +27,7 @@ parallel = ["rapier/parallel", "num_cpus"] features = ["parallel"] [dependencies] -nalgebra = { version = "0.32", features = ["rand"] } +nalgebra = { version = "0.32", features = ["rand", "glam025"] } rand = "0.8" rand_pcg = "0.3" instant = { version = "0.1", features = ["web-sys", "now"] } diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml index e8e7f3f..a3f3c5d 100644 --- a/crates/rapier_testbed3d/Cargo.toml +++ b/crates/rapier_testbed3d/Cargo.toml @@ -28,7 +28,7 @@ other-backends = ["physx", "physx-sys", "glam"] features = ["parallel", "other-backends"] [dependencies] -nalgebra = { version = "0.32", features = ["rand"] } +nalgebra = { version = "0.32", features = ["rand", "glam025"] } rand = "0.8" rand_pcg = "0.3" instant = { version = "0.1", features = ["web-sys", "now"] } diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 08fd996..39d85f6 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -20,6 +20,7 @@ mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; mod heightfield2; +mod inverse_kinematics2; mod joint_motor_position2; mod joints2; mod locked_rotations2; @@ -84,6 +85,7 @@ pub fn main() { ("Damping", damping2::init_world), ("Drum", drum2::init_world), ("Heightfield", heightfield2::init_world), + ("Inverse kinematics", inverse_kinematics2::init_world), ("Joints", joints2::init_world), ("Locked rotations", locked_rotations2::init_world), ("One-way platforms", one_way_platforms2::init_world), diff --git a/examples2d/inverse_kinematics2.rs b/examples2d/inverse_kinematics2.rs new file mode 100644 index 0000000..985bdb2 --- /dev/null +++ b/examples2d/inverse_kinematics2.rs @@ -0,0 +1,96 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::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 = 1.0; + let ground_height = 0.01; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); + 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) + .density(0.0) + .sensor(true); + colliders.insert_with_parent(collider, new_body, &mut bodies); + + let link_ab = RevoluteJointBuilder::new() + .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32]) + .local_anchor2(point![0.0, -size / 2.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_point) = graphics.mouse().point else { + return; + }; + + // We will have the endpoint track the mouse position. + let target_point = mouse_point.coords; + + 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.0], 300.0); +} 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]); +} diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index c789004..dc1fa75 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -1,6 +1,7 @@ //! MultibodyJoints using the reduced-coordinates formalism or using constraints. pub use self::multibody::Multibody; +pub use self::multibody_ik::InverseKinematicsOption; pub use self::multibody_joint::MultibodyJoint; pub use self::multibody_joint_set::{ MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId, @@ -13,5 +14,6 @@ mod multibody_joint_set; mod multibody_link; mod multibody_workspace; +mod multibody_ik; mod multibody_joint; mod unit_multibody_joint; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 617d447..fb56087 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -89,6 +89,7 @@ impl Default for Multibody { Multibody::new() } } + impl Multibody { /// Creates a new multibody with no link. pub fn new() -> Self { @@ -115,6 +116,8 @@ impl Multibody { pub(crate) fn with_root(handle: RigidBodyHandle) -> Self { let mut mb = Multibody::new(); + // NOTE: we have no way of knowing if the root in fixed at this point, so + // we mark it as dynamic and will fixe later with `Self::update_root_type`. mb.root_is_dynamic = true; let joint = MultibodyJoint::free(Isometry::identity()); mb.add_link(None, joint, handle); @@ -747,6 +750,12 @@ impl Multibody { self.velocities.rows(0, self.ndofs) } + /// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`]. + #[inline] + pub fn body_jacobian(&self, link_id: usize) -> &Jacobian { + &self.body_jacobians[link_id] + } + /// The mutable generalized velocities of this multibodies. #[inline] pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut { @@ -762,17 +771,27 @@ impl Multibody { } /// Apply displacements, in generalized coordinates, to this multibody. + /// + /// Note this does **not** updates the link poses, only their generalized coordinates. + /// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`] + /// or [`Self::finalize`]. pub fn apply_displacements(&mut self, disp: &[Real]) { for link in self.links.iter_mut() { link.joint.apply_displacement(&disp[link.assembly_id..]) } } - pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) { + pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) { if let Some(rb) = bodies.get(self.links[0].rigid_body) { if rb.is_dynamic() != self.root_is_dynamic { + let root_pose = if take_body_pose { + *rb.position() + } else { + self.links[0].local_to_world + }; + if rb.is_dynamic() { - let free_joint = MultibodyJoint::free(*rb.position()); + let free_joint = MultibodyJoint::free(root_pose); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = free_joint; self.links[0].assembly_id = 0; @@ -791,7 +810,7 @@ impl Multibody { assert!(self.damping.len() >= SPATIAL_DIM); assert!(self.accelerations.len() >= SPATIAL_DIM); - let fixed_joint = MultibodyJoint::fixed(*rb.position()); + let fixed_joint = MultibodyJoint::fixed(root_pose); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = fixed_joint; self.links[0].assembly_id = 0; @@ -820,30 +839,86 @@ impl Multibody { } // Make sure the positions are properly set to match the rigid-body’s. - if self.links[0].joint.data.locked_axes.is_empty() { - self.links[0].joint.set_free_pos(*rb.position()); + if take_body_pose { + if self.links[0].joint.data.locked_axes.is_empty() { + self.links[0].joint.set_free_pos(*rb.position()); + } else { + self.links[0].joint.data.local_frame1 = *rb.position(); + } + } + } + } + + /// Update the rigid-body poses based on this multibody joint poses. + /// + /// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses + /// to the rigid-bodies. + pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) { + self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true) + } + + pub(crate) fn update_rigid_bodies_internal( + &self, + bodies: &mut RigidBodySet, + update_mass_properties: bool, + update_next_positions_only: bool, + change_tracking: bool, + ) { + // Handle the children. They all have a parent within this multibody. + for link in self.links.iter() { + let rb = if change_tracking { + bodies.get_mut_internal_with_modification_tracking(link.rigid_body) } else { - self.links[0].joint.data.local_frame1 = *rb.position(); + bodies.get_mut_internal(link.rigid_body) + }; + + if let Some(rb) = rb { + rb.pos.next_position = link.local_to_world; + + if !update_next_positions_only { + rb.pos.position = link.local_to_world; + } + + if update_mass_properties { + rb.mprops.update_world_mass_properties(&link.local_to_world); + } } } } // 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_rb_mass_props: bool) { + /// Apply forward-kinematics to this multibody. + /// + /// This will update the [`MultibodyLink`] pose information as wall as the body jacobians. + /// This will also ensure that the multibody has the proper number of degrees of freedom if + /// its root node changed between dynamic and non-dynamic. + /// + /// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints. + /// Run [`Self::update_rigid_bodies`] to trigger that update. + /// + /// This method updates `self` with the result of the forward-kinematics operation. + /// For a non-mutable version running forward kinematics on a single link, see + /// [`Self::forward_kinematics_single_link`]. + /// + /// ## Parameters + /// - `bodies`: the set of rigid-bodies. + /// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint, + /// or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true` + /// when the root rigid-body pose has been modified and needs to affect the multibody. + pub fn forward_kinematics( + &mut self, + bodies: &RigidBodySet, + read_root_pose_from_rigid_body: bool, + ) { + // Be sure the degrees of freedom match and take the root position if needed. + self.update_root_type(bodies, read_root_pose_from_rigid_body); + // Special case for the root, which has no parent. { let link = &mut self.links[0]; link.local_to_parent = link.joint.body_to_parent(); link.local_to_world = link.local_to_parent; - - if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { - rb.pos.next_position = link.local_to_world; - if update_rb_mass_props { - rb.mprops.update_world_mass_properties(&link.local_to_world); - } - } } // Handle the children. They all have a parent within this multibody. @@ -865,20 +940,11 @@ impl Multibody { link.shift23 = c3 - c2; } - let link_rb = bodies.index_mut_internal(link.rigid_body); - link_rb.pos.next_position = link.local_to_world; - assert_eq!( - link_rb.body_type, + bodies[link.rigid_body].body_type, RigidBodyType::Dynamic, "A rigid-body that is not at the root of a multibody must be dynamic." ); - - if update_rb_mass_props { - link_rb - .mprops - .update_world_mass_properties(&link.local_to_world); - } } /* @@ -887,6 +953,107 @@ impl Multibody { self.update_body_jacobians(); } + /// Apply forward-kinematics to compute the position of a single link of this multibody. + /// + /// If `out_jacobian` is `Some`, this will simultaneously compute the new jacobian of this link. + /// If `displacement` is `Some`, the generalized position considered during transform propagation + /// is the sum of the current position of `self` and this `displacement`. + // TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except + // that we are only traversing a single kinematic chain. Could this be refactored? + pub fn forward_kinematics_single_link( + &self, + bodies: &RigidBodySet, + link_id: usize, + displacement: Option<&[Real]>, + mut out_jacobian: Option<&mut Jacobian>, + ) -> Isometry { + let mut branch = vec![]; // Perf: avoid allocation. + let mut curr_id = Some(link_id); + + while let Some(id) = curr_id { + branch.push(id); + curr_id = self.links[id].parent_id(); + } + + branch.reverse(); + + if let Some(out_jacobian) = out_jacobian.as_deref_mut() { + if out_jacobian.ncols() != self.ndofs { + *out_jacobian = Jacobian::zeros(self.ndofs); + } else { + out_jacobian.fill(0.0); + } + } + + let mut parent_link: Option = None; + + for i in branch { + let mut link = self.links[i]; + + if let Some(displacement) = displacement { + link.joint + .apply_displacement(&displacement[link.assembly_id..]); + } + + let parent_to_world; + + if let Some(parent_link) = parent_link { + link.local_to_parent = link.joint.body_to_parent(); + link.local_to_world = parent_link.local_to_world * link.local_to_parent; + + { + let parent_rb = &bodies[parent_link.rigid_body]; + let link_rb = &bodies[link.rigid_body]; + let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com; + let c2 = link.local_to_world + * Point::from(link.joint.data.local_frame2.translation.vector); + let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com; + + link.shift02 = c2 - c0; + link.shift23 = c3 - c2; + } + + parent_to_world = parent_link.local_to_world; + + if let Some(out_jacobian) = out_jacobian.as_deref_mut() { + let (mut link_j_v, parent_j_w) = + out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM); + let shift_tr = (link.shift02).gcross_matrix_tr(); + link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0); + } + } else { + link.local_to_parent = link.joint.body_to_parent(); + link.local_to_world = link.local_to_parent; + parent_to_world = Isometry::identity(); + } + + if let Some(out_jacobian) = out_jacobian.as_deref_mut() { + let ndofs = link.joint.ndofs(); + let mut tmp = SMatrix::::zeros(); + let mut link_joint_j = tmp.columns_mut(0, ndofs); + let mut link_j_part = out_jacobian.columns_mut(link.assembly_id, ndofs); + link.joint.jacobian( + &(parent_to_world.rotation * link.joint.data.local_frame1.rotation), + &mut link_joint_j, + ); + link_j_part += link_joint_j; + + { + let (mut link_j_v, link_j_w) = + out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM); + let shift_tr = link.shift23.gcross_matrix_tr(); + link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0); + } + } + + parent_link = Some(link); + } + + parent_link + .map(|link| link.local_to_world) + .unwrap_or_else(Isometry::identity) + } + /// The total number of freedoms of this multibody. #[inline] pub fn ndofs(&self) -> usize { diff --git a/src/dynamics/joint/multibody_joint/multibody_ik.rs b/src/dynamics/joint/multibody_joint/multibody_ik.rs new file mode 100644 index 0000000..7e0fc15 --- /dev/null +++ b/src/dynamics/joint/multibody_joint/multibody_ik.rs @@ -0,0 +1,238 @@ +use crate::dynamics::{JointAxesMask, Multibody, RigidBodySet}; +use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM}; +use na::{self, DVector, SMatrix}; +use parry::math::SpacialVector; + +#[derive(Copy, Clone, Debug, PartialEq)] +/// Options for the jacobian-based Inverse Kinematics solver for multibodies. +pub struct InverseKinematicsOption { + /// A damping coefficient. + /// + /// Small value can lead to overshooting preventing convergence. Large + /// values can slown down convergence, requiring more iterations to converge. + pub damping: Real, + /// The maximum number of iterations the iterative IK solver can take. + pub max_iters: usize, + /// The axes the IK solver will solve for. + pub constrained_axes: JointAxesMask, + /// The error threshold on the linear error. + /// + /// If errors on both linear and angular parts fall below this + /// threshold, the iterative resolution will stop. + pub epsilon_linear: Real, + /// The error threshold on the angular error. + /// + /// If errors on both linear and angular parts fall below this + /// threshold, the iterative resolution will stop. + pub epsilon_angular: Real, +} + +impl Default for InverseKinematicsOption { + fn default() -> Self { + Self { + damping: 1.0, + max_iters: 10, + constrained_axes: JointAxesMask::all(), + epsilon_linear: 1.0e-3, + epsilon_angular: 1.0e-3, + } + } +} + +impl Multibody { + /// Computes the displacement needed to have the link identified by `link_id` move by the + /// desired transform. + /// + /// The displacement calculated by this function is added to the `displacement` vector. + pub fn inverse_kinematics_delta( + &self, + link_id: usize, + desired_movement: &SpacialVector, + damping: Real, + displacements: &mut DVector, + ) { + let body_jacobian = self.body_jacobian(link_id); + Self::inverse_kinematics_delta_with_jacobian( + body_jacobian, + desired_movement, + damping, + displacements, + ); + } + + /// Computes the displacement needed to have a link with the given jacobian move by the + /// desired transform. + /// + /// The displacement calculated by this function is added to the `displacement` vector. + pub fn inverse_kinematics_delta_with_jacobian( + jacobian: &Jacobian, + desired_movement: &SpacialVector, + damping: Real, + displacements: &mut DVector, + ) { + let identity = SMatrix::::identity(); + let jj = jacobian * &jacobian.transpose() + identity * (damping * damping); + let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity); + displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0); + } + + /// Computes the displacement needed to have the link identified by `link_id` have a pose + /// equal (or as close as possible) to `target_pose`. + /// + /// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be + /// obtained from its current generalized coordinates summed with the `displacement` vector. + /// + /// The `displacements` vector is overwritten with the new displacement. + pub fn inverse_kinematics( + &self, + bodies: &RigidBodySet, + link_id: usize, + options: &InverseKinematicsOption, + target_pose: &Isometry, + displacements: &mut DVector, + ) { + let mut jacobian = Jacobian::zeros(0); + + for _ in 0..options.max_iters { + let pose = self.forward_kinematics_single_link( + bodies, + link_id, + Some(displacements.as_slice()), + Some(&mut jacobian), + ); + + let delta_lin = target_pose.translation.vector - pose.translation.vector; + let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis(); + + #[cfg(feature = "dim2")] + let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x]; + #[cfg(feature = "dim3")] + let mut delta = na::vector![ + delta_lin.x, + delta_lin.y, + delta_lin.z, + delta_ang.x, + delta_ang.y, + delta_ang.z + ]; + + if !options.constrained_axes.contains(JointAxesMask::X) { + delta[0] = 0.0; + } + if !options.constrained_axes.contains(JointAxesMask::Y) { + delta[1] = 0.0; + } + #[cfg(feature = "dim3")] + if !options.constrained_axes.contains(JointAxesMask::Z) { + delta[2] = 0.0; + } + if !options.constrained_axes.contains(JointAxesMask::ANG_X) { + delta[DIM] = 0.0; + } + #[cfg(feature = "dim3")] + if !options.constrained_axes.contains(JointAxesMask::ANG_Y) { + delta[DIM + 1] = 0.0; + } + #[cfg(feature = "dim3")] + if !options.constrained_axes.contains(JointAxesMask::ANG_Z) { + delta[DIM + 2] = 0.0; + } + + // TODO: measure convergence on the error variation instead? + if delta.rows(0, DIM).norm() <= options.epsilon_linear + && delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular + { + break; + } + + Self::inverse_kinematics_delta_with_jacobian( + &jacobian, + &delta, + options.damping, + displacements, + ); + } + } +} + +#[cfg(test)] +mod test { + use crate::dynamics::{ + MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder, + RigidBodySet, + }; + use crate::math::{Jacobian, Real, Vector}; + use approx::assert_relative_eq; + + #[test] + fn one_link_fwd_kinematics() { + let mut bodies = RigidBodySet::new(); + let mut multibodies = MultibodyJointSet::new(); + + let num_segments = 10; + let body = RigidBodyBuilder::fixed(); + let mut last_body = bodies.insert(body); + let mut last_link = MultibodyJointHandle::invalid(); + + for _ in 0..num_segments { + let body = RigidBodyBuilder::dynamic().can_sleep(false); + let new_body = bodies.insert(body); + + #[cfg(feature = "dim2")] + let builder = RevoluteJointBuilder::new(); + #[cfg(feature = "dim3")] + let builder = RevoluteJointBuilder::new(Vector::z_axis()); + let link_ab = builder + .local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into()) + .local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into()); + last_link = multibodies + .insert(last_body, new_body, link_ab, true) + .unwrap(); + + last_body = new_body; + } + + let (multibody, last_id) = multibodies.get_mut(last_link).unwrap(); + multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date. + assert_eq!(multibody.ndofs(), num_segments); + + /* + * No displacement. + */ + let mut jacobian2 = Jacobian::zeros(0); + let link_pose1 = *multibody.link(last_id).unwrap().local_to_world(); + let jacobian1 = multibody.body_jacobian(last_id); + let link_pose2 = + multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2)); + assert_eq!(link_pose1, link_pose2); + assert_eq!(jacobian1, &jacobian2); + + /* + * Arbitrary displacement. + */ + let niter = 100; + let displacement_part: Vec<_> = (0..multibody.ndofs()) + .map(|i| i as Real * -0.1 / niter as Real) + .collect(); + let displacement_total: Vec<_> = displacement_part + .iter() + .map(|d| *d * niter as Real) + .collect(); + let link_pose2 = multibody.forward_kinematics_single_link( + &bodies, + last_id, + Some(&displacement_total), + Some(&mut jacobian2), + ); + + for _ in 0..niter { + multibody.apply_displacements(&displacement_part); + multibody.forward_kinematics(&bodies, false); + } + + let link_pose1 = *multibody.link(last_id).unwrap().local_to_world(); + let jacobian1 = multibody.body_jacobian(last_id); + assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5); + assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5); + } +} diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index fa0ffdb..a95c10b 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -286,6 +286,13 @@ impl MultibodyJointSet { self.multibodies.get(index.0) } + /// Gets a mutable reference to a multibody, based on its temporary index. + /// `MultibodyJointSet`. + pub fn get_multibody_mut(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> { + // TODO: modification tracking. + self.multibodies.get_mut(index.0) + } + /// Gets a mutable reference to a multibody, based on its temporary index. /// /// This method will bypass any modification-detection automatically done by the @@ -363,13 +370,13 @@ impl MultibodyJointSet { let parent1 = link1.parent_id(); if parent1 == Some(id2.id) { - Some((MultibodyJointHandle(rb1.0), mb, &link1)) + Some((MultibodyJointHandle(rb1.0), mb, link1)) } else { let link2 = mb.link(id2.id)?; let parent2 = link2.parent_id(); if parent2 == Some(id1.id) { - Some((MultibodyJointHandle(rb2.0), mb, &link2)) + Some((MultibodyJointHandle(rb2.0), mb, link2)) } else { None } diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs index 7336a6c..bf6d605 100644 --- a/src/dynamics/joint/multibody_joint/multibody_link.rs +++ b/src/dynamics/joint/multibody_joint/multibody_link.rs @@ -6,7 +6,7 @@ use crate::prelude::RigidBodyVelocity; /// One link of a multibody. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] +#[derive(Copy, Clone)] pub struct MultibodyLink { // FIXME: make all those private. pub(crate) internal_id: usize, diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index b822aa9..25631fc 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -292,9 +292,9 @@ impl RigidBody { allow_rotations_z: bool, wake_up: bool, ) { - if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x - || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y - || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z + if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x + || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y + || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z { if self.is_dynamic() && wake_up { self.wake_up(true); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index b0bafb8..9d5c279 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -251,8 +251,9 @@ impl VelocitySolver { .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_substep); + // PERF: don’t write back to the rigid-body poses `bodies` before the last step? + multibody.forward_kinematics(bodies, false); + multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false); if !is_last_substep { // These are very expensive and not needed if we don’t diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index f5d0f64..739e7e8 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -359,16 +359,10 @@ impl ContactManifoldData { pub trait ContactManifoldExt { /// Computes the sum of all the impulses applied by contacts from this contact manifold. fn total_impulse(&self) -> Real; - /// Computes the maximum impulse applied by contacts from this contact manifold. - fn max_impulse(&self) -> Real; } impl ContactManifoldExt for ContactManifold { fn total_impulse(&self) -> Real { self.points.iter().map(|pt| pt.data.impulse).sum() } - - fn max_impulse(&self) -> Real { - self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse)) - } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 3884e19..95e408f 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -469,10 +469,10 @@ impl PhysicsPipeline { // TODO: do this only on user-change. // TODO: do we want some kind of automatic inverse kinematics? for multibody in &mut multibody_joints.multibodies { - multibody.1.update_root_type(bodies); - // FIXME: what should we do here? We should not - // rely on the next state here. multibody.1.forward_kinematics(bodies, true); + multibody + .1 + .update_rigid_bodies_internal(bodies, true, false, false); } self.detect_collisions( diff --git a/src/utils.rs b/src/utils.rs index 5025744..102c85f 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,8 +1,8 @@ //! Miscellaneous utilities. use na::{ - Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex, - UnitQuaternion, Vector1, Vector2, Vector3, + Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion, + Vector1, Vector2, Vector3, }; use num::Zero; use simba::simd::SimdValue; @@ -90,35 +90,6 @@ impl SimdSign for SimdReal { } } -pub(crate) trait SimdComponent: Sized { - type Element; - - fn min_component(self) -> Self::Element; - fn max_component(self) -> Self::Element; -} - -impl SimdComponent for Real { - type Element = Real; - - fn min_component(self) -> Self::Element { - self - } - fn max_component(self) -> Self::Element { - self - } -} - -impl SimdComponent for SimdReal { - type Element = Real; - - fn min_component(self) -> Self::Element { - self.simd_horizontal_min() - } - fn max_component(self) -> Self::Element { - self.simd_horizontal_max() - } -} - /// Trait to compute the orthonormal basis of a vector. pub trait SimdBasis: Sized { /// The type of the array of orthonormal vectors. @@ -166,89 +137,6 @@ impl> SimdBasis for Vector3 { } } -pub(crate) trait SimdVec: Sized { - type Element; - - fn horizontal_inf(&self) -> Self::Element; - fn horizontal_sup(&self) -> Self::Element; -} - -impl SimdVec for Vector2 -where - N::Element: Scalar, -{ - type Element = Vector2; - - fn horizontal_inf(&self) -> Self::Element { - Vector2::new(self.x.min_component(), self.y.min_component()) - } - - fn horizontal_sup(&self) -> Self::Element { - Vector2::new(self.x.max_component(), self.y.max_component()) - } -} - -impl SimdVec for Point2 -where - N::Element: Scalar, -{ - type Element = Point2; - - fn horizontal_inf(&self) -> Self::Element { - Point2::new(self.x.min_component(), self.y.min_component()) - } - - fn horizontal_sup(&self) -> Self::Element { - Point2::new(self.x.max_component(), self.y.max_component()) - } -} - -impl SimdVec for Vector3 -where - N::Element: Scalar, -{ - type Element = Vector3; - - fn horizontal_inf(&self) -> Self::Element { - Vector3::new( - self.x.min_component(), - self.y.min_component(), - self.z.min_component(), - ) - } - - fn horizontal_sup(&self) -> Self::Element { - Vector3::new( - self.x.max_component(), - self.y.max_component(), - self.z.max_component(), - ) - } -} - -impl SimdVec for Point3 -where - N::Element: Scalar, -{ - type Element = Point3; - - fn horizontal_inf(&self) -> Self::Element { - Point3::new( - self.x.min_component(), - self.y.min_component(), - self.z.min_component(), - ) - } - - fn horizontal_sup(&self) -> Self::Element { - Point3::new( - self.x.max_component(), - self.y.max_component(), - self.z.max_component(), - ) - } -} - pub(crate) trait SimdCrossMatrix: Sized { type CrossMat; type CrossMatTr; @@ -463,28 +351,21 @@ impl SimdQuat for UnitQuaternion { pub(crate) trait SimdAngularInertia { type AngVector; - type LinVector; type AngMatrix; fn inverse(&self) -> Self; - fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector; fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector; fn squared(&self) -> Self; - fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix; fn into_matrix(self) -> Self::AngMatrix; } impl SimdAngularInertia for N { type AngVector = N; - type LinVector = Vector2; type AngMatrix = N; fn inverse(&self) -> Self { simd_inv(*self) } - fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { - pt * *self - } fn transform_vector(&self, pt: N) -> N { pt * *self } @@ -493,10 +374,6 @@ impl SimdAngularInertia for N { *self * *self } - fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix { - *mat * *self - } - fn into_matrix(self) -> Self::AngMatrix { self } @@ -504,7 +381,6 @@ impl SimdAngularInertia for N { impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; - type LinVector = Vector3; type AngMatrix = Matrix3; fn inverse(&self) -> Self { @@ -540,10 +416,6 @@ impl SimdAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { - self.transform_vector(v) - } - fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; @@ -559,16 +431,10 @@ impl SimdAngularInertia for SdpMatrix3 { self.m13, self.m23, self.m33, ) } - - #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { - *self * *m - } } impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; - type LinVector = Vector3; type AngMatrix = Matrix3; fn inverse(&self) -> Self { @@ -593,10 +459,6 @@ impl SimdAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { - self.transform_vector(v) - } - fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; @@ -623,27 +485,6 @@ impl SimdAngularInertia for SdpMatrix3 { self.m13, self.m23, self.m33, ) } - - #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { - let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; - let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; - let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; - - let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; - let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; - let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; - - let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33; - let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33; - let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33; - - Matrix3::new( - x0, x1, x2, - y0, y1, y2, - z0, z1, z2, - ) - } } // This is an RAII structure that enables flushing denormal numbers diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index cb24d7e..36c9199 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -25,6 +25,7 @@ mod camera3d; mod debug_render; mod graphics; pub mod harness; +mod mouse; pub mod objects; pub mod physics; #[cfg(all(feature = "dim3", feature = "other-backends"))] diff --git a/src_testbed/mouse.rs b/src_testbed/mouse.rs new file mode 100644 index 0000000..05b69e2 --- /dev/null +++ b/src_testbed/mouse.rs @@ -0,0 +1,46 @@ +use crate::math::Point; +use bevy::prelude::*; +use bevy::window::PrimaryWindow; + +#[derive(Component)] +pub struct MainCamera; + +#[derive(Default, Copy, Clone, Debug, Resource)] +pub struct SceneMouse { + #[cfg(feature = "dim2")] + pub point: Option>, + #[cfg(feature = "dim3")] + pub ray: Option<(Point, crate::math::Vector)>, +} + +pub fn track_mouse_state( + mut scene_mouse: ResMut, + windows: Query<&Window, With>, + camera: Query<(&GlobalTransform, &Camera), With>, +) { + if let Ok(window) = windows.get_single() { + for (camera_transform, camera) in camera.iter() { + if let Some(cursor) = window.cursor_position() { + let ndc_cursor = ((cursor / Vec2::new(window.width(), window.height()) * 2.0) + - Vec2::ONE) + * Vec2::new(1.0, -1.0); + let ndc_to_world = + camera_transform.compute_matrix() * camera.projection_matrix().inverse(); + let ray_pt1 = + ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0)); + + #[cfg(feature = "dim2")] + { + scene_mouse.point = Some(Point::new(ray_pt1.x, ray_pt1.y)); + } + #[cfg(feature = "dim3")] + { + let ray_pt2 = + ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0)); + let ray_dir = ray_pt2 - ray_pt1; + scene_mouse.ray = Some((na::Vector3::from(ray_pt1).into(), ray_dir.into())); + } + } + } + } +} diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 6ddc949..836fc1f 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -9,8 +9,8 @@ use bevy::prelude::*; use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin}; use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState}; use crate::plugin::TestbedPlugin; -use crate::ui; use crate::{graphics::GraphicsManager, harness::RunState}; +use crate::{mouse, ui}; use na::{self, Point2, Point3, Vector3}; #[cfg(feature = "dim3")] @@ -135,7 +135,7 @@ pub struct TestbedState { pub solver_type: RapierSolverType, pub physx_use_two_friction_directions: bool, pub snapshot: Option, - nsteps: usize, + pub nsteps: usize, camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button. } @@ -161,6 +161,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { camera_transform: GlobalTransform, camera: &'a mut OrbitCamera, keys: &'a ButtonInput, + mouse: &'a SceneMouse, } pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> { @@ -400,6 +401,7 @@ impl TestbedApp { brightness: 0.3, ..Default::default() }) + .init_resource::() .add_plugins(DefaultPlugins.set(window_plugin)) .add_plugins(OrbitCameraPlugin) .add_plugins(WireframePlugin) @@ -419,7 +421,9 @@ impl TestbedApp { .insert_resource(self.builders) .insert_non_send_resource(self.plugins) .add_systems(Update, update_testbed) - .add_systems(Update, egui_focus); + .add_systems(Update, egui_focus) + .add_systems(Update, track_mouse_state); + init(&mut app); app.run(); } @@ -472,6 +476,15 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { pub fn keys(&self) -> &ButtonInput { self.keys } + + pub fn mouse(&self) -> &SceneMouse { + self.mouse + } + + #[cfg(feature = "dim3")] + pub fn camera_fwd_dir(&self) -> Vector { + (self.camera_transform * -Vec3::Z).normalize().into() + } } impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { @@ -1047,7 +1060,8 @@ fn setup_graphics_environment(mut commands: Commands) { .insert(OrbitCamera { rotate_sensitivity: 0.05, ..OrbitCamera::default() - }); + }) + .insert(MainCamera); } #[cfg(feature = "dim2")] @@ -1078,7 +1092,8 @@ fn setup_graphics_environment(mut commands: Commands) { zoom: 100.0, pan_sensitivity: 0.02, ..OrbitCamera::default() - }); + }) + .insert(MainCamera); } fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>) { @@ -1091,12 +1106,14 @@ fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera> } } +use crate::mouse::{track_mouse_state, MainCamera, SceneMouse}; use bevy::window::PrimaryWindow; fn update_testbed( mut commands: Commands, windows: Query<&Window, With>, // mut pipelines: ResMut>, + mouse: Res, mut meshes: ResMut>, mut materials: ResMut>, builders: ResMut, @@ -1126,6 +1143,7 @@ fn update_testbed( camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, keys: &keys, + mouse: &mouse, }; let mut testbed = Testbed { @@ -1216,6 +1234,7 @@ fn update_testbed( camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, keys: &keys, + mouse: &mouse, }; let mut testbed = Testbed { @@ -1389,6 +1408,7 @@ fn update_testbed( camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, keys: &keys, + mouse: &mouse, }; harness.step_with_graphics(Some(&mut testbed_graphics)); -- cgit