/* Appends 1 at the end of the col vector */ mat dynamics::get_homogenousTransformPoint(mat & P) { mat R(P.numRows() + 1, 1); for (int i = 1; i <= P.numRows(); i++) R(i, 1) = P(i, 1); R(P.numRows() + 1, 1) = 1; return R; }