/** * Snaps the given polygon to the frame of this planar region and then transforms it to be in * this planar region. If the snappingTransform is not consistent with this PlanarRegion, then it * prints an error message. * * @param polygonToSnap * @param snappingTransform * @return ConvexPolygon2d Snapped polygon in the frame of this PlanarRegion. */ public ConvexPolygon2D snapPolygonIntoRegionAndChangeFrameToRegionFrame(ConvexPolygon2D polygonToSnap, RigidBodyTransform snappingTransform) { RigidBodyTransform fromPolygonToPlanarRegionTransform = new RigidBodyTransform(); fromPolygonToPlanarRegionTransform.set(fromWorldToLocalTransform); fromPolygonToPlanarRegionTransform.multiply(snappingTransform); double m02 = Math.abs(fromPolygonToPlanarRegionTransform.getM02()); double m12 = Math.abs(fromPolygonToPlanarRegionTransform.getM12()); if ((Math.abs(m02) > 1e-4) || (Math.abs(m12) > 1e-4)) { System.err.println("Snapping transform does not seem consistent with PlanarRegion transform!"); } ConvexPolygon2D snappedPolygonToReturn = new ConvexPolygon2D(polygonToSnap); snappedPolygonToReturn.applyTransform(fromPolygonToPlanarRegionTransform, false); return snappedPolygonToReturn; }
/** * Retrieves the normal of this planar region in the world frame and stores it in the given {@link Vector3D}. * * @param normalToPack used to store the normal of this planar region. */ public void getNormal(Vector3DBasics normalToPack) { normalToPack.setX(fromLocalToWorldTransform.getM02()); normalToPack.setY(fromLocalToWorldTransform.getM12()); normalToPack.setZ(fromLocalToWorldTransform.getM22()); }
public double getPlaneZGivenXY(RigidBodyTransform planeToWorldTransform, double xWorld, double yWorld) { // The three components of the plane origin double x0 = planeToWorldTransform.getM03(); double y0 = planeToWorldTransform.getM13(); double z0 = planeToWorldTransform.getM23(); // The three components of the plane normal double a = planeToWorldTransform.getM02(); double b = planeToWorldTransform.getM12(); double c = planeToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
/** * Computes the z-coordinate in world of the plane for a given xy-coordinates in world. * * @param xWorld x-coordinate of the query * @param yWorld y-coordinate of the query * @return the z-coordinate */ public double getPlaneZGivenXY(double xWorld, double yWorld) { // The three components of the plane origin double x0 = fromLocalToWorldTransform.getM03(); double y0 = fromLocalToWorldTransform.getM13(); double z0 = fromLocalToWorldTransform.getM23(); // The three components of the plane normal double a = fromLocalToWorldTransform.getM02(); double b = fromLocalToWorldTransform.getM12(); double c = fromLocalToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
private double getPlaneZGivenXY(ReferenceFrame planeFrame, double xWorld, double yWorld) { RigidBodyTransform fromLocalToWorldTransform = planeFrame.getTransformToWorldFrame(); // The three components of the plane origin double x0 = fromLocalToWorldTransform.getM03(); double y0 = fromLocalToWorldTransform.getM13(); double z0 = fromLocalToWorldTransform.getM23(); // The three components of the plane normal double a = fromLocalToWorldTransform.getM02(); double b = fromLocalToWorldTransform.getM12(); double c = fromLocalToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
/** * Creates and returns the Plane3D that this planar region lies on. */ public Plane3D getPlane() { Plane3D ret = new Plane3D(); ret.setPoint(fromLocalToWorldTransform.getM03(), fromLocalToWorldTransform.getM13(), fromLocalToWorldTransform.getM23()); ret.setNormal(fromLocalToWorldTransform.getM02(), fromLocalToWorldTransform.getM12(), fromLocalToWorldTransform.getM22()); return ret; }
@Test public void testDeterminantRotationPart() throws Exception { Random random = new Random(42353L); RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform transform = new RigidBodyTransform(original); double corruptionFactor = 0.1; double m00 = original.getM00() * corruptionFactor; double m01 = original.getM01() * corruptionFactor; double m02 = original.getM02() * corruptionFactor; double m10 = original.getM10() * corruptionFactor; double m11 = original.getM11() * corruptionFactor; double m12 = original.getM12() * corruptionFactor; double m20 = original.getM20() * corruptionFactor; double m21 = original.getM21() * corruptionFactor; double m22 = original.getM22() * corruptionFactor; transform.setRotationUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); assertTrue(transform.determinantRotationPart() < corruptionFactor); transform.normalizeRotationPart(); assertEquals(1.0, transform.determinantRotationPart(), EPS); }
normal.setY((float) transform.getM12()); normal.setZ((float) transform.getM22()); vertexNormals[vertexIndex] = normal;
double m10 = original.getM10() + corruptionFactor * random.nextDouble(); double m11 = original.getM11() + corruptionFactor * random.nextDouble(); double m12 = original.getM12() + corruptionFactor * random.nextDouble(); double m20 = original.getM20() + corruptionFactor * random.nextDouble(); double m21 = original.getM21() + corruptionFactor * random.nextDouble();
assertTrue(transform.getElement(1, 1) == coeff); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, coeff = random.nextDouble(), 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.getM12() == coeff); assertTrue(transform.getElement(1, 2) == coeff); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, coeff = random.nextDouble(), 0.0, 0.0, 0.0, 0.0);
public static void convertRigidBodyTransformToAffine(RigidBodyTransform rigidBodyTransform, Affine affineToPack) { affineToPack.setMxx(rigidBodyTransform.getM00()); affineToPack.setMxy(rigidBodyTransform.getM01()); affineToPack.setMxz(rigidBodyTransform.getM02()); affineToPack.setMyx(rigidBodyTransform.getM10()); affineToPack.setMyy(rigidBodyTransform.getM11()); affineToPack.setMyz(rigidBodyTransform.getM12()); affineToPack.setMzx(rigidBodyTransform.getM20()); affineToPack.setMzy(rigidBodyTransform.getM21()); affineToPack.setMzz(rigidBodyTransform.getM22()); affineToPack.setTx(rigidBodyTransform.getM03()); affineToPack.setTy(rigidBodyTransform.getM13()); affineToPack.setTz(rigidBodyTransform.getM23()); }
double m10 = expected.getM10(); double m11 = expected.getM11(); double m12 = expected.getM12(); double m13 = expected.getM13(); double m20 = expected.getM20();
double m10 = expected.getM10(); double m11 = expected.getM11(); double m12 = expected.getM12(); double m13 = expected.getM13(); double m20 = expected.getM20(); m10 = expected.getM10(); m11 = expected.getM11(); m12 = expected.getM12(); m13 = expected.getM13(); m20 = expected.getM20(); m10 = expected.getM10(); m11 = expected.getM11(); m12 = expected.getM12(); m13 = expected.getM13(); m20 = expected.getM20(); double m10 = expected.getM10(); double m11 = expected.getM11(); double m12 = expected.getM12(); double m13 = expected.getM13(); double m20 = expected.getM20(); m10 = expected.getM10(); m11 = expected.getM11(); m12 = expected.getM12(); m13 = expected.getM13(); m20 = expected.getM20();
currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM10()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM11()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM12()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM13());