lejos.robotics.navigation
Class ArcAlgorithms

java.lang.Object
  extended by lejos.robotics.navigation.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
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. Delete any time.
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.