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."); } } }
public TransformRayDemo() incrementalRotation.setRotationRollAndZeroTranslation(-Math.PI / 16); graphic.addCylinder(5.0, 0.2, YoAppearance.Red());
private static void setUpCinderBlockUpright(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; // wall RigidBodyTransform location = new RigidBodyTransform(); RigidBodyTransform setUpright = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); setUpright.setRotationRollAndZeroTranslation(Math.toRadians(90)); location.multiply(setUpright); location.setTranslation(new Vector3D(xCenter, yCenter, cinderBlockWidth / 2 + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject( new Box3D(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
@Test public void testExapleUsage() { double radius = 1.0; double thickness = 0.1; RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(Math.PI / 2.0); transform.setTranslation(new Vector3D(2.0, 0.0, 3.0)); Torus3D torus3d = new Torus3D(transform, radius, thickness); Point3D pointToCheck = new Point3D(2.0, 0.0, 4.0); assertTrue(torus3d.isInsideOrOnSurface(pointToCheck)); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newValvePose = new RigidBodyTransform(); pinJointTransform.setRotationRollAndZeroTranslation(valvePinJoint.getQYoVariable().getDoubleValue()); newValvePose.set(originalValvePose); newValvePose.multiply(pinJointTransform); valveFrame.setPoseAndUpdate(newValvePose); super.updateAllGroundContactPointVelocities(); }
@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)); } }
@Test public void testSetMethodSetsUpAllFieldsOfNewRampAccurately() { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(Math.PI / 6); Ramp3D ramp = new Ramp3D(transform, 3.0, 2.0, 4.0); Ramp3D rampCopyBySet = new Ramp3D(6.0, 5.0, 7.0); rampCopyBySet.set(ramp); Point3D pointProjectedOntoRamp = new Point3D(1.0, 0.0, 5.0); Point3D pointProjectedOntoRampCopy = new Point3D(pointProjectedOntoRamp); ramp.orthogonalProjection(pointProjectedOntoRamp); rampCopyBySet.orthogonalProjection(pointProjectedOntoRampCopy); assertEquals(pointProjectedOntoRamp, pointProjectedOntoRampCopy); }
@Test public void testExampleUsage() { double xRadius = 1.0; double yRadius = 2.0; double zRadius = 3.0; RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(Math.PI / 2.0); transform.setTranslation(new Vector3D(0.0, 5.0, 0.0)); Ellipsoid3D ellipsoid = new Ellipsoid3D(transform, xRadius, yRadius, zRadius); assertTrue(ellipsoid.isInsideEpsilon(new Point3D(0.0, 8.0, 0.0), 0.001)); assertTrue(ellipsoid.isInsideEpsilon(new Point3D(0.0, 7.9, 0.0), 0.001)); assertFalse(ellipsoid.isInsideEpsilon(new Point3D(0.0, 8.2, 0.0), 0.001)); assertFalse(ellipsoid.isInsideEpsilon(new Point3D(0.2, 8.2, 0.2), 0.001)); }
@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);
transform.setRotationRollAndZeroTranslation(-Math.PI / 2.0); System.out.println(transform); momentOfInertia = InertiaTools.rotate(transform, momentOfInertia);
jointTransformUpdater = transform -> transform.setRotationRollAndZeroTranslation(joint.getQ());
transform3D.setRotationRollAndZeroTranslation(0.3); transform3D.setRotationPitchAndZeroTranslation(-0.7); transform3D.setTranslation(new Vector3D(0.1, 0.2, 0.3));
actualTransform.setRotationRollAndZeroTranslation(yawPitchRoll[2]); assertTrue(actualTransform.hasRotation()); assertFalse(actualTransform.hasTranslation()); EuclidCoreTestTools.assertTuple3DIsSetToZero(actualTransform.getTranslationVector()); actualTransform.setRotationRollAndZeroTranslation(0.0); assertFalse(actualTransform.hasRotation());