@Override public QuaternionBasedTransform createData() { return new QuaternionBasedTransform(); }
public TransformStamped() { header_ = new std_msgs.msg.dds.Header(); child_frame_id_ = new java.lang.StringBuilder(255); transform_ = new us.ihmc.euclid.transform.QuaternionBasedTransform(); }
@Override public QuaternionBasedTransform createRandomTransform2D(Random random) { RigidBodyTransform rTransform2D = new RigidBodyTransform(); rTransform2D.setRotationYaw(2.0 * Math.PI * random.nextDouble() - Math.PI); rTransform2D.setTranslation(EuclidCoreRandomTools.nextVector3D(random)); QuaternionBasedTransform qTransform2D = new QuaternionBasedTransform(rTransform2D); return qTransform2D; } }
QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); double x = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); double z = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); Tuple3DReadOnly translation = EuclidCoreRandomTools.nextPoint3D(random, 10.0, 10.0, 10.0); translationTransform.setTranslation(translation);
@Test public void testTransformWithQuaternionBasedTransform() throws Exception { Random random = new Random(23423L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform originalRigidBodyTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expectedRigidBodyTransform = new RigidBodyTransform(); QuaternionBasedTransform original = new QuaternionBasedTransform(originalRigidBodyTransform); QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); transform.transform(originalRigidBodyTransform, expectedRigidBodyTransform); expected.set(expectedRigidBodyTransform); transform.transform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.transform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); RigidBodyTransform inverse = new RigidBodyTransform(transform); inverse.invert(); inverse.transform(original, expected); transform.inverseTransform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.inverseTransform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); }
@Test public void testTransformWithQuaternionBasedTransform() throws Exception { Random random = new Random(23423L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform originalRigidBodyTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expectedRigidBodyTransform = new RigidBodyTransform(); QuaternionBasedTransform original = new QuaternionBasedTransform(originalRigidBodyTransform); QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); transform.transform(originalRigidBodyTransform, expectedRigidBodyTransform); expected.set(expectedRigidBodyTransform); transform.transform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.transform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); RigidBodyTransform inverse = new RigidBodyTransform(transform); inverse.invert(); inverse.transform(original, expected); transform.inverseTransform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.inverseTransform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); }
@Test public void testInvert() throws Exception { Random random = new Random(345L); QuaternionBasedTransform original = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); rigidBodyTransform.set(original); rigidBodyTransform.invert(); expected.set(rigidBodyTransform); actual.set(original); actual.invert(); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); rigidBodyTransform.set(original); rigidBodyTransform.invertRotation(); expected.set(rigidBodyTransform); actual.set(original); actual.invertRotation(); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); }
@Test public void testTransformWithQuaternionBasedTransform() throws Exception { Random random = new Random(23423L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform originalRigidBodyTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expectedRigidBodyTransform = new RigidBodyTransform(); QuaternionBasedTransform original = new QuaternionBasedTransform(originalRigidBodyTransform); QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); transform.transform(originalRigidBodyTransform, expectedRigidBodyTransform); expected.set(expectedRigidBodyTransform); transform.transform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.transform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); RigidBodyTransform inverse = new RigidBodyTransform(); transform.getRigidBodyTransform(inverse); inverse.invert(); inverse.transform(original, expected); transform.inverseTransform(original, actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(original); transform.inverseTransform(actual); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); }
@Test public void testToString() throws Exception { Random random = new Random(12345L); QuaternionBasedTransform quaternionA; QuaternionBasedTransform quaternionB; for (int i = 0; i < ITERATIONS; ++i) { quaternionA = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); quaternionB = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); assertNotEquals(quaternionA.toString(), quaternionB.toString()); } for (int i = 0; i < ITERATIONS; ++i) { quaternionA = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); quaternionB = new QuaternionBasedTransform(quaternionA); assertEquals(quaternionA.toString(), quaternionB.toString()); } }
@Test public void testHashCode() throws Exception { Random random = new Random(12345L); Quaternion quaternion; Vector3D translation; QuaternionBasedTransform qbt = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); int previousHashCode, newHashCode; newHashCode = qbt.hashCode(); assertEquals(newHashCode, qbt.hashCode()); previousHashCode = qbt.hashCode(); for (int i = 0; i < ITERATIONS; ++i) { quaternion = EuclidCoreRandomTools.nextQuaternion(random); translation = EuclidCoreRandomTools.nextVector3D(random); qbt = new QuaternionBasedTransform(quaternion, translation); newHashCode = qbt.hashCode(); assertNotEquals(previousHashCode, newHashCode); previousHashCode = newHashCode; } }
@Test public void testPreMultiplyInvertOtherWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform inverseOfMultipliedWithRigidBody = new RigidBodyTransform(multipliedWithRigidBody); inverseOfMultipliedWithRigidBody.invert(); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.preMultiply(inverseOfMultipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertOther(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
QuaternionBasedTransform actual = new QuaternionBasedTransform(expected); assertAssertionMethodsBehaveProperly(false, methodName, argumentsClass, expected, actual, EPSILON);
@Test public void testMultiplyInvertOtherWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform inverseOfMultipliedWithRigidBody = new RigidBodyTransform(multipliedWithRigidBody); inverseOfMultipliedWithRigidBody.invert(); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.multiply(inverseOfMultipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.multiplyInvertOther(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.multiplyInvertOther(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThisWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.invert(); expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiplyInvertThis(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertThis(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertThisWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.invert(); expected.multiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.multiplyInvertThis(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.multiplyInvertThis(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); multipliedWith.set(actual); multipliedWith.invert(); actual.preMultiply(multipliedWith); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
EuclidCoreTestTools.assertTuple3DEquals(pointExpected, pointActual, EPSILON); QuaternionBasedTransform invertedTransform = new QuaternionBasedTransform(transform); invertedTransform.invert(); invertedTransform.transform(pointOriginal, pointExpected); EuclidCoreTestTools.assertTuple3DEquals(vectorExpected, vectorActual, EPSILON); QuaternionBasedTransform invertedTransform = new QuaternionBasedTransform(transform); invertedTransform.invert(); invertedTransform.transform(vectorOriginal, vectorExpected);
@Test public void testMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.multiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.multiply(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); multipliedWith.set(actual); multipliedWith.invert(); actual.multiply(multipliedWith); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
QuaternionBasedTransform actualTransform = new QuaternionBasedTransform(); Quaternion expectedQuaternion = new Quaternion(); actualTransform.getRotation(expectedQuaternion);
@Test public void testContainsNaN() throws Exception { QuaternionBasedTransform transform = new QuaternionBasedTransform(); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertFalse(transform.containsNaN()); transform.setUnsafe(Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN); assertTrue(transform.containsNaN()); }