@Override public Vector4D createTuple(double x, double y, double z, double s) { return new Vector4D(x, y, z, s); }
@Override public Vector4D createEmptyTuple() { return new Vector4D(); }
@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); } }
@Test public void testTransformVector4D() throws Exception { Random random = new Random(435L); Vector4D actual = new Vector4D(); Vector4D expected = new Vector4D(); for (int i = 0; i < ITERATIONS; i++) { RotationMatrix matrix = EuclidCoreRandomTools.nextRotationMatrix(random); 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(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); }
@Test public void testInverseTransformVector4D() 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.inverseTransform(original, expected); actual.set(original); matrix.inverseTransform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); actual.setToNaN(); matrix.inverseTransform(original, actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
@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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConversionQDotToAngularVelocityBackAndForth() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularVelocity.negate(); Vector3D actualAngularVelocity = new Vector3D(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDot(q, expectedAngularVelocity, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, EPSILON)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
public static FrameVector3D computeAngularVelocityByFiniteDifference(double dt, ReferenceFrame bodyFrame, ReferenceFrame bodyFrameInFuture) { FrameQuaternion bodyOrientation = new FrameQuaternion(bodyFrame); bodyOrientation.changeFrame(worldFrame); FrameQuaternion bodyOrientationInFuture = new FrameQuaternion(bodyFrameInFuture); bodyOrientationInFuture.changeFrame(worldFrame); FrameVector3D bodyAngularVelocity = new FrameVector3D(worldFrame); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDotByFiniteDifferenceCentral(bodyOrientation, bodyOrientationInFuture, 0.5 * dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(bodyOrientation, qDot, bodyAngularVelocity); bodyAngularVelocity.changeFrame(bodyFrame); return bodyAngularVelocity; }
Vector4D actual = new Vector4D(expected); assertAssertionMethodsBehaveProperly(false, methodName, argumentsClass, expected, actual, EPSILON);
Vector4D actual = new Vector4D(expected); assertAssertionMethodsBehaveProperly(false, methodName, argumentsClass, expected, actual, EPSILON);
/** * 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); }
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 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 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); }
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(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); }
Vector4D product = new Vector4D();
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIntegrateToQuaternion() throws Exception { for (int i = 0; i < 100; i++) { Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random); Vector3D actualAngularVelocity = new Vector3D(); Quaternion integrationResultPrevious = new Quaternion(); Quaternion integrationResultCurrent = new Quaternion(); Quaternion integrationResultNext = new Quaternion(); Vector4D qDot = new Vector4D(); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); double dt = 1.0e-4; for (double t = dt; t <= 1.0; t += dt) { RotationTools.integrateAngularVelocity(expectedAngularVelocity, t - dt, integrationResultPrevious); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t, integrationResultCurrent); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t + dt, integrationResultNext); quaternionCalculus.computeQDotByFiniteDifferenceCentral(integrationResultPrevious, integrationResultNext, dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(integrationResultCurrent, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, 1.0e-7)); } } }