From 3ac5efeac1e8d861ccf52f37ccaca00d44b0e094 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jochen=20G=C3=B6rtler?= <web@goertler.email>
Date: Sun, 18 Nov 2018 16:51:40 +0100
Subject: [PATCH] WIP: to_homogeneous for MatrixN

---
 src/base/matrix.rs   | 16 +++++++++++++++-
 tests/core/matrix.rs |  8 ++++++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/base/matrix.rs b/src/base/matrix.rs
index 91e47624..8f700ea9 100644
--- a/src/base/matrix.rs
+++ b/src/base/matrix.rs
@@ -20,7 +20,7 @@ use alga::general::{ClosedAdd, ClosedMul, ClosedSub, Real, Ring};
 
 use base::allocator::{Allocator, SameShapeAllocator, SameShapeC, SameShapeR};
 use base::constraint::{DimEq, SameNumberOfColumns, SameNumberOfRows, ShapeConstraint};
-use base::dimension::{Dim, DimAdd, DimSum, U1, U2, U3};
+use base::dimension::{Dim, DimNameAdd, DimAdd, DimNameSum, DimSum, IsNotStaticOne, U1, U2, U3};
 use base::iter::{MatrixIter, MatrixIterMut};
 use base::storage::{
     ContiguousStorage, ContiguousStorageMut, Owned, SameShapeStorage, Storage, StorageMut,
@@ -830,6 +830,20 @@ impl<N: Scalar, D: Dim, S: Storage<N, D, D>> SquareMatrix<N, D, S> {
     }
 }
 
+impl<N: Scalar + One, D: DimNameAdd<U1> + IsNotStaticOne> MatrixN<N, D> {
+
+    /// Yields the homogeneous matrix for this matrix, i.e., appending an additional dimension and
+    /// and setting the diagonal element to `1`.
+    #[inline]
+    pub fn to_homogeneous(&self) -> MatrixN<N, DimNameSum<D, U1>>
+    where DefaultAllocator: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
+        let mut res = MatrixN::<N, DimNameSum<D, U1>>::identity();
+        res.fixed_slice_mut::<D, D>(0, 0).copy_from(&self);
+        res
+    }
+
+}
+
 impl<N: Scalar + Zero, D: DimAdd<U1>, S: Storage<N, D>> Vector<N, D, S> {
     /// Computes the coordinates in projective space of this vector, i.e., appends a `0` to its
     /// coordinates.
diff --git a/tests/core/matrix.rs b/tests/core/matrix.rs
index 9cf2a66b..18dfdcf2 100644
--- a/tests/core/matrix.rs
+++ b/tests/core/matrix.rs
@@ -275,8 +275,16 @@ fn to_homogeneous() {
     let b = DVector::from_row_slice(3, &[1.0, 2.0, 3.0]);
     let expected_b = DVector::from_row_slice(4, &[1.0, 2.0, 3.0, 0.0]);
 
+    let c = Matrix2::new(1.0, 2.0, 3.0, 4.0);
+    let expected_c = Matrix3::new(1.0, 2.0, 0.0, 3.0, 4.0, 0.0, 0.0, 0.0, 1.0);
+
+    let d = DMatrix::from_row_slice(2, &[1.0, 2.0, 3.0, 4.0]);
+    let expected_d = DMatrix::from_row_slice(3, 3, &[1.0, 2.0, 0.0, 3.0, 4.0, 0.0, 0.0, 0.0, 1.0]);
+
     assert_eq!(a.to_homogeneous(), expected_a);
     assert_eq!(b.to_homogeneous(), expected_b);
+    assert_eq!(c.to_homogeneous(), expected_c);
+    assert_eq!(d.to_homogeneous(), expected_d);
 }
 
 #[test]