From 1cf7d12695dacd2b55777612a4e4234b6600c3da Mon Sep 17 00:00:00 2001
From: Adam Nemecek <adamnemecek@gmail.com>
Date: Sun, 5 Jul 2020 13:29:08 -0700
Subject: [PATCH] unrolled new_nonuniform_scaling_wrt_point

---
 src/base/cg.rs   | 47 ++++++++++++++++++++++++++++++++++++-----------
 tests/core/cg.rs |  6 +++---
 2 files changed, 39 insertions(+), 14 deletions(-)

diff --git a/src/base/cg.rs b/src/base/cg.rs
index 6a324bd2..935388af 100644
--- a/src/base/cg.rs
+++ b/src/base/cg.rs
@@ -16,7 +16,7 @@ use crate::base::{
 };
 use crate::geometry::{
     Isometry, IsometryMatrix3, Orthographic3, Perspective3, Point, Point2, Point3, Rotation2,
-    Rotation3, Translation2, Translation3,
+    Rotation3,
 };
 
 use simba::scalar::{ClosedAdd, ClosedMul, RealField};
@@ -76,11 +76,20 @@ impl<N: RealField> Matrix3<N> {
     ///
     /// Can be used to implement "zoom_to" functionality.
     #[inline]
-    pub fn new_nonuniform_scaling_wrt_point(scaling: Vector2<N>, pt: Point2<N>) -> Self {
-        let translate = Translation2::new(pt.x, pt.y).to_homogeneous();
-        let scale = Matrix3::new_nonuniform_scaling(&scaling);
-        let translate_inv = Translation2::new(-pt.x, -pt.y).to_homogeneous();
-        translate * scale * translate_inv
+    pub fn new_nonuniform_scaling_wrt_point(scaling: &Vector2<N>, pt: &Point2<N>) -> Self {
+        let _0 = N::zero();
+        let _1 = N::one();
+        Matrix3::new(
+            scaling.x,
+            _0,
+            pt.x - pt.x * scaling.x,
+            _0,
+            scaling.y,
+            pt.y - pt.y * scaling.y,
+            _0,
+            _0,
+            _1,
+        )
     }
 }
 
@@ -106,11 +115,27 @@ impl<N: RealField> Matrix4<N> {
     ///
     /// Can be used to implement "zoom_to" functionality.
     #[inline]
-    pub fn new_nonuniform_scaling_wrt_point(scaling: Vector3<N>, pt: Point3<N>) -> Self {
-        let translate = Translation3::new(pt.x, pt.y, pt.z).to_homogeneous();
-        let scale = Matrix4::new_nonuniform_scaling(&scaling);
-        let translate_inv = Translation3::new(-pt.x, -pt.y, -pt.z).to_homogeneous();
-        translate * scale * translate_inv
+    pub fn new_nonuniform_scaling_wrt_point(scaling: &Vector3<N>, pt: &Point3<N>) -> Self {
+        let _0 = N::zero();
+        let _1 = N::one();
+        Matrix4::new(
+            scaling.x,
+            _0,
+            _0,
+            pt.x - pt.x * scaling.x,
+            _0,
+            scaling.y,
+            _0,
+            pt.y - pt.y * scaling.y,
+            _0,
+            _0,
+            scaling.z,
+            pt.z - pt.z * scaling.z,
+            _0,
+            _0,
+            _0,
+            _1,
+        )
     }
 
     /// Builds a 3D homogeneous rotation matrix from an axis and an angle (multiplied together).
diff --git a/tests/core/cg.rs b/tests/core/cg.rs
index f1969658..b061efbb 100644
--- a/tests/core/cg.rs
+++ b/tests/core/cg.rs
@@ -9,7 +9,7 @@ fn test_scaling_wrt_point_1() {
     let c = Point2::new(5.0, 2.0);
 
     let scaling = Vector2::new(2.0, 2.0);
-    let scale_about = Matrix3::new_nonuniform_scaling_wrt_point(scaling, c);
+    let scale_about = Matrix3::new_nonuniform_scaling_wrt_point(&scaling, &c);
 
     let expected_a = Point2::new(-5.0, -2.0);
     let expected_b = Point2::new(-3.0, 0.0);
@@ -30,7 +30,7 @@ fn test_scaling_wrt_point_2() {
     let c = Point3::new(5.0, 2.0, 1.0);
 
     let scaling = Vector3::new(2.0, 2.0, 1.0);
-    let scale_about = Matrix4::new_nonuniform_scaling_wrt_point(scaling, c);
+    let scale_about = Matrix4::new_nonuniform_scaling_wrt_point(&scaling, &c);
 
     let expected_a = Point3::new(-5.0, -2.0, 1.0);
     let expected_b = Point3::new(-3.0, 0.0, 1.0);
@@ -50,7 +50,7 @@ fn test_scaling_wrt_point_3() {
     let about = Point3::new(2.0, 1.0, -2.0);
     let scale = Vector3::new(2.0, 0.5, -1.0);
     let pt = Point3::new(1.0, 2.0, 3.0);
-    let scale_about = Matrix4::new_nonuniform_scaling_wrt_point(scale, about);
+    let scale_about = Matrix4::new_nonuniform_scaling_wrt_point(&scale, &about);
 
     let expected = Point3::new(0.0, 1.5, -7.0);
     let result = scale_about.transform_point(&pt);