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

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

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

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

Координаты препятствия (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)

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

Это достаточно простая проверка. Пусть точка имеет координаты (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=(C2*B1-B2*C1)/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))

Всё.