/** * 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.multiply(fromWorldToLocalTransform, 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.applyTransformAndProjectToXYPlane(fromPolygonToPlanarRegionTransform); return snappedPolygonToReturn; }
/** * Retrieves the normal of this planar region and stores it in the given {@link Vector3d}. * @param normalToPack used to store the normal of this planar region. */ public void getNormal(Vector3d normalToPack) { normalToPack.setX(fromLocalToWorldTransform.getM02()); normalToPack.setY(fromLocalToWorldTransform.getM12()); normalToPack.setZ(fromLocalToWorldTransform.getM22()); }
/** * Retrieves the normal of this planar region and stores it in the given {@link Vector3f}. * @param normalToPack used to store the normal of this planar region. */ public void getNormal(Vector3f normalToPack) { normalToPack.setX((float) fromLocalToWorldTransform.getM02()); normalToPack.setY((float) fromLocalToWorldTransform.getM12()); normalToPack.setZ((float) fromLocalToWorldTransform.getM22()); }
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; }
/** * 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; }
normal.setX((float) transform.getM02()); normal.setY((float) transform.getM12()); normal.setZ((float) transform.getM22());
public static DenseMatrix64F differentiate(RigidBodyTransform t1, RigidBodyTransform t2, double dt) { DenseMatrix64F ret = new DenseMatrix64F(4,4); ret.set(0,0,(t2.getM00() - t1.getM00()) / dt); ret.set(0,1,(t2.getM01() - t1.getM01()) / dt); ret.set(0,2,(t2.getM02() - t1.getM02()) / dt); ret.set(0,3,(t2.getM03() - t1.getM03()) / dt); ret.set(1,0,(t2.getM10() - t1.getM10()) / dt); ret.set(1,1,(t2.getM11() - t1.getM11()) / dt); ret.set(1,2,(t2.getM12() - t1.getM12()) / dt); ret.set(1,3,(t2.getM13() - t1.getM13()) / dt); ret.set(2,0,(t2.getM20() - t1.getM20()) / dt); ret.set(2,1,(t2.getM21() - t1.getM21()) / dt); ret.set(2,2,(t2.getM22() - t1.getM22()) / dt); ret.set(2,3,(t2.getM23() - t1.getM23()) / dt); ret.set(3,0,0); ret.set(3,1,0); ret.set(3,2,0); ret.set(3,3,1);; return ret; }
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()); }
currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM00()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM01()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM02()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM03());