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;
}
