From 56f52699a7d2f3eb5c51c6afb7b9670d719d8ea5 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Fri, 13 Dec 2024 16:23:33 -0800 Subject: [PATCH] mul operator for group_mul --- crates/sophus_lie/src/groups/isometry2.rs | 6 +- crates/sophus_lie/src/groups/isometry3.rs | 6 +- crates/sophus_lie/src/groups/rotation2.rs | 8 +- crates/sophus_lie/src/groups/rotation3.rs | 6 +- crates/sophus_lie/src/lib.rs | 9 --- crates/sophus_lie/src/lie_group.rs | 19 +++-- .../sophus_lie/src/{ => lie_group}/average.rs | 21 ++--- crates/sophus_lie/src/lie_group/group_mul.rs | 80 +++++++++++++++++++ .../src/{ => lie_group}/lie_group_manifold.rs | 6 +- .../src/{ => lie_group}/real_lie_group.rs | 10 +-- .../src/example_problems/cam_calib.rs | 10 +-- .../cost_fn/isometry2_prior.rs | 4 +- .../cost_fn/isometry3_prior.rs | 4 +- .../example_problems/cost_fn/pose_graph.rs | 9 +-- .../example_problems/cost_fn/reprojection.rs | 2 +- .../src/example_problems/pose_circle.rs | 10 +-- crates/sophus_opt/src/variables.rs | 10 +-- .../src/scene_renderer/line.rs | 2 +- .../src/scene_renderer/mesh.rs | 2 +- .../src/scene_renderer/point.rs | 2 +- .../src/scene_renderer/textured_mesh.rs | 2 +- crates/sophus_renderer/src/uniform_buffers.rs | 3 +- .../src/interactions/orbit_interaction.rs | 18 ++--- 23 files changed, 149 insertions(+), 100 deletions(-) rename crates/sophus_lie/src/{ => lie_group}/average.rs (87%) create mode 100644 crates/sophus_lie/src/lie_group/group_mul.rs rename crates/sophus_lie/src/{ => lie_group}/lie_group_manifold.rs (93%) rename crates/sophus_lie/src/{ => lie_group}/real_lie_group.rs (99%) diff --git a/crates/sophus_lie/src/groups/isometry2.rs b/crates/sophus_lie/src/groups/isometry2.rs index c0cb8ec..60b150d 100644 --- a/crates/sophus_lie/src/groups/isometry2.rs +++ b/crates/sophus_lie/src/groups/isometry2.rs @@ -1,7 +1,7 @@ -use crate::average::iterative_average; -use crate::average::IterativeAverageError; use crate::groups::rotation2::Rotation2Impl; use crate::groups::translation_product_product::TranslationProductGroupImpl; +use crate::lie_group::average::iterative_average; +use crate::lie_group::average::IterativeAverageError; use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::EmptySliceError; @@ -98,7 +98,7 @@ impl HasAverage for Isometry2 HasAverage for Isometry3 HasAverage for Rotation2 Rotation3 { #[test] fn rotation3_prop_tests() { use crate::factor_lie_group::RealFactorLieGroupTest; - use crate::real_lie_group::RealLieGroupTest; + use crate::lie_group::real_lie_group::RealLieGroupTest; use sophus_core::calculus::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] use sophus_core::calculus::dual::DualBatchScalar; diff --git a/crates/sophus_lie/src/lib.rs b/crates/sophus_lie/src/lib.rs index 33b835f..35e2a60 100644 --- a/crates/sophus_lie/src/lib.rs +++ b/crates/sophus_lie/src/lib.rs @@ -22,18 +22,9 @@ pub use crate::lie_group::LieGroup; /// Lie groups pub mod factor_lie_group; -/// Lie group as a manifold -pub mod lie_group_manifold; - /// Lie group traits pub mod traits; -/// Real lie group -pub mod real_lie_group; - -/// Lie group average -pub mod average; - /// sophus_lie prelude pub mod prelude { pub use crate::traits::IsLieGroup; diff --git a/crates/sophus_lie/src/lie_group.rs b/crates/sophus_lie/src/lie_group.rs index ddb8de9..2d2d74d 100644 --- a/crates/sophus_lie/src/lie_group.rs +++ b/crates/sophus_lie/src/lie_group.rs @@ -7,6 +7,15 @@ use sophus_core::params::ParamsImpl; extern crate alloc; +/// Lie group average +pub mod average; +/// Group multiplication +pub mod group_mul; +/// Lie group as a manifold +pub mod lie_group_manifold; +/// Real lie group +pub mod real_lie_group; + /// Lie group #[derive(Debug, Copy, Clone, Default)] pub struct LieGroup< @@ -149,7 +158,7 @@ impl< /// w is typically in [0, 1]. If w=0, self is returned. If w=1 other is returned. /// pub fn interpolate(&self, other: &Self, w: S) -> Self { - self.group_mul(&Self::exp(&self.inverse().group_mul(other).log().scaled(w))) + self * &Self::exp(&(&self.inverse() * other).log().scaled(w)) } /// logarithmic map @@ -327,8 +336,8 @@ impl< for g1 in &group_examples { for g2 in &group_examples { for g3 in &group_examples { - let left_hugging = (g1.group_mul(g2)).group_mul(g3); - let right_hugging = g1.group_mul(&g2.group_mul(g3)); + let left_hugging = &(g1 * g2) * g3; + let right_hugging = g1 * &(g2 * g3); assert_relative_eq!( left_hugging.compact(), right_hugging.compact(), @@ -339,8 +348,8 @@ impl< } for g1 in &group_examples { for g2 in &group_examples { - let daz_from_foo_transform_1 = g2.inverse().group_mul(&g1.inverse()); - let daz_from_foo_transform_2 = g1.group_mul(g2).inverse(); + let daz_from_foo_transform_1 = g2.inverse() * g1.inverse(); + let daz_from_foo_transform_2 = (g1 * g2).inverse(); assert_relative_eq!( daz_from_foo_transform_1.compact(), daz_from_foo_transform_2.compact(), diff --git a/crates/sophus_lie/src/average.rs b/crates/sophus_lie/src/lie_group/average.rs similarity index 87% rename from crates/sophus_lie/src/average.rs rename to crates/sophus_lie/src/lie_group/average.rs index 0ae441f..380629d 100644 --- a/crates/sophus_lie/src/average.rs +++ b/crates/sophus_lie/src/lie_group/average.rs @@ -53,18 +53,13 @@ pub fn iterative_average< for parent_from_body in parent_from_body_transforms { average_tangent = average_tangent - + parent_from_body_average - .inverse() - .group_mul(parent_from_body) + + (parent_from_body_average.inverse() * parent_from_body) .log() .scaled(w.clone()); } let refined_parent_from_body_average = - parent_from_body_average.group_mul(&LieGroup::exp(&average_tangent)); - let step = refined_parent_from_body_average - .inverse() - .group_mul(&parent_from_body_average) - .log(); + parent_from_body_average.clone() * LieGroup::exp(&average_tangent); + let step = (refined_parent_from_body_average.inverse() * parent_from_body_average).log(); if step.squared_norm() < S::from_f64(EPS_F64) { return Ok(refined_parent_from_body_average); } @@ -109,11 +104,7 @@ macro_rules! def_average_test_template { for parent_from_b in Self::element_examples() { // test: average of two elements is identical to interpolation - if parent_from_a - .inverse() - .group_mul(&parent_from_b) - .has_shortest_path_ambiguity() - { + if (parent_from_a.inverse() * parent_from_b).has_shortest_path_ambiguity() { continue; } let averaged_element = @@ -135,9 +126,7 @@ macro_rules! def_average_test_template { let w = 1.0 / (list.len() as f64); for parent_from_x in list { - average_tangent += parent_from_average - .inverse() - .group_mul(&parent_from_x) + average_tangent += (parent_from_average.inverse() * parent_from_x) .log() .scaled(w); } diff --git a/crates/sophus_lie/src/lie_group/group_mul.rs b/crates/sophus_lie/src/lie_group/group_mul.rs new file mode 100644 index 0000000..dc0e3e7 --- /dev/null +++ b/crates/sophus_lie/src/lie_group/group_mul.rs @@ -0,0 +1,80 @@ +use crate::lie_group::LieGroup; +use crate::traits::IsLieGroupImpl; +use core::ops::Mul; +use sophus_core::prelude::IsScalar; + +// a * b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul> + for LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: Self) -> Self::Output { + self.group_mul(&rhs) + } +} + +// a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul<&LieGroup> + for LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: &Self) -> Self::Output { + self.group_mul(rhs) + } +} + +// &a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul> + for &LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: LieGroup) -> Self::Output { + self.group_mul(&rhs) + } +} + +// a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul<&LieGroup> + for &LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: &LieGroup) -> Self::Output { + self.group_mul(rhs) + } +} diff --git a/crates/sophus_lie/src/lie_group_manifold.rs b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs similarity index 93% rename from crates/sophus_lie/src/lie_group_manifold.rs rename to crates/sophus_lie/src/lie_group/lie_group_manifold.rs index 2e32eff..7685303 100644 --- a/crates/sophus_lie/src/lie_group_manifold.rs +++ b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs @@ -86,12 +86,12 @@ impl< { fn oplus(&self, tangent: &>::Vector) -> Self { Self { - group: LieGroup::::exp(tangent) - .group_mul(&self.group), + group: &LieGroup::::exp(tangent) + * &self.group, } } fn ominus(&self, rhs: &Self) -> >::Vector { - self.group.inverse().group_mul(&rhs.group).log() + (&self.group.inverse() * &rhs.group).log() } } diff --git a/crates/sophus_lie/src/real_lie_group.rs b/crates/sophus_lie/src/lie_group/real_lie_group.rs similarity index 99% rename from crates/sophus_lie/src/real_lie_group.rs rename to crates/sophus_lie/src/lie_group/real_lie_group.rs index 9eb9a19..833b7f1 100644 --- a/crates/sophus_lie/src/real_lie_group.rs +++ b/crates/sophus_lie/src/lie_group/real_lie_group.rs @@ -1,9 +1,9 @@ -use super::traits::IsLieGroupImpl; -use super::traits::IsRealLieGroupImpl; use crate::groups::isometry2::Isometry2F64; use crate::groups::isometry3::Isometry3F64; use crate::lie_group::LieGroup; use crate::prelude::*; +use crate::traits::IsLieGroupImpl; +use crate::traits::IsRealLieGroupImpl; use crate::Isometry2; use crate::Isometry3; use crate::Rotation2; @@ -71,7 +71,7 @@ where /// derivative of log(exp(x)) at the identity pub fn dx_log_a_exp_x_b_at_0(a: &Self, b: &Self) -> S::Matrix { - let ab = a.group_mul(b); + let ab = a * b; Self::dx_log_x(ab.params()) .mat_mul(Self::da_a_mul_b(&Self::identity(), &ab)) .mat_mul(Self::dx_exp_x_at_0()) @@ -322,7 +322,7 @@ macro_rules! def_real_group_test_template { let log_x = |t: <$scalar as IsScalar<$batch>>::Vector| -> <$scalar as IsScalar<$batch>>::Vector { - Self::exp(&t).group_mul(&g).log() + (Self::exp(&t) * g).log() }; let o = <$scalar as IsScalar<$batch>>::Vector::zeros(); @@ -381,7 +381,7 @@ macro_rules! def_real_group_test_template { let dual_log_x = |t: <$dual_scalar as IsScalar<$batch>>::Vector| -> <$dual_scalar as IsScalar<$batch>>::Vector { - dual_a.group_mul( + (&dual_a * &<$dual_group>::exp(&t) .group_mul(&dual_b) ).log() diff --git a/crates/sophus_opt/src/example_problems/cam_calib.rs b/crates/sophus_opt/src/example_problems/cam_calib.rs index 2f5ede3..8bcd8d9 100644 --- a/crates/sophus_opt/src/example_problems/cam_calib.rs +++ b/crates/sophus_opt/src/example_problems/cam_calib.rs @@ -86,9 +86,8 @@ impl CamCalibProblem { let true_uv_in_img0_proof = true_intrinsics.cam_proj(&true_point_in_cam0); approx::assert_abs_diff_eq!(true_uv_in_img0, true_uv_in_img0_proof, epsilon = 0.1); - let true_cam1_from_cam0 = true_world_from_cameras[1] - .inverse() - .group_mul(&true_world_from_cameras[0]); + let true_cam1_from_cam0 = + true_world_from_cameras[1].inverse() * true_world_from_cameras[0]; let true_point_in_cam1 = true_cam1_from_cam0.transform(&true_point_in_cam0); let true_uv_in_img1 = true_intrinsics.cam_proj(&true_point_in_cam1); let img_noise = VecF64::<2>::new(rng.gen::() - 0.5, rng.gen::() - 0.5); @@ -107,9 +106,8 @@ impl CamCalibProblem { }); } - let true_cam2_from_cam0 = true_world_from_cameras[2] - .inverse() - .group_mul(&true_world_from_cameras[0]); + let true_cam2_from_cam0 = + true_world_from_cameras[2].inverse() * true_world_from_cameras[0]; let true_point_in_cam2 = true_cam2_from_cam0.transform(&true_point_in_cam0); let true_uv_in_img2 = true_intrinsics.cam_proj(&true_point_in_cam2); let img_noise = VecF64::<2>::new(rng.gen::() - 0.5, rng.gen::() - 0.5); diff --git a/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs b/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs index 0c1bd42..a2efa44 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs @@ -43,7 +43,7 @@ fn res_fn>( isometry: Isometry2, isometry_prior_mean: Isometry2, ) -> Scalar::Vector<3> { - Isometry2::::group_mul(&isometry, &isometry_prior_mean.inverse()).log() + (isometry * isometry_prior_mean.inverse()).log() } impl IsResidualFn<3, 1, (), Isometry2F64, Isometry2F64> for Isometry2PriorCostFn { @@ -60,7 +60,7 @@ impl IsResidualFn<3, 1, (), Isometry2F64, Isometry2F64> for Isometry2PriorCostFn let residual = res_fn(isometry, *isometry_prior_mean); let dx_res_fn = |x: DualVector<3>| -> DualVector<3> { - let pp = Isometry2::::exp(&x).group_mul(&isometry.to_dual_c()); + let pp = Isometry2::::exp(&x) * isometry.to_dual_c(); res_fn( pp, Isometry2::from_params(&DualVector::from_real_vector( diff --git a/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs b/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs index 29b1ddf..4b40767 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs @@ -44,7 +44,7 @@ fn res_fn + IsSingleScalar>( isometry: Isometry3, isometry_prior_mean: Isometry3, ) -> Scalar::Vector<6> { - Isometry3::::group_mul(&isometry, &isometry_prior_mean.inverse()).log() + (isometry * isometry_prior_mean.inverse()).log() } impl IsResidualFn<6, 1, (), Isometry3F64, (Isometry3F64, MatF64<6, 6>)> for Isometry3PriorCostFn { @@ -61,7 +61,7 @@ impl IsResidualFn<6, 1, (), Isometry3F64, (Isometry3F64, MatF64<6, 6>)> for Isom let residual = res_fn(isometry, isometry_prior.0); let dx_res_fn = |x: DualVector<6>| -> DualVector<6> { - let pp = Isometry3::::exp(&x).group_mul(&isometry.to_dual_c()); + let pp = Isometry3::::exp(&x) * isometry.to_dual_c(); res_fn( pp, Isometry3::from_params(&DualVector::from_real_vector(*isometry_prior.0.params())), diff --git a/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs b/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs index f3b2a46..1e1d306 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs @@ -14,10 +14,7 @@ pub fn res_fn( world_from_pose_b: Isometry2, pose_a_from_pose_b: Isometry2, ) -> S::Vector<3> { - (world_from_pose_a - .inverse() - .group_mul(&world_from_pose_b.group_mul(&pose_a_from_pose_b.inverse()))) - .log() + (world_from_pose_a.inverse() * world_from_pose_b * pose_a_from_pose_b.inverse()).log() } /// Cost function for 2d pose graph @@ -43,13 +40,13 @@ impl IsResidualFn<12, 2, (), (Isometry2F64, Isometry2F64), Isometry2F64> for Pos || { -Isometry2::dx_log_a_exp_x_b_at_0( &world_from_pose_a.inverse(), - &world_from_pose_b.group_mul(&obs.inverse()), + &(world_from_pose_b * obs.inverse()), ) }, || { Isometry2::dx_log_a_exp_x_b_at_0( &world_from_pose_a.inverse(), - &world_from_pose_b.group_mul(&obs.inverse()), + &(world_from_pose_b * obs.inverse()), ) }, ) diff --git a/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs b/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs index 7a1eef6..96a335a 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs @@ -90,7 +90,7 @@ impl IsResidualFn<13, 3, (), (PinholeCamera, Isometry3F64, VecF64<3>), V &DualVector::from_real_vector(*intrinsics.params()), intrinsics.image_size(), ), - Isometry3::::exp(&x).group_mul(&world_from_camera_pose.to_dual_c()), + Isometry3::::exp(&x) * world_from_camera_pose.to_dual_c(), DualVector::from_real_vector(point_in_world), DualVector::from_real_vector(*uv_in_image), ) diff --git a/crates/sophus_opt/src/example_problems/pose_circle.rs b/crates/sophus_opt/src/example_problems/pose_circle.rs index 5496e6e..242a807 100644 --- a/crates/sophus_opt/src/example_problems/pose_circle.rs +++ b/crates/sophus_opt/src/example_problems/pose_circle.rs @@ -61,11 +61,8 @@ impl PoseCircleProblem { let true_world_from_pose_b = true_world_from_robot_poses[b_idx]; let p = VecF64::<3>::from_real_array([0.001, 0.001, 0.0001]); - let pose_a_from_pose_b = Isometry2::exp(&p).group_mul( - &true_world_from_pose_a - .inverse() - .group_mul(&true_world_from_pose_b), - ); + let pose_a_from_pose_b = + Isometry2::exp(&p) * true_world_from_pose_a.inverse() * true_world_from_pose_b; obs_pose_a_from_pose_b_poses .terms @@ -87,8 +84,7 @@ impl PoseCircleProblem { let world_from_pose_a = est_world_from_robot_poses[a_idx]; let pose_a_from_pose_b = obs.pose_a_from_pose_b; let p = VecF64::<3>::from_real_array([0.1, 0.1, 0.1]); - let world_from_pose_b = - Isometry2::exp(&p).group_mul(&world_from_pose_a.group_mul(&pose_a_from_pose_b)); + let world_from_pose_b = Isometry2::exp(&p) * world_from_pose_a * pose_a_from_pose_b; est_world_from_robot_poses.push(world_from_pose_b); } diff --git a/crates/sophus_opt/src/variables.rs b/crates/sophus_opt/src/variables.rs index 89c6213..bc6252e 100644 --- a/crates/sophus_opt/src/variables.rs +++ b/crates/sophus_opt/src/variables.rs @@ -421,10 +421,7 @@ impl IsVariable for Isometry2F64 { for d in 0..::DOF { delta_vec[d] = delta[d]; } - self.set_params( - (Isometry2::::group_mul(&Isometry2::::exp(&delta_vec), &self.clone())) - .params(), - ); + self.set_params((Isometry2::::exp(&delta_vec) * *self).params()); } } @@ -436,10 +433,7 @@ impl IsVariable for Isometry3F64 { for d in 0..::DOF { delta_vec[d] = delta[d]; } - self.set_params( - (Isometry3::::group_mul(&Isometry3::::exp(&delta_vec), &self.clone())) - .params(), - ); + self.set_params((Isometry3::::exp(&delta_vec) * *self).params()); } } diff --git a/crates/sophus_renderer/src/scene_renderer/line.rs b/crates/sophus_renderer/src/scene_renderer/line.rs index 23f5e68..1396529 100644 --- a/crates/sophus_renderer/src/scene_renderer/line.rs +++ b/crates/sophus_renderer/src/scene_renderer/line.rs @@ -103,7 +103,7 @@ impl SceneLineRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &line.world_from_entity, ); render_pass.set_vertex_buffer(0, line.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/mesh.rs b/crates/sophus_renderer/src/scene_renderer/mesh.rs index 3e4fcce..02e4178 100644 --- a/crates/sophus_renderer/src/scene_renderer/mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/mesh.rs @@ -114,7 +114,7 @@ impl MeshRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &mesh.world_from_entity, ); render_pass.set_vertex_buffer(0, mesh.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/point.rs b/crates/sophus_renderer/src/scene_renderer/point.rs index d7d49a8..b25f781 100644 --- a/crates/sophus_renderer/src/scene_renderer/point.rs +++ b/crates/sophus_renderer/src/scene_renderer/point.rs @@ -89,7 +89,7 @@ impl ScenePointRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &point.world_from_entity, ); render_pass.set_vertex_buffer(0, point.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs index 990322f..4fc8fc8 100644 --- a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs @@ -201,7 +201,7 @@ impl TexturedMeshRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &mesh._scene_from_entity, ); render_pass.set_bind_group(1, &mesh._texture_bind_group, &[]); diff --git a/crates/sophus_renderer/src/uniform_buffers.rs b/crates/sophus_renderer/src/uniform_buffers.rs index 6673b07..9032bf3 100644 --- a/crates/sophus_renderer/src/uniform_buffers.rs +++ b/crates/sophus_renderer/src/uniform_buffers.rs @@ -57,8 +57,7 @@ impl CameraFromEntityPoseUniform { scene_from_camera: &Isometry3F64, world_from_entity: &Isometry3F64, ) { - let camera_from_entity_mat4x4 = - (scene_from_camera.inverse().group_mul(world_from_entity)).matrix(); + let camera_from_entity_mat4x4 = (&scene_from_camera.inverse() * world_from_entity).matrix(); let mut camera_from_entity_uniform: [[f32; 4]; 4] = [[0.0; 4]; 4]; for i in 0..4 { diff --git a/crates/sophus_viewer/src/interactions/orbit_interaction.rs b/crates/sophus_viewer/src/interactions/orbit_interaction.rs index 645c764..6d5e8e0 100644 --- a/crates/sophus_viewer/src/interactions/orbit_interaction.rs +++ b/crates/sophus_viewer/src/interactions/orbit_interaction.rs @@ -166,11 +166,10 @@ impl OrbitalInteraction { let delta = 0.002 * VecF64::<6>::new(0.0, 0.0, 0.0, 0.0, 0.0, delta_z); let camera_from_scene_point = Isometry3::from_translation(&focus_point_in_camera); - self.scene_from_camera = - self.scene_from_camera - .group_mul(&camera_from_scene_point.group_mul( - &Isometry3::exp(&delta).group_mul(&camera_from_scene_point.inverse()), - )); + self.scene_from_camera = self.scene_from_camera + * camera_from_scene_point + * Isometry3::exp(&delta) + * camera_from_scene_point.inverse(); } } @@ -245,11 +244,10 @@ impl OrbitalInteraction { 0.01 * VecF64::<6>::new(0.0, 0.0, 0.0, -delta_y as f64, delta_x as f64, 0.0); let camera_from_scene_point = Isometry3::from_translation(&cam.cam_unproj_with_z(&pixel, depth)); - self.scene_from_camera = - self.scene_from_camera - .group_mul(&camera_from_scene_point.group_mul( - &Isometry3::exp(&delta).group_mul(&camera_from_scene_point.inverse()), - )); + self.scene_from_camera = self.scene_from_camera + * camera_from_scene_point + * Isometry3::exp(&delta) + * camera_from_scene_point.inverse(); } else if response.dragged_by(egui::PointerButton::Primary) { // translate scene