WIP: to_homogeneous for MatrixN

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.

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]