Java Code Examples for com.vividsolutions.jts.geom.Geometry#intersects()

@Override
public Collection<ValidationError> validate(GMLMap map) {
    List<ValidationError> errors = new ArrayList<ValidationError>();
    List<Geometry> polygons = new ArrayList<Geometry>();
    List<GMLShape> shapes = new ArrayList<GMLShape>(map.getAllShapes());

    for (GMLShape shape : shapes) {
        try {
            Geometry polygon = checkShape(shape);
            polygons.add(polygon);
        }
        catch (ValidationException e) {
            errors.add(e.getError());
            polygons.add(null);
        }
    }

    for (int i = 0; i < polygons.size(); i++) {
        Geometry s1 = polygons.get(i);
        if (s1 == null) {
            continue;
        }
        for (int j = i + 1; j < polygons.size(); j++) {
            Geometry s2 = polygons.get(j);
            if (s2 != null && s1.intersects(s2) && !s1.touches(s2)) {
                int s1Id = shapes.get(i).getID();
                int s2Id = shapes.get(j).getID();
                String message = " Shape overlaps with shape " + s2Id;
                errors.add(new ValidationError(s1Id, message));
            }
        }
    }
    return errors;
}
 

/**
 * Return if a robot, which is starting from a critical sections, 
 * is able to exit safetly from it.
 */
protected boolean canExitCriticalSection(int drivingCurrentIndex, int waitingCurrentIndex, TrajectoryEnvelope drivingTE, TrajectoryEnvelope waitingTE, int lastIndexOfCSDriving) {
	drivingCurrentIndex = Math.max(drivingCurrentIndex,0);
	waitingCurrentIndex = Math.max(waitingCurrentIndex,0);
	Geometry placementWaiting = waitingTE.makeFootprint(waitingTE.getTrajectory().getPoseSteering()[waitingCurrentIndex]);
	for (int i = drivingCurrentIndex; i <= lastIndexOfCSDriving; i++) {
		Geometry placementDriving = drivingTE.makeFootprint(drivingTE.getTrajectory().getPoseSteering()[i]);
		if (placementWaiting.intersects(placementDriving)) return false;
	}
	return true;
}
 

/**
 * Get the last placement along the {@link Trajectory} of a {@link TrajectoryEnvelope} that does
 * not overlap with the final pose of the robot.
 * @param te The trajectory envelope to search on
 * @return The last placement along the {@link Trajectory} of a {@link TrajectoryEnvelope} that does
 * not overlap with the final pose of the robot.
 */
public static Geometry getBackBlockingObstacle(TrajectoryEnvelope te) {
	Trajectory traj = te.getTrajectory();
	PoseSteering[] path = traj.getPoseSteering();
	Geometry placementLast = te.makeFootprint(path[path.length-1]);
	for (int i = path.length-2; i >= 0; i--) {
		Geometry placement = te.makeFootprint(path[i]);
		if (!placement.intersects(placementLast)) return placement;
	}
	return null;
}
 

private List<Geometry> getSegments(Geometry sourceLines,
		List<Geometry> lines) {
	List<Geometry> segments = new ArrayList<Geometry>();
	for (Geometry line : lines) {
		if (sourceLines.intersects(line) && !line.crosses(sourceLines)) {
			segments.add(line);
		}
	}
	return segments;
}
 

public void testResultsEqual(Geometry g, LineString line) 
 {
	boolean slowIntersects = g.intersects(line);

   PreparedGeometryFactory pgFact = new PreparedGeometryFactory();
   PreparedGeometry prepGeom = pgFact.create(g);
   
	boolean fastIntersects = prepGeom.intersects(line);

	if (slowIntersects != fastIntersects) {
		System.out.println(line);
		System.out.println("Slow = " + slowIntersects + ", Fast = " + fastIntersects);
		throw new RuntimeException("Different results found for intersects() !");
	}
}
 

public boolean checkIntersects(Geometry target, Geometry test) 
 {
boolean expectedResult = target.intersects(test);

PreparedGeometryFactory pgFact = new PreparedGeometryFactory();
PreparedGeometry prepGeom = pgFact.create(target);

boolean prepResult = prepGeom.intersects(test);

if (prepResult != expectedResult) {
	return false;
}
return true;
 }
 

/**
 * Get the path index beyond which a robot should not navigate, given the {@link TrajectoryEnvelope} of another robot.  
 * @param te1 The {@link TrajectoryEnvelope} of the leading robot.
 * @param te2 The {@link TrajectoryEnvelope} of the yielding robot.
 * @param currentPIR1 The current path index of the leading robot.
 * @param te1Start The path index
 * @param te1End
 * @param te2Start
 * @return The path index beyond which a robot should not navigate, given the {@link TrajectoryEnvelope} of another robot.
 */
protected int getCriticalPoint(int yieldingRobotID, CriticalSection cs, int leadingRobotCurrentPathIndex) {

	//Number of additional path points robot 2 should stay behind robot 1
	int TRAILING_PATH_POINTS = 3;

	int leadingRobotStart = -1;
	int yieldingRobotStart = -1;
	int leadingRobotEnd = -1;
	int yieldingRobotEnd = -1;
	TrajectoryEnvelope leadingRobotTE = null;
	TrajectoryEnvelope yieldingRobotTE = null;
	if (cs.getTe1().getRobotID() == yieldingRobotID) {
		leadingRobotStart = cs.getTe2Start();
		yieldingRobotStart = cs.getTe1Start();
		leadingRobotEnd = cs.getTe2End();
		yieldingRobotEnd = cs.getTe1End();
		leadingRobotTE = cs.getTe2();
		yieldingRobotTE = cs.getTe1();
	}
	else {
		leadingRobotStart = cs.getTe1Start();
		yieldingRobotStart = cs.getTe2Start();
		leadingRobotEnd = cs.getTe1End();
		yieldingRobotEnd = cs.getTe2End();
		leadingRobotTE = cs.getTe1();
		yieldingRobotTE = cs.getTe2();			
	}

	if (leadingRobotCurrentPathIndex < leadingRobotStart) {
		return Math.max(0, yieldingRobotStart-TRAILING_PATH_POINTS);
	}

	//Compute sweep of robot 1's footprint from current position to LOOKAHEAD
	Pose leadingRobotPose = leadingRobotTE.getTrajectory().getPose()[leadingRobotCurrentPathIndex];
	Geometry leadingRobotInPose = TrajectoryEnvelope.getFootprint(leadingRobotTE.getFootprint(), leadingRobotPose.getX(), leadingRobotPose.getY(), leadingRobotPose.getTheta());
	if (leadingRobotCurrentPathIndex <= leadingRobotEnd) {
		for (int i = leadingRobotCurrentPathIndex+1; i <= leadingRobotEnd; i++) {
			Pose leadingRobotNextPose = leadingRobotTE.getTrajectory().getPose()[i];
			try {
				leadingRobotInPose = leadingRobotInPose.union(TrajectoryEnvelope.getFootprint(leadingRobotTE.getFootprint(), leadingRobotNextPose.getX(), leadingRobotNextPose.getY(), leadingRobotNextPose.getTheta()));			
			} catch (Exception e) { e.printStackTrace(); }
		}
	}

	//Return pose at which yielding robot should stop given driving robot's projected sweep
	for (int i = yieldingRobotStart; i <= yieldingRobotEnd; i++) {
		Pose yieldingRobotPose = yieldingRobotTE.getTrajectory().getPose()[i];
		Geometry yieldingRobotInPose = TrajectoryEnvelope.getFootprint(yieldingRobotTE.getFootprint(), yieldingRobotPose.getX(), yieldingRobotPose.getY(), yieldingRobotPose.getTheta());
		if (leadingRobotInPose.intersects(yieldingRobotInPose)) {
			return Math.max(0, i-TRAILING_PATH_POINTS);
		}
	}

	//The only situation where the above has not returned is when robot 2 should
	//stay "parked", therefore wait at index 0
	return Math.max(0, yieldingRobotStart-TRAILING_PATH_POINTS);

}
 

public OSMElement queryFromTap(ILatLng latLng, float zoom) {
    double lat = latLng.getLatitude();
    double lng = latLng.getLongitude();
    Coordinate coord = new Coordinate(lng, lat);
    Envelope envelope = createTapEnvelope(coord, lat, lng, zoom);

    List results = spatialIndex.query(envelope);

    int len = results.size();
    if (len == 0 ) {
        return null;
    }
    if (len == 1) {
        return (OSMElement) results.get(0);
    }

    Point clickPoint = geometryFactory.createPoint(coord);
    OSMElement closestElement = null;
    double closestDist = Double.POSITIVE_INFINITY; // should be replaced in first for loop iteration
    for (Object res : results) {
        OSMElement el = (OSMElement) res;
        if (closestElement == null) {
            closestElement = el;
            closestDist = el.getJTSGeom().distance(clickPoint);
            continue;
        }
        Geometry geom = el.getJTSGeom();
        double dist = geom.distance(clickPoint);

        if (dist > closestDist) {
            continue;
        }

        if (dist < closestDist) {
            closestElement = el;
            closestDist = dist;
            continue;
        }

        // If we are here, then the distances are the same,
        // so we prioritize which element is better based on their type.
        closestElement = prioritizeElementByType(closestElement, el);

    }

    Geometry closestElementGeom = closestElement.getJTSGeom();
    if (closestElementGeom != null && closestElementGeom.intersects(geometryFactory.createPoint(coord))) {
        return closestElement;
    }
    return null;
}
 

public static boolean intersects(Geometry g1, Geometry g2)
{
  return g1.intersects(g2);
}
 

/**
 * Checks if an edge is part (i.e intersects) of a given polygon.
 * @param edge
 * @param polygon
 * @return
 */
private static boolean edgePartOfPolygon(GMLDirectedEdge edge,
        Geometry polygon) {
    // No idea if this works...
    return polygon.intersects(JTSTools.edgeToLine(edge));
}
 

public static boolean intersects(Geometry a, Geometry b) {    return a.intersects(b);    }