Szukanie drogi w labiryncie - algorytm

#include <stdio.h>
#include <stdlib.h>
#define W 7
#define K 6
char plansza[ W ][ K ];

typedef struct {
    int i, j;
} wspol;
wspol start, end;

void szukam() {
    wspol * wsks = & start, * wske = & end;
    int i, j;
    for( i = 0; i < W; i++ ) {
        for( j = 0; j < K; j++ ) {
            if( plansza[ i ][ j ] == '$' ) {
                wsks->i = i;
                wsks->j = j;
            }
            if( plansza[ i ][ j ] == '@' ) {
                wske->i = i;
                wske->j = j;
            }
        }
    }
}


void droga() {
    wspol * wsks = & start, * wske = & end;
    int sw, sk, ew, ek;
    sw = wsks->i;
    sk = wsks->j;
    ew = wske->i;
    ek = wske->j;
    while( plansza[ sw ][ sk ] != plansza[ ew ][ ek ] ) {
        if( plansza[ sw ][ sk + 1 ] != '#' ) {
            ++sk;
            plansza[ sw ][ sk ] = 'X';
           
        }
        else if( plansza[ sw + 1 ][ sk ] != '#' ) {
            ++sw;
            plansza[ sw ][ sk ] = 'X';
        }
        else if( plansza[ sw ][ sk - 1 ] != '#' ) {
            --sk;
            plansza[ sw ][ sk ] = 'X';
        }
    }
}

void WczytajLab() {
    int i = 0, j = 0;
    char c;
    FILE * obraz;
    obraz = fopen( "obraz.txt", "r" );
    if( obraz ) {
        while( c != EOF ) {
            c = getc( obraz );
            if( c != '\n' ) {
                plansza[ i ][ j ] = c;
                j++;
            }
            else {
                i++;
                j = 0;
            }
        }
    }
    else {
        printf( "Blad otwarcia planszy" );
        return - 1;
    }
    fclose( obraz );
}

void PokazLab() {
    int i, j;
    for( i = 0; i < W; i++ ) {
        for( j = 0; j < K; j++ ) {
            printf( "%c ", plansza[ i ][ j ] );
        }
        printf( "\n" );
    }
}

int main()
{
    WczytajLab();
    PokazLab();
    szukam();
    droga();
    printf( "\n\n" );
    PokazLab();
    return 0;
}