Энциклопедия по робототехнике - Геометрия - Многоугольники

Автомат, адаптивный автомат ... разум

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 09 янв 2008, 20:09

=DeaD= писал(а):
Digit писал(а):Не получается, как у людей! :D Т.к. человек помнит, что он где-то не ходил и там имеет смысл поискать, а робот знать этого не будет. И тогда у него будет только два режима:
1. Движение по уже разведанному пути
2. Тотальный поиск несоответствий карты и реального мира

Неа :)


А как? :)

=DeaD= писал(а):
Digit писал(а):А если его заслали не пойми куда, он не сможет сказать, что "хозяин, ты - дурак! дороги нет", а будет тыкаться в стены, пока аккум не сдохнет :)

Почему? Он же когда будет тыкаться в стены он будет их на карту наносить и перерасчитывать путь. Если реально пути не нашлось - тогда хозяин и правда дурак и ему об этом стоит сообщить :)


А каким образом робот будет догадываться, что все неизведанности уже изведаны и все стены, нанесенные на карту - это реальные стены? У нас же и реальные стены, и неизведанное - на карте отмечается как препятствие... Как бот сообразит, что пора говорить хозяину, что тот дурак? Только если после нескольких проходов карты (желательно по всей карте) ничего в рассчитанных путях не меняется. Так? Так что, будем хранить индексы проходов по карте? :) И чем оно существенно отличается от "неизведанная территория"?

=DeaD= писал(а):Анализ неразведанной территории может иметь смысл только если при удачном результате разведки может быть найден более короткий чем уже известный путь, ну либо если может появится путь, когда сейчас вообще пути нет. А если у нас надо проехать в точку находящуюся от нас в метре и у нас уже известен самый короткий путь по разведанному маршруту длиной 4м, а на карте никаких неизвестностей ближе 4м к нам нету, то какой смысл ехать за неизвестностями? :)

Согласен :)
Отсекать по таким критериям разумно. Вот только это не снимает необходимости в отмечании неразведанных территорий ;)
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 09 янв 2008, 20:55

Digit писал(а):Вот только это не снимает необходимости в отмечании неразведанных территорий ;)

Не понял, а я разве говорил, что не нужно хранить неразведанную территорию отдельно от препятствий?

Digit писал(а):А каким образом робот будет догадываться, что все неизведанности уже изведаны и все стены, нанесенные на карту - это реальные стены? У нас же и реальные стены, и неизведанное - на карте отмечается как препятствие...

Я такого не говорил :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 09 янв 2008, 23:24

Вот блин! Мы дискуссию на пустом месте устроили. :)
Я вот про это обсуждал:
=DeaD= писал(а):Еще надо приземлить один вопрос - как храним различную информацию. Всего для информации о площадях у нас есть 3 базовых типа - непроходимая область, проходимая область и неразведанная область.
Теоретически нужно хранить непроходимую область и проходимую, а всё остальное считать неразведанным.
Кто что по этому поводу думает?

Strijar писал(а):Я в первом приближении решил так - "Кругом враги" ;) Т.е. пока карты нет - вокруг, вплотную "стена", а дальномер эту стену отодвигает.

Digit писал(а):Strijar, тут мы где-то обсуждали, что если не отличать неисследованные зоны от исследованных, тогда робот не сможет планировать свой путь из одной комнаты в другую. Он вначале всегда будет утыкаться в стенку, а потом вдоль нее идти... Либо всегда будет идти разведанным маршрутом, даже если маршрут будет не оптимальным. Короче, не понятно, как подходить к процессу разведки территории интеллектуально.

=DeaD= писал(а):А что непонятного то? Всё как у людей :)
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 16 янв 2008, 01:06

Короче освоил я библиотеку PolyBool, всё просто и очень удобно, единственная грабля - все координаты только целые и только не более 500'000 по модулю, но нам думаю на пару лет хватит :)

Завтра выложу утиль для прорисовки многоугольников в объекты TImage в Borland C++ Builder + тестовый проект с библиотекой и начну писать класс для работы с картами. Можно начинать выписывать свои мысли по поводу того, как работаем с картой.

PS: Прицеплен скриншот XOR двух многоугольников с отверстиями.
Вложения
polyxor.jpg
polyxor.jpg (41.67 КиБ) Просмотров: 5003
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 16 янв 2008, 11:41

=DeaD= писал(а):Можно начинать выписывать свои мысли по поводу того, как работаем с картой.


А чего тут выписывать? :) Решили же, что все изначально неизвестное.
Имеем два полигона карты. Один - разведанное\неразведанное. Второй проходимое\непроходимое. Первый весь залит неразведанным (т.е. он "полный"), второй - проходимым (т.е. он пустой).
Из первого вычитается треугольник - луч сонара\сенсора некоторого масштаба. Если получен сигнал "стенка", то масштаб выбираем исходя из полученного расстояния (равно высоте треугольника). Если сигнал не получен, то тут треугольник будет некоторого импирически определенного максимального масштаба.
В случае сигнала "стенка" ко второму полигону прибавляем отрезок\узкий_прямоугольник длиной, равной основанию треугольника в текущем масштабе.

Или ты про что-то другое?
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 16 янв 2008, 12:36

Digit писал(а):
=DeaD= писал(а):Можно начинать выписывать свои мысли по поводу того, как работаем с картой.


А чего тут выписывать? :) Решили же, что все изначально неизвестное.

Нифига, ща напишу :)

Digit писал(а):Имеем два полигона карты. Один - разведанное\неразведанное. Второй проходимое\непроходимое. Первый весь залит неразведанным (т.е. он "полный"), второй - проходимым (т.е. он пустой).

Тогда уже точнее - один разведанное, второй - препятствия. Если бы у нас был 1 тип датчика с узким лучом - было бы всё почти просто :) и делали бы как ты сказал:
Digit писал(а):Из первого вычитается треугольник - луч сонара\сенсора некоторого масштаба. Если получен сигнал "стенка", то масштаб выбираем исходя из полученного расстояния (равно высоте треугольника). Если сигнал не получен, то тут треугольник будет некоторого импирически определенного максимального масштаба.
В случае сигнала "стенка" ко второму полигону прибавляем отрезок\узкий_прямоугольник длиной, равной основанию треугольника в текущем масштабе.

Только раз мы храним разведанное, то к разведанному прибавляем прямоугольник до препятствия.

Итого вариант минимум - храним 2 набора многоугольников - разведанное поле и поле "препятствия".

В крайнем случае так и придется считать, однако в реальности у нас будет сонар и ИК-дальномер, а может еще чтонить типа ИК-бампера :) так что жизнь будет выглядеть сложнее... попробуем прикинуть как это в жизни будет?

1. Вариант с остановкой для замеров - сканируем сонаром, если чисто - радуемся, если нет - сканируем ИК-дальномером внутри области, где должно быть препятствие, результаты загоняем в карту.

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

3. Вариант заводим еще 1 тип информации - "здесь может быть препятствие", в форме границы области, в которой сонар гарантировал, что нет препятствий, из неё вычитаем измерения сонаром и ИК-дальномером.

Вот так вроде...
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 16 янв 2008, 14:09

По первой части согласен. А вот дальше:

=DeaD= писал(а):...skipped...
...однако в реальности у нас будет сонар и ИК-дальномер, а может еще чтонить типа ИК-бампера :) так что жизнь будет выглядеть сложнее... попробуем прикинуть как это в жизни будет?

1. Вариант с остановкой для замеров - сканируем сонаром, если чисто - радуемся, если нет - сканируем ИК-дальномером внутри области, где должно быть препятствие, результаты загоняем в карту.

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

3. Вариант заводим еще 1 тип информации - "здесь может быть препятствие", в форме границы области, в которой сонар гарантировал, что нет препятствий, из неё вычитаем измерения сонаром и ИК-дальномером.

Вот так вроде...


Что-то я в этом не разобрался нефига.
Что такое сонар? Он не определяет расстояние до препятствия?
Если ничего не напутал, то вариант 2 вполне хорош. Все остальное не особо нужно. Счас перескажу, как я понял :)
Сонар у нас ничего о расстоянии сказать не может, а только свободно\занято, зато на относительно большом расстоянии.
ИК-дальномер может сказать дистанцию до препятствия, но на расстоянии меньшем, чем видит сонар.
ИК-бампер всегда подскажет, что предыдущие датчики лоханулись.
Итак, двигаемся куда хотим и сканируем сонаром, пока он не скажет, что препятствие. Пока сканируем, постоянно прибавляем многоугольники к полигону "разведано" и к полигону "свободно от препятствий". Многоугольники эти размером "по-умолчанию", т.е. на сколько ориентировочно "видит" сенсор, причем, видимо, он размером с область видения сенсора, способного определить расстояние (в нашем случае, ИК-дальномера).
В зависимости от того, насколько шустро бот работает сенсорами, возможно, придется остановиться, когда сонар увидит препятствие.
Проверяем ИК-дальномером расстояние в секторе, куда смотрел сонар. При обнаружении расстояния, масштабируем многоугольник "по-умолчанию" до размеров расстояния и прибавляем уже его.

Типа того.
Чего-то оно не сильно отличается от того, что я писал выше :)

ЗЫ
Кстати, чтоб не отрываться по каждому писку о препятствии, надо сделать какую-то быструю проверку на то, увидели ли мы новое препятствие или нет. Т.к. иначе робот навсегда потеряется на кухне, где ножки от стола и табуреток\стульев (в случае, конечно, если проверка расстояний и сканирование будут занимать достаточно большое количество процессорного времени).
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 16 янв 2008, 23:03

Digit писал(а):Что-то я в этом не разобрался нефига.
Что такое сонар? Он не определяет расстояние до препятствия?

Определяет :) но как-бы не совсем, дело в том, что он имеет очень широкий угол в котором видит препятствия, поэтому при получении отраженного сигнала не ясно где оно конкретно с точки зрения направления;

Digit писал(а):Если ничего не напутал, то вариант 2 вполне хорош. Все остальное не особо нужно. Счас перескажу, как я понял :)
Сонар у нас ничего о расстоянии сказать не может, а только свободно\занято, зато на относительно большом расстоянии.

Расстояние он может сказать точно, не может точно сказать направление.

Digit писал(а):ИК-дальномер может сказать дистанцию до препятствия, но на расстоянии меньшем, чем видит сонар.

Дистанция ИК-дальномера не зависит от расстояния на котором работает сонар, но обычно она меньше.

Digit писал(а):ИК-бампер всегда подскажет, что предыдущие датчики лоханулись.

Это как раз не так, ИК-бампер заведомо хуже ИК-дальномера, он просто от безденежья :)

Digit писал(а):Итак, двигаемся куда хотим и сканируем сонаром, пока он не скажет, что препятствие. Пока сканируем, постоянно прибавляем многоугольники к полигону "разведано" и к полигону "свободно от препятствий". Многоугольники эти размером "по-умолчанию", т.е. на сколько ориентировочно "видит" сенсор, причем, видимо, он размером с область видения сенсора, способного определить расстояние (в нашем случае, ИК-дальномера).

Вроде всё так.

Digit писал(а):В зависимости от того, насколько шустро бот работает сенсорами, возможно, придется остановиться, когда сонар увидит препятствие. Проверяем ИК-дальномером расстояние в секторе, куда смотрел сонар. При обнаружении расстояния, масштабируем многоугольник "по-умолчанию" до размеров расстояния и прибавляем уже его.

Ну вроде да, всё так если поведение рассматривать, хотя мы поведение будем выносить на другой уровень в программе.

Digit писал(а):ЗЫ
Кстати, чтоб не отрываться по каждому писку о препятствии, надо сделать какую-то быструю проверку на то, увидели ли мы новое препятствие или нет. Т.к. иначе робот навсегда потеряется на кухне, где ножки от стола и табуреток\стульев (в случае, конечно, если проверка расстояний и сканирование будут занимать достаточно большое количество процессорного времени).

А это просто - если на карте есть препятствие на таком расстоянии значит не паримся, если нет - значит что-то новенькое и можно попробовать обшарить ИК-дальномером.

Добавлено спустя 7 часов 59 минут 39 секунд:
В общем начнем с малого - с варинта 2 :) отсутствие препятствий выявляем сонаром и ИК-дальномером, а наличие - только ИК-дальномером.

Попробуем спроектировать объекта "карта":

Свойства объекта:

- разведанное пространство;
- выявленные препятствия;
- подготовленная карта и граф расстояний для поиска путей;

Методы объекта:

- Обнулить карту;
- Загрузить карту (из указанного файла);
- Сохранить карту (в указанный файл);
- Показать карту (в объекте TImage, - разведанную территорию и поверх неё выявленные препятствия);

- Добавить информацию об отсутствии препятствий (в заданном многоугольнике);
- Добавить информацию о наличии препятствия (в заданном многоугольнике);

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

- Найти ломанный путь из точки А в точку Б;
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 19 янв 2008, 16:11

Выложил [[Библиотека myPolyBool]] чтобы удобно рисовать многоугольники из PolyBoolean на объектах TImage.

Приступил к разработке класса PolyMap - в него заверну всё что для карты, чтобы никакие особенности PolyBoolean наружу не торчали, чтобы потом можно было перейти на другую библиотеку при необходимости.

Добавлено спустя 35 минут 49 секунд:
В ходе раздумий над PolyMap вспомнил, что надо будет писать Дейкстру. И тут мой "двигатель прогресса" подсказал, что может быть и это уже готовое взять? А то либо тормозить будет и памяти жрать немерянно, либо запаришься оптимизировать... :)

В итоге нашел библиотеку продвинутую и распространенную для работы с графами - http://www.boost.org/libs/graph/doc/index.html по ней даже книгу издали. Так что если кому надо - пользуйтесь :) Правда я сам пока еще не решил - может и не заморачиваться на эту навороченную библиотеку... в общем сейчас будем разбираться.

Добавлено спустя 1 час 30 минут 51 секунду:
Покопался в универсальной библиотеке и расхотел пока её пользовать :) слишком сложной - начнем лучше с тупого подхода, но ограничимся дейкстрой по 1000 вершин (то есть 998 внешних вершин в картах "запретных зон") и пофиг, что она пока будет не очень быстрая.

Вспомогательный тип ввёл пока:

Код: Выделить всё
struct pmPoint(
  int x,y;
  pmPoint* next;
};


и набросок класса подготовил. Если кому есть что сказать - говорите :)

Код: Выделить всё
class pmMap{
private:
  //Сама карта
  PAREA* revealed;
  PAREA* obstacles;

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

  //Структуры данных для хранения графа
  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];

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 revealed);
  pmPoint* findShortestWay(pmPoint* start, pmPoint* finish);
};
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 20 янв 2008, 05:11

Да чего-то как-то говорить пока нечего :) Ну набросок класса... Не вполне понятно, что будет по указателям PAREA, вернее, что это за формат данных. Но это не существенно пока :)
В общем, никаких умных мыслей вызвано не было :)
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 20 янв 2008, 10:14

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

Добавлено спустя 2 часа 10 минут 27 секунд:
Выяснил, что описание класса внимательно никто не читал :) там нету конструктора и деструктора. Сейчас добавил :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 20 янв 2008, 19:09

Так написано ж було "набросок" - так и отнеслись :)
Формально можно же классы без конструкторов создавать вроде...
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 21 янв 2008, 23:28

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

Пока сделаны пункты 1-3 (первый толком не тестировался, а пункт 2 пока сделан только для очень грубого контура - прямоугольника).
Зато полностью работает пункт 3 (правда может быть немного тормознуто).

Дальше будем дотестировать пункт 1 и доделывать пункт 4.

Добавлено спустя 4 минуты 10 секунд:
Текущая отладочная версия класса:
Код: Выделить всё
class pmMap{
public:
  //Сама карта
  PAREA* revealed;
  PAREA* obstacles;

  //Карта запретных зон для поиска пути
  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);
  void stepBackSafeDistance(int safe_distance, int rounding_size);
  void getRestricted(int only_revealed);
  void pmMap::generateVertexList();

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


и сам код основных функций:
Код: Выделить всё
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){
  PAREA* r0=restricted->Copy();
  PAREA* r1=NULL;

  //Подготовим обводы для ребер
  PLINE2* cur;
  cur=restricted->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==restricted->cntr){cur=NULL;};
  };

  //Добавим их к PAREA restricted и получим PAREA safe_distance
  PAREA::Del(&safe_distanced);
  safe_distanced=r0;
  r0=NULL;
};

void pmMap::generateVertexList(){
  vert_qty=0;

  //Выделим вершины на которых будем строить граф
  PLINE2* cur=safe_distanced->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==safe_distanced->cntr){ cur=NULL; };
  };
};

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

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

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

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

  //Построим граф

};

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);
};
Вложения
safe_distance.jpg
Вот так отступает от запрещенной зоны наш алгоритм.
safe_distance.jpg (38.95 КиБ) Просмотров: 5221
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение Digit » 22 янв 2008, 11:26

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


А почему в п.4 отсутствуют вершины старта и финиша, а потом мы их "достраиваем" в п.7? Чет смысла не понимаю...

Слушай, ты выкладывай не только исходники, но и откомпилированный проект. А то у меня на работе компилировать не чем, но в перерыв я бы потестил то, что надо тестить... :)
злой полицейский
Аватара пользователя
Digit
 
Сообщения: 3339
Зарегистрирован: 27 ноя 2004, 00:42
Откуда: совсем Москва
ФИО: Григорий

Re: Энциклопедия по робототехнике - Геометрия - Многоугольники

Сообщение =DeaD= » 22 янв 2008, 11:33

Digit писал(а):А почему в п.4 отсутствуют вершины старта и финиша, а потом мы их "достраиваем" в п.7? Чет смысла не понимаю...

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

Digit писал(а):Слушай, ты выкладывай не только исходники, но и откомпилированный проект. А то у меня на работе компилировать не чем, но в перерыв я бы потестил то, что надо тестить... :)

Ты хочешь лично нажать Button1, чтобы увидеть подцепленное изображение? :pardon:
Как только будет чем покрутить - выложу еще и готовый скомпилированный проект.
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24218
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: C++ / PHP / 1C
ФИО: Антон Ботов

Пред.След.

Вернуться в Алгоритмы

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 0