示例#1
0
  /**
   * @author Nick Gigliotti
   *     <p>//* @param node to calculate the distance
   * @return int distance between nodes
   */
  public int calcDistance(MapNode toNode) {
    double distance = 0;
    double distanceXLeg = (toNode.getXPos() - this.getXPos());
    double distanceYLeg = (toNode.getYPos() - this.getYPos());

    distance = (Math.sqrt((distanceXLeg * distanceXLeg) + (distanceYLeg * distanceYLeg))) / 5;
    distance = Math.round(distance);
    return (int) distance;
  }
示例#2
0
  public double aStarHeuristic(MapNode toNode) {
    double dist =
        (double)
                Math.sqrt(
                    Math.pow((xPos - toNode.getXPos()), 2) + Math.pow(yPos - toNode.getYPos(), 2))
            + Math.abs(zPos - toNode.getZPos());

    return dist;
  }
示例#3
0
  /**
   * @author Rayan Alsoby
   *     <p>function to calculate the angle needed to turn from the current to be facing the target
   *     node
   * @param nextNode the target node to turn towards
   * @return the angle needed to turn in order to be facing the next node 90 degrees is left, 270
   *     degrees is right, 180 degrees is forward, 0 degrees is backwards (hope we don't get the
   *     last one)
   */
  public double calculateAngle(MapNode nextNode) {
    MapNode currentNode = this;
    MapNode previousNode = currentNode.getCameFrom();
    double prevX = previousNode.getXPos();
    double prevY = previousNode.getYPos();
    double currentX = currentNode.getXPos();
    double currentY = currentNode.getYPos();
    double nextX = nextNode.getXPos();
    double nextY = nextNode.getYPos();

    double angle1 = Math.atan2(prevY - currentY, prevX - currentX);
    double angle2 = Math.atan2(nextY - currentY, nextX - currentX);

    double radAngle;
    radAngle = angle1 - angle2;

    double resultAngle = Math.toDegrees(radAngle);
    if (resultAngle < 0) {
      resultAngle += 360;
    }
    return resultAngle;
  }