Exemplo n.º 1
0
  /**
   * 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());
  }