public RigidBodyTransform getSweepTransform(int i) { if (i >= params.pointsPerSweep * params.scanHeight) { throw new IndexOutOfBoundsException("Index " + i + " greater than or equal to pointsPerSweep " + params.pointsPerSweep + " * scanHeight " + params.scanHeight); } double yawPerIndex = (params.sweepYawMax - params.sweepYawMin) / (params.pointsPerSweep - 1); double pitchPerIndex = (params.heightPitchMax - params.heightPitchMin) / (params.scanHeight - 1); RigidBodyTransform sweepTransform = new RigidBodyTransform(); if (params.pointsPerSweep > 1) { //sweepTransform.rotZ(params.sweepYawMin + yawPerIndex * i); sweepTransform.setRotationYawAndZeroTranslation(params.sweepYawMin + yawPerIndex * (i % params.pointsPerSweep)); } if (params.scanHeight > 1) { sweepTransform.setRotationPitchAndZeroTranslation(params.heightPitchMin + pitchPerIndex * (i / params.pointsPerSweep)); } return sweepTransform; }
private static RigidBodyTransform createRandomTransform(Random random) { RigidBodyTransform transformReturn = new RigidBodyTransform(); RigidBodyTransform transformTemp = new RigidBodyTransform(); transformReturn.setRotationRollAndZeroTranslation(random.nextDouble()); transformTemp.setRotationPitchAndZeroTranslation(random.nextDouble()); transformReturn.multiply(transformTemp); transformTemp.setRotationYawAndZeroTranslation(random.nextDouble()); transformReturn.multiply(transformTemp); transformReturn.setTranslation(new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble())); return transformReturn; }
private RigidBodyTransform randomTransform(Random random) { RigidBodyTransform transform = new RigidBodyTransform(); RigidBodyTransform tempTransform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(2 * Math.PI * random.nextDouble()); tempTransform.setRotationPitchAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); tempTransform.setRotationYawAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); double[] matrix = new double[16]; transform.get(matrix); matrix[3] = random.nextDouble(); matrix[7] = random.nextDouble(); matrix[11] = random.nextDouble(); return transform; } }
private static void computeRotationTransform(RigidBodyTransform transform3DToPack, double rotationAngle, Axis rotationAxis) { transform3DToPack.setIdentity(); switch(rotationAxis) { case X: { transform3DToPack.setRotationRollAndZeroTranslation(rotationAngle); break; } case Y: { transform3DToPack.setRotationPitchAndZeroTranslation(rotationAngle); break; } case Z: { transform3DToPack.setRotationYawAndZeroTranslation(rotationAngle); break; } default: { throw new RuntimeException("Shouldn't get here."); } } }
private static Point3D transformFromAngledToWorldFrame(Ramp3D ramp, Point3D point) { RigidBodyTransform angleTransform = new RigidBodyTransform(); angleTransform.setRotationPitchAndZeroTranslation(-ramp.getRampIncline()); angleTransform.transform(point); ramp.transformToWorld(point); return new Point3D(point); }
private void setUpSlopedCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(new Vector3D(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject(new Box3D(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(new Vector3D(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject( new Box3D(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpRampBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; setUpCinderBlockSquare(combinedTerrainObject, xCenter, yCenter, numberFlatSupports - 1, yawDegrees); double xOffset = 0, yOffset = -cinderBlockWidth; double[] xyRotated = rotateAroundOrigin(new double[] {xOffset, yOffset}, yawDegrees); RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject( new Box3D(location, cinderBlockLength, 2.0 * cinderBlockWidth, cinderBlockHeight), cinderBlockAppearance); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3D(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3D(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
@Test public void testIndependenceOfCopiedTransforms() { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(Math.PI / 6); Ramp3D ramp = new Ramp3D(transform, 1.0, 1.0, 1.0); Ramp3D rampCopy = new Ramp3D(ramp); RigidBodyTransform transformAppliedOnlyToCopy = new RigidBodyTransform(); transformAppliedOnlyToCopy.setRotationPitchAndZeroTranslation(Math.PI / 4); rampCopy.applyTransform(transformAppliedOnlyToCopy); assertFalse(rampCopy.equals(ramp)); Ramp3D rampCopyBySet = new Ramp3D(6.0, 5.0, 7.0); rampCopyBySet.set(ramp); RigidBodyTransform transformAppliedOnlyToCopyBySet = new RigidBodyTransform(); transformAppliedOnlyToCopyBySet.setRotationYawAndZeroTranslation(Math.PI / 5); rampCopyBySet.applyTransform(transformAppliedOnlyToCopyBySet); assertFalse(rampCopyBySet.equals(ramp)); }
@Test public void testIndependenceOfCopiedTransforms() { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(Math.PI / 6); Torus3D torus = new Torus3D(transform, 7.0, 2.0); Torus3D torusCopy = new Torus3D(torus); RigidBodyTransform transformAppliedOnlyToCopy = new RigidBodyTransform(); transformAppliedOnlyToCopy.setRotationPitchAndZeroTranslation(Math.PI / 4); torusCopy.applyTransform(transformAppliedOnlyToCopy); assertFalse(torusCopy.equals(torus)); Torus3D torusCopyBySet = new Torus3D(5.0, 1.0); torusCopyBySet.set(torus); RigidBodyTransform transformAppliedOnlyToCopyBySet = new RigidBodyTransform(); transformAppliedOnlyToCopyBySet.setRotationYawAndZeroTranslation(Math.PI / 5); torusCopyBySet.applyTransform(transformAppliedOnlyToCopyBySet); assertFalse(torusCopyBySet.equals(torus)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotate() { RigidBodyTransform transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, Math.PI / 4, Axis.X); RigidBodyTransform transform2 = new RigidBodyTransform(); transform2.setRotationRollAndZeroTranslation(Math.PI / 4); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, 3 * Math.PI / 4, Axis.Y); transform2 = new RigidBodyTransform(); transform2.setRotationPitchAndZeroTranslation(3 * Math.PI / 4); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, -Math.PI / 2, Axis.Z); transform2 = new RigidBodyTransform(); transform2.setRotationYawAndZeroTranslation(-Math.PI / 2); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); }
transform.setRotationRollAndZeroTranslation(angle); if (i == 1) transform.setRotationPitchAndZeroTranslation(angle); if (i == 2) transform.setRotationYawAndZeroTranslation(angle);
jointTransformUpdater = transform -> transform.setRotationPitchAndZeroTranslation(joint.getQ());
@Test public void testApplyTransform() { RigidBodyTransform transformation = new RigidBodyTransform(); transformation.setRotationYawAndZeroTranslation(2.3); Plane3D plane = new Plane3D(0.0, 0.0, 0.0, 0.0, 0.0, 1.0); plane.applyTransform(transformation); EuclidCoreTestTools.assertTuple3DEquals(plane.getNormalCopy(), new Vector3D(0.0, 0.0, 1.0), 1e-14); EuclidCoreTestTools.assertTuple3DEquals(plane.getPointCopy(), new Point3D(0.0, 0.0, 0.0), 1e-14); RigidBodyTransform transformation2 = new RigidBodyTransform(); transformation2.setTranslation(new Vector3D(1.0, 2.0, 3.0)); Plane3D plane2 = new Plane3D(0.0, 0.0, 0.0, 0.0, 0.0, 1.0); plane2.applyTransform(transformation2); EuclidCoreTestTools.assertTuple3DEquals(plane2.getNormalCopy(), new Vector3D(0.0, 0.0, 1.0), 1e-14); EuclidCoreTestTools.assertTuple3DEquals(plane2.getPointCopy(), new Point3D(1.0, 2.0, 3.0), 1e-14); RigidBodyTransform transformation3 = new RigidBodyTransform(); transformation3.setRotationPitchAndZeroTranslation(Math.PI / 2); transformation3.setTranslation(new Vector3D(1.0, 2.0, 3.0)); Plane3D plane3 = new Plane3D(0.0, 0.0, 0.0, 0.0, 0.0, 1.0); plane3.applyTransform(transformation3); EuclidCoreTestTools.assertTuple3DEquals(plane3.getNormalCopy(), new Vector3D(1.0, 0.0, 0.0), 1e-14); EuclidCoreTestTools.assertTuple3DEquals(plane3.getPointCopy(), new Point3D(1.0, 2.0, 3.0), 1e-14); RigidBodyTransform transformation4 = new RigidBodyTransform(); transformation4.setRotationPitchAndZeroTranslation(Math.PI / 2); transformation4.setTranslation(new Vector3D(1.0, 2.0, 3.0)); }
transformFromActuatorSlide5FrameToBoneFrame.setRotationPitchAndZeroTranslation(-actuatorSlider5PitchRotation); transformFromActuatorSlide5FrameToBoneFrame.setTranslation(new Vector3D(rod5PointInBoneFrame)); transformFromActuatorSlide6FrameToBoneFrame.setRotationPitchAndZeroTranslation(-actuatorSlider6PitchRotation); transformFromActuatorSlide6FrameToBoneFrame.setTranslation(new Vector3D(rod6PointInBoneFrame));
transforms.add(new RigidBodyTransform(tr_base)); tr_other.setRotationPitchAndZeroTranslation(1.2); tr_base.multiply(tr_other); tr_other.setRotationYawAndZeroTranslation(-0.6);
transformation3.setRotationPitchAndZeroTranslation(Math.PI / 2); transformation3.setTranslation(new Vector3D(1.0, 2.0, 3.0)); FramePlane3d plane3 = new FramePlane3d(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); transformation4.setRotationPitchAndZeroTranslation(Math.PI / 2); transformation4.setTranslation(new Vector3D(1.0, 2.0, 3.0)); FramePlane3d plane4 = new FramePlane3d(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0));
transform3D.setRotationPitchAndZeroTranslation(-0.7); transform3D.setTranslation(new Vector3D(0.1, 0.2, 0.3));