@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
public ConvexHullFootstepSnapper(FootstepValueFunction valueFunction, FootstepSnappingParameters parameters) { this.footstepValueFunction = valueFunction; this.parameters = parameters; this.useMask = true; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); if (writer == null) writer = new PointWriter("DataFromConvexHullSnapper"); }
public void updateParameters(FootstepSnappingParameters newParameters) { parameters.updateParameters(newParameters); footstepValueFunction.updateFunction(); this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
private ArrayList<Point2d> reduceListOfPointsToFourFootstepBased(List<Point2d> listOfPoints) { ConvexPolygon2d basePolygon = parameters.getCollisionPolygon(); ConvexPolygon2d supportPolygon = new ConvexPolygon2d(listOfPoints); supportPolygon.update(); ArrayList<Point2d> finalListOfSupportPoints = new ArrayList<Point2d>(); //for each vertex of the basePolygon, find the closest point inside the support polygon. int size = basePolygon.getNumberOfVertices(); for (int i = 0; i < size; i++) { Point2d vertex = basePolygon.getVertex(i); Point2d correspondingSupportPoint = getPointInPolygonNearestPoint(supportPolygon, vertex); finalListOfSupportPoints.add(correspondingSupportPoint); } return finalListOfSupportPoints; }
private void addLowerBoundaryPointsToHullPointList(List<Point3d> convexHullPointsList, double xOrigin, double yOrigin, double yaw) { double minHeight = Double.POSITIVE_INFINITY; for (Point3d point : convexHullPointsList) { if (point.getZ() < minHeight) minHeight = point.getZ(); } ConvexPolygon2d lowerBoundPolygon = parameters.getCollisionPolygon(); double dropDistance = parameters.getBoundingSquareSizeLength(); double cosYaw = Math.cos(yaw); double sinYaw = Math.sin(yaw); int numVertices = lowerBoundPolygon.getNumberOfVertices(); Point2d vertex; double xCoord; double yCoord; for (int i = 0; i < numVertices; i++) { vertex = lowerBoundPolygon.getVertex(i); xCoord = xOrigin + cosYaw * vertex.getX() - sinYaw * vertex.getY(); yCoord = yOrigin + cosYaw * vertex.getY() + sinYaw * vertex.getX(); convexHullPointsList.add(new Point3d(xCoord, yCoord, minHeight - dropDistance)); } }
public void updateParameters(FootstepSnappingParameters newParameters){ collisionPolygon = newParameters.getCollisionPolygon(); supportPolygon = newParameters.getSupportPolygon(); boundingSquareSizeLength = newParameters.getBoundingSquareSizeLength(); maxAngle = newParameters.getMaxAngle(); minArea = newParameters.getMinArea(); zDistanceTolerance = newParameters.getZDistanceTolerance(); this.distanceAdjustment = newParameters.getDistanceAdjustment(); this.angleAdjustment = newParameters.getAngleAdjustment(); this.badnumberOfPointsthreshold = newParameters.getBadnumberOfPointsthreshold(); } }