@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3D position = originalFootstepFound.getLocation(); double yaw = originalFootstepFound.getOrientation().getYaw(); if (!useMask) { pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength()); } else { footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength(), footstepMask); } double height = heightMap.getHeightAtPoint(position.getX(), position.getY()); return snapFootstep(footstep, pointList, height); }
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(); Point2DReadOnly 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(QuadTreeFootstepSnappingParameters 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(); } }