@Test
public void testWaypoint()
{
Waypoint wp1, wp2, wp3, wp4, wp5;
wp1 = new Waypoint(new Vector2f(3, 4), 0);
assertNotNull(wp1);
assertNull(wp1.getPredecessor());
assertEquals(new Vector2f(3,4),wp1.getVector2f());
assertEquals(0.0,wp1.getDistance(),1e-4);
wp2 = new Waypoint(new Vector2f(3,8), 4, wp1);
assertNotNull(wp2);
assertEquals(wp1,wp2.getPredecessor());
wp3 = new Waypoint(new Vector2f(1,8), 2, wp2);
assertEquals(6, wp3.getDistance(),1e-4);
Vector<Vector2f> points = new Vector<Vector2f>();
Vector<Vector2f> actual_points = new Vector<Vector2f>();
points = wp3.getRouteList(points);
assertNotSame(actual_points,points);
actual_points.add(wp3.getVector2f());
actual_points.add(wp2.getVector2f());
actual_points.add(wp1.getVector2f());
assertEquals(actual_points, points);
wp4 = new Waypoint(new Vector2f(3,4), 1, wp2);
wp5 = new Waypoint(new Vector2f(2,4), 0);
assertTrue(wp1.equals(wp1));
assertTrue(wp1.equals(wp4));
assertFalse(wp1.equals(wp2));
assertFalse(wp1.equals(wp5));