private void update() { frame.getTransformToDesiredFrame(transform, ReferenceFrame.getWorldFrame()); transform.invert(); }
@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); }
public static Transform getInverse(Transform transform) { RigidBodyTransform transform3D = jmeTransformToTransform3D(transform); transform3D.invert(); Transform ret = j3dTransform3DToJMETransform(transform3D); return ret; }
/** * Creates a reference frame with an immutable transform from its parent. * <p> * The {@code transformFromParent} should describe the pose of the parent frame expressed in this * new frame. * </p> * * @param frameName the name of the new frame. * @param parentFrame the parent frame of the new reference frame. * @param transformFromParent the transform that can be used to transform a geometry object from the * parent frame to this frame. Not modified. * @return the new reference frame. */ public static ReferenceFrame constructFrameWithUnchangingTransformFromParent(String frameName, ReferenceFrame parentFrame, RigidBodyTransform transformFromParent) { RigidBodyTransform transformToParent = new RigidBodyTransform(transformFromParent); transformToParent.invert(); return constructFrameWithUnchangingTransformToParent(frameName, parentFrame, transformToParent); }
public static Transform invert(Transform transform) { RigidBodyTransform transform3D = jmeTransformToTransform3D(transform); transform3D.invert(); return transform.set(j3dTransform3DToJMETransform(transform3D)); }
public void addPolygons(RigidBodyTransform transform, List<? extends ConvexPolygon2DReadOnly> convexPolygon2D, AppearanceDefinition appearance) { transform(transform); for (int i = 0; i < convexPolygon2D.size(); i++) { ConvexPolygon2DReadOnly convexPolygon = convexPolygon2D.get(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, -0.0001); addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearance)); } transform = new RigidBodyTransform(transform); transform.invert(); transform(transform); }
/** * Transforms the planar region * * @param rigidBodyTransform transform from current frame to desired frame */ public void transform(RigidBodyTransform fromDesiredToCurrentTransform) { fromLocalToWorldTransform.multiply(fromDesiredToCurrentTransform); fromWorldToLocalTransform.set(fromLocalToWorldTransform); fromWorldToLocalTransform.invert(); updateBoundingBox(); updateConvexHull(); }
private Point2D projectOnPlane(RigidBodyTransform plane, Vector3D point) { RigidBodyTransform planeInv = new RigidBodyTransform(plane); planeInv.invert(); planeInv.transform(point); return new Point2D(point.getX(), point.getY()); }
private Point2D projectOnPlane(RigidBodyTransform plane, Vector3D point) { RigidBodyTransform planeInv = new RigidBodyTransform(plane); planeInv.invert(); planeInv.transform(point); return new Point2D(point.getX(), point.getY()); }
@Test public void testGetTransformToParents() { Random random = new Random(87); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] treeFrame = EuclidFrameRandomTools.nextReferenceFrameTree(random); ReferenceFrame frame = treeFrame[random.nextInt(treeFrame.length)]; checkRepInvariants(frame); ReferenceFrame parent = frame.getParent(); if (parent != null) { RigidBodyTransform transformToParentOne = frame.getTransformToParent(); RigidBodyTransform transformToParentTwo = frame.getTransformToDesiredFrame(parent); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformToParentOne, transformToParentTwo, EPSILON); RigidBodyTransform transformToParentThree = parent.getTransformToDesiredFrame(frame); transformToParentThree.invert(); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformToParentOne, transformToParentThree, EPSILON); } } }
private RigidBodyTransform getWiggleTransformInWorldFrame(RigidBodyTransform wiggleTransformLocalToLocal) { RigidBodyTransform wiggleTransformWorldToWorld = new RigidBodyTransform(); RigidBodyTransform localToWorld = new RigidBodyTransform(); planarRegionToPack.getTransformToWorld(localToWorld); RigidBodyTransform worldToLocal = new RigidBodyTransform(localToWorld); worldToLocal.invert(); wiggleTransformWorldToWorld.set(localToWorld); wiggleTransformWorldToWorld.multiply(wiggleTransformLocalToLocal); wiggleTransformWorldToWorld.multiply(worldToLocal); return wiggleTransformWorldToWorld; }
public void initialize(ReferenceFrame pelvisFrame, MocapRigidBody markerRigidBody) { RigidBodyTransform marker2ToMocapTransform = new RigidBodyTransform(markerRigidBody.getOrientation(), markerRigidBody.getPosition()); RigidBodyTransform worldToPelvisTransform = pelvisFrame.getTransformToWorldFrame(); worldToPelvisTransform.invert(); RigidBodyTransform worldToMocapTransform = new RigidBodyTransform(); worldToMocapTransform.multiply(marker2ToMocapTransform); worldToMocapTransform.multiply(pelvisToMarker2Transform); worldToMocapTransform.multiply(worldToPelvisTransform); mocapFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("mocapFrame", ReferenceFrame.getWorldFrame(), worldToMocapTransform); initialized = true; }
public void addCrossBar() { double height = 2.0 * steeringWheelRadius; double radius = 0.015; double heightAboveWheel = 0.1; FramePose3D crossBar = new FramePose3D(steeringWheelFrame); GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.X, Math.PI / 2.0, crossBar); GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.Z, Math.PI / 2.0, crossBar); crossBar.setPosition(new Vector3D(-height/2.0, 0.0, heightAboveWheel)); RigidBodyTransform transform = new RigidBodyTransform(); crossBar.get(transform); FrameCylinder3d spinnerHandleCylinder = new FrameCylinder3d(steeringWheelFrame, transform, height, radius); spokesCylinders.add(spinnerHandleCylinder); steeringWheelLinkGraphics.transform(transform); steeringWheelLinkGraphics.addCylinder(height, radius, YoAppearance.IndianRed()); transform.invert(); steeringWheelLinkGraphics.transform(transform); steeringWheelLink.setLinkGraphics(steeringWheelLinkGraphics); }
/** * Adds a PlanarRegion transforming from the current coordinate system. * Uses the given appearances in order, one for each Polygon in the PlanarRegion. Then loops on the appearances. * * @param planarRegion */ public static void addPlanarRegion(Graphics3DObject graphics3DObject, PlanarRegion planarRegion, AppearanceDefinition... appearances) { int numberOfConvexPolygons = planarRegion.getNumberOfConvexPolygons(); RigidBodyTransform transform = new RigidBodyTransform(); planarRegion.getTransformToWorld(transform); graphics3DObject.transform(transform); for (int i = 0; i < numberOfConvexPolygons; i++) { ConvexPolygon2D convexPolygon = planarRegion.getConvexPolygon(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, -0.0001); graphics3DObject.addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearances[i % appearances.length])); } transform.invert(); graphics3DObject.transform(transform); }
@Test public void testPreMultiplyInvertOtherWithAffineTransform() 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(); AffineTransform multipliedWith = new AffineTransform(multipliedWithRigidBody); multipliedWith.setScale(EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0)); expected.set(original); expected.preMultiply(inverseOfMultipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testTransformWithOtherRigidBodyTransform() throws Exception { Random random = new Random(23423L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = new RigidBodyTransform(); RigidBodyTransform actual = new RigidBodyTransform(); expected.set(transform); 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); 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 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 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); } }
private PlanarRegion createPlanarRegion(RigidBodyTransform regionToWorldFrameTransform, ConvexPolygon2D desiredFootholdInWorldXYPlane) { RigidBodyTransform worldToRegionTransform = new RigidBodyTransform(regionToWorldFrameTransform); worldToRegionTransform.invert(); ConvexPolygon2D planarRegionPolygon = new ConvexPolygon2D(); for (int i = 0; i < desiredFootholdInWorldXYPlane.getNumberOfVertices(); i++) { Point2DReadOnly vertex = desiredFootholdInWorldXYPlane.getVertex(i); double zHeight = getPlaneZGivenXY(regionToWorldFrameTransform, vertex.getX(), vertex.getY()); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), zHeight, 1.0); worldToRegionTransform.transform(transformPoint); planarRegionPolygon.addVertex(transformPoint.getX(), transformPoint.getY()); } planarRegionPolygon.update(); return new PlanarRegion(regionToWorldFrameTransform, planarRegionPolygon); }
@Test public void testPreMultiplyInvertThisWithAffineTransform() 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); AffineTransform multipliedWith = new AffineTransform(multipliedWithRigidBody); multipliedWith.setScale(EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0)); expected.set(original); expected.invert(); expected.preMultiply(multipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertThis(new AffineTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }