Repository: bitshifter/glam-rs
Pull request #37 (2019-12-17 09:13:01)

Added Mat4::to_scale_rotation_translation
The function extracts scale, rotation and translation from a 4x4 homogeneous transformation matrix. Also added a number of tests.


Patch:
diff --git a/.travis.yml b/.travis.yml
index e919e56..c6ece78 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -14,8 +14,8 @@ rust:
 # cache: cargo
 
 env:
-  - CARGO_FEATURES="mint rand serde debug-glam-assert"
-  - CARGO_FEATURES="mint rand serde scalar-math debug-glam-assert"
+  - CARGO_FEATURES="mint rand serde debug-glam-assert transform-types"
+  - CARGO_FEATURES="mint rand serde scalar-math debug-glam-assert transform-types"
 
 matrix:
   allow_failures:
@@ -31,7 +31,7 @@ script:
   - cargo clean
   - cargo build --features "$CARGO_FEATURES"
   - cargo test --features "$CARGO_FEATURES"
-  - cargo bench --features "$CARGO_FEATURES transform-types" --no-run
+  - cargo bench --features "$CARGO_FEATURES" --no-run
 
 after_success: |
   if [[ "$TRAVIS_RUST_VERSION" == nightly ]]; then
diff --git a/src/f32/mat2.rs b/src/f32/mat2.rs
index 1ad0e25..d8afe1f 100644
--- a/src/f32/mat2.rs
+++ b/src/f32/mat2.rs
@@ -150,6 +150,30 @@ impl Mat2 {
         Vec2::new(x, y)
     }
 
+    // #[inline]
+    // pub(crate) fn col(&self, index: usize) -> Vec2 {
+    //     match index {
+    //         0 => self.x_axis(),
+    //         1 => self.y_axis(),
+    //         _ => panic!(
+    //             "index out of bounds: the len is 2 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
+    // #[inline]
+    // pub(crate) fn col_mut(&mut self, index: usize) -> &mut Vec2 {
+    //     match index {
+    //         0 => unsafe { &mut *(self.0.as_mut().as_mut_ptr() as *mut Vec2) },
+    //         1 => unsafe { &mut *(self.0.as_mut()[2..].as_mut_ptr() as *mut Vec2) },
+    //         _ => panic!(
+    //             "index out of bounds: the len is 2 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
     /// Returns the transpose of `self`.
     #[inline]
     pub fn transpose(&self) -> Self {
diff --git a/src/f32/mat3.rs b/src/f32/mat3.rs
index 8314b61..546a9fe 100644
--- a/src/f32/mat3.rs
+++ b/src/f32/mat3.rs
@@ -281,6 +281,32 @@ impl Mat3 {
         self.z_axis
     }
 
+    // #[inline]
+    // pub(crate) fn col(&self, index: usize) -> Vec3 {
+    //     match index {
+    //         0 => self.x_axis,
+    //         1 => self.y_axis,
+    //         2 => self.z_axis,
+    //         _ => panic!(
+    //             "index out of bounds: the len is 3 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
+    // #[inline]
+    // pub(crate) fn col_mut(&mut self, index: usize) -> &mut Vec3 {
+    //     match index {
+    //         0 => &mut self.x_axis,
+    //         1 => &mut self.y_axis,
+    //         2 => &mut self.z_axis,
+    //         _ => panic!(
+    //             "index out of bounds: the len is 3 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
     /// Returns the transpose of `self`.
     #[inline]
     pub fn transpose(&self) -> Self {
diff --git a/src/f32/mat4.rs b/src/f32/mat4.rs
index 0b20715..d0258f2 100644
--- a/src/f32/mat4.rs
+++ b/src/f32/mat4.rs
@@ -1,4 +1,4 @@
-use super::{scalar_sin_cos, Quat, Vec3, Vec4};
+use super::{scalar_sin_cos, Mat3, Quat, Vec3, Vec4};
 #[cfg(all(
     target_arch = "x86",
     target_feature = "sse2",
@@ -192,6 +192,32 @@ impl Mat4 {
         }
     }
 
+    /// Extracts `scale`, `rotation` and `translation` from `self`. The input matrix is expected to
+    /// be a 4x4 homogeneous transformation matrix otherwise the output will be invalid.
+    pub fn to_scale_rotation_translation(&self) -> (Vec3, Quat, Vec3) {
+        let det = self.determinant();
+        glam_assert!(det != 0.0);
+
+        let scale = Vec3::new(
+            self.x_axis.length() * det.signum(),
+            self.y_axis.length(),
+            self.z_axis.length(),
+        );
+        glam_assert!(scale.cmpne(Vec3::zero()).all());
+
+        let inv_scale = scale.reciprocal();
+
+        let rotation = Quat::from_rotation_mat3(&Mat3::from_cols(
+            self.x_axis().truncate() * inv_scale.dup_x(),
+            self.y_axis().truncate() * inv_scale.dup_y(),
+            self.z_axis().truncate() * inv_scale.dup_z(),
+        ));
+
+        let translation = self.w_axis.truncate();
+
+        (scale, rotation, translation)
+    }
+
     /// Creates a 4x4 homogeneous transformation matrix from the given `rotation`.
     #[inline]
     pub fn from_quat(rotation: Quat) -> Self {
@@ -339,6 +365,34 @@ impl Mat4 {
         self.w_axis
     }
 
+    // #[inline]
+    // pub(crate) fn col(&self, index: usize) -> Vec4 {
+    //     match index {
+    //         0 => self.x_axis,
+    //         1 => self.y_axis,
+    //         2 => self.z_axis,
+    //         3 => self.w_axis,
+    //         _ => panic!(
+    //             "index out of bounds: the len is 4 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
+    // #[inline]
+    // pub(crate) fn col_mut(&mut self, index: usize) -> &mut Vec4 {
+    //     match index {
+    //         0 => &mut self.x_axis,
+    //         1 => &mut self.y_axis,
+    //         2 => &mut self.z_axis,
+    //         3 => &mut self.w_axis,
+    //         _ => panic!(
+    //             "index out of bounds: the len is 4 but the index is {}",
+    //             index
+    //         ),
+    //     }
+    // }
+
     /// Returns the transpose of `self`.
     #[inline]
     pub fn transpose(&self) -> Self {


Test patch:
diff --git a/tests/mat4.rs b/tests/mat4.rs
index dc873d2..664ea65 100644
--- a/tests/mat4.rs
+++ b/tests/mat4.rs
@@ -242,6 +242,78 @@ fn test_mat4_inverse() {
     assert_approx_eq!(m_inv, trans_inv * rotz_inv * scale_inv, 1.0e-6);
 }
 
+#[test]
+fn test_mat4_decompose() {
+    // identity
+    let (out_scale, out_rotation, out_translation) =
+        Mat4::identity().to_scale_rotation_translation();
+    assert_approx_eq!(Vec3::one(), out_scale);
+    assert!(out_rotation.is_near_identity());
+    assert_approx_eq!(Vec3::zero(), out_translation);
+
+    // no scale
+    let in_scale = Vec3::one();
+    let in_translation = Vec3::new(-2.0, 4.0, -0.125);
+    let in_rotation = Quat::from_rotation_ypr(
+        f32::to_radians(-45.0),
+        f32::to_radians(180.0),
+        f32::to_radians(270.0),
+    );
+    let in_mat = Mat4::from_scale_rotation_translation(in_scale, in_rotation, in_translation);
+    let (out_scale, out_rotation, out_translation) = in_mat.to_scale_rotation_translation();
+    assert_approx_eq!(in_scale, out_scale, 1e-6);
+    // out_rotation is different but produces the same matrix
+    // assert_approx_eq!(in_rotation, out_rotation);
+    assert_approx_eq!(in_translation, out_translation);
+    assert_approx_eq!(
+        in_mat,
+        Mat4::from_scale_rotation_translation(out_scale, out_rotation, out_translation),
+        1e-6
+    );
+
+    // positive scale
+    let in_scale = Vec3::new(1.0, 2.0, 4.0);
+    let in_mat = Mat4::from_scale_rotation_translation(in_scale, in_rotation, in_translation);
+    let (out_scale, out_rotation, out_translation) = in_mat.to_scale_rotation_translation();
+    assert_approx_eq!(in_scale, out_scale, 1e-6);
+    // out_rotation is different but produces the same matrix
+    // assert_approx_eq!(in_rotation, out_rotation);
+    assert_approx_eq!(in_translation, out_translation);
+    assert_approx_eq!(
+        in_mat,
+        Mat4::from_scale_rotation_translation(out_scale, out_rotation, out_translation),
+        1e-6
+    );
+
+    // negative scale
+    let in_scale = Vec3::new(-4.0, 1.0, 2.0);
+    let in_mat = Mat4::from_scale_rotation_translation(in_scale, in_rotation, in_translation);
+    let (out_scale, out_rotation, out_translation) = in_mat.to_scale_rotation_translation();
+    assert_approx_eq!(in_scale, out_scale, 1e-6);
+    // out_rotation is different but produces the same matrix
+    // assert_approx_eq!(in_rotation, out_rotation);
+    assert_approx_eq!(in_translation, out_translation);
+    assert_approx_eq!(
+        in_mat,
+        Mat4::from_scale_rotation_translation(out_scale, out_rotation, out_translation),
+        1e-5
+    );
+
+    // negative scale
+    let in_scale = Vec3::new(4.0, -1.0, -2.0);
+    let in_mat = Mat4::from_scale_rotation_translation(in_scale, in_rotation, in_translation);
+    let (out_scale, out_rotation, out_translation) = in_mat.to_scale_rotation_translation();
+    // out_scale and out_rotation are different but they produce the same matrix
+    // assert_approx_eq!(in_scale, out_scale, 1e-6);
+    // assert_approx_eq!(in_rotation, out_rotation);
+    assert_approx_eq!(in_translation, out_translation);
+    assert_approx_eq!(
+        in_mat,
+        Mat4::from_scale_rotation_translation(out_scale, out_rotation, out_translation),
+        1e-6
+    );
+}
+
 #[test]
 fn test_mat4_look_at() {
     let eye = Vec3::new(0.0, 0.0, -5.0);