/** * Packs the components of this vector in an array starting from {@code startIndex}. * <p> * The components are packed in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param startIndex the index in the array where the first component is stored. * @param arrayToPack the array in which this vector is stored. Modified. */ default void get(int startIndex, float[] arrayToPack) { getAngularPart().get(startIndex, arrayToPack); getLinearPart().get(startIndex + 3, arrayToPack); }
/** * Packs the components of this vector in an array starting from {@code startIndex}. * <p> * The components are packed in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param startIndex the index in the array where the first component is stored. * @param arrayToPack the array in which this vector is stored. Modified. */ default void get(int startIndex, double[] arrayToPack) { getAngularPart().get(startIndex, arrayToPack); getLinearPart().get(startIndex + 3, arrayToPack); }
/** * Packs the components of this vector in a column vector starting from {@code startRow} at the * column index {@code column}. * <p> * The components are packed in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param startRow the first row index to start writing in the dense-matrix. * @param column the column index to write in the dense-matrix. * @param matrixToPack the column vector in which this vector is stored. Modified. */ default void get(int startRow, int column, DenseMatrix64F matrixToPack) { getAngularPart().get(startRow, column, matrixToPack); getLinearPart().get(startRow + 3, column, matrixToPack); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { getJointWrench().getAngularPart().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointVelocity(int rowStart, DenseMatrix64F matrixToPack) { getJointTwist().getAngularPart().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointAcceleration(int rowStart, DenseMatrix64F matrixToPack) { getJointAcceleration().getAngularPart().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }