Package lejos.robotics.navigation

Class ArcAlgorithms

  • public class ArcAlgorithms
extends java.lang.Object
    The static methods in this class can be used to to calculate theoretical routes and for displaying graphical representations of the path of a robot. The methods getAvailablePaths() and getBestPath() are useful for this.
    Version:
    November 2009
    Author:
    bb

    • Method Summary

      All Methods Static Methods Concrete Methods Deprecated Methods 
      Modifier and Type Method Description
      static float distBetweenPoints​(Point a, Point b)
      Calculates the distance between any two points.
      static Point findCircleCenter​(Point p1, float radius, float heading)
      Calculates the center of a circle that rests on the tangent of the vehicle's starting heading.
      static Point findP2​(Point c, Point p3, float radius)
      This method finds P2 if the vehicle is traveling to a point (with no heading).
      static Point findPointOnHeading​(Point original, float heading, float distance)
      Given a starting point and heading, this method will calculate another Point that is distance away from the first point.
      static float getArc​(Point p1, Point p2, float radius, float heading, boolean forward)
      If p1 is the starting point of the robot, and p2 is the point at which the robot is pointing directly at the target point, this method calculates the angle to travel along the circle (an arc) to get from p1 to p2.
      static float getArcBackward​(float forwardArc)
      Quick calculation of reverse arc instead of going through getArcLength() math again.
      static double getArcOld​(Point p1, Point p2, double radius)
      Deprecated.
      This method is no longer used because it can't calculate >180 angles.
      static Move[][] getAvailablePaths​(Pose start, float turnRadius1, Pose destination, float turnRadius2)
      This method gets all the available paths given a start Pose and destination Post.
      static Move[][] getAvailablePaths​(Pose start, Point destination, float turnRadius)
      This method calculates the moves needed to drive from a starting Pose to a final Point.
      static Move[] getBestPath​(Move[][] paths)
      This helper method accepts a number of paths (an array of Move) and selects the shortest path.
      static Move[] getBestPath​(Pose start, float turnRadius1, Pose destination, float turnRadius2)
      Find the shortest path for a steering vehicle between two points.
      static Move[] getBestPath​(Pose start, Point destination, float radius)
      This method generates the shortest path from a starting Pose to a final Point.
      static float getHeading​(float oldHeading, float changeInHeading)
      Given the former heading and the change in heading, this method will calculate a new heading.
      static float getHeading​(Point from, Point to)
      Calculates the heading designated by two points.
      static float getTriangleAngle​(Point p1, Point p2, Point pa)
      This method calculates the angle generated by three points.

      • Methods inherited from class java.lang.Object

        clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

    • Method Detail

      • getBestPath

        public static Move[] getBestPath​(Pose start,
                                 float turnRadius1,
                                 Pose destination,
                                 float turnRadius2)
        Find the shortest path for a steering vehicle between two points. The start Pose and destination Pose include the heading, so the robot will execute an arc, then travel a straight line, followed by another arc to obtain the final heading. This algorithm searches through 16 combinations of paths in order to find the shortest path. Even though the radii supplied are positive (left turn), this algorithm searches through negative radii too (right turns)
        Parameters:
        start - The starting Pose
        turnRadius1 - The turning radius for the first arc.
        destination - The destination Pose
        turnRadius2 - The turning radius for the final arc
        Returns:
        the sequence of moves

      • getAvailablePaths

        public static Move[][] getAvailablePaths​(Pose start,
                                         float turnRadius1,
                                         Pose destination,
                                         float turnRadius2)
        This method gets all the available paths given a start Pose and destination Post. Only four possible paths will be generated. The two turn radii can be positive (left hand turn) or negative (right hand turn). Each path consists of an arc, then a straight line, followed by another arc to obtain the final heading.
        Parameters:
        start - The starting Pose
        turnRadius1 - The turning radius for the first arc
        destination - The destination Pose
        turnRadius2 - The turning radius for the final arc
        Returns:
        the sequence of moves

      • getAvailablePaths

        public static Move[][] getAvailablePaths​(Pose start,
                                         Point destination,
                                         float turnRadius)
        This method calculates the moves needed to drive from a starting Pose to a final Point. The final heading is indeterminate at the destination. To arrive at the destination, the robot drives an arc, then a straight line to the destination point. If the destination point is within the turning circle, the moves generated by that circle will all have Float.NaN for the distanceTraveled and arcAngle values.
        Parameters:
        start - The starting Pose
        destination - The destination Point
        turnRadius - The turn radius
        Returns:
        A list of 4 paths.

      • getBestPath

        public static Move[] getBestPath​(Pose start,
                                 Point destination,
                                 float radius)
        This method generates the shortest path from a starting Pose to a final Point. The final heading is indeterminate at the destination. To arrive at the destination, the robot drives an arc, then a straight line to the destination point.
        Parameters:
        start - The starting Pose
        destination - The destination Point
        radius - The turn radius
        Returns:
        The shortest available path (given the parameters).

      • getBestPath

        public static Move[] getBestPath​(Move[][] paths)
        This helper method accepts a number of paths (an array of Move) and selects the shortest path.
        Parameters:
        paths - Any number of paths.
        Returns:
        The shortest path.

      • findPointOnHeading

        public static Point findPointOnHeading​(Point original,
                                       float heading,
                                       float distance)
        Given a starting point and heading, this method will calculate another Point that is distance away from the first point.
        Parameters:
        original - The starting point
        heading - The heading of the point
        distance - The distance away from this point to calculate a new point
        Returns:
        A new point "distance" from the first point along the same heading.

      • getTriangleAngle

        public static float getTriangleAngle​(Point p1,
                                     Point p2,
                                     Point pa)
        This method calculates the angle generated by three points. The point pa is the center point in the angle, while p1 and p2 are outside the angle.
        Parameters:
        p1 - An outer point, connects only with pa.
        p2 - An outer point, connects only with pa.
        pa - The central point, connects with both p1 and p2.
        Returns:
        The angle in degrees (between 0 and 180)

      • getHeading

        public static float getHeading​(float oldHeading,
                               float changeInHeading)
        Given the former heading and the change in heading, this method will calculate a new heading.
        Parameters:
        oldHeading - The old heading (original heading of robot) in degrees
        changeInHeading - The change in angle, in degrees.
        Returns:
        The new heading, in degrees. (0 to 360)

      • getArc

        public static float getArc​(Point p1,
                           Point p2,
                           float radius,
                           float heading,
                           boolean forward)
        If p1 is the starting point of the robot, and p2 is the point at which the robot is pointing directly at the target point, this method calculates the angle to travel along the circle (an arc) to get from p1 to p2.
        Parameters:
        p1 - Start position
        p2 - Take-off point on the circle
        radius - Radius of circle AKA the turnRadius
        heading - Start heading vehicle is pointed, in degrees.
        forward - Will the vehicle be moving forward along the circle arc?
        Returns:
        Length of travel along circle, in degrees

      • getArcBackward

        public static float getArcBackward​(float forwardArc)
        Quick calculation of reverse arc instead of going through getArcLength() math again.
        Parameters:
        forwardArc -
        Returns:
        the backward arc

      • getArcOld

        @Deprecated
public static double getArcOld​(Point p1,
                               Point p2,
                               double radius)
        Deprecated.
        This method is no longer used because it can't calculate >180 angles. Delete any time.
        If p1 is the starting point of the robot, and p2 is the point at which the robot is pointing directly at the target point, this method calculates the angle to travel along the circle (an arc) to get from p1 to p2.
        Parameters:
        p1 - Start position
        p2 - Take-off point on circle
        radius - Radius of circle
        Returns:
        Length of travel along circle, in degrees

      • distBetweenPoints

        public static float distBetweenPoints​(Point a,
                                      Point b)
        Calculates the distance between any two points.
        Parameters:
        a - The first point
        b - The second point
        Returns:
        The distance between points a and b.

      • getHeading

        public static float getHeading​(Point from,
                               Point to)
        Calculates the heading designated by two points. Heading always travels from the "from" point to the "to" point.
        Parameters:
        from - Starting point.
        to - Final point.
        Returns:
        Heading in degrees (0-360)

      • findP2

        public static Point findP2​(Point c,
                           Point p3,
                           float radius)
        This method finds P2 if the vehicle is traveling to a point (with no heading).
        Parameters:
        c - The center point of the turning circle.
        p3 - The final target point
        radius - The turn radius.
        Returns:
        P2, the takeoff point on the circle.

      • findCircleCenter

        public static Point findCircleCenter​(Point p1,
                                     float radius,
                                     float heading)
        Calculates the center of a circle that rests on the tangent of the vehicle's starting heading. It can calculate a circle to the right OR left of the heading tangent. To calculate a circle on the left side of the heading tangent, feed it a negative radius.
        Parameters:
        p1 - the starting point of the vehicle.
        radius - Turning radius of vehicle. A negative value produces a circle to the right of the heading.
        heading - Start heading of vehicle, in degrees (not radians).
        Returns:
        The center point of the circle.