/**
* Calculate the range of a robot to the nearest wall
*
* @param pose the pose of the robot
* @return the range or -1 if not in range
*/
public float range(Pose pose) {
Line l =
new Line(
pose.getX(),
pose.getY(),
pose.getX() + 254f * (float) Math.cos(Math.toRadians(pose.getHeading())),
pose.getY() + 254f * (float) Math.sin(Math.toRadians(pose.getHeading())));
Line rl = null;
for (int i = 0; i < lines.length; i++) {
Point p = lines[i].intersectsAt(l);
if (p == null) continue; // Does not intersect
Line tl = new Line(pose.getX(), pose.getY(), p.x, p.y);
// If the range line intersects more than one map line
// then take the shortest distance.
if (rl == null || tl.length() < rl.length()) rl = tl;
}
return (rl == null ? -1 : rl.length());
}