public void setTransformFromWorld(AffineTransform transformFromWorld, double time) { synchronized (lidarTransform) { transformFromWorld.getRigidBodyTransform(lidarTransform); this.time = time; } }
@Test public void testTransformWithOtherRigidBodyTransform() throws Exception { Random random = new Random(23423L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = new RigidBodyTransform(); RigidBodyTransform actual = new RigidBodyTransform(); transform.getRigidBodyTransform(expected); expected.multiply(original); transform.transform(original, actual); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.set(original); transform.transform(actual); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); RigidBodyTransform inverse = new RigidBodyTransform(); transform.getRigidBodyTransform(inverse); inverse.invert(); inverse.transform(original, expected); transform.inverseTransform(original, actual); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.set(original); transform.inverseTransform(actual); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); }
@Test public void testMultiply() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertOtherWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertThisWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertOtherWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertOtherWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertThis() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThis() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertOther() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertOther() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThisWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThisWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertThisWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertThis(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testMultiplyInvertOtherWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.multiplyInvertOther(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.multiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiply() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(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); }