#include <windows.h>
#include <stdio.h>
#include <queue>
/*
* In der Cell Strucktur werden Ergebnisse aus dem Algorithmus gespeichert
*/
struct Cell
{
bool bMarked; // Zeigt an, ob das Feld schon besucht wurde ( ture = besucht )
int nLastX; // Die X Koordinate des vorherigen Feldes ( -1 = kein vorheriges Feld )
int nLastY; // Die Y Koordinate des vorherigen Feldes
Cell(){
this->bMarked = false;
this->nLastX = -1;
this->nLastY = -1;
}
};
/*
* Die Coordinate Strucktur dient der
*/
struct Coordinate
{
int x, y;
};
// Uninteressant
bool operator<(const Coordinate c1, const Coordinate c2) {
return false;
}
// Uninteressant
bool operator==(const Coordinate c1, const Coordinate c2) {
return false;
}
/*
* In dieser Tabelle steht die Reihenfolge in der die Umliegenden
* Felder abgearbeitet werden sollen
*/
const int DirTable[8][2] = {
{ 0, -1}, // oben
{ 1, 0}, // rechts
{ 0, 1}, // unten
{ -1, 0}, // links
{ 1, -1}, // rechts-oben
{ 1, 1}, // rechts-unten
{ -1, 1}, // links-unten
{ -1, -1} };// links-oben
// Globale Variablen
int heigth; // Hoehe des Konfigurationsraums
int width; // Breite des Konfigurationsraums
BYTE **cspace; // cspace[i][j] gibt die "Farbe" des Konfigurationsraums an
// Die Farben selbst sind in der Funktion SaveAsBitmap als
// Palette definiert. z.B. 1 = Weiss, 2 = Rot, usw. ( siehe unten)
Cell **aCells; // In diesem 2D Feld werden die Ergebisse des Algorithmus abgelegt
// Funktionsprototypen:
bool LoadFromBitmap( char *szFile, BYTE ***array, int *width, int *heigth );
bool SaveAsBitmap( char *szFile, BYTE **array, int width, int heigth );
bool FindPath( int nStartX, int nStartY, int nGoalX, int nGoalY );
void ClearCells();
/*
* main
*/
int main()
{
int x, y;
// Laden des Konfigurationsraums
if( !LoadFromBitmap( "cspace.bmp", &cspace, &width, &heigth ) )
return -1;
// Zellenarray allokieren ( dynamisch auf dem Heap )
aCells = new Cell*[heigth];
for( y=0; y<heigth; y++ )
aCells[y] = new Cell[width];
// Startposition
int nStartX = 100;
int nStartY = 300;
// Zielposition
int nGoalX = 340;
int nGoalY = 20;
DWORD dwStart = GetTickCount();
// Pfad suchen
bool bFound = FindPath( nStartX, nStartY, nGoalX, nGoalY );
DWORD dwElapsed = GetTickCount() - dwStart;
printf( "Die Suche dauerete: %d ms\nErgebnis:\n", dwElapsed );
// �berpr�fen ob ein Pfad gefunden wurde
if( bFound )
{
printf( "Pfad gefunden in %d ms.\n", dwElapsed );
// Den Pfad vom Ziel zum Start zur�ckgehen und den Pfad markieren
x = nGoalX;
y = nGoalY;
while( aCells[y][x].nLastX != -1 ) // Bei -1 sind wir am Start
{
cspace[y][x] = 255; // Schwarzes Pixel setzen
Cell cell = aCells[y][x];
x = cell.nLastX;
y = cell.nLastY;
}
// Bild ausgeben, so das man den Pfad sehen kann
SaveAsBitmap( "csp_path.bmp", cspace, width, heigth );
}
else
{
printf( "Keinen Pfad gefunden!\n" );
}
// Cleanup
for( y=0; y<heigth; y++ ) // Alle Cells vom Heap l�schen
delete [] aCells[y];
delete [] aCells;
system("pause");
return 0;
}
/*
* FindPath - Breadth-First (Breitensuche)
* Algorithmus zum finden eines Pfades vom Start zum Endpunkt ( goal )
*/
bool FindPath( int nStartX, int nStartY, int nGoalX, int nGoalY )
{
printf( "Suche Pfad von %d/%d nach %d/%d\n", nStartX, nStartY, nGoalX, nGoalY );
// ToDo: Zellen initialiseren...
// Queue erzeugen ( aus der STL - Standard Template Library - kommt mit C++ )
std::queue<Coordinate> queue;
// Startzelle:
Coordinate c;
c.x = nStartX;
c.y = nStartY;
// Folgende Zeilen demonstrieren die Manipulation von Queues ( Warteschlange )
// Einfach auskommentieren und testen
/* printf("Queue leer ? : %d\n", queue.empty()); // Am Anfang ist die Queue leer
queue.push( c ); // Element in die Queue anstellen
printf("Queue leer ? : %d\n", queue.empty()); // Jetzt sollte die Queue nicht mehr leer sein
printf( "Elemente in Queue: %d\n", queue.size() ); // Anzahl Elemente in der Queue
Coordinate test = queue.front(); // front() gibt das forderste Element in der Queue zur�ck,
printf("x: %d, y: %d\n", test.x, test.y ); // jedoch wird da Element dadurch nicht entfernt!
queue.pop(); // Entfernt das erste Element aus der Queue
printf("Queue leer ? : %d\n", queue.empty()); // Nun sollte die Queue wieder leer sein
*/
// ToDo: Ihr Algorithmus
// ....
queue.push(c);
aCells[c.y][c.x].bMarked = true;
while( !queue.empty() ) // Solange bis keine Elemente mehr in der Queue sind
{
Coordinate currentCell = queue.front();
queue.pop();
if (currentCell.x == nGoalX && currentCell.y == nGoalY){
return true;
}
else{
for (unsigned int i = 0; i < 8; i++){
Coordinate newCell = currentCell;
newCell.x += DirTable[i][1]; // Offset auf die Base Adress rechnen, um zur umliegenden Zelle zu kommen
newCell.y += DirTable[i][0];
int nX = newCell.x;
int nY = newCell.y;
if ((nX < heigth && nY < width) && (nX && nY)){ // Die Zelle liegt noch im Konfigurationsraum
if (!aCells[nY][nX].bMarked){ // Wenn die Zelle noch nicht markiert ist
if (!cspace[nY][nX]){ // Und keine Kollision stattfindet...
aCells[nY][nX].bMarked = true; // aktuelle Zelle als vorg�nger setzten
aCells[nY][nX].nLastX = currentCell.x;
aCells[nY][nX].nLastY = currentCell.y;
queue.push(newCell);
}
}
}
}
}
}
return false; // Default Return
}
/*
* ClearCells()
* Setzt alle Zellen auf die Initialwerte zur�ck
*/
void ClearCells()
{
// Zellen zur�cksetzen
for( int y=0; y<heigth; y++)
{
for( int x=0; x<width; x++)
{
aCells[y][x].bMarked = false; // Nicht besucht
aCells[y][x].nLastX = -1; // Keine vorherige Zelle
aCells[y][x].nLastY = -1;
}
}
}
/*
* LoadFromBitmap
* L�d ein Konfigurationsraum aus einem Bitmap
* Dabei muss das Bild 8 Bit/Pixel haben
*/
bool LoadFromBitmap( char *szFile, BYTE ***array, int *width, int *heigth )
{
int x, y;
FILE *pFile;
if ( fopen_s(&pFile, szFile, "rb") )
{
printf( "Konnte Datei '%s' nicht oeffnen.\n", szFile );
return false;
}
BITMAPFILEHEADER bmpFileHeader;
BITMAPINFOHEADER bmpInfoHeader;
fread( &bmpFileHeader, sizeof( BITMAPFILEHEADER ), 1, pFile );
fread( &bmpInfoHeader, sizeof( BITMAPINFOHEADER ), 1, pFile );
// Breite und H�he kopieren
int w = (*width) = bmpInfoHeader.biWidth;
int h = (*heigth) = bmpInfoHeader.biHeight;
// Speicher allokieren ( dynamisch auf dem heap )
(*array) = new BYTE*[h];
for( y=0; y<h; y++ )
(*array)[y] = new BYTE[w];
// Zum Anfang der Bits springen
fseek( pFile, bmpFileHeader.bfOffBits, SEEK_SET );
int n =0;
for( y=h-1; y>=0; y-- )
{
for( x=0; x<w; x++ )
{
if( feof( pFile) ) // Nicht gut
break;
BYTE c;
fread( &c, 1, sizeof(BYTE), pFile );
(*array)[y][x] = c;
n++;
}
}
fclose(pFile);
return true;
}
/*
* SaveAsBitmap
* Speichert den Konfigurationsraum als Bitmap.
* Parameter:
* szFile - Dateiname
* array - Das 2D Array mit dem Konfigurationsraum
* width - Breite des Konfigurationsraum
* height - H�he des Konfigrationsraum
*/
bool SaveAsBitmap( char *szFile, BYTE **array, int width, int heigth )
{
BITMAPFILEHEADER bmpFileHeader;
bmpFileHeader.bfType = *((int*)("BM")); // Muss BM sein
bmpFileHeader.bfSize = 0; // Gr��e der Bitmap Datei
bmpFileHeader.bfReserved1 = 0; // Muss 0 sein
bmpFileHeader.bfReserved2 = 0; // Muss 0 sein
bmpFileHeader.bfOffBits = 0; // Offset, in Bytes, zwischen BITMAPFILEHEADE und den Bits
BITMAPINFOHEADER bmpInfoHeader;
bmpInfoHeader.biSize = sizeof( BITMAPINFOHEADER );
bmpInfoHeader.biWidth = width;
bmpInfoHeader.biHeight = heigth;
bmpInfoHeader.biPlanes = 1;
bmpInfoHeader.biBitCount = 8;
bmpInfoHeader.biCompression = BI_RGB;
bmpInfoHeader.biSizeImage = width * heigth * 1;
bmpInfoHeader.biXPelsPerMeter = 2834;
bmpInfoHeader.biYPelsPerMeter = 2834;
bmpInfoHeader.biClrUsed = 256;
bmpInfoHeader.biClrImportant = 256;
FILE *pFile;
if ( fopen_s( &pFile, szFile, "wb" ) )
{
printf( "Konnte Datei '%s' nicht erzeugen.\n", szFile );
return false;
}
fwrite( &bmpFileHeader, sizeof( BITMAPFILEHEADER ), 1, pFile );
fwrite( &bmpInfoHeader, sizeof( BITMAPINFOHEADER ), 1, pFile );
// Farbpalette zum Bild hinzuf�gen
static RGBQUAD colortable[] =
{
{ 255, 255, 255, 0},
{ 255, 0, 0, 0}, // Rot
{ 0, 255, 0, 0}, // Gr�n
{ 0, 0, 255, 0}, // Blau
{ 255, 255, 0, 0}, // Gelb
{ 255, 0, 255, 0}, // Magenta
{ 0, 255, 255, 0}, // Cyan
{ 255, 128, 0, 0}, // Oragne
{ 255, 0, 128, 0}, // Weinrot
{ 0, 255, 128, 0}, // Hell Gr�n
{ 128, 255, 0, 0} // Gift Gr�n
// Wenn die Farben nicht reichen, hier neue hinzuf�gen
};
int nCol = sizeof(colortable) / sizeof(RGBQUAD);
fwrite( colortable, sizeof(RGBQUAD), sizeof(colortable), pFile );
// Alle restlichen Farben auf Schwarz setzen
for( int i=nCol; i<256; i++ )
{
RGBQUAD rgb;
rgb.rgbRed = 0;
rgb.rgbGreen = 0;
rgb.rgbBlue = 0;
rgb.rgbReserved = 0;
fwrite( &rgb, sizeof( RGBQUAD), 1, pFile );
}
// Bits schreiben
bmpFileHeader.bfOffBits = ftell( pFile );
for( int y=heigth-1; y>=0; y-- )
{
for( int x=0; x<width; x++ )
{
putc( array[y][x], pFile );
}
}
bmpFileHeader.bfSize = ftell( pFile );
// File Header nochmal mit neuen Werten �berschreiben
fseek( pFile, 0L, SEEK_SET );
fwrite( &bmpFileHeader, sizeof( BITMAPFILEHEADER ), 1, pFile );
fclose(pFile);
return true;
}