@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
public static Vector4D nextVector4D(Random random, Tuple4DReadOnly lowerBound, Tuple4DReadOnly upperBound) { Vector4D ret = new Vector4D(); ret.setX(RandomNumbers.nextDouble(random, lowerBound.getX(), upperBound.getX())); ret.setY(RandomNumbers.nextDouble(random, lowerBound.getY(), upperBound.getY())); ret.setZ(RandomNumbers.nextDouble(random, lowerBound.getZ(), upperBound.getZ())); ret.setS(RandomNumbers.nextDouble(random, lowerBound.getS(), upperBound.getS())); return ret; }
private Vector3DReadOnly convertToAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot) { // w = qDDot * q^-1 + qDot * qDot^-1 QuaternionTools.multiplyConjugateRight(qDDot, q, tempConvertVector4D); tempConvertVector3D.setX(tempConvertVector4D.getX()); tempConvertVector3D.setY(tempConvertVector4D.getY()); tempConvertVector3D.setZ(tempConvertVector4D.getZ()); QuaternionTools.multiplyConjugateRight(qDot, qDot, tempConvertVector4D); tempConvertVector3D.addX(tempConvertVector4D.getX()); tempConvertVector3D.addY(tempConvertVector4D.getY()); tempConvertVector3D.addZ(tempConvertVector4D.getZ()); tempConvertVector3D.scale(2.0); return tempConvertVector3D; }
Vector4D vectorOriginal = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector4D vectorExpected = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); QuaternionTools.transform(quaternion, vector3D, vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON); vectorActual.set(vectorOriginal); QuaternionTools.transform(quaternion, vectorActual, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON);
Vector4D vectorOriginal = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector4D vectorExpected = new Vector4D(); Matrix3D matrixCopy = new Matrix3D(matrix); vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorOriginalCopy = new Vector4D(vectorOriginal); vectorExpected.set(0.0, 0.0, 0.0, vectorOriginal.getS()); Matrix3DTools.transform(matrix, vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorExpected.set(vectorOriginal); Matrix3DTools.transform(matrix, vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorExpected.setX(matrix.getM00() * vectorOriginal.getX()); vectorExpected.setY(matrix.getM11() * vectorOriginal.getY()); vectorExpected.setZ(matrix.getM22() * vectorOriginal.getZ()); vectorExpected.setS(vectorOriginal.getS()); matrix.get(denseMatrix); for (int index = 0; index < 3; index++) denseVectorOriginal.set(index, vectorOriginal.getElement(index)); vectorExpected.setElement(index, denseVectorTransformed.get(index)); vectorExpected.setS(vectorOriginal.getS()); vectorActual.set(vectorOriginal); Matrix3DTools.transform(matrix, vectorActual, vectorActual);
Vector4D vector = new Vector4D(); assertTrue(vector.getX() == 0.0); assertTrue(vector.getY() == 0.0); assertTrue(vector.getZ() == 0.0); assertTrue(vector.getS() == 0.0); double z = random.nextDouble(); double s = random.nextDouble(); Vector4D vector = new Vector4D(x, y, z, s); assertTrue(vector.getX() == x); assertTrue(vector.getY() == y); assertTrue(vector.getZ() == z); assertTrue(vector.getS() == s); double s = random.nextDouble(); double[] vectorArray = {x, y, z, s}; Vector4D vector = new Vector4D(vectorArray); assertTrue(vector.getX() == x); assertTrue(vector.getY() == y); assertTrue(vector.getZ() == z); assertTrue(vector.getS() == s); Vector4D vector = new Vector4D(quaternion); EuclidCoreTestTools.assertTuple4DEquals(quaternion, vector, EPS); Vector4D vector = new Vector4D(quaternion); EuclidCoreTestTools.assertTuple4DEquals(quaternion, vector, EPS); Vector4D vector = new Vector4D(vector3D);
@Override public Vector4D createEmptyTuple() { return new Vector4D(); }
/** * Computes the convex hull of the intersections of footPolygonInWorldFrame when snapped to planarRegion using * snapTransform * * @param planarRegion * @param footPolygon foot polygon in world frame * @param snapTransform * @return intersection polygon in region frame */ public static ConvexPolygon2D getConvexHullOfPolygonIntersections(PlanarRegion planarRegion, ConvexPolygon2D footPolygon, RigidBodyTransform snapTransform) { ArrayList<ConvexPolygon2D> intersections = new ArrayList<>(); ConvexPolygon2D footPolygonInPlaneFrame = new ConvexPolygon2D(); RigidBodyTransform inverseSnapTransform = new RigidBodyTransform(snapTransform); inverseSnapTransform.invert(); for (int i = 0; i < footPolygon.getNumberOfVertices(); i++) { Point2DReadOnly vertex = footPolygon.getVertex(i); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), 0.0, 1.0); snapTransform.transform(transformPoint); transformPoint.setZ(0.0); footPolygonInPlaneFrame.addVertex(transformPoint.getX() + 1e-10, transformPoint.getY() + 1e-10); } footPolygonInPlaneFrame.update(); planarRegion.getPolygonIntersectionsWhenProjectedVertically(footPolygonInPlaneFrame, intersections); return getConvexHull(intersections); }
@Test public void testInverseTransformWithVector4D() throws Exception { Random random = new Random(3454L); T transform = createRandomTransform(random); { // Test inverseTransform(Vector4DBasics vectorToTransform) Vector4D expected = EuclidCoreRandomTools.nextVector4D(random); Vector4D actual = new Vector4D(); actual.set(expected); transform.transform(actual); transform.inverseTransform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } { // Test inverseTransform(Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) Vector4D expected = EuclidCoreRandomTools.nextVector4D(random); Vector4D actual = new Vector4D(); transform.inverseTransform(expected, actual); transform.transform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
FrameVector4D frameVector4D = new FrameVector4D(randomFrame, randomTuple.getX(), randomTuple.getY(), randomTuple.getZ(), randomTuple.getS()); assertTrue(frameVector4D.getReferenceFrame() == randomFrame); EuclidCoreTestTools.assertTuple4DEquals(randomTuple, frameVector4D, EPSILON); Vector4D randomTuple = EuclidCoreRandomTools.nextVector4D(random); double[] array = new double[4]; randomTuple.get(array); FrameVector4D frameVector4D = new FrameVector4D(randomFrame, array); assertTrue(frameVector4D.getReferenceFrame() == randomFrame);
private PlanarRegion createPlanarRegion(RigidBodyTransform regionToWorldFrameTransform, ConvexPolygon2D desiredFootholdInWorldXYPlane) { RigidBodyTransform worldToRegionTransform = new RigidBodyTransform(regionToWorldFrameTransform); worldToRegionTransform.invert(); ConvexPolygon2D planarRegionPolygon = new ConvexPolygon2D(); for (int i = 0; i < desiredFootholdInWorldXYPlane.getNumberOfVertices(); i++) { Point2DReadOnly vertex = desiredFootholdInWorldXYPlane.getVertex(i); double zHeight = getPlaneZGivenXY(regionToWorldFrameTransform, vertex.getX(), vertex.getY()); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), zHeight, 1.0); worldToRegionTransform.transform(transformPoint); planarRegionPolygon.addVertex(transformPoint.getX(), transformPoint.getY()); } planarRegionPolygon.update(); return new PlanarRegion(regionToWorldFrameTransform, planarRegionPolygon); }
@Test public void testTransformVector4D() throws Exception { Random random = new Random(435L); Vector4D actual = new Vector4D(); Vector4D expected = new Vector4D(); for (int i = 0; i < NUMBER_OF_ITERATIONS; i++) { RotationScaleMatrix matrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D original = EuclidCoreRandomTools.nextVector4D(random); matrix.transform(original, expected); actual.set(original); matrix.transform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); actual.setToNaN(); matrix.transform(original, actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
@Test public void testTransformVector4D() throws Exception { Random random = new Random(34534L); RotationScaleMatrix rotationScaleMatrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D originalVector = EuclidCoreRandomTools.nextVector4D(random); Vector4D actualVector = new Vector4D(); Vector4D expectedVector = new Vector4D(); expectedVector.set(originalVector); expectedVector.scale(rotationScaleMatrix.getScaleX(), rotationScaleMatrix.getScaleY(), rotationScaleMatrix.getScaleZ(), 1.0); rotationScaleMatrix.getRotationMatrix().transform(expectedVector, expectedVector); rotationScaleMatrix.transform(originalVector, actualVector); EuclidCoreTestTools.assertTuple4DEquals(expectedVector, actualVector, EPS); }
Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Quaternion qResult = new Quaternion(); QuaternionTools.multiply(q1, q2, qResult); vectorExpected.set(qResult); if (vectorActual.dot(vectorExpected) < 0.0) vectorActual.negate(); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON); Quaternion qResult = new Quaternion(); QuaternionTools.multiplyConjugateLeft(q1, q2, qResult); vectorExpected.set(qResult); if (vectorActual.dot(vectorExpected) < 0.0) vectorActual.negate(); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON); Quaternion qResult = new Quaternion(); QuaternionTools.multiplyConjugateRight(q1, q2, qResult); vectorExpected.set(qResult); if (vectorActual.dot(vectorExpected) < 0.0) vectorActual.negate(); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON);
/** * Returns the value of the x-component of this vector. * * @return the x-component's value. */ @Override public double getX() { return vector.getX(); }
/** * Returns the value of the y-component of this vector. * * @return the y-component's value. */ @Override public double getY() { return vector.getY(); }
Vector4D product = new Vector4D(); expectedDot = product.getS(); assertEquals(expectedDot, actualDot, getEpsilon()); expectedDot = product.getS(); assertEquals(expectedDot, actualDot, getEpsilon()); expectedDot = product.getS(); assertEquals(expectedDot, actualDot, getEpsilon()); expectedDot = product.getS(); assertEquals(expectedDot, actualDot, getEpsilon());
@Test public void testInverseTransformTuple4D() throws Exception { Random random = new Random(24534L); RotationScaleMatrix rotationScaleMatrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D originalVector = EuclidCoreRandomTools.nextVector4D(random); Vector4D actualVector = new Vector4D(); Vector4D expectedVector = new Vector4D(originalVector); rotationScaleMatrix.transform(originalVector, actualVector); assertFalse(expectedVector.epsilonEquals(actualVector, EPS)); rotationScaleMatrix.inverseTransform(actualVector, actualVector); EuclidCoreTestTools.assertTuple4DEquals(expectedVector, actualVector, EPS); } }
/** * Returns the value of the z-component of this vector. * * @return the z-component's value. */ @Override public double getZ() { return vector.getZ(); }