/*
* Simple Implementierung des Dijkstra-Algorithmus in lite-C. Verwendet ein
* regelmäßiges Gitter als Graphen. Ohne Optimierungen.
*
* Diesen Code in eine .c-Datei kopieren und in SED ausführen.
*
* Getestet in A8.03.2
*/
#include <acknex.h>
#include <default.c>
// Eigenschaften des Graphen
#define NUMBER_OF_COLS 10
#define NUMBER_OF_ROWS 10
#define NUMBER_OF_NODES 100
// Die Kosten, um von einem Knoten zum Nachbarknoten zu gelangen.
// Der Wert an sich ist hier ohne große Bedeutung.
#define EDGE_COST 10.0
// Kennzeichnet ungültige bzw. nicht vorhandene Knoten.
#define NULLNODE -1
// row-major order Berechnung
#define NODE_INDEX(col, row) ((row) * NUMBER_OF_COLS + (col))
// Nummer des Vorgängerknotens für jeden Knoten
int nodeParent[NUMBER_OF_NODES];
// Pfadkosten vom Startknoten bis zu jedem Knoten
var nodePathCost[NUMBER_OF_NODES];
// speichert, ob sich der jeweilige Knoten in der Open List befindet
BOOL inOpenList[NUMBER_OF_NODES];
// speichert die Passierbarkeit eines Knotens: 0 = passierbar, 1 = unpassierbar
BOOL nodeBlocked[NUMBER_OF_NODES] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 1, 1, 0, 1, 1,
0, 0, 1, 1, 1, 1, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 1, 1, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
// speichert den Pfad vom Zielknoten bis zum Startknoten
int path[NUMBER_OF_NODES];
// Prototypen
BOOL isOpenListEmpty();
int getBestNodeFromOpenList();
void reconstructPath(int node);
void expandNode(int parent);
// Die Hauptfunktion des Suchalgorithmus. Liefert TRUE zurück, wenn ein Pfad
// zwischen Start- und Zielknoren gefunden wird, ansonsten FALSE.
// Der gefundene Pfad wird in umgekehrter Reihenfolge im globalen Array path[]
// gespeichert.
BOOL dijkstra(int start_node, int dest_node)
{
int node;
if ((nodeBlocked[start_node] == TRUE) ||
(nodeBlocked[dest_node] == TRUE))
{
return FALSE;
}
// Knoten initialisieren
for (node = 0; node < NUMBER_OF_NODES; node++)
{
nodeParent[node] = NULLNODE;
nodePathCost[node] = VAR_MAX;
inOpenList[node] = TRUE;
}
nodePathCost[start_node] = 0;
do
{
node = getBestNodeFromOpenList();
inOpenList[node] = FALSE;
if (node == dest_node) // Pfad gefunden!
{
reconstructPath(node);
return TRUE;
}
expandNode(node);
}
while (isOpenListEmpty() == FALSE);
return FALSE;
}
// Liefert TRUE zurück, wenn die Open List keine Knoten mehr enthält,
// ansonsten FALSE.
BOOL isOpenListEmpty()
{
int node = 0;
for (node = 0; node < NUMBER_OF_NODES; node++)
{
if (inOpenList[node] == TRUE)
{
return FALSE;
}
}
return TRUE;
}
// Liefert den Knoten mit den kleinsten Wegkosten aus den Open List zurück.
int getBestNodeFromOpenList()
{
var smallest_path_cost = VAR_MAX;
int best_node = NULLNODE;
int node;
for (node = 0; node < NUMBER_OF_NODES; node++)
{
if ((inOpenList[node] == TRUE) &&
(nodePathCost[node] < smallest_path_cost))
{
best_node = node;
smallest_path_cost = nodePathCost[node];
}
}
return best_node;
}
// Rekonstruiert den Pfad vom Zielknoten (node) bis zum Startknoten.
// Das Ergebnis wird im globalen Array path[] gespeichert.
void reconstructPath(int node)
{
int path_length = 0;
for (; node >= 0; path_length++)
{
path[path_length] = node;
node = nodeParent[node];
}
if (path_length < (NUMBER_OF_NODES - 1))
{
// Markiere das Ende des Pfads
path[path_length] = NULLNODE;
}
}
// Aktualisiert die Wegkosten der Nachbarknoten von parent.
void expandNode(int parent)
{
int parent_col = parent % NUMBER_OF_COLS;
int parent_row = parent / NUMBER_OF_COLS;
int node_col, node_row;
for (node_row = parent_row - 1; node_row <= parent_row + 1; node_row++)
{
for (node_col = parent_col - 1; node_col <= parent_col + 1; node_col++)
{
int node = NODE_INDEX(node_col, node_row);
if ((node_col < 0) || (node_col >= NUMBER_OF_COLS) ||
(node_row < 0) || (node_row >= NUMBER_OF_ROWS) ||
(node == parent))
{
// kein gültiger Nachbarknoten
continue;
}
if ((inOpenList[node] == TRUE) && (nodeBlocked[node] == FALSE))
{
var path_cost = nodePathCost[parent] + EDGE_COST;
if ((node_col != parent_col) && (node_row != parent_row))
{
// diagonale Pfade bekommen höhere Wegkosten
path_cost += EDGE_COST * 0.5;
}
if (path_cost < nodePathCost[node])
{
nodePathCost[node] = path_cost;
nodeParent[node] = parent;
}
}
}
}
}
// Liefert zurück, ob sich der angegebene Knoten im Pfad befindet.
BOOL isNodeInPath(int node)
{
int n;
for (n = 0; n < NUMBER_OF_NODES; n++)
{
if (path[n] == node)
return TRUE;
else if (path[n] == NULLNODE) // Ende des Pfads
break;
}
return FALSE;
}
PANEL *pfindDescPnl =
{
digits(10, 10, "rot: blockierte Knoten\ngruen: Pfad", *, 0, 0);
flags = SHOW;
}
void main()
{
level_load(NULL);
vec_set(&camera->x, vector(50, -80, 180));
vec_set(&camera->pan, vector(90, -80, 0));
path[0] = NULLNODE; // noch kein Pfad gefunden
// Finde den Pfad von Knoten 12 zu Knoten 98
dijkstra(12, 98);
while (1)
{
int row, col;
// Zeichne den Graphen
for (row = 0; row < NUMBER_OF_ROWS; row++)
{
for (col = 0; col < NUMBER_OF_COLS; col++)
{
COLOR node_color;
int node = NODE_INDEX(col, row);
if (isNodeInPath(node) == TRUE)
vec_set(&node_color, vector(0, 255, 0));
else if (nodeBlocked[node] == FALSE)
vec_set(&node_color, vector(128, 128, 128));
else
vec_set(&node_color, vector(0, 0, 255));
draw_point3d(vector(col * 10, -row * 10, 0), &node_color, 100, 5);
}
}
wait(1);
}
}