lejos.robotics.navigation
Class ArcAlgorithms

java.lang.Object
  lejos.robotics.navigation.ArcAlgorithms

public class ArcAlgorithms
extends 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.