roboforum.ru

=DeaD= писал(а):Ты хочешь лично нажать Button1, чтобы увидеть подцепленное изображение?
Как только будет чем покрутить - выложу еще и готовый скомпилированный проект.

Ну типа побыть причастным к чему-то большому!
А вообще, это я к тому, что ты сказал, что не оттестил еще п.1, так вот я и хочу помочь.

А насчет точек старта и финиша я что-то стормозил Пральный подход

Из подзадач:
1. Формирования запретной зоны (только пряпятствия или еще и неизведанное);
2. Формирование контура вокруг отрезка на расстоянии safe_distance;
3. Формирование контура вокруг многоугольников запретной зоны на расстоянии safe_distance (использует пункт 2);
4. Выбор вершин на которых (кроме старта и финиша) будем строить граф;
5. Проверка - пересекает ли отрезок многоугольник.
6. Построение графа на выбранных вершинах (использует пункт 5);
7. Достройка графа стартовой и финишной вершинами (использует пункт 5);
8. Поиск в графе и извлечение пути;

Пока брошен пункт 1 в случае запрета на перемещение по неизведанному, потом дотестируем.
Добиты пункты 2-3-4, сейчас примемся за 5-й пункт.

Из подзадач:
1. Формирования запретной зоны (только пряпятствия или еще и неизведанное);
2. Формирование контура вокруг отрезка на расстоянии safe_distance;
3. Формирование контура вокруг многоугольников запретной зоны на расстоянии safe_distance (использует пункт 2);
4. Выбор вершин на которых (кроме старта и финиша) будем строить граф;
5. Проверка - пересекает ли отрезок многоугольник.
6. Построение графа на выбранных вершинах (использует пункт 5);
7. Достройка графа стартовой и финишной вершинами (использует пункт 5);
8. Поиск в графе и извлечение пути;

PolyMap.h отладочная версия (добавлено TMemo, чтобы выводить информацию на форму пока все баги не выловим):

Код: Выделить всё • Развернуть

#ifndef PolyMapH
#define PolyMapH
//---------------------------------------------------------------------------
#include "myPolyBool.h"
#include <dstring.h>
#include <StdCtrls.hpp>

struct pmPoint{
  int x,y;
  pmPoint* next;
};

class pmMap{
public:
  //Сама карта
  PAREA* revealed;
  PAREA* obstacles;

  TMemo* tm;

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

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

  //Структуры данных для дейкстры
  int vert_queue[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);
};

//---------------------------------------------------------------------------
#endif

PolyMap.cpp отладочная версия:

Код: Выделить всё • Развернуть

#pragma hdrstop
#include "PolyMap.h"
#include <Math.h>
#include "myBasicGeometry.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)

int pmMap::intersectLine2Lines(int x1, int y1, int x2, int y2, int xp, int yp, int xc, int yc, int xn, int yn){
  if(x1==x2 && y1==y2){ return 0; };
  rfwSection s0,s1,sr;
  rfwPoint pr;
  s0.xa=x1; s0.ya=y1;
  s0.xb=x2; s0.yb=y2;
  s1.xa=xp; s1.ya=yp;
  s1.xb=xc; s1.yb=yc;
  rfwInt res=rfwSectionsCrossing(s0,s1,pr,sr);
  if(res==-1){ return 0; };
  if(res==0){
    //Intersection is section "sr"
    if( ( (sr.xa-xc)*(xc-xp)+(sr.ya-yc)*(yc-yp)>0)
     && (-(sr.xa-xc)*(yn-yc)+(sr.ya-yc)*(xn-xc)>0) ){ return 1; };
    if( ( (sr.xb-xc)*(xc-xp)+(sr.yb-yc)*(yc-yp)>0)
     && (-(sr.xb-xc)*(yn-yc)+(sr.yb-yc)*(xn-xc)>0) ){ return 1; };
    return 0;
  };
  if(res==1){
    //Intersection is point "pr"
    if(pr.x==xp && pr.y==yp){ return 0; }; //PR=P
    if(pr.x==xc && pr.y==yc){ //PR=C
      if( -(xn-xc)*(yc-yp)+(yn-yc)*(xc-xp)>0 ){ //Angle PCN<=180 deg
        if( (-(x1-xc)*(yc-yp)+(y1-yc)*(xc-xp)>0)
         && (-(x1-xc)*(yn-yc)+(y1-yc)*(xn-xc)>0) ){ tm->Lines->Add("q3"); return 1; };
        if( (-(x2-xc)*(yc-yp)+(y2-yc)*(xc-xp)>0)
         && (-(x2-xc)*(yn-yc)+(y2-yc)*(xn-xc)>0) ){ tm->Lines->Add("q4"); return 1; };
        return 0;
      }else{ //Angle PCN>180 deg
        if( (-(x1-xc)*(yc-yp)+(y1-yc)*(xc-xp)>0)
         || (-(x1-xc)*(yn-yc)+(y1-yc)*(xn-xc)>0) ){ tm->Lines->Add("q5"); return 1; };
        if( (-(x2-xc)*(yc-yp)+(y2-yc)*(xc-xp)>0)
         || (-(x2-xc)*(yn-yc)+(y2-yc)*(xn-xc)>0) ){ tm->Lines->Add("q6"); return 1; };
        return 0;
      };
    }else{ //PR belongs to interval (P,C)
      if( -(x1-xc)*(yc-yp)+(y1-yc)*(xc-xp)>0 ){ tm->Lines->Add("q1"); return 1; };
      if( -(x2-xc)*(yc-yp)+(y2-yc)*(xc-xp)>0 ){ tm->Lines->Add("q2"); return 1; };
      return 0;
    };
  };

};

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){
  tm->Lines->Add("## x1="+IntToStr(x1)+" y1="+IntToStr(y1)+
                 " x2="+IntToStr(x2)+" y2="+IntToStr(y2));
  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=intersectLine2Lines(x1,y1,x2,y2,xp,yp,xc,yc,xn,yn);
      if(rs==1){
        tm->Lines->Add(" - x1="+IntToStr(x1)+" y1="+IntToStr(y1)+
                          " x2="+IntToStr(x2)+" y2="+IntToStr(y2)+
                          " xp="+IntToStr(xp)+" yp="+IntToStr(yp)+
                          " xc="+IntToStr(xc)+" yc="+IntToStr(yc)+
                          " xn="+IntToStr(xn)+" yn="+IntToStr(yn)+
                          " res="+IntToStr(rs));
      };
      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){
      tm->Lines->Add("i="+IntToStr(i)+" j="+IntToStr(j)+" res=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{
      tm->Lines->Add("i="+IntToStr(i)+" j="+IntToStr(j)+" res=1");
      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();

};

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);
};

Добиты пункты 5-6 (итого первые 6 готовы, осталось мелочь), чуть не застрелился пункт 5 прописывать перед программированием на бумажке во всех вариантах

Добавлено спустя 8 часов 12 минут 28 секунд:
Я вот думаю какую демку сделать, чтобы тестировать всё это с одной стороны, а с другой стороны - чтобы польза была, а не просто так делать... Может редактор карты сделать и к нему демку - построение карты с отступом safe_distance и поиск пути прикрутить? Хотя с другой стороны редактор карты - не ясно зачем, робот же сам карту строит?

Теоретически, редактор - это хорошо, т.к. можно на комп загрузить карту, ботом построенную, посмотреть чего он ванную найти не может - мож в карте глюк, возможно, подредактировать чуток, и боту назад выдать
Так что редактор - это хорошо !

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

Видимо заниматься этим буду до конца недели.

Добавлено спустя 22 часа 59 минут 51 секунду:
Выложил библиотеки для поиска пути и реализующие геометрию в соответствующие разделы и связал их перекрестными ссылками с теорией.

Добавлено спустя 28 минут 25 секунд:
Всё! Я сделал пробничек, который можно покрутить. Если увидите, что где-то получился глюк с поиском пути - выпишите координаты старта и финиша и выложите их сюда.

Крутить пробничек можно 4 ползунками задающими координаты старта и финиша. После чего надо нажать кнопочку "Find optimal way" и программка найдет оптимальный путь.

Скачал. Буду крутить

Добавлено спустя 9 минут 32 секунды:
Dead, перекомприль с библиотеками плиз! А то пишет вот такое:

Код: Выделить всё • Развернуть

---------------------------
PolyMapTest.exe - Не удалось найти компонент
---------------------------
Приложению не удалось запуститься, поскольку vcl60.bpl не был найден. Повторная установка приложения может исправить эту проблему. 
---------------------------

Digit писал(а):Dead, перекомприль с библиотеками плиз! А то пишет вот такое:

Вроде сделал, проверь плиз (выложено там же).

неа
всё равно не все библы

cc3260mt.dll просит

я ему подсунул а он говорит не та версия

Myp писал(а):неа
всё равно не все библы

cc3260mt.dll просит

я ему подсунул а он говорит не та версия

Блин, вот зараза, понатыкали везде настроек

Выяснил еще один флажок, снял. Пробуй сейчас - файлик выложен там же.

В вике сделал страницу по сборке проектов без DLL - [[Сборка проекта в C++ Builder без использования внешних DLL]] чтобы другим потом было легко эту проблему решать.

всё теперь ворк =)

Подтверждаю, все фунциклирует

хе, под вином работает