Библиотека PolyMap

Материал из roboforum.ru Wiki
Перейти к: навигация, поиск
Информация
         Теория

Теоретические выкладки по этой теме можно найти на странице Перемещение робота в нужную точку векторной карты



Назначение библиотеки

Решение задачи поиска пути на векторной карте с отступом от препятствий на требуемое расстояние.

Функции (описание)

saveToFile

Сохранить карту в файл.

loadFromFile

Загрузить карту из файла.

addObstacle

Добавить на карту информацию о препятствии в переданном многоугольнике.

addFreeSpace

Добавить на карту информацию о свободном пространстве в переданном многоугольнике.

prepareWayFindingMap

Подготовить информацию для поиска пути. Параметры - дистанция безопасности, количество вершин в скруглении и требуется ли поиск только путей в разведанной области.

findShortestWay

Найти самый короткий путь между переданными точками. Вызывать только при подготовленной информации для поиска пути.

Файлы библиотеки

Пока выложена альфа-версия библиотеки без работы с файлами, ограниченная по количеству вершин в графе - 1000 и с доступным количеством вершин на скруглениях - только 2.

PolyMap.h

<source lang="cpp">

  1. ifndef PolyMapH
  2. define PolyMapH

//---------------------------------------------------------------------------

  1. include "myPolyBool.h"
  2. include <dstring.h>
  3. include <StdCtrls.hpp>

struct pmPoint{ 

 int x,y;
 pmPoint* next;

};

class pmMap{ public: 

 //Сама карта
 PAREA* revealed;
 PAREA* obstacles;

 //Карта запретных зон для поиска пути
 PAREA* restricted;
 PAREA* safe_distanced;

 TMemo* tm;

 //Структуры данных для хранения графа
 double vert_x[1000];
 double vert_y[1000];
 int vert_qty;
 int graph_ready;
 double graph[1000][1000];

 //Структуры данных для дейкстры
 int vert_queue[1000];
 int vert_from[1000];
 int vert_flag[1000];
 double vert_dist[1000];

 PLINE2* getAroundLine(int x1, int y1, int x2, int y2, int safe_distance, int rounding_size);
 int intersectLine2Lines(int x1, int y1, int x2, int y2, int xp, int yp, int xc, int yc, int xn, int yn);
 int intersectLinePAREA(int x1, int y1, int x2, int y2, PAREA* p);
 void stepBackSafeDistance(int safe_distance, int rounding_size);
 void getRestricted(int only_revealed);
 void generateVertexList();
 void generateGraph();

public: 

 pmMap();
 ~pmMap();
 void saveToFile(const char* filename);
 void loadFromFile(const char* filename);
 void addObstacle(pmPoint* poly);
 void addFreeSpace(pmPoint* poly);
 void prepareWayFindingMap(int safe_distance, int rounding_size, int only_revealed);
 pmPoint* findShortestWay(pmPoint* start, pmPoint* finish);

};

//---------------------------------------------------------------------------

  1. endif

</source>

PolyMap.cpp

<source lang="cpp">

  1. pragma hdrstop
  2. include "PolyMap.h"
  3. include <Math.h>
  4. include "myBasicGeometry.h"

//---------------------------------------------------------------------------

  1. pragma package(smart_init)

void pmMap::saveToFile(const char* filename){ 

 //Save

};

void pmMap::loadFromFile(const char* filename){ 

 PAREA::Del(&revealed);
 PAREA::Del(&obstacles);
 PAREA::Del(&restricted);
 //Load

};

PLINE2* pmMap::getAroundLine(int x1, int y1, int x2, int y2, int safe_distance, int rounding_size){ 

 double dx=x2-x1;
 double dy=y2-y1;
 double dd=sqrt(dx*dx+dy*dy);
 double du_x=dx/dd*safe_distance; //Сонаправленный вектор длиной safe_distance
 double du_y=dy/dd*safe_distance; //Сонаправленный вектор длиной safe_distance
 double dv_x=du_y;  //Перпендикулярный вектор длиной safe_distance
 double dv_y=-du_x; //Перпендикулярный вектор длиной safe_distance

 PLINE2* p=NULL;
 GRID2 g;

 //пока будем всё тупо обводить прямоугольниками
 if( rounding_size>2 ){ rounding_size=2; };

 if( rounding_size<3 ){
   g.x=x2+dv_x+du_x;
   g.y=y2+dv_y+du_y;
   PLINE2::Incl(&p, g);
   g.x=x2-dv_x+du_x;
   g.y=y2-dv_y+du_y;
   PLINE2::Incl(&p, g);
   g.x=x1-dv_x-du_x;
   g.y=y1-dv_y-du_y;
   PLINE2::Incl(&p, g);
   g.x=x1+dv_x-du_x;
   g.y=y1+dv_y-du_y;
   PLINE2::Incl(&p, g);
 }else{
   //Тут будем по хитрому отступать на safe_distance, а пока упростим задачу
 };
 p->Prepare();
 if (not p->IsOuter()) // make sure the contour is outer
 p->Invert();
 return p;

};

void pmMap::getRestricted(int only_revealed){ 

 if(only_revealed==1){

   //Подготовим область перекрывающую всё поле
   PAREA* r=NULL;
   PLINE2* pline = NULL;
   GRID2 gr;
   gr.x=500000;
   gr.y=500000;
   PLINE2::Incl(&pline, gr);
   gr.x=-500000;
   gr.y=500000;
   PLINE2::Incl(&pline, gr);
   gr.x=-500000;
   gr.y=-500000;
   PLINE2::Incl(&pline, gr);
   gr.x=500000;
   gr.y=-500000;
   PLINE2::Incl(&pline, gr);
   pline->Prepare();
   if (not pline->IsOuter()) // make sure the contour is outer
     pline->Invert();
   PAREA::InclPline(&r, pline);

   // Запретная зона - всё неразведанное и препятствия
   PAREA* r1=NULL;

   //Неразведанное
   int err = PAREA::Boolean(r, revealed, &r1, PAREA::SB);

   //Препятствия
   PAREA::Del(&restricted);
   restricted=NULL;
   err = PAREA::Boolean(r1, obstacles, &restricted, PAREA::UN);

 }else{

   //Запретная зона - только препятствия
   restricted=obstacles->Copy();

 };

};

void pmMap::stepBackSafeDistance(int safe_distance, int rounding_size){ 

 //Начнем с области restricted
 PAREA* r0=restricted->Copy();
 PAREA* r1=NULL;

 PAREA* curl=restricted;
 while( curl!=NULL ){

 //Будем добавлять к ней по очереди контуры вокруг её ребер
 PLINE2* cur;
 cur=curl->cntr;
 while(cur!=NULL){
   VNODE2* nc;
   nc=cur->head;
   while(nc!=NULL){
     PLINE2* x=getAroundLine(nc->g.x,nc->g.y,nc->next->g.x,nc->next->g.y,safe_distance,rounding_size);
     x->Prepare();
     if (not x->IsOuter()) // make sure the contour is outer
     x->Invert();

     //Добавим полученный контур к уже имеющейся области
     PAREA* b=NULL;
     PAREA::InclPline(&b, x);
     int err = PAREA::Boolean(r0, b, &r1, PAREA::UN); //Будем избегать на расстоянии Safe_Distance
     PAREA::Del(&r0);
     PAREA::Del(&b);
     r0=r1;
     r1=NULL;

     nc=nc->next;
     if(nc==cur->head){nc=NULL;};
   };
   cur=cur->next;
   if(cur==curl->cntr){cur=NULL;};
 };

 curl=curl->f;
 if(curl==restricted){ curl=NULL; };
 };

 //сохраним полученную область в safe_distanced
 PAREA::Del(&safe_distanced);
 safe_distanced=r0;
 r0=NULL;

};

int pmMap::intersectLinePAREA(int x1, int y1, int x2, int y2, PAREA* p){ 

 PAREA* curl=p;
 while( curl!=NULL ){

 PLINE2* cur=curl->cntr;
 while(cur != NULL){
   VNODE2* vcur=cur->head;
   while(vcur != NULL){
     int xp=vcur->prev->g.x;
     int yp=vcur->prev->g.y;
     int xc=vcur->g.x;
     int yc=vcur->g.y;
     int xn=vcur->next->g.x;
     int yn=vcur->next->g.y;
     int rs=rfwLineAndPolygonSectionCrossing(x1,y1,x2,y2,xp,yp,xc,yc,xn,yn);
     if(rs==1){ return 1; };
     vcur=vcur->next;
     if(vcur==cur->head){ vcur=NULL; };
   };
   cur=cur->next;
   if(cur==curl->cntr){ cur=NULL; };
 };

 curl=curl->f;
 if(curl==p){ curl=NULL; };
 };

 return 0;

};

void pmMap::generateGraph(){ 

 for(int i=0; i<vert_qty; i++){
 for(int j=i+1; j<vert_qty; j++){
   if(intersectLinePAREA(vert_x[i],vert_y[i],vert_x[j],vert_y[j],safe_distanced)==0){
     graph[i][j]=sqrt((vert_x[i]-vert_x[j])*(vert_x[i]-vert_x[j])+(vert_y[i]-vert_y[j])*(vert_y[i]-vert_y[j]));
     graph[j][i]=graph[i][j];
   }else{
     graph[i][j]=-1;
     graph[j][i]=graph[i][j];
   };
 };};

};

void pmMap::generateVertexList(){ 

 vert_qty=0;

 PAREA* curl=safe_distanced;
 while( curl!=NULL ){



 //Выделим вершины на которых будем строить граф
 PLINE2* cur=curl->cntr;
 while(cur != NULL){
   VNODE2* vcur=cur->head;
   while(vcur != NULL){
     int dx1=vcur->g.x-vcur->prev->g.x;
     int dy1=vcur->g.y-vcur->prev->g.y;
     int dx2=vcur->next->g.x-vcur->g.x;
     int dy2=vcur->next->g.y-vcur->g.y;
     if(dx1*dy2>dy1*dx2){
       vert_x[vert_qty]=vcur->g.x;
       vert_y[vert_qty]=vcur->g.y;
       vert_qty++;
       if(vert_qty>998){vert_qty=1/0;};
     };
     vcur=vcur->next;
     if(vcur==cur->head){ vcur=NULL; };
   };
   cur=cur->next;
   if(cur==curl->cntr){ cur=NULL; };
 };

 curl=curl->f;
 if(curl==safe_distanced){ curl=NULL; };
 };

};

void pmMap::prepareWayFindingMap(int safe_distance, int rounding_size, int only_revealed){ 

 //Сначала получим что будем объезжать - препятствия или еще неразведанное
 getRestricted(only_revealed);

 //Затем отступим от всех препятствий на SAFE_DISTANCE
 stepBackSafeDistance(safe_distance, rounding_size);

 //Выделим вершины
 generateVertexList();

 //Построим граф
 generateGraph();

};

pmPoint* pmMap::findShortestWay(pmPoint* start, pmPoint* finish) { 

 //Добавим стартовую и финишную вершины
 int v_qty=vert_qty+2;
 vert_x[vert_qty]=start->x;
 vert_y[vert_qty]=start->y;
 vert_x[vert_qty+1]=finish->x;
 vert_y[vert_qty+1]=finish->y;

 //Достроим граф для них
 for(int i=0; i<v_qty; i++){
 for(int j=vert_qty; j<v_qty; j++){
   if(intersectLinePAREA(vert_x[i],vert_y[i],vert_x[j],vert_y[j],safe_distanced)==0){
     graph[i][j]=sqrt((vert_x[i]-vert_x[j])*(vert_x[i]-vert_x[j])+(vert_y[i]-vert_y[j])*(vert_y[i]-vert_y[j]));
     graph[j][i]=graph[i][j];
   }else{
     graph[i][j]=-1;
     graph[j][i]=graph[i][j];
   };
 };};

 //Подготовим всё для Дейкстры, начнем с финишной вершины
 for(int i=0; i<v_qty; i++) vert_flag[i]=0;
 int queue_start=0;
 int queue_end=1;
 vert_dist[vert_qty+1]=0;
 vert_queue[0]=vert_qty+1;
 vert_flag[vert_qty+1]=1;
 vert_from[vert_qty+1]=-1;

 //Основной цикл Дейкстры
 while(queue_start<queue_end){
   int cur=vert_queue[queue_start];
   if( cur==vert_qty ){
     queue_start=queue_end;
   }else{
     for(int i=0; i<v_qty; i++) if(i!=cur){



     if(graph[cur][i]>=0){
       if( vert_flag[i]==0 ){
         vert_flag[i]=1;
         int j=queue_end;
         vert_queue[queue_end++]=i;
         vert_dist[i]=vert_dist[cur]+graph[cur][i];
         vert_from[i]=cur;
         while(vert_dist[vert_queue[j]]<vert_dist[vert_queue[j-1]]){
           int k=vert_queue[j];
           vert_queue[j]=vert_queue[j-1];
           vert_queue[j-1]=k;
           j--;
         };
       }else{
         if( vert_dist[i]>vert_dist[cur]+graph[cur][i] ){
           vert_dist[i]=vert_dist[cur]+graph[cur][i];
           vert_from[i]=cur;
         };
       };
     };

     };
   };
   queue_start++;
 };

 //Если смогли дойти до стартовой вершины, тогда
 if(vert_flag[vert_qty]==0){
   return NULL;
 }else{
   //Заполним структуру показывающую найденный путь и вернём её.
   pmPoint* p=new pmPoint();
   pmPoint* x=p;
   int cur=vert_qty;
   while(cur!=-1){
     p->x=vert_x[cur];
     p->y=vert_y[cur];
     cur=vert_from[cur];
     if(cur!=-1){ p->next=new pmPoint(); p=p->next; }else{ p->next=NULL; };
   };
   return x;
 };

};

void pmMap::addObstacle(pmPoint* poly){ 

 pmPoint* cur=poly;
 PAREA* p=NULL;
 PLINE2 * pline = NULL;
 while(cur!=NULL){
   GRID2 gr;
   gr.x=cur->x;
   gr.y=cur->y;
   PLINE2::Incl(&pline, gr);
   cur=cur->next;
   if(cur==poly){cur=NULL;};
 };
 pline->Prepare();
 if (not pline->IsOuter()) // make sure the contour is outer
 pline->Invert();
 PAREA::InclPline(&p, pline);

 // Добавим к разведанной области
 PAREA * R = NULL;
 int err = PAREA::Boolean(revealed, p, &R, PAREA::UN);
 PAREA::Del(&revealed);
 revealed=R;

 // Добавим к препятствиям
 R = NULL;
 err = PAREA::Boolean(obstacles, p, &R, PAREA::UN);
 PAREA::Del(&obstacles);
 obstacles=R;

};

void pmMap::addFreeSpace(pmPoint* poly){ 

 pmPoint* cur=poly;
 PAREA* p=NULL;
 PLINE2 * pline = NULL;
 while(cur!=NULL){
   GRID2 gr;
   gr.x=cur->x;
   gr.y=cur->y;
   PLINE2::Incl(&pline, gr);
   cur=cur->next;
   if(cur==poly){cur=NULL;};
 };
 pline->Prepare();
 if (not pline->IsOuter()) // make sure the contour is outer
 pline->Invert();
 PAREA::InclPline(&p, pline);

 // Добавим к разведанной области
 PAREA * R = NULL;
 int err = PAREA::Boolean(revealed, p, &R, PAREA::UN);
 PAREA::Del(&revealed);
 revealed=R;

};

pmMap::pmMap(){ 

 revealed=NULL;
 obstacles=NULL;
 restricted=NULL;
 safe_distanced=NULL;
 graph_ready=0;
 vert_qty=0;

};

pmMap::~pmMap(){ 

 PAREA::Del(&revealed);
 PAREA::Del(&obstacles);
 PAREA::Del(&restricted);
 PAREA::Del(&safe_distanced);

};

</source>

Источник — «https://wiki.roboforum.ru/index.php?title=Библиотека_PolyMap&oldid=3790»