public static void fillMapWithMatrixCentered(HeightMapWithPoints map, DenseMatrix64F matrix, double gridSize) { for (int i = 0; i < matrix.getNumRows(); i++) { for (int j = 0; j < matrix.getNumRows(); j++) { map.addPoint((i-matrix.getNumRows()/2)* gridSize, (j-matrix.getNumRows()/2)* gridSize, matrix.get(i, j)); } } } }
/** {@inheritDoc} */ @Override public boolean setA(DenseMatrix64F A) { this.A.set(A); matrixSize = A.getNumRows(); return true; }
public void setRho(double[] rho) { for (int i = 0; i < this.rho.getNumRows(); i++) { this.rho.set(i, 0, rho[i]); } }
public static void extractDiagonal(DenseMatrix64F matrix, double[] diagonal) { for (int i = 0; i < Math.min(matrix.getNumRows(), matrix.getNumCols()); i++) { diagonal[i] = matrix.get(i, i); } }
public static void setMatrixColumnToVector(int columnIndex, DenseMatrix64F Matrix, DenseMatrix64F vector) { for (int i = 0; i < Matrix.getNumRows(); i++) { Matrix.set(i, columnIndex, vector.get(i, columnIndex)); } }
public static void setMatrixColumnToVector(int columnIndex, DenseMatrix64F Matrix, DenseMatrix64F vector) { for (int i = 0; i < Matrix.getNumRows(); i++) { Matrix.set(i, columnIndex, vector.get(i, columnIndex)); } }
public void setJointAngles(DenseMatrix64F angles) { for (int i = 0; i < angles.getNumRows(); i++) { oneDoFJoints[i].setQ(angles.get(i, 0)); } }
private void setJointAngles(DenseMatrix64F angles) { for (int i = 0; i < angles.getNumRows(); i++) { oneDoFJoints[i].setQ(angles.get(i, 0)); } }
public static void getFromYoVariablesMatrix(DenseMatrix64F m, List<List<YoDouble>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<YoDouble> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, row.get(j).getDoubleValue()); } } }
public static void storeInYoVariables(DenseMatrix64F v, DoubleYoVariable[] yoV) { for (int i = 0; i < v.getNumRows(); i++) { yoV[i].set(v.get(i)); } }
public static void storeInYoVariablesMatrix(DenseMatrix64F m, List<List<DoubleYoVariable>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<DoubleYoVariable> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { row.get(j).set(m.get(i, j)); } } }
public static void getFromYoVariablesMatrix(DenseMatrix64F m, List<List<DoubleYoVariable>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<DoubleYoVariable> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, row.get(j).getDoubleValue()); } } }
public static DenseMatrix64F mult(DenseMatrix64F A, DenseMatrix64F B) { DenseMatrix64F C = new DenseMatrix64F(A.getNumRows(), B.getNumCols()); CommonOps.mult(A, B, C); return C; }
public DenseMatrix64F checkJQEqualsZeroAfterSetConstraint() { DenseMatrix64F checkJQEqualsZero = new DenseMatrix64F(this.j.getNumRows(), q.getNumCols()); CommonOps.mult(this.j, q, checkJQEqualsZero); return checkJQEqualsZero; }
public DenseMatrix64F constrainResult(DenseMatrix64F xBar) { x.reshape(xBar.getNumRows(), 1); CommonOps.mult(q, xBar, x); CommonOps.addEquals(x, jPlusp); return x; }
public static void getFromYoVariables(DenseMatrix64F m, YoDouble[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, yoM[i][j].getDoubleValue()); } } }
public void setSelectionMatrix(DenseMatrix64F selectionMatrix) { numberOfConstraints = selectionMatrix.getNumRows(); this.selectionMatrix.reshape(numberOfConstraints, selectionMatrix.getNumCols()); this.selectionMatrix.set(selectionMatrix); }
public static void getFromYoVariables(DenseMatrix64F m, DoubleYoVariable[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, yoM[i][j].getDoubleValue()); } } }
public static void assertMatrixEqualsZero(String message, DenseMatrix64F matrix, double epsilon) { int numberOfRows = matrix.getNumRows(); int numberOfColumns = matrix.getNumCols(); for (int row=0; row<numberOfRows; row++) { for (int column=0; column<numberOfColumns; column++) { assertEquals(message, 0.0, matrix.get(row, column), epsilon); } } }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE); MathTools.checkIfInRange(matrix.getNumCols(), 1, Integer.MAX_VALUE); matrix.set(rowStart, 0, qd); }