Страницы

Поиск по вопросам

понедельник, 25 марта 2019 г.

Нахождение угла между 2 точками (для движения машины к цели)

Проблема такая:
Нужно найти угол между точками X, Y.
К примеру, есть точки:
Координаты машины:
x: 1141.513916 y: -1162.998169
Координаты точки, куда нужно ей приехать:
x: 1162.112061 y: -1199.149658
Нужно как-то рассчитать угол между машиной и точкой.
Должно быть что-то такое:
if (angel < 5){ Turn_Car_Left(); }
if (angel == 5){ Ride_Forward(); }
if (angel > 5){ Turn_Car_Right(); }


Ответ

Вступление
Итак, начать стоит с того, что Вы поставили некорректное условие, так как угол -
геометрическая фигура, образованная двумя лучами (сторонами угла), выходящими из одной точки (которая называется вершиной угла).
В свою очередь луч -
часть прямой, состоящая из данной точки и всех точек, лежащих по одну сторону от неё. Любая точка на прямой разделяет прямую на два луча.
В свою же очередь одна единственная прямая проходит через 2 точки => для построения угла требуется части 2-х пересекающихся прямых (с одной общей точкой) => 2 * 2 - 1 = 3 точки
Таким образом мы получаем очевидный для всех факт: не может быть между двумя точками какого-либо угла

Немного теории
Отойдем ненадолго от разъяснений геометрии за N класс средней школы и все таки попытаемся догадаться, что же Вам нужно
Как я понимаю, Вы моделируете движение машины в плоскости xOy. Так как машина движется, она имеет некоторый вектор, характеризующий ее перемещение.
Предположу, что машина выехала из точки (0; 0) => если ее текущие координаты равны (x; y), то вектор перемещения равен { x - 0; y - 0; } = { x; y; }
Однако так как Вам требуется найти угол для поворота машины, Вам бы следовало использовать вектор ее скорости, но Вы нас обделили информацией о нем, так что предположу, что он сонаправлен с вектором перемещения
Итак. На данном шаге у нас есть вектор и точка, итого: 3 точки. Для расчета угла более чем достаточно
Далее находим направляющий вектор из начала координат в необходимую точку и находим наименьший угол между двумя имеющимися векторами (a и b) по формуле:
cos(α) = (a * b) / (|a| * |b|)

Пример
Попробуем на примере:
Пусть машина располагается в точке (1; 2.5), а пункт назначения - в точке (3; 3)

a = { 1; 2.5 } b = { 3; 3 }
cos(α) = (1*3 + 2.5*3) / (sqrt(1*1 + 2.5*2.5) * sqrt(3*3 + 3*3)) ≈ 0.91914503001
=>
α = arccos(0.91914503001) ≈ 0.404891786 rad ≈ 23.1985905 deg
Вот мы и получили заветный угол, который примерно равен 23 градусам
На сием курс геометрии окончен, переходим к программной реализации

Реализация
Набросаем такую функцию:
private static double GetAngle(Point Machine, Point Destination) { // Получим косинус угла по формуле double cos = Math.Round((Machine.X * Destination.X + Machine.Y * Destination.Y) / (Math.Sqrt(Machine.X * Machine.X + Machine.Y * Machine.Y) * Math.Sqrt(Destination.X * Destination.X + Destination.Y * Destination.Y)), 9); // Вернем arccos полученного значения (в радианах!) return Math.Acos(cos); }
Судя по значениям в Вашем примере, которые явно больше единицы, Вы используете не радианную, а градусную меру, а посему значение, которое вернет Вам функция, необходимо будет преобразовать по формуле:
dAngle = rAngle * 180 / Pi
То есть так:
// Переведем угол в градусы private static double ToDegrees(double Angle) => Angle * 180 / Math.PI;

Протестируем:
Пусть машина располагается в точке (-3; -3), а пункт назначения - в точке (3; 3)

Найдем угол:
Console.WriteLine(ToDegrees(GetAngle(new Point(-3, -3), new Point(3, 3)))); // 180
180 градусов, что, очевидно, является чистейшей правдой!

Итоги
Старайтесь не забывать, что программирование состоит не только из набора текста, но и из применения знаний некой предметной области, с которой Вы соприкасаетесь в рамках проекта.
Чего-то не знаете? Читайте и узнавайте по теме как можно больше!
И да, подчеркну, что представленный выше метод будет работать только если Ваша машинка прямолинейно удаляется от начала координат (т.е. векторы перемещения и скорости сонаправлены), однако стоит машине развернуться и поехать в сторону точки (0; 0), как все сломается! Чтобы решить проблему, Вам необходимо знать, в какую сторону движется автомобиль. Я не знаю деталей Вашей реализации, так что могу предложить кэшировать предыдущую точку, в которой был автомобиль, после чего уже передвигать его на новую. Тем самым Вы спокойно в любой момент времени найдете вектор скорости машины и примените его в расписанном выше алгоритме

Комментариев нет:

Отправить комментарий