I have an ArrayList which contains coordinates of points:
class Point
{
int x, y;
}
ArrayList<Point> myPoints;
of such an image for example:
The problem is that these points are set chaotically in the ArrayList and I would like to sort them so that 2 points lying next to each other on the image are also one after another in the ArrayList. I can't come up with some good idea or algorithm to solve such a sorting... Are there some known methods of solving such problems?
edit:
The shape cannot cross over itself and let's assume that only shapes looking similarly like this can occur.
My thinking is that you first need a mathematical definition of your ordering. I suggest (Note, this definition wasn't clear in the original question, left here for completeness):
Starting with placing any point in the sequence, then perpetually append to the sequence the point closest to the current point and that hasn't already been appended to the sequence, until all points are appended to the sequence.
Thus with this definition of the ordering, you can derive a simple algorithm for this
ArrayList<point> orderedList = new ArrayList<point>();
orderedList.add(myList.remove(0)); //Arbitrary starting point
while (myList.size() > 0) {
//Find the index of the closest point (using another method)
int nearestIndex=findNearestIndex(orderedList.get(orderedList.size()-1), myList);
//Remove from the unorderedList and add to the ordered one
orderedList.add(myList.remove(nearestIndex));
}
The above is pretty universal (regardless of the algorithm for finding the next point). Then the "findNearestIndex" method could be defined as:
//Note this is intentially a simple algorithm, many faster options are out there
int findNearestIndex (point thisPoint, ArrayList listToSearch) {
double nearestDistSquared=Double.POSITIVE_INFINITY;
int nearestIndex;
for (int i=0; i< listToSearch.size(); i++) {
point point2=listToSearch.get(i);
distsq = (thisPoint.x - point2.x)*(thisPoint.x - point2.x)
+ (thisPoint.y - point2.y)*(thisPoint.y - point2.y);
if(distsq < nearestDistSquared) {
nearestDistSquared = distsq;
nearestIndex=i;
}
}
return nearestIndex;
}
Update:
Since the question was revised to largely adopt the definition I used, I took out some of the caveats.
Here is a possible solution for you: our goal is to construct a path that visits each of points in your list exactly once before it loops back. We can construct paths recursively: we can pick any point from the original list as our starting point and make a trivial path that consists only of a single node. Then we can extend an already constructed path by appending a point that we haven't visited yet.
Then we assume that we can find a good order for the original list of points by making sure by choosing the path that has the smallest length. Here, by length I don't mean number of points in the path, but the total sum of the Euclidian distance between each pair of adjacent points on the path.
The only problem is: given such a path, which point should we append next? In theory, we'd have to try out all possibilities to see which one leads to the best overall path.
The main trick that the code below employs is that it uses the following heuristic: in each step where we have to append a new point to the path constructed so far, pick the point that minimizes the average distance between two adjacent points.
It should be noted that it would be a bad idea to include in this the "loop distance" between the last point on the path and the first point: as we keep adding points, we move away from the first path point more and more. If we included the distance between the two end points, this would severely affect the average distance between all adjacent pairs, and thus hurt our heuristic.
Here's a simple auxiliary class to implement the path construction outlined above:
/**
* Simple recursive path definition: a path consists
* of a (possibly empty) prefix and a head point.
*/
class Path {
private Path prefix;
private Point head;
private int size;
private double length;
public Path(Path prefix, Point head) {
this.prefix = prefix;
this.head = head;
if (prefix == null) {
size = 1;
length = 0.0;
} else {
size = prefix.size + 1;
// compute distance from head of prefix to this new head
int distx = head.x - prefix.head.x;
int disty = head.y - prefix.head.y;
double headLength = Math.sqrt(distx * distx + disty * disty);
length = prefix.length + headLength;
}
}
}
And here's the actual heuristic search algorithm.
/**
* Implements a search heuristic to determine a sort
* order for the given <code>points</code>.
*/
public List<Point> sort(List<Point> points) {
int len = points.size();
// compares the average edge length of two paths
Comparator<Path> pathComparator = new Comparator<Path>() {
public int compare(Path p1, Path p2) {
return Double.compare(p1.length / p1.size, p2.length / p2.size);
}
};
// we use a priority queue to implement the heuristic
// of preferring the path with the smallest average
// distance between its member points
PriorityQueue<Path> pq = new PriorityQueue<Path>(len, pathComparator);
pq.offer(new Path(null, points.get(0)));
List<Point> ret = new ArrayList<Point>(len);
while (!pq.isEmpty()) {
Path path = pq.poll();
if (path.size == len) {
// result found, turn path into list
while (path != null) {
ret.add(0, path.head);
path = path.prefix;
}
break;
}
loop:
for (Point newHead : points) {
// only consider points as new heads that
// haven't been processed yet
for (Path check = path; check != null; check = check.prefix) {
if (newHead == check.head) {
continue loop;
}
}
// create new candidate path
pq.offer(new Path(path, newHead));
}
}
return ret;
}
If you run this code on the sample points of your question, and then connect each adjacent pair of points from the returned list, you get the following picture:
This is not a Sort algorithm - it is more of a rearrangement to minimise a metric (the distance between consecutive points).
I'd attempt some kind of heuristic algorithm - something like:
Pick three consecutive points a, b, c.
If distance(a,c) < distance(a,b) then swap(a,b).
Repeat from 1.
It should be possible to calculate how many times you should need to cycle this to achieve a minimal arrangement or perhaps you could detect a minimal arrangement by finding no swaps during a run.
You may need to alternate the direction of your sweeps rather like the classic optimisation of bubble-sort.
Added
Experiment shows that this algorithm doesn't work but I've found one that does. Essentially, for each entry in the list find the closest other point and move it up to the next location.
private static class Point {
final int x;
final int y;
public Point(int x, int y) {
this.x = x;
this.y = y;
}
public String toString() {
return "(" + x + "," + y + ")";
}
public double distance(Point b) {
int dx = x - b.x;
int dy = y - b.y;
// Simple cartesian distance.
return Math.sqrt(dx * dx + dy * dy);
}
}
// Sample test data - forms a square.
Point[] points = new Point[]{
new Point(0, 0),
new Point(0, 1),
new Point(0, 2),
new Point(0, 3),
new Point(0, 4),
new Point(0, 5),
new Point(0, 6),
new Point(0, 7),
new Point(0, 8),
new Point(0, 9),
new Point(1, 9),
new Point(2, 9),
new Point(3, 9),
new Point(4, 9),
new Point(5, 9),
new Point(6, 9),
new Point(7, 9),
new Point(8, 9),
new Point(9, 9),
new Point(9, 8),
new Point(9, 7),
new Point(9, 6),
new Point(9, 5),
new Point(9, 4),
new Point(9, 3),
new Point(9, 2),
new Point(9, 1),
new Point(9, 0),
new Point(8, 0),
new Point(7, 0),
new Point(6, 0),
new Point(5, 0),
new Point(4, 0),
new Point(3, 0),
new Point(2, 0),
new Point(1, 0),};
public void test() {
System.out.println("Hello");
List<Point> test = Arrays.asList(Arrays.copyOf(points, points.length));
System.out.println("Before: " + test);
Collections.shuffle(test);
System.out.println("Shuffled: " + test);
List<Point> rebuild = new ArrayList<>(test);
rebuild.add(0, new Point(0, 0));
rebuild(rebuild);
rebuild.remove(0);
System.out.println("Rebuilt: " + rebuild);
}
private void rebuild(List<Point> l) {
for (int i = 0; i < l.size() - 1; i++) {
Point a = l.get(i);
// Find the closest.
int closest = i;
double howClose = Double.MAX_VALUE;
for (int j = i + 1; j < l.size(); j++) {
double howFar = a.distance(l.get(j));
if (howFar < howClose) {
closest = j;
howClose = howFar;
}
}
if (closest != i + 1) {
// Swap it in.
Collections.swap(l, i + 1, closest);
}
}
}
prints:
Before: [(0,0), (0,1), (0,2), (0,3), (0,4), (0,5), (0,6), (0,7), (0,8), (0,9), (1,9), (2,9), (3,9), (4,9), (5,9), (6,9), (7,9), (8,9), (9,9), (9,8), (9,7), (9,6), (9,5), (9,4), (9,3), (9,2), (9,1), (9,0), (8,0), (7,0), (6,0), (5,0), (4,0), (3,0), (2,0), (1,0)]
Shuffled: [(9,6), (0,9), (0,8), (3,9), (0,5), (9,4), (0,7), (1,0), (5,0), (9,3), (0,1), (3,0), (1,9), (8,9), (9,8), (2,0), (2,9), (9,5), (5,9), (9,7), (6,0), (0,3), (0,2), (9,1), (9,2), (4,0), (4,9), (7,9), (7,0), (8,0), (6,9), (0,6), (0,4), (9,0), (0,0), (9,9)]
Rebuilt: [(0,0), (0,1), (0,2), (0,3), (0,4), (0,5), (0,6), (0,7), (0,8), (0,9), (1,9), (2,9), (3,9), (4,9), (5,9), (6,9), (7,9), (8,9), (9,9), (9,8), (9,7), (9,6), (9,5), (9,4), (9,3), (9,2), (9,1), (9,0), (8,0), (7,0), (6,0), (5,0), (4,0), (3,0), (2,0), (1,0)]
which looks like what you are looking for.
The efficiency of the algorithm is not good - somewhere around O(n log n) - I hope you don't need to do this millions of times.
If you want the points to appear in a predictable order (say leftmost one at the start) you could add a fake point at the start of the list before rebuilding it and remove it after. The algorithm will always leave the first point alone.
I started this shortly after the question, but it had been delayed due to the question being put on hold. It's the simple approach that in the meantime also has been mentioned in the comments and other answers, but I'll post it here anyhow:
Here is a MCVE showing the simplest and most straightforward approach. The approach simply consists of picking an arbitrary point, and then continuing by always picking the point that is closest to the previous one. Of course, this has limitations:
It may pick the wrong point, when there are sharp corners or cusps
It's not very efficient, because it repeatedly does a search for the closest point
One approach for accelerating it could be to sort the points based on the x-coordinate, and then exploit this partial ordering in order to skip most of the points when looking for the next neighbor. But as long as you don't want to apply this to ten-thousands of points in a time-critical context, this should not be an issue.
The possible ambiguities, in turn, may be a problem, but considering that, one has to say that the problem is underspecified anyhow. In some cases, not even a human could decide which point is the appropriate "next" point - at least, when the problem is not extended to detect the "interior/exterior" of shapes (this is somewhat similar to the problem of ambiguities in the marching cube algorithm: You just don't know what the intended path is).
Note that most of the code is not really important for your actual question, but ... you did not provide such a "stub" implementation. The relevant part is === marked ===
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.Shape;
import java.awt.geom.Area;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Path2D;
import java.awt.geom.PathIterator;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.List;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.SwingUtilities;
public class SortShapePoints
{
public static void main(String[] args)
{
SwingUtilities.invokeLater(new Runnable()
{
#Override
public void run()
{
createAndShowGUI();
}
});
}
private static void createAndShowGUI()
{
JFrame f = new JFrame();
f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
Shape shape = createExampleShape();
List<Point2D> points = computePoints(shape, 6);
Collections.shuffle(points);
List<Point2D> sortedPoints = sortPoints(points);
Path2D path = createPath(sortedPoints, true);
f.getContentPane().add(new ShapePanel(points, path));
f.setSize(800, 800);
f.setLocationRelativeTo(null);
f.setVisible(true);
}
//=== Relevant part starts here =========================================
private static List<Point2D> sortPoints(List<Point2D> points)
{
points = new ArrayList<Point2D>(points);
List<Point2D> sortedPoints = new ArrayList<Point2D>();
Point2D p = points.remove(0);
sortedPoints.add(p);
while (points.size() > 0)
{
int index = indexOfClosest(p, points);
p = points.remove(index);
sortedPoints.add(p);
}
return sortedPoints;
}
private static int indexOfClosest(Point2D p, List<Point2D> list)
{
double minDistanceSquared = Double.POSITIVE_INFINITY;
int minDistanceIndex = -1;
for (int i = 0; i < list.size(); i++)
{
Point2D other = list.get(i);
double distanceSquared = p.distanceSq(other);
if (distanceSquared < minDistanceSquared)
{
minDistanceSquared = distanceSquared;
minDistanceIndex = i;
}
}
return minDistanceIndex;
}
//=== Relevant part ends here ===========================================
private static Shape createExampleShape()
{
Area a = new Area();
a.add(new Area(new Ellipse2D.Double(200, 200, 200, 100)));
a.add(new Area(new Ellipse2D.Double(260, 160, 100, 500)));
a.add(new Area(new Ellipse2D.Double(220, 380, 180, 60)));
a.add(new Area(new Rectangle2D.Double(180, 520, 260, 40)));
return a;
}
private static List<Point2D> computePoints(Shape shape, double deviation)
{
List<Point2D> result = new ArrayList<Point2D>();
PathIterator pi = shape.getPathIterator(null, deviation);
double[] coords = new double[6];
Point2D newPoint = null;
Point2D previousMove = null;
Point2D previousPoint = null;
while (!pi.isDone())
{
int segment = pi.currentSegment(coords);
switch (segment)
{
case PathIterator.SEG_MOVETO:
previousPoint = new Point2D.Double(coords[0], coords[1]);
previousMove = new Point2D.Double(coords[0], coords[1]);
break;
case PathIterator.SEG_CLOSE:
createPoints(previousPoint, previousMove, result, deviation);
break;
case PathIterator.SEG_LINETO:
newPoint = new Point2D.Double(coords[0], coords[1]);
createPoints(previousPoint, newPoint, result, deviation);
previousPoint = new Point2D.Double(coords[0], coords[1]);
break;
case PathIterator.SEG_QUADTO:
case PathIterator.SEG_CUBICTO:
default:
// Should never occur
throw new AssertionError("Invalid segment in flattened path!");
}
pi.next();
}
return result;
}
private static void createPoints(Point2D p0, Point2D p1,
List<Point2D> result, double deviation)
{
double dx = p1.getX() - p0.getX();
double dy = p1.getY() - p0.getY();
double d = Math.hypot(dx, dy);
int steps = (int) Math.ceil(d / deviation);
for (int i = 0; i < steps; i++)
{
double alpha = (double) i / steps;
double x = p0.getX() + alpha * dx;
double y = p0.getY() + alpha * dy;
result.add(new Point2D.Double(x, y));
}
}
public static Path2D createPath(Iterable<? extends Point2D> points,
boolean close)
{
Path2D path = new Path2D.Double();
Iterator<? extends Point2D> iterator = points.iterator();
boolean hasPoints = false;
if (iterator.hasNext())
{
Point2D point = iterator.next();
path.moveTo(point.getX(), point.getY());
hasPoints = true;
}
while (iterator.hasNext())
{
Point2D point = iterator.next();
path.lineTo(point.getX(), point.getY());
}
if (close && hasPoints)
{
path.closePath();
}
return path;
}
}
class ShapePanel extends JPanel
{
private final List<Point2D> points;
private final Shape shape;
public ShapePanel(List<Point2D> points, Shape shape)
{
this.points = points;
this.shape = shape;
}
#Override
protected void paintComponent(Graphics gr)
{
super.paintComponent(gr);
Graphics2D g = (Graphics2D) gr;
g.setRenderingHint(RenderingHints.KEY_ANTIALIASING,
RenderingHints.VALUE_ANTIALIAS_ON);
g.setColor(Color.RED);
g.draw(shape);
g.setColor(Color.BLACK);
for (Point2D p : points)
{
g.fill(new Ellipse2D.Double(p.getX() - 1, p.getY() - 1, 2, 2));
}
}
}
This is a pretty open ended question but if you want them stored in a certain way you need to define the ordering more than "So that they are next to each other in the array" You need to have a function where you can take two points and say, Point A is less than Point B or vice versa, or they are equal.
If you have that, then the algorithm you need to sort them is already implemented and you can use it by implementing a Comparator as SANN3 said.
As a side note, you might not want to store a shape as a set of points. I think you might want to store them as a line? You can use a cubic spline to get almost any shape you want then you could save on storage...
I had a task to sort the points to represent a line. I decided to keep the full weight of the path and update it upon standard Collection operations accordingly. The solution should work in your case too. Just take the elements of this LinkedList ps and connect its head and tail. Also, you can add more operations like PointXY get(int index) etc. with a bit more forwarding to the underlying LinkedList in this composition. Finally, you should guard the collection with excessive defensive copies where necessary. I tried to keep it simple for the sake of brevity.
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.Iterator;
import java.util.LinkedList;
public class ContinuousLineSet implements Collection<PointXY> {
LinkedList<PointXY> ps = new LinkedList<>(); // Exposed for simplicity
private int fullPath = 0; // Wighted sum of all edges in ps
#Override
public int size() {
return ps.size();
}
#Override
public boolean isEmpty() {
return ps.isEmpty();
}
#Override
public boolean contains(Object o) {
return ps.contains(o);
}
#Override
public Iterator<PointXY> iterator() {
return ps.iterator();
}
#Override
public Object[] toArray() {
return ps.toArray();
}
#Override
public <T> T[] toArray(T[] a) {
return ps.toArray(a);
}
private int dist(PointXY a, PointXY b) {
return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
}
#Override
public boolean add(PointXY e) {
if (isEmpty())
return ps.add(e);
if (ps.getFirst().equals(e))
return false;
Iterator<PointXY> it = ps.iterator();
PointXY previous = it.next();
int asFirst = fullPath + dist(e, previous);
int minPath = asFirst;
int iMin = 0;
int i = 0;
while (it.hasNext()) {
i++;
PointXY next = it.next();
if (next.equals(e))
return false;
int asBetween = fullPath - dist(previous, next) + dist(previous, e) + dist(e, next);
if (asBetween < minPath) {
iMin = i;
minPath = asBetween;
}
previous = next;
}
int asLast = fullPath + dist(e, previous);
if (asLast < minPath) {
minPath = asLast;
iMin = size();
}
fullPath = minPath;
ps.add(iMin, e);
return true;
}
public void reverse() {
Collections.reverse(ps);
}
#Override
public boolean remove(Object o) {
PointXY last = null;
for (Iterator<PointXY> it = iterator(); it.hasNext();) {
PointXY p = it.next();
if (o.equals(p)) {
int part1 = last != null ? dist(last, p) : 0;
int part2 = it.hasNext() ? dist(p, it.next()) : 0;
fullPath -= part1 + part2;
break;
}
last = p;
}
return ps.remove(o);
}
#Override
public boolean containsAll(Collection<?> c) {
return ps.containsAll(c);
}
#Override
public boolean addAll(Collection<? extends PointXY> c) {
boolean wasAdded = false;
for (PointXY p : c) {
wasAdded |= add(p);
}
return wasAdded;
}
#Override
public boolean removeAll(Collection<?> c) {
boolean wasRemoved = false;
for (Object o : c) {
if (o instanceof PointXY) {
PointXY p = (PointXY) o;
wasRemoved |= remove(p);
}
}
return wasRemoved;
}
#Override
public boolean retainAll(Collection<?> c) {
ContinuousLineSet cls = new ContinuousLineSet();
for (Object o : c) {
if (o instanceof PointXY && ps.contains(o)) {
PointXY p = (PointXY) o;
cls.add(p);
}
}
int oldSize = ps.size();
ps = cls.ps;
fullPath = cls.fullPath;
return size() != oldSize;
}
#Override
public void clear() {
ps.clear();
fullPath = 0;
}
}
class PointXY {
public static PointXY of(int x, int y) {
return new PointXY(x, y);
}
public final int x, y;
private int hash;
private boolean wasHashInit = false;
private PointXY(int x, int y) {
this.x = x;
this.y = y;
}
#Override
public boolean equals(Object obj) {
if (!(obj instanceof PointXY))
return false;
PointXY p = (PointXY) obj;
return x == p.x && y == p.y;
}
#Override
public int hashCode() {
if (!wasHashInit) {
hash = 17;
hash = 31 * hash + y;
hash = 31 * hash + x;
wasHashInit = true;
}
return hash;
}
#Override
public String toString() {
return String.format("(%d, %d)", x, y);
}
}
public class Point implements Comparable
{
...
...
#Override
public int compareTo(Pointarg0)
{
....
}
...
}
...
I have been reading some articles to learn A* pathfinding and I was able to make a test program based on it, but the the program I made is giving strange paths which are not a shortest path to the target node.
In the images, the green square unit represents the starting node, the red unit represents target node, blue units are impassable tiles (walls) and the purple units represent the path found from starting node to target node
http://imgur.com/5dJEfYc
http://imgur.com/lHfXEyW
If anybody could find a problem with the pathfinding source code I would be much thankful. I'm burned out from trying to know what caused it to act strange.
Its allowed to cut corners and go diagonal
package com.streak324.pathfinding;
import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;
import com.badlogic.gdx.utils.Array;
public class PathFinder {
private boolean foundTarget;
private int width, height;
//list of nodes that leads from starting node to target node is stored here
private Array<PathNode> path;
//all nodes stored in this array
private PathNode[][] nodes;
private PriorityQueue<PathNode> open;
private HashSet<PathNode> closed;
//nodes that must be referenced
private PathNode start, target, current;
//how far the current node can reach for other nodes from its own position
private int range = 1;
public PathFinder(int width, int height, boolean map[][]) {
this.width = width;
this.height = height;
nodes = new PathNode[width][height];
for(int i=0; i<width; i++) {
for(int j=0; j<height; j++) {
nodes[i][j] = new PathNode(i, j);
//if wall tile is spotted, mark the node unwalkable
if(map[i][j] != true) { nodes[i][j].passable = false; }
}
}
open = new PriorityQueue<PathNode>(new CostComparator());
closed = new HashSet<PathNode>();
}
public Array<PathNode> getPath(int sx, int sy ,int tx, int ty) {
path = new Array<PathNode>();
open.clear();
closed.clear();
start = nodes[sx][sy];
start.movementCost = 0;
addToOpenList(start);
target = nodes[tx][ty];
while(foundTarget != true) {
if(open.size() == 0) { return null; }
current = open.poll();
addToClosedList(current);
checkNeighbors(current);
}
traceBack();
return path;
}
// makes its way back adding the parent node until start
private void traceBack() {
while(current != start) {
path.add(current);
current = current.parent;
}
}
//checks for nodes within certain range
private void checkNeighbors(PathNode node) {
//continues loop if i or j goes out of bounds of nodes array
for(int i = node.x - range; i <= (node.x + range); i++) {
if(i >= width || i < 0) { continue; }
for(int j = node.y - range; j <= (node.y + range); j++) {
if( j >= height || j < 0) { continue; }
if((i == node.x && j == node.y) ) { continue; }
PathNode neighbor = nodes[i][j];
identifyNode(neighbor);
}
}
}
//if node is not on open list, add node and calculate it
private void identifyNode(PathNode node) {
if(!node.passable || closed.contains(node) ) return;
if(node == target) {
foundTarget = true;
System.out.println("Target Found: " + node.x + ", " + node.y);
return;
}
else if(!open.contains(node)) {
addToOpenList(node);
calcHeuristic(node);
updateNode(node, current);
}
else {
checkForReparenting(node);
}
}
//is the movement cost less to go from the current node?
private void checkForReparenting(PathNode node) {
float cost = node.movementCost;
float reCost = calcMovementCost(node, current);
if(reCost < cost) {
System.out.println("Reparenting");
updateNode(node, current);
}
}
//updates parent and cost
private void updateNode(PathNode child, PathNode parent) {
child.parent = parent;
child.movementCost = calcMovementCost(child, parent);
child.totalCost = child.movementCost + child.heuristic;
}
private float calcMovementCost(PathNode n1, PathNode n2) {
float dx = n1.x - n2.x;
float dy = n1.y - n2.y;
return (float) Math.sqrt( (dx*dx + dy*dy) ) + n2.movementCost;
}
private float calcHeuristic(PathNode node) {
float dx = node.x - target.x;
float dy = node.y - target.y;
return (float) Math.sqrt( (dx*dx + dy*dy) );
}
private void addToOpenList(PathNode node) {
if(!open.contains(node) && !closed.contains(node)) {
open.add(node);
}
}
private void addToClosedList(PathNode node) {
if(!closed.contains(node)) {
closed.add(node);
}
}
public class PathNode {
public int x, y;
public PathNode parent;
//g, h and f
public float movementCost, heuristic, totalCost;
public boolean passable;
public PathNode(int x, int y) {
this.x = x;
this.y = y;
passable = true;
}
}
private class CostComparator implements Comparator<PathNode> {
#Override
public int compare(PathNode a, PathNode b) {
if(a.totalCost < b.totalCost) return 1;
else return -1;
}
}
}
no comments
http://pastebin.com/rSv7pUrB
I'm guessing something is wrong in the way that the priority queue is ordering the elements or I may have not properly calculated the totalCost, movementCost, and heuristic variables, but I see nothing wrong with it.
Someone that could point me to the right direction of a probable problem or solution is much appreciated
There are several issues with your code:
You never really use the heuristic. The following statement (the only call to calcHeuristic) just "throws the result away".
calcHeuristic(node);
That alone can't be the error here, since it's a valid admissible heuristic to guess the distance to the target to be 0. However the algorithm degenerates that way (to what I think is the Dijkstra algorithm).
You never update the position of the node in the priority queue. That means a node with updated totalDistance will never move up in the proirity queue, even if it's totalCost becomes less than the totalCost of another node. You have to remove the node and add it again to do that with a PriorityQueue:
open.remove(node);
// ... update totalDistance
open.add(node);
You terminate too early for general A* (however that wouldn't be an issue here, since totalDistance is equal to the real distance, for expanded neighbors of the target IF you use the heuristic; here the distance real distance is different by either sqrt(2) or 1). In general the distance heuristic for the last step can be arbitrary bad (and here it's bad, see (1.)) and you can only be sure you found the real solution, if you run the algorithm to the point where you would expand the target node.
Estimated Streak324:
Altough your implementation of A* is now working properly, I recommend you to do a quick search in Internet for java search libraries. Your code will look much more simple, scalable and modular, and the implementations are very efficient and well tested. This will be your code using Hipster:
//HERE YOU DEFINE THE SEARCH PROBLEM
// The maze is a 2D map, where each tile defined by 2D coordinates x and y
// can be empty or occupied by an obstacle. We have to define de transition
// function that tells the algorithm which are the available movements from
// a concrete tile point.
SearchProblem p = ProblemBuilder.create()
.initialState(origin)
.defineProblemWithoutActions()
.useTransitionFunction(new StateTransitionFunction<Point>() {
#Override
public Iterable<Point> successorsOf(Point state) {
// The transition function returns a collection of transitions.
// A transition is basically a class Transition with two attributes:
// source point (from) and destination point (to). Our source point
// is the current point argument. We have to compute which are the
// available movements (destination points) from the current point.
// Class Maze has a helper method that tell us the empty points
// (where we can move) available:
//TODO: FILL WITH YOUR CODE GENERATING THE NEIGHBORS, FILTERING
//THOSE WHICH ARE NOT ACCESIBLE DUE TO OBSTACLES IN YOUR MAP
return [...]
}
})
.useCostFunction(new CostFunction<Void, Point, Double>() {
// We know now how to move (transitions) from each tile. We need to define the cost
// of each movement. A diagonal movement (for example, from (0,0) to (1,1)) is longer
// than a top/down/left/right movement. Although this is straightforward, if you don't
// know why, read this http://www.policyalmanac.org/games/aStarTutorial.htm.
// For this purpose, we define a CostFunction that computes the cost of each movement.
// The CostFunction is an interface with two generic types: S - the state, and T - the cost
// type. We use Points as states in this problem, and for example doubles to compute the distances:
#Override
public Double evaluate(Transition<Void, Point> transition) {
Point source = transition.getFromState();
Point destination = transition.getState();
// The distance from the source to de destination is the euclidean
// distance between these two points http://en.wikipedia.org/wiki/Euclidean_distance
return source.distance(destination);
}
})
.build();
//HERE YOU INSTANTIATE THE ALGORITHM AND EXECUTE THE SEARCH
//MazeSearch.printSearch(Hipster.createAStar(p).iterator(), maze);
System.out.println(Hipster.createAStar(p).search(goal));
As you can see, you only need to define the components to be used in the search problem, and then execute the algorithm. The library will do the rest of the operations for you.
Also, the library is open-source and licsensed Apache2. You have access to several examples that will help you to start working with the library.
In your case, as you are using a custom 2D grid, the only thing you need to adapt is the transition function which checks your grid to filter those neighbors not accesible due to obstacles.
The inmediate benefit of using this implementation is, apart of the scalability and modularity of the code, avoid instantiating all the nodes in the path, as the library will do it dynamically for you, reducing memory and increasing performance (specially in cases of huge grids).
I hope my answer helps,