Прикладная геометрия

	Реализация на С++
Библиотеку с реализацией большей части этих функций можно найти на странице Библиотека myBasicGeometry

В этом разделе будут рассмотрены решения основных геометрических задач встречающихся при программировании роботов.

Системы координат

Переход от одной системы координат к другой

Координаты препятствия (X0,Y0) в глобальной (Xг,Yг) системе координат и его же координаты (X1,Y1) в локальной (Xл,Yл) системе координат.

Одна из наиболее распространенных геометрических задач, которые предстоит решать робототехнику - переход между системами координат. Чаще всего это нужно будет делать на плоскости, особенно при решении задач глобальной навигации. Типичная задача в робототехнике будет выглядеть так:

Робот находится в координатах (XR,YR) в глобальной системе координат (на карте) и повернут на угол AR по часовой стрелке от направления оси O-X, робот обнаружил препятствие относительно себя в координатах (X1,Y1) локальной системы координат робота. Требуется определить глобальные координаты (X0,Y0) препятствия. Либо наоборот - зная глобальные координаты препятствия требуется вычислить его локальные координаты.

Тройка чисел (XR,YR,AR) - координаты базиса локальной системы координат в глобальной системе координат.

Формулы перевода из локальных координат в глобальные
X0=XR+X1*cos(AR)-Y1*sin(AR) Y0=YR+Y1*cos(AR)+X1*sin(AR)
Формулы обратного перевода координат
X1=(X0-XR)*cos(AR)+(Y0-YR)*sin(AR) Y1=(Y0-YR)*cos(AR)-(X0-XR)*sin(AR)

Проверим: (переведем координаты туда и обратно)

X1=(X0-XR)*cos(AR)+(Y0-YR)*sin(AR)=
=(XR+X1*cos(AR)-Y1*sin(AR)-XR)*cos(AR)+(YR+Y1*cos(AR)+X1*sin(AR)-YR)*sin(AR)=
=(X1*cos(AR)-Y1*sin(AR))*cos(AR)+(Y1*cos(AR)+X1*sin(AR))*sin(AR)=
=X1*cos(AR)*cos(AR)-Y1*sin(AR)*cos(AR)+Y1*cos(AR)*sin(AR)+X1*sin(AR)*sin(AR)=
=X1*cos(AR)*cos(AR)+X1*sin(AR)*sin(AR)=X1*(cos(AR)*cos(AR)+sin(AR)*sin(AR))=X1*(1)=X1,

и вторую координату:

Y1=(Y0-YR)*cos(AR)-(X0-XR)*sin(AR)=
=(YR+Y1*cos(AR)+X1*sin(AR)-YR)*cos(AR)-(XR+X1*cos(AR)-Y1*sin(AR)-XR)*sin(AR)=
=(Y1*cos(AR)+X1*sin(AR))*cos(AR)-(X1*cos(AR)-Y1*sin(AR))*sin(AR)=
=Y1*cos(AR)*cos(AR)+X1*sin(AR)*cos(AR)-X1*cos(AR)*sin(AR)+Y1*sin(AR)*sin(AR)=
=Y1*cos(AR)*cos(AR)+Y1*sin(AR)*sin(AR)=Y1*(cos(AR)*cos(AR)+sin(AR)*sin(AR))=Y1*(1)=Y1.

Точки, прямые, отрезки и операции с ними

Общее уравнение прямой и получение его по двум точкам на прямой

Общее уравнение первой прямой на плоскости выглядит так:

A*x+B*y+C=0

Понятно, что при этом коэффициенты A,B,C задаются с точностью до умножения на ненулевое число. Если где-то нам потребуется однозначность, тогда мы можем дополнительно нормировать эту тройку до A^2+B^2+C^2=1 и тогда останется подобрать лишь знак (надо будет с этим что-то тоже сделать :)).

Теперь, если нам известны две различные точки на этой прямой (x1,y1) и (x2,y2), тогда мы можем выписать вариант этих коэффициентов как:

A=y1-y2, B=x2-x1, C=x1*y2-y1*x2

Далее если нам надо получить коэффициенты (A,B,C) для прямой определяемой отрезком (x1,y1)-(x2,y2), то мы будем пользоваться функцией (внутренность которой определена выше):

(A,B,C)=getLineCoefsFromTwoPoints(x1,y1,x2,y2)

Пересечение двух прямых

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

Чтобы найти пересечение двух прямых A1*x+B1*y+C1=0 и A2*x+B2*y+C2=0 нужно рассмотреть эти два уровнения как систему и решить её.

Для этого найдем три специальные величины:

d=A1*B2-A2*B1
d1=C2*B1-B2*C1
d2=C1*A2-A1*C2

Теперь если d=0 тогда прямые параллельны и совпадают только если d1=d2=0.

А если же d<>0, тогда прямые пересекаются в точке:

x=d1/d
y=d2/d

Принадлежность точки отрезку

Это достаточно простая проверка. Пусть точка имеет координаты (x,y), а отрезок (xa,ya)-(xb,yb). Тогда сначала найдем коэфициенты общего уравнения прямой, на которой лежит отрезок через определенную выше функцию (A,B,C)=getLineCoefsFromTwoPoints(xa,ya,xb,yb). Теперь, если не выполняется условие:

1) A*x+B*y+C=0

Значит точка не лежит на прямой, если же выполняется, то осталось проверить, что точка лежит внутри отрезка:

2) min(xa,xb)<=x<=max(xa,xb)
3) min(ya,yb)<=y<=max(ya,yb)

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

Пересчение двух отрезков

Пересчение отрезков (x1a,y1a)-(x1b,y1b) и (x2a,y2a)-(x2b,y2b).

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

Постановка задачи: Даны два НЕВЫРОЖДЕННЫХ (то есть не являющихся точками) отрезка P1a(x1a,y1a)-P1b(x1b,y1b) и P2a(x2a,y2a)-P2b(x2b,y2b). Требуется определить, пересекаются ли эти два отрезка, если пересекаются, то в какой точке или по какому отрезку.

Решение:

В целом решение будет по следующей схеме:

1. Определяем прямые, на которых лежат отрезки;
2. Если прямые не параллельны, тогда находим точку их пересечения и проверяем, что она лежит на обоих отрезках;
3. Если же прямые параллельны, тогда рассматриваем два варианта:
   1. Если прямые не совпадают, тогда общих точек нет
   2. Если прямые совпали, смотрим взаимное расположение отрезков и если они пересекаются, то выписываем, по какому отрезку.

Теперь подробно по каждому шагу:

Шаг 1.

Координаты точек каждого отрезка определяются формулами:

P1(u)=P1a+u*(P1b-P1a), где 0<=u<=1,
P2(v)=P2a+v*(P2b-P2a), где 0<=v<=1,

Выпишем это в координатах:

x1(u)=x1a+u*(x1b-x1a)
y1(u)=y1a+u*(y1b-y1a)
x2(v)=x2a+v*(x2b-x2a)
y2(v)=y2a+v*(y2b-y2a)

Нам требуется найти такие u,v, что:

x1(u)=x2(v) => x1a+u*(x1b-x1a)=x2a+v*(x2b-x2a)
y1(u)=y2(v) => y1a+u*(y1b-y1a)=y2a+v*(y2b-y2a)

Это система из двух линейных уравнений с двумя неизвестными вида:

A1*u+B1*v+C1=0, где A1=x1b-x1a, B1=x2a-x2b, C1=x1a-x2a
A2*u+B2*v+C2=0, где A2=y1b-y1a, B2=y2a-y2b, C2=y1a-y2a

Выпишем важную величину:

D=A1*B2-A2*B1

Она равна нулю, тогда и только тогда, когда прямые параллельны.

Шаг 2

Если прямые не параллелны (D<>0), тогда можно выписать решение системы как:

u=(B2*C1-C2*B1)/D
v=(C1*A2-A1*C2)/D

Останется лишь проверить условие

0<=u<=1 и 0<=v<=1

чтобы убедится, что точка пересечения прямых принадлежит обоим отрезкам.

Шаг 3

Отрезки на разных прямых (x1a,y1a)-(x1b,y1b) и (x2a,y2a)-(x2b,y2b).

Отрезки лежат на одной прямой (x1a,y1a)-(x1b,y1b) и (x2a,y2a)-(x2b,y2b).

Если же прямые параллельны (D=0), тогда нужно проверять совпадают ли они. Если выполняется условие

D1<>0 или D2<>0, где D1=A1*C2-A2*C1 и D2=B1*C2-B2*C1

тогда прямые не совпадают и общих точек у отрезков нет. Если же D1=D2=0, тогда прямые совпадают и нужно проверить накладываются ли отрезки. Для этого просто выпишем координаты концов второго отрезка в базисе соответствующему первому отрезку:

P2a=P1a+u2a*(P1b-P1a) =>
=> u2a*(P1b-P1a)=P2a-P1a =>
=> u2a*(P1b-P1a)*(P1b-P1a)=(P2a-P1a)*(P1b-P1a) =>
=> u2a=((P2a-P1a)*(P1b-P1a))/((P1b-P1a)*(P1b-P1a))

Аналогично:

P2b=P1a+u2b*(P1b-P1a) => ... =>
u2b=((P2b-P1a)*(P1b-P1a))/((P1b-P1a)*(P1b-P1a))

Теперь если max(u2a,u2b)<0 или min(u2a,u2b)>1 тогда отрезки общих точек не имеют, иначе они пересекаются по отрезку:

P1a+u*(P1b-P1a), где max(0,min(u2a,u2b))<u<min(1,max(u2a,u2b))

Всё.

Скалярное произведение векторов на плоскости

Одним из самых полезных понятий в вычислительной геометрии является скалярное произведение A*B векторов, которое для векторов A=(dx1,dy1) и B=(dx2,dy2) вычисляется по формуле dx1*dx2+dy1*dy2 и равно косинусу угла между векторами умноженному на их длины. Его полезно применять для проверки - направлены ли вектора в одну сторону или нет. А также если у нас есть перпендикулярные вектора X и Y, тогда можно любой вектор Z разложить в линейную комбинацию векторов X,Y по формуле Z=a*X+b*Y, где a=Z*X/|X|^2, b=Z*Y/|Y|^2.

Векторное произведение на плоскости

Другим полезным понятием в вычислительной геометрии является векторное произведение AxB на плоскости, равное ориентированной площади параллелограма натянутого на вектора A=(dx1,dy1) и B=(dx2,dy2). Вычисляется это число по формуле dy1*dx2-dy2*dx1, и равно синусу угла между векторами умноженному на их длины и позволяет определить направлен ли вектор (dx2,dy2) влево или вправо от вектора (dx1,dy1). Кроме того понятно, что модуль этого произведения деленный на 2 будет равен площади треугольника натянутого на эти вектора отложенные из одной точки.

Многоугольники, операции с ними

Пересечение отрезка с многоугольником

Для целей поиска пути на карте нам потребуется научиться определять - пересекает ли отрезок [SF] необязательно выпуклый многоугольник без самопересечений (причем в более тонком смысле, чем проверка на существование общих точек - нас будет интересовать принадлежность точек отрезка внутренним точкам многоугольника). Будем считать что многоугольник задан обходом его точек против часовой стрелки (то есть многоугольник всегда слева от границы при движении в порядке определения его точек).

Будем рассматривать каждую вершину многоугольника C вкупе с интервалом (P,C) до предыдущей вершины C, а также обозначим для удобства следующую его вершину после C как вершину N. Если пересечений отрезка [S,F] с полуинтервалом (P,C] нет (задача определения пересечения отрезков решается выше), тогда будем считать что в относительно вершины C нам рассматривать нечего и переходим дальше.

Теперь у нас будет два варианта - отрезок [S,F] пересекается с отрезком [P,C] в одной точке и эта точке не C, либо он пересекается с отрезком [P,C] по некоторому отрезку (очевидно что в этом случае он параллелен [P,C]):

Если пересечение в 1 точке и эта точка внутри интервала (P,C), тогда достаточно проверить, что ни точка S ни точка F не лежат слева от вектора PC (это легко сделать с помощью векторного произведения на плоскости - см. выше).

Если пересечение в 1 точке и эта точка С и вектор CN лежит левее вектора PC или параллелен ему, тогда надо проверить, что ни точка S ни точка F не лежат слева от одновременно CN и PC.

Если пересечение в 1 точке и эта точка С и вектор CN лежит правее вектора PC, тогда надо проверить, что ни точка S ни точка F не лежат слева от либо CN и PC.

Если же пересечение отрезок, тогда надо лишь проверить, что если вектор CN лежит правее вектора PC, тогда ни CS ни CF не направлены в ту же сторону, что и вектор PC (проверяется скалярным произведением).

Всё.