/** * Sets all the components of this frame tuple to zero and sets the current reference frame to * {@code referenceFrame}. * * @param referenceFrame the new reference frame to be associated with this tuple. */ default void setToZero(ReferenceFrame referenceFrame) { setReferenceFrame(referenceFrame); setToZero(); }
/** * Transforms the given {@code tupleOriginal} by the rotation about the z-axis described by this and * stores the result in {@code tupleTransformed}. * * <pre> * / cos(yaw) -sin(yaw) 0 \ * tupleTransformed = | sin(yaw) cos(yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. */ default void transform(Tuple3DReadOnly tupleOriginal, FrameTuple3DBasics tupleTransformed) { tupleTransformed.setToZero(getReferenceFrame()); Orientation2DReadOnly.super.transform(tupleOriginal, tupleTransformed); }
/** * Performs the inverse of the transform to the given {@code tupleOriginal} by the rotation about * the z-axis described by this and stores the result in {@code tupleTransformed}. * * <pre> * / cos(-yaw) -sin(-yaw) 0 \ * tupleTransformed = | sin(-yaw) cos(-yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. */ default void inverseTransform(Tuple3DReadOnly tupleOriginal, FrameTuple3DBasics tupleTransformed) { tupleTransformed.setToZero(getReferenceFrame()); Orientation2DReadOnly.super.inverseTransform(tupleOriginal, tupleTransformed); }
/** * Transforms the given {@code tupleOriginal} by the rotation about the z-axis described by this and * stores the result in {@code tupleTransformed}. * * <pre> * / cos(yaw) -sin(yaw) 0 \ * tupleTransformed = | sin(yaw) cos(yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. * @throws ReferenceFrameMismatchException if reference frame of {@code this} and * {@code tupleOriginal} do not match. */ default void transform(FrameTuple3DReadOnly tupleOriginal, FrameTuple3DBasics tupleTransformed) { checkReferenceFrameMatch(tupleOriginal); tupleTransformed.setToZero(getReferenceFrame()); Orientation2DReadOnly.super.transform(tupleOriginal, tupleTransformed); }
/** * Performs the inverse of the transform to the given {@code tupleOriginal} by the rotation about * the z-axis described by this and stores the result in {@code tupleTransformed}. * * <pre> * / cos(-yaw) -sin(-yaw) 0 \ * tupleTransformed = | sin(-yaw) cos(-yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. * @throws ReferenceFrameMismatchException if reference frame of {@code this} and * {@code tupleOriginal} do not match. */ default void inverseTransform(FrameTuple3DReadOnly tupleOriginal, FrameTuple3DBasics tupleTransformed) { checkReferenceFrameMatch(tupleOriginal); tupleTransformed.setToZero(getReferenceFrame()); Orientation2DReadOnly.super.inverseTransform(tupleOriginal, tupleTransformed); }
public static void extractFrameTupleFromEJMLVector(FrameTuple3DBasics frameTuple, DenseMatrix64F ejmlVector, ReferenceFrame desiredFrame, int startIndex) { frameTuple.setToZero(desiredFrame); frameTuple.setX(ejmlVector.get(startIndex + 0, 0)); frameTuple.setY(ejmlVector.get(startIndex + 1, 0)); frameTuple.setZ(ejmlVector.get(startIndex + 2, 0)); }
@Test public void testSetToZero() throws Exception { Random random = new Random(234234L); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] referenceFrames = EuclidFrameRandomTools.nextReferenceFrameTree(random); Tuple3DBasics expectedGeometryObject = createRandomFramelessTuple(random); expectedGeometryObject.setToZero(); ReferenceFrame initialFrame = referenceFrames[random.nextInt(referenceFrames.length)]; F frameGeometryObject = createRandomFrameTuple(random, initialFrame); assertEquals(initialFrame, frameGeometryObject.getReferenceFrame()); assertFalse(expectedGeometryObject.epsilonEquals(frameGeometryObject, EPSILON)); frameGeometryObject.setToZero(); EuclidCoreTestTools.assertTuple3DEquals(expectedGeometryObject, frameGeometryObject, EPSILON); frameGeometryObject = createRandomFrameTuple(random, initialFrame); ReferenceFrame newFrame = referenceFrames[random.nextInt(referenceFrames.length)]; assertEquals(initialFrame, frameGeometryObject.getReferenceFrame()); assertFalse(expectedGeometryObject.epsilonEquals(frameGeometryObject, EPSILON)); frameGeometryObject.setToZero(newFrame); assertEquals(newFrame, frameGeometryObject.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(expectedGeometryObject, frameGeometryObject, EPSILON); } }