com.vividsolutions.jts.geom.util.AffineTransformation 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();
}
 

/**
 * Generate obstacles representing the placement(s) of a given robot in given poses.
 * @param robotID The ID of the robot whose footprint should be used.
 * @param obstaclePoses The poses of the footprint.
 * @return A {@link Geometry} that has the shape of the given robot's footprint, placed in each of the the given {@link Pose}s. 
 */
public Geometry[] makeObstacles(int robotID, Pose ... obstaclePoses) {
	ArrayList<Geometry> ret = new ArrayList<Geometry>();
	for (Pose p : obstaclePoses) {
		GeometryFactory gf = new GeometryFactory();
		Coordinate[] footprint = this.getFootprint(robotID);
		Coordinate[] newFoot = new Coordinate[footprint.length+1];
		for (int j = 0; j < footprint.length; j++) {
			newFoot[j] = footprint[j];
		}
		newFoot[footprint.length] = footprint[0];
		Geometry obstacle = gf.createPolygon(newFoot);
		AffineTransformation at = new AffineTransformation();
		at.rotate(p.getTheta());
		at.translate(p.getX(), p.getY());
		obstacle = at.transform(obstacle);
		ret.add(obstacle);
		metaCSPLogger.fine("Made obstacle for Robot" + robotID + " in pose " + p);
	}
	return ret.toArray(new Geometry[ret.size()]);
}
 

private Geometry makeObstacle(Pose p) {
	GeometryFactory gf = new GeometryFactory();
	Geometry geom = null;
	if (obstacleFootprint == null) {
		geom = gf.createPolygon(new Coordinate[] { new Coordinate(0.0,0.0), new Coordinate(0.0,OBSTACLE_SIZE), new Coordinate(OBSTACLE_SIZE,OBSTACLE_SIZE), new Coordinate(OBSTACLE_SIZE,0.0), new Coordinate(0.0,0.0) });
	}
	else {
		geom = gf.createPolygon(obstacleFootprint);
	}		
	AffineTransformation at = new AffineTransformation();
	at.rotate(p.getTheta());
	at.translate(p.getX(), p.getY());
	Geometry transGeom = at.transform(geom);
	Pose center = new Pose(p.getX(), p.getY(), p.getTheta());
	obstacles.add(transGeom);
	obstacleCenters.add(center);
	return transGeom;
}
 

private Geometry createArrow(Pose pose, double length, double size) {		
	GeometryFactory gf = new GeometryFactory();
	double aux = 1.8;
	double aux1 = 0.8;
	double aux2 = 0.3;
	double theta = pose.getTheta();
	Coordinate[] coords = new Coordinate[8];
	coords[0] = new Coordinate(0.0,-aux2);
	coords[1] = new Coordinate(length-aux,-aux2);
	coords[2] = new Coordinate(length-aux,-aux1);
	coords[3] = new Coordinate(length,0.0);
	coords[4] = new Coordinate(length-aux,aux1);
	coords[5] = new Coordinate(length-aux,aux2);
	coords[6] = new Coordinate(0.0,aux2);
	coords[7] = new Coordinate(0.0,-aux2);
	Polygon arrow = gf.createPolygon(coords);
	AffineTransformation at = new AffineTransformation();
	at.scale(size, size);
	at.rotate(theta);
	at.translate(pose.getX(), pose.getY());
	Geometry ret = at.transform(arrow);
	return ret;
}
 

private Geometry createArrow(Pose pose1, Pose pose2) {		
	GeometryFactory gf = new GeometryFactory();
	double aux = 1.8;
	double aux1 = 0.8;
	double aux2 = 0.3;
	double factor = Math.sqrt(robotFootprintArea)*0.5;
	double distance = Math.sqrt(Math.pow((pose2.getX()-pose1.getX()),2)+Math.pow((pose2.getY()-pose1.getY()),2))/factor;
	double theta = Math.atan2(pose2.getY() - pose1.getY(), pose2.getX() - pose1.getX());
	Coordinate[] coords = new Coordinate[8];
	coords[0] = new Coordinate(0.0,-aux2);
	coords[1] = new Coordinate(distance-aux,-aux2);
	coords[2] = new Coordinate(distance-aux,-aux1);
	coords[3] = new Coordinate(distance,0.0);
	coords[4] = new Coordinate(distance-aux,aux1);
	coords[5] = new Coordinate(distance-aux,aux2);
	coords[6] = new Coordinate(0.0,aux2);
	coords[7] = new Coordinate(0.0,-aux2);
	Polygon arrow = gf.createPolygon(coords);
	AffineTransformation at = new AffineTransformation();
	at.scale(factor, factor);
	at.rotate(theta);
	at.translate(pose1.getX(), pose1.getY());
	Geometry ret = at.transform(arrow);
	return ret;
}
 

/**
 * Add one or more obstacles with a given geometry placed in given poses. 
 * @param geom The geometry of the obstacles to add.
 * @param poses The poses in which to add obstacles.
 * @return A list of geometries representing the added obstacles.
 */
public ArrayList<Geometry> addObstacles(Geometry geom, Pose ... poses) {
	ArrayList<Geometry> obstacles = new ArrayList<Geometry>();
	for (Pose pose : poses) {
		AffineTransformation atObs = new AffineTransformation();
		atObs.rotate(pose.getTheta());
		atObs.translate(pose.getX(), pose.getY());
		Geometry obs = atObs.transform(geom);
		obstacles.add(obs);			
	}
	this.addObstacles(obstacles.toArray(new Geometry[obstacles.size()]));
	return obstacles;
}
 

private Geometry makeObstacle(Pose pose) {
	GeometryFactory gf = new GeometryFactory();
	Coordinate[] newFoot = new Coordinate[this.footprintCoords.length+1];
	for (int j = 0; j < this.footprintCoords.length; j++) {
		newFoot[j] = this.footprintCoords[j];
	}
	newFoot[this.footprintCoords.length] = this.footprintCoords[0];
	Geometry obstacle = gf.createPolygon(newFoot);
	AffineTransformation at = new AffineTransformation();
	at.rotate(pose.getTheta());
	at.translate(pose.getX(), pose.getY());
	return at.transform(obstacle);
}
 

public Geometry makeFootprint(double x, double y, double theta) {
	AffineTransformation at = new AffineTransformation();
	at.rotate(theta);
	at.translate(x,y);
	Geometry rect = at.transform(PP_footprintGeom);
	return rect;
}
 

@Override
public List<Geometry> listToroidalGeometries(final Geometry geom) {
	final Geometry copy = (Geometry) geom.clone();
	final List<Geometry> geoms = new ArrayList<>();
	final AffineTransformation at = new AffineTransformation();
	geoms.add(copy);
	for (int cnt = 0; cnt < 8; cnt++) {
		at.setToTranslation(getAdjustedXYVector()[cnt][0], getAdjustedXYVector()[cnt][1]);
		geoms.add(at.transform(copy));
	}
	return geoms;
}
 

public Geometry returnToroidalGeom(final GamaPoint loc) {
	final List<Geometry> geoms = new ArrayList<>();
	final Point pt = GeometryUtils.GEOMETRY_FACTORY.createPoint(loc);
	final AffineTransformation at = new AffineTransformation();
	geoms.add(pt);
	for (int cnt = 0; cnt < 8; cnt++) {
		at.setToTranslation(getAdjustedXYVector()[cnt][0], getAdjustedXYVector()[cnt][1]);
		geoms.add(at.transform(pt));
	}
	return GeometryUtils.GEOMETRY_FACTORY.buildGeometry(geoms);
}
 

protected Geometry rotate(Geometry geom)
{
  if (rotationAngle != 0.0) {
    AffineTransformation trans = AffineTransformation.rotationInstance(rotationAngle, 
        dim.getCentre().x, dim.getCentre().y);
    geom.apply(trans);
  }
  return geom;
}
 

/**
 * 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(); }

}
 

@Override
public ILocation normalizeLocation(final ILocation point, final boolean nullIfOutside) {

	// TODO Subclass (or rewrite) this naive implementation to take care of
	// irregular
	// geometries.

	// TODO Take into account the fact that some topologies may consider
	// invalid locations.
	if (environment.getGeometry().covers(point)) { return point; }

	if (isTorus()) {
		final Point pt = GeometryUtils.GEOMETRY_FACTORY.createPoint(GeometryUtils.toCoordinate(point));

		for (int cnt = 0; cnt < 8; cnt++) {
			final AffineTransformation at = new AffineTransformation();
			at.translate(getAdjustedXYVector()[cnt][0], getAdjustedXYVector()[cnt][1]);
			final GamaPoint newPt = new GamaPoint(at.transform(pt).getCoordinate());
			if (environment.getGeometry().covers(newPt)) { return newPt; }
		}
	}
	// See if rounding errors of double do not interfere with the
	// computation.
	// In which case, the use of Maths.approxEquals(value1, value2,
	// tolerance) could help.

	// if ( envWidth == 0.0 ) {
	// xx = xx != envMinX ? nullIfOutside ? nil : envMinX : xx;
	// } else if ( xx < envMinX /* && xx > hostMinX - precision */) {
	// xx = /* !isTorus ? */nullIfOutside ? nil : envMinX /* : xx % envWidth
	// + envWidth */;
	// } else if ( xx >= envMaxX /*- precision*/) {
	// xx = /* !isTorus ? */nullIfOutside ? nil : envMaxX /* : xx % envWidth
	// */;
	// }
	// if ( xx == nil ) { return null; }
	// if ( envHeight == 0.0 ) {
	// yy = yy != envMinY ? nullIfOutside ? nil : envMinY : yy;
	// } else if ( yy < envMinY/* && yy > hostMinY - precision */) {
	// yy = /* !isTorus ? */nullIfOutside ? nil : envMinY /* : yy %
	// envHeight + envHeight */;
	// } else if ( yy >= envMaxY /*- precision*/) {
	// yy = /* !isTorus ? */nullIfOutside ? nil : envMaxY /* : yy %
	// envHeight */;
	// }
	// if ( yy == nil ) { return null; }
	// point.setLocation(xx, yy, point.getZ());

	return null;
}