public static DenseDoubleMatrix2D createUniMatrix( DenseDoubleMatrix2D rot, DenseDoubleMatrix1D trl) { DenseDoubleMatrix2D res = new DenseDoubleMatrix2D(4, 4); for (int i = 0; i != 3; i++) { for (int j = 0; j != 3; j++) { res.set(i, j, rot.get(i, j)); } } for (int i = 0; i != 3; i++) { res.set(i, 3, trl.get(i)); } for (int i = 0; i != 3; i++) { res.set(3, i, 0); } res.set(3, 3, 1); return res; }