public void rotate(Matrix3d rotation) { transformGenerator.rotate(rotation); }
public void identity() { transformGenerator.identity(); }
public void rotateEuler(Vector3d eulerAngles) { transformGenerator.rotateEuler(eulerAngles); }
RigidBodyTransformGenerator generator = new RigidBodyTransformGenerator(); generator.translate(1.0, 0.0, 0.0); RigidBodyTransform expectedTransform = new RigidBodyTransform(); expectedTransform.setTranslation(1.0, 0.0, 0.0); RigidBodyTransform transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.translate(2.0, 0.0, 0.0); expectedTransform = new RigidBodyTransform(); expectedTransform.setRotationEulerAndZeroTranslation(0.0, -Math.PI / 2.0, 0.0); expectedTransform.setTranslation(1.0, 0.0, 2.0); transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.rotate(Math.PI / 2.0, Axis.Z); generator.translate(3.0, 0.0, 0.0); expectedTransform = new RigidBodyTransform(); expectedTransform.setRotationEulerAndZeroTranslation(-Math.PI / 2.0, 0.0, Math.PI / 2.0); expectedTransform.setTranslation(1.0, 3.0, 2.0); transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.identity(); generator.translateThenRotateEuler(new Vector3D(1.0, 0.0, 0.0), new Vector3D(0.0, -Math.PI / 2.0, 0.0)); generator.translateThenRotate(translateThenRotate);
RigidBodyTransformGenerator generator = new RigidBodyTransformGenerator(); RigidBodyTransform identity = new RigidBodyTransform(); RigidBodyTransform transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(identity, 1e-10)); generator.translate(1.0, 2.0, 3.0); RigidBodyTransform expectedTransform = new RigidBodyTransform(); expectedTransform.setTranslation(1.0, 2.0, 3.0); generator.getRigidyBodyTransform(transform); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.translate(4.0, 5.0, 6.0); expectedTransform = new RigidBodyTransform(); expectedTransform.setTranslation(5.0, 7.0, 9.0); generator.getRigidyBodyTransform(transform); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.identity(); generator.translate(new Vector3D(-3.0, 9.0, 11.0)); expectedTransform = new RigidBodyTransform(); expectedTransform.setTranslation(-3.0, 9.0, 11.0); generator.getRigidyBodyTransform(transform); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); generator.identity(); generator.rotate(1.7, Axis.Z); expectedTransform = new RigidBodyTransform(); expectedTransform.setRotationEulerAndZeroTranslation(0.0, 0.0, 1.7);
public void addCubeReferencedAtCenter(double lengthX, double widthY, double heightZ) { RigidBodyTransformGenerator transformGeneratorTwo = new RigidBodyTransformGenerator(transformGenerator); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, 0.0, heightZ / 2.0); addRectangle(transformGeneratorTwo, lengthX, widthY); transformGeneratorTwo.translate(0.0, 0.0, -heightZ); transformGeneratorTwo.rotate(Math.PI, Axis.Y); addRectangle(transformGeneratorTwo, lengthX, widthY); transformGeneratorTwo.rotate(Math.PI, Axis.Y); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.rotate(Math.PI / 2.0, Axis.Y); transformGeneratorTwo.translate(0.0, 0.0, lengthX / 2.0); addRectangle(transformGeneratorTwo, heightZ, widthY); transformGeneratorTwo.translate(0.0, 0.0, -lengthX); transformGeneratorTwo.rotate(Math.PI, Axis.Y); addRectangle(transformGeneratorTwo, heightZ, widthY); transformGeneratorTwo.rotate(Math.PI, Axis.Y); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.rotate(Math.PI / 2.0, Axis.X); transformGeneratorTwo.translate(0.0, 0.0, widthY / 2.0); addRectangle(transformGeneratorTwo, lengthX, heightZ); transformGeneratorTwo.translate(0.0, 0.0, -widthY); transformGeneratorTwo.rotate(Math.PI, Axis.X); addRectangle(transformGeneratorTwo, lengthX, heightZ); transformGeneratorTwo.rotate(Math.PI, Axis.X); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testTranslateThenRotateTransformGeneration() { RigidBodyTransformGenerator generator = new RigidBodyTransformGenerator(); generator.translate(3.3, 4.4, 5.5); generator.rotateEuler(new Vector3D(0.67, 0.89, 0.34)); RigidBodyTransform expectedTransform = new RigidBodyTransform(); expectedTransform.setRotationEulerAndZeroTranslation(0.67, 0.89, 0.34); expectedTransform.setTranslation(3.3, 4.4, 5.5); RigidBodyTransform transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); }
RigidBodyTransformGenerator generator = new RigidBodyTransformGenerator(); generator.rotate(yaw, Axis.Z); RigidBodyTransform transform = generator.getRigidBodyTransformCopy();
public void addPolygon(ConvexPolygon2d polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void translate(double x, double y, double z) { transformGenerator.translate(x, y, z); }
public void setTransform(RigidBodyTransform transform) { transformGenerator.setTransform(transform); }
public void translateThenRotate(RigidBodyTransform transform) { transformGenerator.translateThenRotate(transform); }
public void addCubeReferencedAtCenter(double lengthX, double widthY, double heightZ) { RigidBodyTransformGenerator transformGeneratorTwo = new RigidBodyTransformGenerator(transformGenerator); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, 0.0, heightZ / 2.0); addRectangle(transformGeneratorTwo, lengthX, widthY); transformGeneratorTwo.translate(0.0, 0.0, -heightZ); addRectangle(transformGeneratorTwo, lengthX, widthY); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.rotate(Math.PI / 2.0, Axis.Y); transformGeneratorTwo.translate(0.0, 0.0, lengthX / 2.0); addRectangle(transformGeneratorTwo, heightZ, widthY); transformGeneratorTwo.translate(0.0, 0.0, -lengthX); addRectangle(transformGeneratorTwo, heightZ, widthY); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.rotate(Math.PI / 2.0, Axis.X); transformGeneratorTwo.translate(0.0, 0.0, widthY / 2.0); addRectangle(transformGeneratorTwo, lengthX, heightZ); transformGeneratorTwo.translate(0.0, 0.0, -widthY); addRectangle(transformGeneratorTwo, lengthX, heightZ); }
RigidBodyTransformGenerator transformGenerator = new RigidBodyTransformGenerator(); transformGenerator.rotate(Math.PI / 4.0, Axis.Z); RigidBodyTransform transform = transformGenerator.getRigidBodyTransformCopy(); Point3D startPosition = new Point3D(-6.0, 0.0, 0.0); transform.transform(startPosition);
public void addPolygon(ConvexPolygon2D polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void translate(double x, double y, double z) { transformGenerator.translate(x, y, z); }
public RigidBodyTransformGenerator(RigidBodyTransformGenerator generator) { setTransform(generator.transform); }
public void translateThenRotate(RigidBodyTransform transform) { transformGenerator.translateThenRotate(transform); }
public void addRampReferencedAtBottomMiddle(double lengthX, double widthY, double heightZ) { RigidBodyTransformGenerator transformGeneratorTwo = new RigidBodyTransformGenerator(transformGenerator); double slope = Math.atan2(heightZ, lengthX); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(lengthX / 2.0, 0.0, heightZ / 2.0); transformGeneratorTwo.rotate(-slope, Axis.Y); addRectangle(transformGeneratorTwo, EuclidGeometryTools.pythagorasGetHypotenuse(lengthX, heightZ), widthY); ConvexPolygon2D leftSide = new ConvexPolygon2D(); leftSide.addVertex(0.0, 0.0); leftSide.addVertex(- lengthX, 0.0); leftSide.addVertex(- lengthX, heightZ); leftSide.update(); ConvexPolygon2D rightSide = new ConvexPolygon2D(); rightSide.addVertex(0.0, 0.0); rightSide.addVertex(lengthX, 0.0); rightSide.addVertex(lengthX, heightZ); rightSide.update(); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, 0.5 * widthY, 0.0); transformGeneratorTwo.rotate(0.5 * Math.PI, Axis.X); transformGeneratorTwo.rotate(Math.PI, Axis.Y); addPolygon(transformGeneratorTwo, leftSide); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, -0.5 * widthY, 0.0); transformGeneratorTwo.rotate(0.5 * Math.PI, Axis.X); addPolygon(transformGeneratorTwo, rightSide); }
public void addPolygons(ArrayList<ConvexPolygon2D> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }