I'm working on a path planning algorithm that will be converted to RobotC. I'm trying to optimize it so that it uses the least amount of memory, as the robot it will be implemented on has supposedly as little as 15k available memory.
It may just be the nature of the algorithm used, but it is apparent that it is making some bad choices when it gets to the middle left part of the grid. Any idea how I might optimize this a bit? Perhaps using a real distance measurement? This is important because the program searching the entire map could result in running out of memory. I understand that this probably can't be completely prevented, but I'd like to mitigate it as much as possible.
Extracted algorithm from code snippet below
setTimeout is for visual feedback / understanding of algorithm.
function scanCells() {
if (openCells > 0) {
setTimeout(function() {
if (current == undefined) {
finished();
return;
}
open[current.x][current.y] = false;
$(td[current.x][current.y]).css({ "background-color": '#FFFF00' });
if (current.x == goal.x && current.y == goal.y) {
pathFound = true;
finished();
return;
}
var nodeDistances = {
node1: 1000,
node2 : 1000,
node3 : 1000,
node4 : 1000
};
// Get the best node.
// Search adjacent tiles.
var distance = 1000;
var node = {};
node.x = -1;
node.y = -1;
if (current.x - 1 >= 0 && open[current.x - 1][current.y]) {
node.x = current.x - 1;
node.y = current.y;
distance = genericDistance(current.x - 1, current.y, goal.x, goal.y);
}
if (current.y - 1 >= 0 && open[current.x][current.y - 1] && (genericDistance(current.x, current.y - 1, goal.x, goal.y) < distance)) {
node.x = current.x;
node.y = current.y - 1;
distance = genericDistance(current.x, current.y - 1, goal.x, goal.y);
}
if (current.x + 1 < 10 && open[current.x + 1][current.y] && genericDistance(current.x + 1, current.y, goal.x, goal.y) < distance) {
node.x = current.x + 1;
node.y = current.y;
distance = genericDistance(current.x + 1, current.y, goal.x, goal.y);
}
if (current.y + 1 < 10 && open[current.x][current.y + 1] && genericDistance(current.x, current.y + 1, goal.x, goal.y) < distance) {
node.x = current.x;
node.y = current.y + 1;
distance = genericDistance(current.x, current.y + 1, goal.x, goal.y);
}
if (node.x == -1 || node.y == -1) {
// We need to step backwards until a node is available.
//path[pathIndex] = null;
path[pathIndex - 1] = null;
pathIndex -= 1;
current = path[pathIndex - 1];
scanCells();
return;
//continue;
}
$(td[current.x][current.y]).css({ "background-color": '#AAAA00' });
current = node;
path[pathIndex++] = current;
scanCells();
}, 1000);
}
else {
finished();
}
}
scanCells();
var grid = [];
var open = [];
var current = {};
var start = {};
var goal = {};
var openCells = 0;
var path = [];
var pathIndex = 0;
// init grid
for (var x = 0; x < 10; x++) {
grid[x] = [];
open[x] = [];
for (var y = 0; y < 10; y++) {
grid[x][y] = 0;
open[x][y] = true;
openCells++;
}
}
open[3][5] = false;
//open[2][5] = false;
//open[2][4] = false;
open[2][3] = false;
open[3][3] = false;
open[8][8] = false;
open[8][6] = false;
open[8][7] = false;
open[8][5] = false;
open[8][4] = false;
open[8][3] = false;
open[8][2] = false;
open[8][1] = false;
open[6][1] = false;
open[6][2] = false;
open[6][3] = false;
open[4][5] = false;
open[4][6] = false;
open[5][4] = false;
open[6][4] = false;
open[7][4] = false;
open[4][7] = false;
open[4][8] = false;
open[4][9] = false;
open[7][8] = false;
open[6][8] = false;
open[5][6] = false;
open[6][6] = false;
open[1][0] = false;
open[1][1] = false;
open[1][2] = false;
open[1][3] = false;
open[1][4] = false;
open[1][5] = false;
open[1][6] = false;
open[1][7] = false;
open[1][8] = false;
open[0][0] = false;
openCells--;
start.x = 0;
start.y = 0;
goal.x = 5;
goal.y = 5;
var pathFound = false;
function genericDistance(x1, y1, x2, y2) {
return Math.abs(x2 - x1) + Math.abs(y2 - y1);
}
// Draw grid / path
var table = $("<table></table>").appendTo("body");
var tr = [];
var td = [];
for (var x = 0; x < 10; x++) {
tr[x] = $("<tr></tr>").appendTo(table);
td[x] = [];
for (var y = 0; y < 10; y++) {
td[x][y] = $("<td></td>").appendTo(tr[x]).css({
width : "50px",
height : "50px",
border : "1px solid #000000"
});
}
}
for (var x = 0; x < 10; x++) {
for (var y = 0; y < 10; y++) {
var t = '#FF0000';
if (open[x][y]) t = '#00FF00';
td[x][y].css({ "background-color": t });
}
}
td[goal.x][goal.y].css({ "background-color" : '#0000FF' });
current = start;
path[pathIndex++] = current;
//while (openCells > 0) {
function scanCells() {
if (openCells > 0) {
setTimeout(function() {
if (current == undefined) {
finished();
return;
}
open[current.x][current.y] = false;
$(td[current.x][current.y]).css({ "background-color": '#FFFF00' });
if (current.x == goal.x && current.y == goal.y) {
pathFound = true;
finished();
return;
}
var nodeDistances = {
node1: 1000,
node2 : 1000,
node3 : 1000,
node4 : 1000
};
// Get the best node.
// Search adjacent tiles.
var distance = 1000;
var node = {};
node.x = -1;
node.y = -1;
if (current.x - 1 >= 0 && open[current.x - 1][current.y]) {
node.x = current.x - 1;
node.y = current.y;
distance = genericDistance(current.x - 1, current.y, goal.x, goal.y);
}
if (current.y - 1 >= 0 && open[current.x][current.y - 1] && (genericDistance(current.x, current.y - 1, goal.x, goal.y) < distance)) {
node.x = current.x;
node.y = current.y - 1;
distance = genericDistance(current.x, current.y - 1, goal.x, goal.y);
}
if (current.x + 1 < 10 && open[current.x + 1][current.y] && genericDistance(current.x + 1, current.y, goal.x, goal.y) < distance) {
node.x = current.x + 1;
node.y = current.y;
distance = genericDistance(current.x + 1, current.y, goal.x, goal.y);
}
if (current.y + 1 < 10 && open[current.x][current.y + 1] && genericDistance(current.x, current.y + 1, goal.x, goal.y) < distance) {
node.x = current.x;
node.y = current.y + 1;
distance = genericDistance(current.x, current.y + 1, goal.x, goal.y);
}
if (node.x == -1 || node.y == -1) {
// We need to step backwards until a node is available.
//path[pathIndex] = null;
path[pathIndex - 1] = null;
pathIndex -= 1;
current = path[pathIndex - 1];
scanCells();
return;
//continue;
}
$(td[current.x][current.y]).css({ "background-color": '#AAAA00' });
current = node;
path[pathIndex++] = current;
scanCells();
}, 250);
}
else {
finished();
}
}
scanCells();
function finished() {
if (pathFound) {
// Clean up path.
// Work from the goal back to the start position, see if we can shortcut any neighboring nodes.
var pathCleanedUp = true;
while (pathCleanedUp) {
pathCleanedUp = false;
for (var x = pathIndex - 1; x > 0; x--) {
if (pathCleanedUp) break;
for (var i = 0; i < pathIndex && i < x - 1; i++) {
if (pathCleanedUp) break;
// Node comes before x and is a neighbor of x
if (genericDistance(path[i].x, path[i].y, path[x].x, path[x].y) == 1) {
pathCleanedUp = true;
// Trim the inbetween.
var newPath = [];
for (var z = 0; z <= i; z++) {
newPath[z] = { x : path[z].x, y : path[z].y };
}
var newPathIndex = z;
for (var z = x; z < pathIndex; z++) {
newPath[newPathIndex++] = { x : path[z].x, y : path[z].y };
}
path = newPath;
pathIndex = newPathIndex;
}
else if ((path[i].x != path[x].x && path[i].y == path[x].y) || (path[i].x == path[x].x && path[i].y != path[x].y)) {
console.log("Connection:");
console.log(path[i]);
console.log(path[x]);
var connected = true;
// If nodes are within line of site and no used nodes are in between.
if (path[i].x == path[x].x) {
if (path[x].x == 1 && path[x].y == 2) console.log("X shared");
// Walk the Y value over until the values are the same or the node is closed.
if (path[i].y < path[x].y) {
for (var ty = path[i].y + 1; ty < path[x].y; ty++) {
console.log("x: " + path[i].x + " y: " + ty + " open: " + open[path[i].x][ty]);
if (!open[path[i].x][ty]) {
connected = false;
break;
}
}
}
else {
for (var ty = path[i].y - 1; ty > path[x].y; ty--) {
if (!open[path[i].x][ty]) {
connected = false;
break;
}
}
}
}
else if (path[i].y == path[i].y) {
// Walk the X value over until the values are the same or the node is closed.
if (path[i].x < path[x].x) {
for (var ty = path[i].x + 1; ty < path[x].x; ty++) {
if (!open[ty][path[i].y]) {
connected = false;
break;
}
}
}
else {
for (var ty = path[i].x - 1; ty > path[x].x; ty--) {
if (!open[ty][path[i].y]) {
connected = false;
break;
}
}
}
}
if (connected) {
console.log("Connected.");
var dist = 0;
var newPath = [];
for (var z = 0; z <= i; z++) {
newPath[z] = { x : path[z].x, y : path[z].y };
}
// Connect the two paths cutting out the inbetween steps.
if (path[i].x < path[x].x) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].x + dist != path[x].x; ix++) {
dist++;
newPath[ix] = { x : path[i].x + dist, y : path[i].y };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].x > path[x].x) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].x + dist != path[x].x; ix++) {
dist--;
newPath[ix] = { x : path[i].x + dist, y : path[i].y };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].y < path[x].y) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].y + dist != path[x].y; ix++) {
dist++;
newPath[ix] = { x : path[i].x, y : path[i].y + dist };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].y > path[x].y) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].y + dist != path[x].y; ix++) {
dist--;
newPath[ix] = { x : path[i].x, y : path[i].y + dist };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
for (var iz = x; iz < pathIndex; iz++) {
newPath[ix++] = path[iz];
}
pathCleanedUp = true;
path = newPath;
console.log(path);
pathIndex = ix;
}
}
}
}
}
for (var x = 0; x < pathIndex; x++) {
td[path[x].x][path[x].y].html("x");
}
}
else {
console.log("No path found.");
}
}<script src="https://ajax.googleapis.com/ajax/libs/jquery/2.1.1/jquery.min.js"></script>
