/**
* Creates a new Direction from the two given points. When no previous point is given
* the robot assumes it starts off facing the right direction.
*
* @param currentPoint The point the robot is currently at.
* @param nextPoint The point the robot is trying to get to next.
* @param scale Coordinates/Unit of Measurement (ex: coordinates/meter) This is how the Direction
* can take two points with x and y coordinates and determine how many meters (or other unit of measurement)
* that represents. You must be consistent with your scale throughout the project or you will be very unhappy.
*/
public Direction(Point currentPoint, Point nextPoint, double scale)
{
this.previousPoint = null;
this.currentPoint = currentPoint;
this.nextPoint = nextPoint;
_distance = CoordinateCalculator.euclideanDistance(currentPoint, nextPoint) / scale;
}
/**
* Creates a new Direction from the three given nodes. When no previous node is given
* the robot assumes it starts off facing the right direction.
*
* @param currentNode The poNodeint the robot is currently at.
* @param nextNode The Node the robot is trying to get to next.
* @param scale Coordinates/Unit of Measurement (ex: coordinates/meter) This is how the Direction
* can take two Nodes' points with x and y coordinates and determine how many meters (or other unit of measurement)
* that represents. You must be consistent with your scale throughout the project or you will be very unhappy.
*/
public Direction(Node currentNode, Node nextNode, double scale)
{
this.previousPoint = null;
this.currentPoint = currentNode.CrossingPoint;
this.nextPoint = nextNode.CrossingPoint;
_distance = CoordinateCalculator.euclideanDistance(currentPoint, nextPoint) / scale;
}
/*
* Creates new edge between two nodes and adds it into the graph
* @param n1 first node
* @param n2 second node
* @param scale
* @param scale scale of the map in coordinates/units
* (ex. if a line started at (0,0) and ended at (5,5) and had an actual length of 3 inches,
* its scale would be 5/3 coordinates/inch)
*/
public void addEdge(Node n1, Node n2, double scale)
{
double weight = CoordinateCalculator.euclideanDistance(n1.CrossingPoint, n2.CrossingPoint) / scale;
Edge new_edge = new Edge(n1, n2, weight);
addEdge(new_edge);
}
/**
* Returns true if the x and y coordinates of two points are closer than epsilon
*
* @param point the point to compare to
* @param epsilon the acceptable error
*/
public bool isCloseTo(Point point, double epsilon)
{
if (point == null)
{
return(false);
}
return(CoordinateCalculator.hasDifferenceLessThan(this.X, point.X, epsilon) &&
CoordinateCalculator.hasDifferenceLessThan(this.Y, point.Y, epsilon));
}
private bool containsInVector(Point point, double epsilon)
{
bool x_in_range = CoordinateCalculator.isBetween(point.X, this.getStartPoint().X, this.getEndPoint().X, epsilon) ||
CoordinateCalculator.isBetween(point.X, this.getEndPoint().X, this.getStartPoint().X, epsilon);
bool y_in_range = CoordinateCalculator.isBetween(point.Y, this.getStartPoint().Y, this.getEndPoint().Y, epsilon) ||
CoordinateCalculator.isBetween(point.Y, this.getEndPoint().Y, this.getStartPoint().Y, epsilon);
return(x_in_range && y_in_range);
}
/**
* Returns true if the angles and distances of the two directions are within epsilon of each other.
*
* @param point the point to compare to
* @param epsilon the acceptable error
*/
public bool isCloseTo(Direction direction, double epsilon)
{
if (direction == null)
{
return(false);
}
return(CoordinateCalculator.hasDifferenceLessThan((float)this.angle, (float)direction.angle, epsilon) &&
CoordinateCalculator.hasDifferenceLessThan((float)this.distance, (float)direction.distance, epsilon));
}
/**
* Returns true if the crossing points of two nodes are closer than epsilon
*
* @param node the node to compare to
* @param epsilon the acceptable error
*/
public bool isCloseTo(Edge e, double epsilon)
{
if (e == null)
{
return(false);
}
bool closeEndpoints = (this.N1.isCloseTo(e.N1, epsilon)) && (this.N2.isCloseTo(e.N2, epsilon)) || (this.N2.isCloseTo(e.N1, epsilon)) && (this.N1.isCloseTo(e.N2, epsilon));
bool closeWeight = CoordinateCalculator.hasDifferenceLessThan((float)this.Weight, (float)e.Weight, epsilon);
return(closeEndpoints && closeWeight);
}
/*
* Returns the crossing point is the line crosses the other line, null if the lines do not cross
* (!) lines are assued to be finite, starting at the start point, and ending at the end point
* @param line the other line
* @param epsilon the amount of error that is permittable for two points to be considered equal
* (two points that are closer than epsilon will be considered equal)
*/
public Point crosses(Line line, double epsilon)
{
if (CoordinateCalculator.hasDifferenceLessThan(end_pt.X, start_pt.X, epsilon) &&
!CoordinateCalculator.hasDifferenceLessThan(line.end_pt.X, line.start_pt.X, epsilon))
{
float crosing_x = end_pt.X;
float crossing_y = line.getSlope() * crosing_x + line.getConstant();
Point crossing_pt = new Point(crosing_x, crossing_y);
if (this.containsInVector(crossing_pt, epsilon) && line.containsInVector(crossing_pt, epsilon))
{
return(crossing_pt);
}
}
else if (!CoordinateCalculator.hasDifferenceLessThan(end_pt.X, start_pt.X, epsilon) &&
CoordinateCalculator.hasDifferenceLessThan(line.end_pt.X, line.start_pt.X, epsilon))
{
float crosing_x = line.end_pt.X;
float crossing_y = getSlope() * crosing_x + getConstant();
Point crossing_pt = new Point(crosing_x, crossing_y);
if (this.containsInVector(crossing_pt, epsilon) && line.containsInVector(crossing_pt, epsilon))
{
return(crossing_pt);
}
}
else if ((CoordinateCalculator.hasDifferenceLessThan(end_pt.X, start_pt.X, epsilon) &&
CoordinateCalculator.hasDifferenceLessThan(line.end_pt.X, line.start_pt.X, epsilon)) ||
(this.getSlope() == line.getSlope()))
{
if (start_pt.isCloseTo(line.getStartPoint(), epsilon) ||
start_pt.isCloseTo(line.getEndPoint(), epsilon))
{
return(start_pt);
}
else if (end_pt.isCloseTo(line.getEndPoint(), epsilon) ||
end_pt.isCloseTo(line.getStartPoint(), epsilon))
{
return(end_pt);
}
}
else
{
float crosing_x = (line.getConstant() - this.getConstant()) / (this.getSlope() - line.getSlope());
float crossing_y = this.getSlope() * crosing_x + this.getConstant();
Point crossing_pt = new Point(crosing_x, crossing_y);
if (this.containsInVector(crossing_pt, epsilon) && line.containsInVector(crossing_pt, epsilon))
{
return(crossing_pt);
}
}
return(null);
}
/*
* Returns true if the point is on a geometric infinite line is in the interval set by the given vector (finite "line" with endpoints)
* @point point
* @param epsilon the amount of error that is permittable for two points to be considered equal
* (two points that are closer than epsilon will be considered equal)
*/
public bool contains(Point point, double epsilon)
{
if (CoordinateCalculator.hasDifferenceLessThan(end_pt.X, start_pt.X, epsilon))
{
return(CoordinateCalculator.hasDifferenceLessThan(point.X, start_pt.X, epsilon) &&
containsInVector(point, epsilon));
}
else
{
float testY = getSlope() * point.X + getConstant();
return(CoordinateCalculator.hasDifferenceLessThan(point.Y, testY, epsilon) &&
containsInVector(point, epsilon));
}
}
private void calculateScale(string path, string transform_data, int length)
{
string[] coordinates = path.Split(' ');
if (coordinates.Length < 4)
{
throw new Exception("Path tag of the scale formatted incorrectely");
}
Point start_pt = new Point(getCoordinateFromString(coordinates[0]), getCoordinateFromString(coordinates[1]));
transform(transform_data, start_pt);
Point end_pt = new Point(getCoordinateFromString(coordinates[2]), getCoordinateFromString(coordinates[3]));
transform(transform_data, end_pt);
graph.Scale = CoordinateCalculator.getScale(start_pt, end_pt, length);
}
/*
* Returns true if coord_a <= coord <= coord_b or coord_a >= coord >= coord_b
* Two coordinates are assumed to be equal if they are closer than epsilon to each other
*
* @param coord coordinate that is either between two other or not
* @param coord_a first coordinate
* @param coord_b second coordinate
* @param epsilon maximal difference between two coordinates to be considered equal
*/
public static bool isBetween(float coord, float coord_a, float coord_b, double epsilon)
{
return((coord > coord_a || CoordinateCalculator.hasDifferenceLessThan(coord, coord_a, epsilon)) &&
(coord < coord_b || CoordinateCalculator.hasDifferenceLessThan(coord, coord_b, epsilon)));
}