Added Godot's matching Transform3D operations

Author: Hexorg

gdnative-core/src/core_types/geom/basis.rs

@@ -247,6 +247,46 @@ impl Basis {
         copy
     }
 
+    /// Returns linear interpolation on a sphere between two basis by weight amount (on the range of 0.0 to 1.0).
+    #[inline]
+    pub fn slerp(&self, other: &Basis, weight: f32) -> Self {
+        #[inline]
+        fn lerp_f(from: f32, to: f32, weight: f32) -> f32 {
+            from + (to - from) * weight
+        }
+        let from = self.to_quat();
+        let to = other.to_quat();
+        let mut result = Basis::from_quat(from.slerp(to, weight));
+        result.elements[0] *= lerp_f(
+            self.elements[0].length(),
+            other.elements[0].length(),
+            weight,
+        );
+        result.elements[1] *= lerp_f(
+            self.elements[1].length(),
+            other.elements[1].length(),
+            weight,
+        );
+        result.elements[2] *= lerp_f(
+            self.elements[2].length(),
+            other.elements[2].length(),
+            weight,
+        );
+
+        result
+    }
+
+    /// Returns linear interpolation between two basis by weight amount (on the range of 0.0 to 1.0).
+    #[inline]
+    pub fn lerp(&self, other: Basis, weight: f32) -> Self {
+        // this is how godot is doing it at https://github.com/godotengine/godot/blob/master/core/math/basis.cpp#L964
+        // but Godot engine output for me differs than godot-rust
+        let a = self.elements[0].linear_interpolate(other.elements[0], weight);
+        let b = self.elements[1].linear_interpolate(other.elements[1], weight);
+        let c = self.elements[2].linear_interpolate(other.elements[2], weight);
+        Basis::from_rows(a, b, c)
+    }
+
     /// Transposes the matrix.
     #[inline]
     fn transpose(&mut self) {
@@ -309,6 +349,20 @@ impl Basis {
         m.is_equal_approx(&Self::IDENTITY)
     }
 
+    #[inline]
+    pub fn orthogonalize(&mut self) {
+        let scale = self.scale();
+        self.orthonormalize();
+        self.scale_local(scale);
+    }
+
+    #[inline]
+    pub fn orthogonalized(&self) -> Self {
+        let mut copy = *self;
+        copy.orthogonalize();
+        copy
+    }
+
     /// Returns an orthonormalized version of the matrix: 3 orthogonal basis vectors of unit length.
     #[inline]
     pub fn orthonormalized(&self) -> Self {
@@ -369,6 +423,18 @@ impl Basis {
         copy
     }
 
+    /// In-place basis scaling in object-local coordinate system
+    #[inline]
+    fn scale_local(&mut self, scale: Vector3) {
+        *self = self.scaled_local(scale);
+    }
+
+    /// Returns basis scaled in object-local coordinate system
+    #[inline]
+    fn scaled_local(&self, scale: Vector3) -> Self {
+        (*self) * Basis::from_diagonal(scale)
+    }
+
     /// Multiplies the matrix from left with the scaling matrix: M -> S·M
     ///
     /// See the comment for [Basis::rotated](#method.rotated) for further explanation.

gdnative-core/src/core_types/geom/transform.rs

@@ -116,12 +116,9 @@ impl Transform {
     /// affine_inverse for transforms with scaling).
     #[inline]
     pub fn inverse(&self) -> Self {
-        let basis_inv = self.basis.transposed();
-        let origin_inv = basis_inv.xform(-self.origin);
-
-        Self {
-            basis: basis_inv,
-            origin: origin_inv,
+        Transform {
+            basis: self.basis.transposed(),
+            origin: self.basis.xform(-self.origin),
         }
     }
 
@@ -138,6 +135,25 @@ impl Transform {
         }
     }
 
+    /// In-place rotation of the transform around the given axis by the given
+    /// angle (in radians), using matrix multiplication. The axis must be a
+    /// normalized vector.
+    /// Due to nature of the operation, a new transform is created first.
+    #[inline]
+    pub fn rotated(&self, axis: Vector3, phi: f32) -> Self {
+        Transform {
+            basis: Basis::from_axis_angle(axis, phi),
+            origin: Vector3::default(),
+        } * (*self)
+    }
+
+    /// Returns the rotated transform around the given axis by the given angle (in radians),
+    /// using matrix multiplication. The axis must be a normalized vector.
+    #[inline]
+    pub fn rotate(&mut self, axis: Vector3, phi: f32) {
+        *self = self.rotated(axis, phi);
+    }
+
     /// Returns a copy of the transform rotated such that its -Z axis points
     /// towards the target position.
     ///
@@ -157,6 +173,92 @@ impl Transform {
         }
     }
 
+    /// Scales basis and origin of the transform by the given scale factor,
+    /// using matrix multiplication.
+    #[inline]
+    pub fn scaled(&self, scale: Vector3) -> Self {
+        Transform {
+            basis: self.basis.scaled(scale),
+            origin: self.origin * scale,
+        }
+    }
+
+    /// In-place translates the transform by the given offset, relative to
+    /// the transform's basis vectors.
+    #[inline]
+    fn translate_withbasis(&mut self, translation: Vector3) {
+        // Note: Godot source uses origin + basis dot translation,
+        // but self.translate() uses only origin + translation
+        self.origin.x += self.basis.elements[0].dot(translation);
+        self.origin.y += self.basis.elements[1].dot(translation);
+        self.origin.z += self.basis.elements[2].dot(translation);
+    }
+
+    /// Translates the transform by the given offset, relative to
+    /// the transform's basis vectors.
+    #[inline]
+    pub fn translated_withbasis(&self, translation: Vector3) -> Self {
+        let mut copy = *self;
+        copy.translate_withbasis(translation);
+        copy
+    }
+
+    /// Returns the transform with the basis orthogonal (90 degrees),
+    /// and normalized axis vectors.
+    #[inline]
+    pub fn orthonormalized(&self) -> Self {
+        Transform {
+            basis: self.basis.orthonormalized(),
+            origin: self.origin,
+        }
+    }
+
+    /// Returns the transform with the basis orthogonal (90 degrees),
+    /// but without normalizing the axis vectors.
+    #[inline]
+    pub fn orthogonalized(&self) -> Self {
+        Transform {
+            basis: self.basis.orthogonalized(),
+            origin: self.origin,
+        }
+    }
+
+    #[inline]
+    pub fn is_equal_approx(&self, other: Transform) -> bool {
+        self.basis.is_equal_approx(&other.basis) && self.origin.is_equal_approx(other.origin)
+    }
+
+    /// Interpolates the transform to other Transform by
+    /// weight amount (on the range of 0.0 to 1.0).
+    /// Assuming the two transforms are located on a sphere surface.
+    #[inline]
+    pub fn sphere_interpolate_with(&self, other: Transform, weight: f32) -> Self {
+        let src_scale = self.basis.scale();
+        let src_rot = self.basis.to_quat();
+        let src_loc = self.origin;
+
+        let dst_scale = other.basis.scale();
+        let dst_rot = other.basis.to_quat();
+        let dst_loc = other.origin;
+
+        let new_basis = Basis::from_quat(src_rot.slerp(dst_rot, weight).normalized());
+        let new_basis = new_basis.scaled(src_scale.linear_interpolate(dst_scale, weight));
+        Transform {
+            basis: new_basis,
+            origin: src_loc.linear_interpolate(dst_loc, weight),
+        }
+    }
+
+    /// Interpolates the transform to other Transform by
+    /// weight amount (on the range of 0.0 to 1.0).
+    #[inline]
+    pub fn interpolate_with(&self, other: Transform, weight: f32) -> Self {
+        Transform {
+            basis: self.basis.lerp(other.basis, weight),
+            origin: self.origin.linear_interpolate(other.origin, weight),
+        }
+    }
+
     #[doc(hidden)]
     #[inline]
     pub fn sys(&self) -> *const sys::godot_transform {
@@ -182,3 +284,120 @@ impl Mul<Transform> for Transform {
         Self { origin, basis }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn test_inputs() -> (Transform, Transform) {
+        let basis = Basis::from_euler(Vector3::new(37.51756, 20.39467, 49.96816));
+        let mut t = Transform::from_basis_origin(
+            basis.a(),
+            basis.b(),
+            basis.c(),
+            Vector3::new(0.0, 0.0, 0.0),
+        );
+        t = t.translated_withbasis(Vector3::new(0.5, -1.0, 0.25));
+        t = t.scaled(Vector3::new(0.25, 0.5, 2.0));
+
+        let basis = Basis::from_euler(Vector3::new(12.23, 50.46, 93.94));
+        let mut t2 = Transform::from_basis_origin(
+            basis.a(),
+            basis.b(),
+            basis.c(),
+            Vector3::new(0.0, 0.0, 0.0),
+        );
+        t2 = t2.translated_withbasis(Vector3::new(1.5, -2.0, 1.25));
+        t2 = t2.scaled(Vector3::new(0.5, 0.58, 1.0));
+        // Godot reports:
+        // t = 0.019358, -0.041264, 0.24581, -0.144074, 0.470205, 0.090279, -1.908901, -0.594598, 0.050514 - 0.112395, -0.519672, -0.347224
+        // t2 = 0.477182, 0.118214, 0.09123, -0.165859, 0.521769, 0.191437, -0.086105, -0.367178, 0.926157 - 0.593383, -1.05303, 1.762894
+
+        (t, t2)
+    }
+
+    #[test]
+    fn translation_is_sane() {
+        let translation = Vector3::new(1.0, 2.0, 3.0);
+        let t = Transform::default().translated(translation);
+        assert!(t.basis.elements[0] == Vector3::new(1.0, 0.0, 0.0));
+        assert!(t.basis.elements[1] == Vector3::new(0.0, 1.0, 0.0));
+        assert!(t.basis.elements[2] == Vector3::new(0.0, 0.0, 1.0));
+        assert!(t.origin == translation);
+    }
+
+    #[test]
+    fn scale_is_sane() {
+        let scale = Vector3::new(1.0, 2.0, 3.0);
+        let t = Transform::default().scaled(scale);
+        assert!(t.basis.elements[0] == Vector3::new(1.0, 0.0, 0.0));
+        assert!(t.basis.elements[1] == Vector3::new(0.0, 2.0, 0.0));
+        assert!(t.basis.elements[2] == Vector3::new(0.0, 0.0, 3.0));
+        assert!(t.origin == Vector3::new(0.0, 0.0, 0.0));
+    }
+
+    #[test]
+    fn affine_inverse_is_sane() {
+        // Godot reports:
+        // From 0.019358, -0.041264, 0.24581, -0.144074, 0.470205, 0.090279, -1.908901, -0.594598, 0.050514 - 0.112395, -0.519672, -0.347224
+        // To 0.309725, -0.576295, -0.477225, -0.66022, 1.880819, -0.148649, 3.932961, 0.361114, 0.012628 - -0.5, 1, -0.25
+        let t = test_inputs().0.affine_inverse();
+        let expected = Transform::from_basis_origin(
+            Vector3::new(0.309725, -0.66022015, 3.9329607),
+            Vector3::new(-0.57629496, 1.8808193, 0.3611141),
+            Vector3::new(-0.47722515, -0.14864945, 0.012628445),
+            Vector3::new(-0.5, 1.0, -0.25),
+        );
+        assert!(expected.is_equal_approx(t))
+    }
+
+    #[test]
+    fn orthonormalization_is_sane() {
+        // Godot reports:
+        // From 0.019358, -0.041264, 0.24581, -0.144074, 0.470205, 0.090279, -1.908901, -0.594598, 0.050514 - 0.112395, -0.519672, -0.347224
+        // To 0.010112, -0.090928, 0.995806, -0.075257, 0.992963, 0.091432, -0.997113, -0.075866, 0.003197 - 0.112395, -0.519672, -0.347224
+        let t = test_inputs().0.orthonormalized();
+        let expected = Transform::from_basis_origin(
+            Vector3::new(0.010111539, -0.0752568, -0.99711293),
+            Vector3::new(-0.090927705, 0.9929635, -0.075865656),
+            Vector3::new(0.99580616, 0.0914323, 0.0031974507),
+            Vector3::new(0.11239518, -0.519672, -0.34722406),
+        );
+        assert!(expected.is_equal_approx(t))
+    }
+
+    #[test]
+    fn linear_interpolation_is_sane() {
+        // Godot reports:
+        // t = 0.019358, -0.041264, 0.24581, -0.144074, 0.470205, 0.090279, -1.908901, -0.594598, 0.050514 - 0.112395, -0.519672, -0.347224
+        // t2 = 0.477182, 0.118214, 0.09123, -0.165859, 0.521769, 0.191437, -0.086105, -0.367178, 0.926157 - 0.593383, -1.05303, 1.762894
+        // TODO: Get new godot result. https://github.com/godotengine/godot/commit/61759da5b35e44003ab3ffe3d4024dd611d17eff changed how Transform3D.linear_interpolate works
+        // For now assuming this is sane - examined the new implementation manually.
+        let (t, t2) = test_inputs();
+        let result = t.interpolate_with(t2, 0.5);
+        let expected = Transform::from_basis_origin(
+            Vector3::new(0.24826992, -0.15496635, -0.997503),
+            Vector3::new(0.038474888, 0.49598676, -0.4808879),
+            Vector3::new(0.16852, 0.14085774, 0.48833522),
+            Vector3::new(0.352889, -0.786351, 0.707835),
+        );
+        assert!(expected.is_equal_approx(result))
+    }
+
+    #[test]
+    fn sphere_linear_interpolation_is_sane() {
+        // Godot reports:
+        // t = 0.019358, -0.041264, 0.24581, -0.144074, 0.470205, 0.090279, -1.908901, -0.594598, 0.050514 - 0.112395, -0.519672, -0.347224
+        // t2 = 0.477182, 0.118214, 0.09123, -0.165859, 0.521769, 0.191437, -0.086105, -0.367178, 0.926157 - 0.593383, -1.05303, 1.762894
+        // result =  0.727909, -0.029075, 0.486138, -0.338385, 0.6514, 0.156468, -0.910002, -0.265481, 0.330678 - 0.352889, -0.786351, 0.707835
+        let (t, t2) = test_inputs();
+        let result = t.sphere_interpolate_with(t2, 0.5);
+        let expected = Transform::from_basis_origin(
+            Vector3::new(0.7279087, -0.19632529, -0.45626357),
+            Vector3::new(-0.05011323, 0.65140045, -0.22942543),
+            Vector3::new(0.9695858, 0.18105738, 0.33067825),
+            Vector3::new(0.3528893, -0.78635097, 0.7078349),
+        );
+        assert!(expected.is_equal_approx(result))
+    }
+}