Duda sobre la creación de un pathfinder.

[sens]

Veréis, en mi universidad, me han mandado hacer un programa que trata sobre dibujar un mapa de unas dimensiones determinadas por el usuario, ponerle 3 obstáculos rectangulares dentro, ponerle punto de salida y llegada y finalmente encontrar un camino óptimo. Pues bien, la primera parte ya la tengo hecho, que es la definición del mapa, obstáculos rectangulares y punto de partida y llegada.



El mapa con toda la información está escrito en una tabla llamada t dentro de una estructura.

Código:

struct recinto{
        char t[100][100];
        int energia;
        };
        
        recinto a;

Y voy pintando el mapa sobre la tabla a.t[100][100] según las condiciones de los obstáculos y puntos finales e iniciales.

Mi gran duda es: ¿Alguna idea para trazar el camino evitando obstáculos de una forma sencilla y óptima?

Tengo conocimientos básicos sobre c++, es decir, manejo, tablas, tuplas, condiciones, acciones, funciones y poca cosa más...

Aquí dejo el código por si ayuda a entender algo: (Cualquier duda me la preguntáis)

Código:

#include <stdlib.h>
#include <iostream>
#include <windows.h>
#include <math.h>

using namespace std;

struct recinte{
        char t[100][100];
        int energia;
        };
        
        recinte a;
        
void gotoxy(int x,int y);

void crear_mapa(recinte& a, int dimx, int dimy, int wallx, int wallx2, int wally, int wally2){
    int i, j;
    for(i=0;i<dimy+1;i++){
    for(j=0;j<dimx+1;j++){
    if(j==0||j==dimx||i==0||i==dimy)
    a.t[j][i]=char(178);
    
    else if(i>=wally && i<=wally2 && j>=wallx && j<=wallx2)
    a.t[j][i]=char(177);
    
    else if (a.t[j][i]!=char(177))
    a.t[j][i]='.';
    }
    }
    }
    
void points(recinte& a, int dimx, int dimy, int inix, int iniy, int finx, int finy){
    int i, j;
    for(i=0;i<dimy+1;i++){
    for(j=0;j<dimx+1;j++){
    if(i==iniy && j==inix)       
    a.t[j][i]=char(219);
    
    else if(i==finy && j==finx)
    a.t[j][i]=char(219);
     
    }
    }
    }
    
void dibujar_mapa(recinte a, int dimx, int dimy){
     int i, j;
     for(i=0;i<dimy+1;i++){
     for(j=0;j<dimx+1;j++){
     cout<<a.t[j][i];     
     }
     cout<<endl;
     }
     }
     
void trazar_camino(recinte& a, int dimx, int dimy, int inix, int iniy, int finx, int finy){
     
//Aquí debería trazar el camino sobre la matriz donde he escrito los obstáculos y puntos.

          }
        
int main(void)
{
    int inix, iniy, finx, finy;
    int dimx=40, dimy=40;
    char recinte[dimx][dimy];
    int wallx, wallx2, wally, wally2;
    char escojer;
    
    system("color F0");
    cout<<"PATHFINDER<<endl;
    cout<<"--------------------------"<<endl;
    
    cout<<"DIMENSIONES DEL RECINTO"<<endl;
    cout<<"________________________"<<endl;
    cout<<"DIMENSION X: "; cin>>dimx;
    cout<<"DIMENSION Y: "; cin>>dimy;
    
    while(dimx>70 && dimy>70){
    cout<<"LA DIMENSION MAXIMA DEL RECINTO ES DE 70x70!"<<endl;
    cout<<"VUELVE A INTRODUCIR EL VALOR: "<<endl;
    cout<<"DIMENSION X: "; cin>>dimx;
    cout<<"DIMENSION Y: "; cin>>dimy;
                  }
                  
    cout<<endl;
    cout<<"DEFINICION DE LOS OBSTACULOS: (3 OBSTACULOS MAXIMO)"<<endl;
    cout<<"(SE DEFINEN POR LOS VERTICES QUE FORMAN LA DIAGONAL DEL RECTANGULO)"<<endl;
    cout<<"___________________________________________________________________"<<endl;
    cout<<endl;
    
    cout<<"DEFINICION DEL PRIMER OBSTACULO RECTANGULAR"<<endl;
    cout<<"_____________________________________________"<<endl;
    cout<<"OBSTACLE 1 VERTEX 1 (SUPERIOR-IZQUIERDA):"<<endl; 
    cout<<"coordenada x:"; cin>>wallx; cout<<endl;
    cout<<"coordenada y:"; cin>>wally; cout<<endl;
    cout<<"OBSTACLE 1 VERTEX 2 (INFERIOR-DERECHA):"<<endl;
    cout<<"coordenada x:"; cin>>wallx2; cout<<endl;
    cout<<"coordenada y:"; cin>>wally2; cout<<endl;
    cout<<endl;
    
    while(wallx>wallx2 || wally>wally2){
    cout<<"EL PRIMER VERTEX HA DE SER MES PETIT QUE EL SEGON!"<<endl;
    cout<<"OBSTACLE 1 VERTEX 1 (SUPERIOR-IZQUIERDA):"<<endl; 
    cout<<"coordenada x:"; cin>>wallx; cout<<endl;
    cout<<"coordenada y:"; cin>>wally; cout<<endl;
    cout<<"OBSTACLE 1 VERTEX 2 (INFERIOR-DERECHA):"<<endl;
    cout<<"coordenada x:"; cin>>wallx2; cout<<endl;
    cout<<"coordenada y:"; cin>>wally2; cout<<endl;
                       }
                       
    while(wallx<=0 || wallx2>=dimx || wally<=0 || wally2>=dimy){
    cout<<"EL OBSTACULO NO ESTA DENTRO DE LOS LIMITES DEL RECINTO!"<<endl;
    cout<<"INTRODUCE UN OBSTACULO ENTRE "<<dimx<<" I "<<dimy<<endl;
    cout<<"OBSTACLE 1 VERTEX 1 (SUPERIOR-IZQUIERDA):"<<endl; 
    cout<<"coordenada x:"; cin>>wallx; cout<<endl;
    cout<<"coordenada y:"; cin>>wally; cout<<endl;
    cout<<"OBSTACLE 1 VERTEX 2 (INFERIOR-DERECHA):"<<endl;
    cout<<"coordenada x:"; cin>>wallx2; cout<<endl;
    cout<<"coordenada y:"; cin>>wally2; cout<<endl;
    }
    
                       
    crear_mapa(a, dimx, dimy, wallx, wallx2, wally, wally2);
    system("cls");
    dibujar_mapa(a, dimx, dimy);
    system("pause");
    
    cout<<"QUIERES INTRODUCIR UN OBSTACULO MAS? (y:si, n:no)"; cin>>escojer;
    
    while(escojer!='y' && escojer!='n'){
    cout<<"SI O NO? (y:si, n:no)"; cin>>escojer;
    }
    
    if (escojer=='y'){
    
    cout<<endl;
    cout<<"DEFINICION DEL SEGUNDO OBSTACULO RECTANGULAR"<<endl;
    cout<<"_____________________________________________"<<endl;
    cout<<"OBSTACLE 2 VERTEX 1 (SUPERIOR-IZQUIERDA):"<<endl; 
    cout<<"coordenada x:"; cin>>wallx; cout<<endl;
    cout<<"coordenada y:"; cin>>wally; cout<<endl;
    cout<<"OBSTACLE 2 VERTEX 2 (INFERIOR-DERECHA):"<<endl;
    cout<<"coordenada x:"; cin>>wallx2; cout<<endl;
    cout<<"coordenada y:"; cin>>wally2; cout<<endl;
    cout<<endl;
    
    crear_mapa(a, dimx, dimy, wallx, wallx2, wally, wally2);
    system("cls");
    dibujar_mapa(a, dimx, dimy);
    system("pause");
    }
    
    if (escojer=='y'){
    cout<<"QUIERES INTRODUCIR EL TERCER OBSTACULO? ((y:si, n:no)";  cin>>escojer; 
    while(escojer!='y' && escojer!='n'){
    cout<<"SI O NO? (y:si, n:no)"; cin>>escojer;
    }
    }
    
    if (escojer=='y'){
    
    cout<<endl;
    cout<<"DEFINICION DEL TERCER OBSTACULO RECTANGULAR"<<endl;
    cout<<"_____________________________________________"<<endl;
    cout<<"OBSTACLE 3 VERTEX 1 (SUPERIOR-IZQUIERDA):"<<endl; 
    cout<<"coordenada x:"; cin>>wallx; cout<<endl;
    cout<<"coordenada y:"; cin>>wally; cout<<endl;
    cout<<"OBSTACLE 3 VERTEX 2 (INFERIOR-DERECHA):"<<endl;
    cout<<"coordenada x:"; cin>>wallx2; cout<<endl;
    cout<<"coordenada y:"; cin>>wally2; cout<<endl;
    cout<<endl;
    
    } 
    
    crear_mapa(a, dimx, dimy, wallx, wallx2, wally, wally2);
    system("cls");
    dibujar_mapa(a, dimx, dimy);
    system("pause");
    
    cout<<endl;
    cout<<"DEFINICION DEL PUNTO DE PARTIDA Y PUNTO FINAL"<<endl;
    cout<<"_____________________________________________"<<endl;
    cout<<"PUNTO DE PARTIDA (SALIDA DEL ROBOT):"<<endl; 
    cout<<"coordenada x:"; cin>>inix; cout<<endl;
    cout<<"coordenada y:"; cin>>iniy; cout<<endl;
    cout<<"PUNTO FINAL(DESTINO DEL ROBOT):"<<endl;
    cout<<"coordenada x:"; cin>>finx; cout<<endl;
    cout<<"coordenada y:"; cin>>finy; cout<<endl;
    
    system("cls");
    points(a, dimx, dimy, inix, iniy, finx, finy);
    trazar_camino(a, dimx, dimy, inix, iniy, finx, finy);
    points(a, dimx, dimy, inix, iniy, finx, finy);
    dibujar_mapa(a, dimx, dimy);
    
    system("PAUSE");
    return EXIT_SUCCESS;
}

void gotoxy(int x,int y)
{
     COORD pos;
     pos.X=x;
     pos.Y=y;
     SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos);
}

Pues nada, eso es todo, gracias de antemano, y espero poder resolver este problema que lleva días quebrandome la cabeza jaja.

Salu2!