public void setOrCreate(int i, FrameTuple3DReadOnly frameTuple) { getAndGrowIfNeeded(i).setIncludingFrame(frameTuple); }
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)); }
/** * Sets this tuple's components {@code x}, {@code y}, {@code z} in order from the given array * {@code tupleArray} and sets this tuple frame to {@code referenceFrame}. * * @param referenceFrame the new reference frame for this tuple. * @param tupleArray the array containing the new values for this tuple's components. Not modified. */ default void setIncludingFrame(ReferenceFrame referenceFrame, double[] tupleArray) { setReferenceFrame(referenceFrame); set(tupleArray); }
/** * Sets all the components of this tuple to {@link Double#NaN} and sets the current reference frame * to {@code referenceFrame}. * * @param referenceFrame the new reference frame to be associated with this tuple. */ default void setToNaN(ReferenceFrame referenceFrame) { setReferenceFrame(referenceFrame); setToNaN(); }
/** * 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(); }
/** * 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); }
@Test public void testSetToNaN() throws Exception { Random random = new Random(574); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] referenceFrames = EuclidFrameRandomTools.nextReferenceFrameTree(random); ReferenceFrame initialFrame = referenceFrames[random.nextInt(referenceFrames.length)]; F frameGeometryObject = createRandomFrameTuple(random, initialFrame); assertEquals(initialFrame, frameGeometryObject.getReferenceFrame()); assertFalse(frameGeometryObject.containsNaN()); frameGeometryObject.setToNaN(); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(frameGeometryObject); frameGeometryObject = createRandomFrameTuple(random, initialFrame); ReferenceFrame newFrame = referenceFrames[random.nextInt(referenceFrames.length)]; assertEquals(initialFrame, frameGeometryObject.getReferenceFrame()); assertFalse(frameGeometryObject.containsNaN()); frameGeometryObject.setToNaN(newFrame); assertEquals(newFrame, frameGeometryObject.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(frameGeometryObject); } }
@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); } }
F frameTuple = createRandomFrameTuple(random, initialFrame); Tuple3DBasics tuple = new Vector3D(); assertEquals(initialFrame, frameTuple.getReferenceFrame()); frameTuple.setIncludingFrame(newFrame, x, y, z); tuple.set(x, y, z); assertEquals(newFrame, frameTuple.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(tuple, frameTuple, EPSILON); F frameTuple = createRandomFrameTuple(random, initialFrame); Tuple3DBasics tuple = new Vector3D(); assertEquals(initialFrame, frameTuple.getReferenceFrame()); frameTuple.setIncludingFrame(newFrame, input, z); tuple.set(input, z); assertEquals(newFrame, frameTuple.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(tuple, frameTuple, EPSILON); F frameTuple = createRandomFrameTuple(random, initialFrame); Tuple3DBasics tuple = new Vector3D(); assertEquals(initialFrame, frameTuple.getReferenceFrame()); frameTuple.setIncludingFrame(newFrame, input); tuple.set(input); assertEquals(newFrame, frameTuple.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(tuple, frameTuple, EPSILON); assertEquals(initialFrame, frameTuple.getReferenceFrame()); Exception expectedException = null; frameTuple.setIncludingFrame(newFrame, input); if (expectedException != null)
actual.set(initialFrame, expected); assertEquals(initialFrame, actual.getReferenceFrame()); actual.set(createRandomFramelessTuple(random)); actual.set(differentFrame, createRandomFramelessTuple(random)); fail("Should have thrown a ReferenceFrameMismatchException"); actual.set(initialFrame, expected.getX(), expected.getY(), expected.getZ()); assertEquals(initialFrame, actual.getReferenceFrame()); actual.set(createRandomFramelessTuple(random)); actual.set(differentFrame, random.nextDouble(), random.nextDouble(), random.nextDouble()); fail("Should have thrown a ReferenceFrameMismatchException");
private static FrameVector3D difference(double dt, ReferenceFrame frame, FrameTuple3DBasics before, FrameTuple3DBasics after) { after.checkReferenceFrameMatch(before); FrameVector3D v = new FrameVector3D(after.getReferenceFrame()); v.sub(after, before); v.scale(1 / (2.0 * dt)); v.changeFrame(frame); return v; }
/** * Packs a column of this matrix into a 3D frame tuple. * * @param column the index of the column to pack. * @param columnToPack the tuple in which the column of this matrix is stored. Modified. * @throws ArrayIndexOutOfBoundsException if {@code column} ∉ [0, 2]. */ default void getColumn(int column, FrameTuple3DBasics columnToPack) { columnToPack.setReferenceFrame(getReferenceFrame()); Matrix3DReadOnly.super.getColumn(column, columnToPack); }
/** * 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); }
/** * Packs a row of this matrix into a 3D frame tuple. * * @param row the index of the row to pack. * @param rowToPack the array in which the row of this matrix is stored. Modified. * @throws ArrayIndexOutOfBoundsException if {@code row} ∉ [0, 2]. */ default void getRow(int row, FrameTuple3DBasics rowToPack) { rowToPack.setReferenceFrame(getReferenceFrame()); Matrix3DReadOnly.super.getRow(row, rowToPack); }
/** * Sets this frame tuple to {@code tuple3DReadOnly} and sets its current frame to * {@code referenceFrame}. * * @param referenceFrame the new reference frame for this frame tuple. * @param tuple3DReadOnly the tuple to copy the values from. Not modified. */ default void setIncludingFrame(ReferenceFrame referenceFrame, Tuple3DReadOnly tuple3DReadOnly) { setReferenceFrame(referenceFrame); set(tuple3DReadOnly); }
public void set(int i, FrameTuple3DReadOnly frameTuple) { get(i).setIncludingFrame(frameTuple); }
/** * 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); }
/** * Computes and packs the orientation described by this orientation as the Euler angles. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @param eulerAnglesToPack the tuple in which the Euler angles are stored. Modified. */ default void getEuler(FrameTuple3DBasics eulerAnglesToPack) { eulerAnglesToPack.setReferenceFrame(getReferenceFrame()); getEuler((Tuple3DBasics) eulerAnglesToPack); }
/** * Sets this frame tuple x and y components to {@code tuple2DReadOnly}'s x and y components, the * z-component to the given {@code z}, and sets the current frame to {@code referenceFrame}. * * @param referenceFrame the new reference frame for this frame tuple. * @param tuple2DReadOnly the tuple to copy the values from. Not modified. * @param z the new z-coordinate for this frame tuple. */ default void setIncludingFrame(ReferenceFrame referenceFrame, Tuple2DReadOnly tuple2DReadOnly, double z) { setReferenceFrame(referenceFrame); set(tuple2DReadOnly, z); }
private void unsafeSet(int i, FrameTuple3DReadOnly frameTuple) { unsafeGet(i).setIncludingFrame(frameTuple); }