Instantly share code, notes, and snippets.

Embed
What would you like to do?
#include <iostream>
#include <cmath>
#include <queue>
using namespace std;
const int NUMNODES = 16;
const int LARGE_INT = 100;
const int BLOCK_WEIGHT = 50;
//map
int blockedSquares[4][4] = {{1,1,1,1},
{1,1,0,1},
{1,1,0,1},
{1,1,0,1}};
void dijkstra(int startIndex, int goalIndex);
int minNotChecked(int checked[], int arr[], int size);
int isEmpty(int arr[], int size);
int inBounds(int neighbor, int cur);
int pathWeight(int current, int neighbor);
int isPath(int current, int neighbor);
int main()
{
/*
for (int i = 0; i < 16; i++)
{
cout << "block " << i << " is " << blockedSquares[i%4][i/4] << endl;
}
*/
dijkstra(0,15);
return 0;
}
void dijkstra(int startIndex, int goalIndex)
{
int cur = startIndex;
int checked[16] = {0};
int parents[16] = {0};
int dist[16];
for (int i = 0; i < NUMNODES; i++)
{
dist[i] = LARGE_INT;
}
dist[cur] = 0;
while (!isEmpty(checked, NUMNODES))
{
cur = minNotChecked(checked, dist, NUMNODES);
cout << "cur: " << cur << endl;
checked[cur] = 1;
int neighbors[4] = {cur-1,
cur+4,
cur+1,
cur-4};
for (int i = 0; i < 4; i++)
{
if (inBounds(neighbors[i], cur) && isPath(cur, neighbors[i]))
{
int altDist = dist[cur] + 1;
if (altDist < dist[neighbors[i]])
{
cout << "Path from " << cur << " to " << neighbors[i] << " is shorter than " << dist[neighbors[i]] << endl;
dist[neighbors[i]] = altDist;
parents[neighbors[i]] = cur;
cout << "Distance to " << neighbors[i] << " is now " << dist[neighbors[i]] << endl;
cout << "Parent of " << neighbors[i] << " is now " << cur << endl << endl;
}
}
}
}
for (int i = 0; i < NUMNODES; i++)
{
cout << "B" << i << ": " << parents[i] << " ";
}
cout << endl;
}
int minNotChecked(int checked[], int arr[], int size)
{
//ret index of minimum value
int min = 99999;
int minIndex;
for (int i = 0; i < size; i++)
{
if ((arr[i] < min) && (!checked[i]))
{
//cout << i << " is not checked and dist is " << arr[i] << endl;
min = arr[i];
minIndex = i;
}
}
return minIndex;
}
int isEmpty(int arr[], int size)
{
//zero in arr is not checked, one is checked
for (int i = 0; i < size; i++)
{
if (arr[i] == 0)
{
return 0;
}
}
return 1;
}
int inBounds(int neighbor, int current)
{
cout << "inBounds is checking: " << neighbor << endl;
int currentX = current%4;
int currentY = current/4;
int neighborX = neighbor%4;
int neighborY = neighbor/4;
//valid coords would be (+1,0), (-1,0), (0,+1), (0,-1)
int diffX = abs(neighborX-currentX);
int diffY = abs(neighborY-currentY);
if ((neighbor > 15) || (neighbor < 0))
{
cout << "invalid block index" << endl;
return 0;
}
if ((diffX == 1) && (diffY == 0))
{
cout << diffX << " and " << diffY << " so neighbor" << endl;
return 1;
}
else if ((diffX == 0) && (diffY == 1))
{
cout << diffX << " and " << diffY << " so neighbor" << endl;
return 1;
}
else
{
cout << diffX << " and " << diffY << " so not in bounds" << endl;
}
return 0;
}
int isPath(int current, int neighbor)
{
int currentX = current%4;
int currentY = current/4;
int neighborX = neighbor%4;
int neighborY = neighbor/4;
if ((blockedSquares[currentX][currentY] == 0) || (blockedSquares[neighborX][neighborY] == 0))
{
return 0;
}
return 1;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment