@Override public void applyTransformAndProjectToXYPlane(RigidBodyTransform transform) { convexPolygon.applyTransformAndProjectToXYPlane(transform); }
@Override public ConvexPolygon2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { ConvexPolygon2d copy = new ConvexPolygon2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
/** * This method moves a convex polygon into a given convex region. It will return the new polygon without modifying the given one. * The algorithm assumes a small rotation angle (it will linearize sin and cos around 0.0). For that reason it is possible to * specify a maximum and a minimum rotation. * * @param polygonToWiggle * @param planeToWiggleInto * @param wiggleParameters * @return */ public static ConvexPolygon2d wigglePolygon(ConvexPolygon2d polygonToWiggle, ConvexPolygon2d planeToWiggleInto, WiggleParameters parameters) { ConvexPolygon2d wiggledPolygon = new ConvexPolygon2d(polygonToWiggle); RigidBodyTransform wiggleTransform = findWiggleTransform(polygonToWiggle, planeToWiggleInto, parameters); if (wiggleTransform == null) return null; wiggledPolygon.applyTransformAndProjectToXYPlane(wiggleTransform); return wiggledPolygon; }
/** * 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; }
intersectingPolygon.applyTransformAndProjectToXYPlane(regionToPolygonTransform); intersectionPolygonToPack.addVertices(intersectingPolygon);
footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); soleToRegion = soleFrameAfterWiggle.getTransformToDesiredFrame(regionFrame); footPolygonInRegion.setAndUpdate(footStepPolygon); footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); ConvexPolygon2d foothold = regionToMoveTo.getConvexHull().intersectionWith(footPolygonInRegion); soleToRegion.invert(); foothold.applyTransformAndProjectToXYPlane(soleToRegion); return foothold;
currentFootPolygon.applyTransformAndProjectToXYPlane(soleTransformBeforeSnap); checkFootPolygonInWorld.applyTransformAndProjectToXYPlane(snapAndWiggleTransform);