com.vividsolutions.jts.awt.ShapeWriter Java Examples

/**
 * Add obstacles to this occupancy map.
 * @param obstacles One or more geometries of obstacles to add to this occupancy map.
 */
public void addObstacles(Geometry ... obstacles) {
	Graphics2D g2 = bimg.createGraphics();
	ShapeWriter writer = new ShapeWriter();
	g2.setPaint(Color.black);
	for (Geometry g : obstacles) {
		AffineTransformation at = new AffineTransformation();
		at.scale(1.0/mapResolution, -1.0/mapResolution);
		at.translate(0, bimg.getHeight());
		Geometry scaledGeom = at.transform(g);
		Shape shape = writer.toShape(scaledGeom);
		//System.out.println("Shape: " + shape.getBounds2D());
		g2.fill(shape);
		this.obstacles.add(g);
	}
	g2.dispose();
	this.createOccupancyMap();
}
 

public static ShapeWriter BADgetConverter(Viewport viewport)
{
  if (viewportCache != viewport) {
    viewportCache = viewport;
    converterCache = new ShapeWriter(viewport, new PointShapeFactory.Point());
  }
  return converterCache;
}
 

public static ShapeWriter getConverter(Viewport viewport)
{
  ShapeWriter sw = new ShapeWriter(viewport, new PointShapeFactory.Point());
  //sw.setRemoveDuplicatePoints(true);
  sw.setDecimation(viewport.toModel(DECIMATION_DISTANCE));
  return sw;
}
 

public static void paint(Geometry geometry, Viewport viewport, 
    Graphics2D g,
    Color lineColor, Color fillColor, Stroke stroke) 
{
  ShapeWriter converter = getConverter(viewport);
  //ShapeWriter converter = new ShapeWriter(viewport);
  paint(geometry, converter, g, lineColor, fillColor, stroke);
}
 

/**
 * Save an image of the occupancy map with extra markings to indicate start and goal poses
 * of a robot with a given footprint.
 * @param startPose The start pose to mark.
 * @param goalPose The end pose to mark.
 * @param robotFoot The footprint to use in marking the start and goal poses.
 */
public void saveDebugObstacleImage(Pose startPose, Pose goalPose, Geometry robotFoot) {
	BufferedImage copyForDebug = new BufferedImage(bimg.getWidth(), bimg.getHeight(), BufferedImage.TYPE_INT_RGB);
	Graphics2D g2 = copyForDebug.createGraphics();
	g2.drawImage(bimg, 0, 0, bimg.getWidth(), bimg.getHeight(), 0, 0, bimg.getWidth(), bimg.getHeight(), null);
	
	ShapeWriter writer = new ShapeWriter();
	float dash1[] = {2.0f};
    BasicStroke dashed = new BasicStroke(2.0f, BasicStroke.CAP_ROUND, BasicStroke.JOIN_ROUND, 10.0f, dash1, 0.0f);
    g2.setStroke(dashed);
	
	g2.setPaint(Color.red);
	AffineTransformation atStart = new AffineTransformation();
	atStart.rotate(startPose.getTheta());
	atStart.translate(startPose.getX(), startPose.getY());
	Geometry robotAtStart = atStart.transform(robotFoot);
	AffineTransformation atStartScale = new AffineTransformation();
	atStartScale.scale(1.0/mapResolution, -1.0/mapResolution);
	atStartScale.translate(0, copyForDebug.getHeight());
	Geometry scaledGeomStart = atStartScale.transform(robotAtStart);
	Shape shapeAtStart = writer.toShape(scaledGeomStart);
	g2.draw(shapeAtStart);
	//g2.fill(shapeAtStart);
	
	g2.setPaint(Color.green);
	AffineTransformation atGoal = new AffineTransformation();
	atGoal.rotate(goalPose.getTheta());
	atGoal.translate(goalPose.getX(), goalPose.getY());
	Geometry robotAtGoal = atGoal.transform(robotFoot);
	AffineTransformation atGoalScale = new AffineTransformation();
	atGoalScale.scale(1.0/mapResolution, -1.0/mapResolution);
	atGoalScale.translate(0, copyForDebug.getHeight());
	Geometry scaledGeomGoal = atGoalScale.transform(robotAtGoal);
	Shape shapeAtGoal = writer.toShape(scaledGeomGoal);
	g2.draw(shapeAtGoal);
	//g2.fill(shapeAtGoal);
	
	g2.dispose();
	
	//Save the map for debugging
	try {
		String filename = TEMP_MAP_DIR + File.separator + "tempMap_" + (numCalls++) + "_" + System.identityHashCode(this) + ".png";
		File outputfile = new File(filename);
		ImageIO.write(copyForDebug, "png", outputfile);
		metaCSPLogger.info("See image " + outputfile.getAbsolutePath() + " for more info on recent planning failure.");
	}
	catch (IOException e) { e.printStackTrace(); }

}
 

private static void paint(Geometry geometry, ShapeWriter converter, Graphics2D g,
    Color lineColor, Color fillColor) 
{
  paint(geometry, converter, g, lineColor, fillColor, null);
}
 

private static void paint(Geometry geometry, ShapeWriter converter, Graphics2D g,
      Color lineColor, Color fillColor, Stroke stroke) 
  {
    if (geometry == null)
			return;

    if (geometry instanceof GeometryCollection) {
      GeometryCollection gc = (GeometryCollection) geometry;
      /**
       * Render each element separately.
       * Otherwise it is not possible to render both filled and non-filled
       * (1D) elements correctly
       */
      for (int i = 0; i < gc.getNumGeometries(); i++) {
        paint(gc.getGeometryN(i), converter, g, lineColor, fillColor, stroke);
      }
      return;
    }

		Shape shape = converter.toShape(geometry);
    
		// handle points in a special way for appearance and speed
		if (geometry instanceof Point) {
			g.setStroke(POINT_STROKE);
		  g.setColor(lineColor);
	    g.draw(shape);
			return;
		}

		if (stroke == null)
		  g.setStroke(GEOMETRY_STROKE);
		else
		  g.setStroke(stroke);
		
    // Test for a polygonal shape and fill it if required
		if (geometry instanceof Polygon && fillColor != null) {
		  // if (!(shape instanceof GeneralPath) && fillColor != null) {
			g.setPaint(fillColor);
			g.fill(shape);
		}
		
		if (lineColor != null) {
		  g.setColor(lineColor);
		  try {
		    g.draw(shape);
		    
				// draw polygon boundaries twice, to discriminate them
		    // MD - this isn't very obvious.  Perhaps a dashed line instead?
		    /*
				if (geometry instanceof Polygon) {
					Shape polyShell = converter.toShape( ((Polygon)geometry).getExteriorRing());
					g.setStroke(new BasicStroke(2));
					g.draw(polyShell);
				}
*/
		  } 
		  catch (Throwable ex) {
		    System.out.println(ex);
		    // eat it!
		  }
		}
	}