Skip to content

Commit

Permalink
Increase cost of right angles in path finding algo
Browse files Browse the repository at this point in the history 
Signed-off-by: Thomas ADAM <[email protected]>
  • Loading branch information
@tadam50
tadam50 committed Dec 5, 2023
1 parent 8da9d05 commit 9c9ddaa
Show file tree
Hide file tree
Showing 3 changed files with 122 additions and 94 deletions.
105 changes: 34 additions & 71 deletions ...ine-diagram-core/src/main/java/com/powsybl/sld/layout/pathfinding/DijkstraPathFinder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,97 +14,60 @@
*/
public final class DijkstraPathFinder extends AbstractPathFinder {

private static final int[] DX = {1, 0, -1, 0}; // Horizontal moves
private static final int[] DY = {0, 1, 0, -1}; // Vertical moves

public DijkstraPathFinder() {
// Nothing to do
}

@Override
public List<Point> findShortestPath(Grid grid, int startX, int startY, int endX, int endY, boolean setSnakeLineAsObstacle) {
Point start = new Point(startX, startY);
Point end = new Point(endX, endY);
Map<Point, Point> parent = new HashMap<>();
Map<Point, Integer> distance = new HashMap<>();

PriorityQueue<Point> queue = new PriorityQueue<>(Comparator.comparingInt(distance::get));
Point goal = new Point(endX, endY);

distance.put(start, start.manhattanDistance(end));
queue.add(start);
Set<Point> visited = new HashSet<>();
PriorityQueue<Grid.Node> queue = new PriorityQueue<>(Comparator.comparingInt(node -> node.getCost() + node.getDistance()));
queue.add(new Grid.Node(start.x(), start.y(), 0, start.manhattanDistance(goal)));

while (!queue.isEmpty()) {
Point current = queue.poll();
if (current.equals(end)) {
List<Point> path = reconstructPath(parent, end);
// Make path not available
grid.setAvailability(path, false);

return smoothPath(path);
Grid.Node current = queue.poll();
Point currentPoint = current.getPoint();
Grid.Node currentParent = current.getParent();
// Path can be rebuilt only when goal point is reached
if (currentPoint.equals(goal)) {
return rebuildPath(current);
}

for (int i = 0; i < 4; i++) {
int newX = current.x() + DX[i];
int newY = current.y() + DY[i];
Point neighbor = new Point(newX, newY);

if (grid.isValid(neighbor)) {
int newDist = neighbor.manhattanDistance(end);

if (!distance.containsKey(neighbor) || newDist < distance.get(neighbor)) {
distance.put(neighbor, newDist);
parent.put(neighbor, current);
queue.add(neighbor);
// Store node already visited
visited.add(currentPoint);
// Loop on available neighbors
for (Grid.Node neighbor : grid.getNeighbors(currentPoint)) {
Point neighborPoint = neighbor.getPoint();
// Avoid to visit previous visited point
if (!visited.contains(neighborPoint)) {
int cost = 1;
if (currentParent != null && currentParent.getPoint().isRightAngle(currentPoint, neighborPoint)) {
// Assuming right angle are useless
cost++;
}
// Update Node parameters
grid.updateNode(neighborPoint,
current.getCost() + cost,
neighborPoint.manhattanDistance(goal),
current);
// Adding next path node
queue.add(neighbor);
}
}
}
return new ArrayList<>(); // No path found
return new ArrayList<>();
}

private List<Point> reconstructPath(Map<Point, Point> parent, Point end) {
private List<Point> rebuildPath(Grid.Node current) {
// Reconstruct path
List<Point> path = new ArrayList<>();
Point current = end;
while (parent.containsKey(current)) {
path.add(current);
current = parent.get(current);
while (current != null) {
path.add(current.getPoint());
current = current.getParent();
}
Collections.reverse(path);
return path;
}

public static List<Point> smoothPath(List<Point> path) {
if (path.size() < 3) {
return path;
}

List<Point> smoothedPath = new ArrayList<>();
smoothedPath.add(path.get(0));

for (int i = 1; i < path.size() - 1; i++) {
Point prev = smoothedPath.get(smoothedPath.size() - 1);
Point current = path.get(i);
Point next = path.get(i + 1);

if (isRightAngle(prev, current, next)) {
smoothedPath.add(current);
}
}

smoothedPath.add(path.get(path.size() - 1));

return smoothedPath;
}

private static boolean isRightAngle(Point p1,
Point p2,
Point p3) {
// Check if right angles when scalar products are null
int dx1 = p2.x() - p1.x();
int dy1 = p2.y() - p1.y();
int dx2 = p3.x() - p2.x();
int dy2 = p3.y() - p2.y();

return dx1 * dx2 + dy1 * dy2 == 0;
}
}
93 changes: 76 additions & 17 deletions ...agram/single-line-diagram-core/src/main/java/com/powsybl/sld/layout/pathfinding/Grid.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,20 +16,53 @@
public class Grid {

static class Node {
private final Point point;
private Point point;
private int cost;
private int distance;
private Node parent;

public Node(Point p, int cost, int distance) {
this.point = p;
this.cost = cost;
this.distance = distance;
this.parent = null;
}

public Node(int x, int y, int cost, int distance) {
this.point = new Point(x, y);
this.cost = cost;
this.distance = distance;
}

public Point getPoint() {
return point;
}

public Node(int x, int y) {
point = new Point(x, y);
cost = NOT_WALKABLE;
public int getCost() {
return cost;
}

public int x() {
return point.x();
public int getDistance() {
return distance;
}

public int y() {
return point.x();
public Node getParent() {
return parent;
}

public Node setCost(int cost) {
this.cost = cost;
return this;
}

public Node setDistance(int distance) {
this.distance = distance;
return this;
}

public Node setParent(Node parent) {
this.parent = parent;
return this;
}

@Override
Expand All @@ -42,12 +75,13 @@ public boolean equals(Object o) {
}
Node node = (Node) o;
return point == node.point &&
cost == node.cost;
cost == node.cost &&
distance == node.distance;
}

@Override
public int hashCode() {
return Objects.hash(point, cost);
return Objects.hash(point, cost, distance);
}
}

Expand All @@ -64,7 +98,7 @@ public Grid(int width, int height) {
this.nodes = new Node[width][height];
for (int x = 0; x < nodes.length; ++x) {
for (int y = 0; y < nodes[0].length; ++y) {
nodes[x][y] = new Node(x, y);
nodes[x][y] = new Node(x, y, NOT_WALKABLE, 0);
}
}
}
Expand All @@ -77,6 +111,13 @@ public int getWidth() {
return width;
}

public void updateNode(Point point, int cost, int distance, Node parent) {
Node node = getNode(point);
node.cost = cost;
node.distance = distance;
node.parent = parent;
}

private Node getNode(Point point) {
return getNode(point.x(), point.y());
}
Expand All @@ -92,15 +133,33 @@ public void setAvailability(int x, int y, boolean available) {
getNode(x, y).cost = available ? WALKABLE : NOT_WALKABLE;
}

public void setAvailability(Point point, boolean available) {
setAvailability(point.x(), point.y(), available);
public boolean isAvailable(Node n) {
return isAvailable(n.point);
}

public void setAvailability(List<com.powsybl.sld.layout.pathfinding.Point> path, boolean available) {
path.forEach(p -> setAvailability(p, available));
public boolean isAvailable(Point point) {
return point.x() >= 0 && point.x() < width && point.y() >= 0 && point.y() < height && nodes[point.x()][point.y()].cost != -1;
}

public boolean isValid(Point point) {
return point.x() >= 0 && point.x() < width && point.y() >= 0 && point.y() < height && nodes[point.x()][point.y()].cost != -1;
public List<Node> getNeighbors(Point point) {
// Considering only adjacent points
List<Node> neighbors = new ArrayList<>();
Node n = getNode(point.x() + 1, point.y());
if (isAvailable(n)) {
neighbors.add(n);
}
n = getNode(point.x() - 1, point.y());
if (isAvailable(n)) {
neighbors.add(n);
}
n = getNode(point.x(), point.y() + 1);
if (isAvailable(n)) {
neighbors.add(n);
}
n = getNode(point.x(), point.y() - 1);
if (isAvailable(n)) {
neighbors.add(n);
}
return neighbors;
}
}
18 changes: 12 additions & 6 deletions ...gram/single-line-diagram-core/src/main/java/com/powsybl/sld/layout/pathfinding/Point.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,18 @@ public int manhattanDistance(Point other) {
return Math.abs(this.x - other.x) + Math.abs(this.y - other.y);
}

public int angleBetween(Point a, Point b) {
int dx1 = a.x - this.x;
int dy1 = a.y - this.y;
int dx2 = b.x - this.x;
int dy2 = b.y - this.y;
return Math.abs(dx1 * dx2 + dy1 * dy2);
public boolean isRightAngle(Point b, Point c) {
// Check if the angle is a right angle using dot product
int vectorABx = b.x() - x();
int vectorABy = b.y() - y();
int vectorBCx = c.x() - b.x();
int vectorBCy = c.y() - b.y();

// Dot product of vectors AB and BC
int dotProduct = vectorABx * vectorBCx + vectorABy * vectorBCy;

// Check if the dot product is zero (cosine of 90 degrees)
return dotProduct == 0;
}

@Override
Expand Down

0 comments on commit 9c9ddaa

Please sign in to comment.