public static boolean isIdentity(Matrix3d matrix3d, double tolerance) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { double expectedValue = i == j ? 1.0 : 0.0; double element = matrix3d.getElement(i, j); if (Math.abs(expectedValue - element) > tolerance) return false; } } return true; }
public static void assertMatrix3dEquals(String message, Matrix3d expected, Matrix3d actual, double delta) { int size = 3; for (int i = 0; i < size; i++) { for (int j = 0; j < size; j++) { assertEquals(message, actual.getElement(i, j), expected.getElement(i, j), delta); } } }
public static void matrix3DToDenseMatrix(Matrix3d matrix, DenseMatrix64F denseMatrixToPack, int startRow, int startColumn) { for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { denseMatrixToPack.unsafe_set(row + startRow, column + startColumn, matrix.getElement(row, column)); } } }
public static void setDenseMatrixFromMatrix3d(int startRow, int startColumn, Matrix3d fromMatrix3d, DenseMatrix64F denseMatrix) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { denseMatrix.set(i + startRow, j + startColumn, fromMatrix3d.getElement(i, j)); } } }
private void set3By3MatrixBlock(DenseMatrix64F denseMatrix, int startRow, int startCol,Matrix3d matrix3d) { if(denseMatrix.numRows < 3 || denseMatrix.numCols < 3) { throw new RuntimeException("Your matrix must be 3x3 or larger."); } for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) { denseMatrix.unsafe_set(startRow+i, startCol+j, matrix3d.getElement(i, j)); } } }
public void setOrientationOffset(Matrix3d orientationOffset) { for (int row = 0; row < MATRIX_SIZE; row++) { for (int column = 0; column < MATRIX_SIZE; column++) { String name = getOrientationOffsetPropertyName(row, column); setProperty(name, orientationOffset.getElement(row, column)); } } }
double s = rotationMatrix.getElement(0, 0) + rotationMatrix.getElement(1, 1) + rotationMatrix.getElement(2, 2); if (s > -0.19) quaternionToPack.setX(inv * (rotationMatrix.getElement(2, 1) - rotationMatrix.getElement(1, 2))); quaternionToPack.setY(inv * (rotationMatrix.getElement(0, 2) - rotationMatrix.getElement(2, 0))); quaternionToPack.setZ(inv * (rotationMatrix.getElement(1, 0) - rotationMatrix.getElement(0, 1))); s = rotationMatrix.getElement(0, 0) - rotationMatrix.getElement(1, 1) - rotationMatrix.getElement(2, 2); quaternionToPack.setW(inv * (rotationMatrix.getElement(2, 1) - rotationMatrix.getElement(1, 2))); quaternionToPack.setY(inv * (rotationMatrix.getElement(1, 0) + rotationMatrix.getElement(0, 1))); quaternionToPack.setZ(inv * (rotationMatrix.getElement(2, 0) + rotationMatrix.getElement(0, 2))); s = rotationMatrix.getElement(1, 1) - rotationMatrix.getElement(0, 0) - rotationMatrix.getElement(2, 2); quaternionToPack.setW(inv * (rotationMatrix.getElement(0, 2) - rotationMatrix.getElement(2, 0))); quaternionToPack.setX(inv * (rotationMatrix.getElement(1, 0) + rotationMatrix.getElement(0, 1))); quaternionToPack.setZ(inv * (rotationMatrix.getElement(1, 2) + rotationMatrix.getElement(2, 1))); s = rotationMatrix.getElement(2, 2) - rotationMatrix.getElement(0, 0) - rotationMatrix.getElement(1, 1); quaternionToPack.setZ(0.5 * FastMath.sqrt(s + 1.0)); double inv = 0.25 / quaternionToPack.getZ(); quaternionToPack.setW(inv * (rotationMatrix.getElement(1, 0) - rotationMatrix.getElement(0, 1))); quaternionToPack.setX(inv * (rotationMatrix.getElement(2, 0) + rotationMatrix.getElement(0, 2))); quaternionToPack.setY(inv * (rotationMatrix.getElement(1, 2) + rotationMatrix.getElement(2, 1)));
/** * @return true if this transformation holds an identity matrix (e.g. doesn't transform at all). */ @Override public boolean isIdentity() { if ( numRow != numCol ) { return false; } for ( int row = 0; row < numRow; row++ ) { for ( int col = 0; col < numCol; col++ ) { double value = ( matrix3D != null ) ? matrix3D.getElement( row, col ) : ( ( matrix4D != null ) ? matrix4D.getElement( row, col ) : matrix.getElement( row, col ) ); if ( Math.abs( value - ( col == row ? 1 : 0 ) ) > EPS11 ) { return false; } } } return true; }
for (int i=0;i<3;i++) { for (int j=0;j<3;j++) { if (!deltaComp(mtranspose.getElement(i, j),scaleMatrix.getElement(i, j))) {
private void getTransformedWeights(FrameVector2d weightsToPack, double forwardWeight, double lateralWeight) { RigidBodyTransform transform = contactableFeet.get(supportSide.getEnumValue()).getSoleFrame().getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotation.transpose(); transformTranspose.setRotation(rotationTranspose); weightsMatrix.setZero(); weightsMatrix.setElement(0, 0, forwardWeight); weightsMatrix.setElement(1, 1, lateralWeight); weightsMatrixTransformed.set(rotation); weightsMatrixTransformed.mul(weightsMatrix); weightsMatrixTransformed.mul(rotationTranspose); weightsToPack.setToZero(worldFrame); weightsToPack.setX(weightsMatrixTransformed.getElement(0, 0)); weightsToPack.setY(weightsMatrixTransformed.getElement(1, 1)); }
private void getTransformedFeedbackGains(FrameVector2d feedbackGainsToPack) { double epsilonZeroICPVelocity = 1e-5; if (desiredICPVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) { icpVelocityDirectionFrame.setXAxis(desiredICPVelocity); RigidBodyTransform transform = icpVelocityDirectionFrame.getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotationTranspose.transpose(); transformTranspose.setRotation(rotationTranspose); gainsMatrix.setZero(); gainsMatrix.setElement(0, 0, 1.0 + feedbackParallelGain.getDoubleValue()); gainsMatrix.setElement(1, 1, 1.0 + feedbackOrthogonalGain.getDoubleValue()); gainsMatrixTransformed.set(rotation); gainsMatrixTransformed.mul(gainsMatrix); gainsMatrixTransformed.mul(rotationTranspose); feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.setX(gainsMatrixTransformed.getElement(0, 0)); feedbackGainsToPack.setY(gainsMatrixTransformed.getElement(1, 1)); } else { feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.set(feedbackOrthogonalGain.getDoubleValue(), feedbackOrthogonalGain.getDoubleValue()); } yoFeedbackGains.set(feedbackGainsToPack); }
corr.setM00(rotationMatrixResultToPack.getElement(0, 0) - x.getM00()); corr.setM01(rotationMatrixResultToPack.getElement(1, 0) - x.getM01()); corr.setM02(rotationMatrixResultToPack.getElement(2, 0) - x.getM02()); corr.setM10(rotationMatrixResultToPack.getElement(0, 1) - x.getM10()); corr.setM11(rotationMatrixResultToPack.getElement(1, 1) - x.getM11()); corr.setM12(rotationMatrixResultToPack.getElement(2, 1) - x.getM12()); corr.setM20(rotationMatrixResultToPack.getElement(0, 2) - x.getM20()); corr.setM21(rotationMatrixResultToPack.getElement(1, 2) - x.getM21()); corr.setM22(rotationMatrixResultToPack.getElement(2, 2) - x.getM22()); x.setM00(rotationMatrixResultToPack.getElement(0, 0)); x.setM01(rotationMatrixResultToPack.getElement(1, 0)); x.setM02(rotationMatrixResultToPack.getElement(2, 0)); x.setM10(rotationMatrixResultToPack.getElement(0, 1)); x.setM11(rotationMatrixResultToPack.getElement(1, 1)); x.setM12(rotationMatrixResultToPack.getElement(2, 1)); x.setM20(rotationMatrixResultToPack.getElement(0, 2)); x.setM21(rotationMatrixResultToPack.getElement(1, 2)); x.setM22(rotationMatrixResultToPack.getElement(2, 2)); fn = fn1;