Add `push` method to Vector

src/base/matrix.rs

@@ -859,6 +859,22 @@ impl<N: Scalar + Zero, D: DimAdd<U1>, S: Storage<N, D>> Vector<N, D, S> {
     }
 }
 
+impl<N: Scalar + Zero, D: DimAdd<U1>, S: Storage<N, D>> Vector<N, D, S> {
+    /// Constructs a new vector of higher dimension by appending `element` to the end of `self`.
+    #[inline]
+    pub fn push(&self, element: N) -> VectorN<N, DimSum<D, U1>>
+    where DefaultAllocator: Allocator<N, DimSum<D, U1>> {
+        let len = self.len();
+        let hnrows = DimSum::<D, U1>::from_usize(len + 1);
+        let mut res = unsafe { VectorN::<N, _>::new_uninitialized_generic(hnrows, U1) };
+        res.generic_slice_mut((0, 0), self.data.shape())
+            .copy_from(self);
+        res[(len, 0)] = element;
+
+        res
+    }
+}
+
 impl<N, R: Dim, C: Dim, S> AbsDiffEq for Matrix<N, R, C, S>
 where
     N: Scalar + AbsDiffEq,

tests/core/matrix.rs

@@ -287,6 +287,18 @@ fn to_homogeneous() {
     assert_eq!(d.to_homogeneous(), expected_d);
 }
 
+#[test]
+fn push() {
+    let a = Vector3::new(1.0, 2.0, 3.0);
+    let expected_a = Vector4::new(1.0, 2.0, 3.0, 4.0);
+
+    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, 4.0]);
+
+    assert_eq!(a.push(4.0), expected_a);
+    assert_eq!(b.push(4.0), expected_b);
+}
+
 #[test]
 fn simple_add() {
     let a = Matrix2x3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);