wp1 = new Waypoint(new Vector2f(1, 1), 1);
wp2 = new Waypoint(new Vector2f(2, 2), 2);
wp3 = new Waypoint(new Vector2f(3, 3), 3);
wp4 = new Waypoint(new Vector2f(4, 4), 4);
PriorityNode n1, n2, n3, n4;
n1 = new PriorityNode(2, wp1);
assertNotNull(n1);
assertEquals(2, n1.getPriority(),1e-4);
assertEquals(wp1, n1.getWaypoint());
assertNull(n1.getNext());
assertTrue(n1.contains(wp1));
n3 = new PriorityNode(5, wp4);
n1.setNext(n3);
assertNotNull(n1.getNext());
assertTrue(n1.insert(0, wp2));
n2 = new PriorityNode(1, wp2, n1);
assertNotNull(n2.getNext());
assertTrue(n2.contains(wp1));
assertFalse(n2.insert(3, wp3));
assertTrue(n2.remove(wp1));
assertEquals(3, n2.getDistanceTo(wp3),1e-4);
assertEquals(Double.MAX_VALUE, n2.getDistanceTo(wp1),1);
assertEquals(1 , n1.compareTo(n2));
assertEquals(-1, n1.compareTo(n3));
assertEquals(0, n1.compareTo(n1));
n4 = new PriorityNode(5, wp1);
assertTrue(n1.equals(n4));
assertFalse(n1.equals(n2));
}