/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(vertices, numberOfVertices)}; * 3- {@code update()}. * @param vertices {@code List<FramePoint2d>} the list of points that is used to creates the vertices. * @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices. * @throws ReferenceFrameMismatchException */ public void setAndUpdate(List<FramePoint2d> vertices, int numberOfVertices) { clear(); addVertices(vertices, numberOfVertices); update(); }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(otherPolygon)}; * 3- {@code update()}. * <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/> * @param otherPolygon {@code FrameConvexPolygon2d} * @throws ReferenceFrameMismatchException */ public void setAndUpdate(ConvexPolygon2d otherPolygon) { clear(); addVertices(otherPolygon); update(); }
/** * This method does: * 1- {@code clear(otherPolygon.getReferenceFrame())}; * 2- {@code addVertices(otherPolygon)}; * 3- {@code update()}. * <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/> * @param otherPolygon {@code FrameConvexPolygon2d} */ public void setIncludingFrameAndUpdate(FrameConvexPolygon2d otherPolygon) { clear(otherPolygon.getReferenceFrame()); addVertices(otherPolygon); update(); }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(firstPolygon)}; * 2- {@code addVertices(secondPolygon)}; * 3- {@code update()}. * <p/> TODO: Make this more efficient by finding the rotating calipers, as in the intersection method.<p/> * @param firstPolygon {@code FrameConvexPolygon2d} * @param secondPolygon {@code FrameConvexPolygon2d} * @throws ReferenceFrameMismatchException */ public void setAndUpdate(FrameConvexPolygon2d firstPolygon, FrameConvexPolygon2d secondPolygon) { referenceFrame.checkReferenceFrameMatch(firstPolygon.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(secondPolygon.getReferenceFrame()); clear(); addVertices(firstPolygon); addVertices(secondPolygon); update(); }
/** * If the list of vertices is empty, this polygon will be empty and its reference frame won't be changed. * If the list of vertices is not empty, this method does: * 1- {@code clear(vertices.get(0).getReferenceFrame())}; * 2- {@code addVertices(vertices)}; * 3- {@code update()}. * @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon. * @param vertices {@code List<FramePoint2d>} the list of points that is used to creates the vertices. * @throws ReferenceFrameMismatchException */ public void setIncludingFrameAndUpdate(List<FramePoint2d> vertices) { int numberOfVertices = vertices.size(); if (numberOfVertices < 1 ) { clear(); update(); } else { clear(vertices.get(0).getReferenceFrame()); addVertices(vertices, numberOfVertices); update(); } }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(otherPolygon)}; * 3- {@code update()}. * <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/> * @param otherPolygon {@code FrameConvexPolygon2d} * @throws ReferenceFrameMismatchException */ public void setAndUpdate(FrameConvexPolygon2d otherPolygon) { referenceFrame.checkReferenceFrameMatch(otherPolygon.getReferenceFrame()); clear(); addVertices(otherPolygon); update(); }
private void fitLine() { if (!footCoPOccupancyGrid.fitLineToData(line)) return; lineL.setIncludingFrame(line); lineL.shiftToLeft(width/2.0); lineR.setIncludingFrame(line); lineR.shiftToRight(width/2.0); backupFootPolygon.set(shrunkFootPolygon); shrunkFootPolygon.clear(); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineL)); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineR)); shrunkFootPolygon.update(); }
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
/** * If the list of vertices is empty, this polygon will be empty and its reference frame won't be changed. * If the list of vertices is not empty, this method does: * 1- {@code clear(vertices.get(0).getReferenceFrame())}; * 2- {@code addVertices(vertices)}; * 3- {@code update()}. * @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon. * @param vertices {@code FrameTuple2dArrayList<FramePoint2d>} the list of points that is used to creates the vertices. * @throws ReferenceFrameMismatchException */ public void setIncludingFrameAndUpdate(FrameTuple2dArrayList<FramePoint2d> vertices) { int numberOfVertices = vertices.size(); if (numberOfVertices < 1 ) { clear(); update(); } else { clear(vertices.get(0).getReferenceFrame()); addVertices(vertices, numberOfVertices); update(); } }
fullSupportAfterShrinking.setIncludingFrameAndUpdate(oppositeFootPolygon); fullSupportAfterShrinking.changeFrameAndProjectToXYPlane(worldFrame); fullSupportAfterShrinking.addVertices(controllerFootPolygonInWorld); fullSupportAfterShrinking.update(); momentumBasedController.getCapturePoint(capturePoint);
tempPolygon2.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); polygonShrinker.shrinkConstantDistanceInto(tempPolygon2, -distanceToExtendUpcomingFoothold.getDoubleValue(), tempPolygon1); safeArea.addVertices(tempPolygon1); safeArea.update();