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

#c_sharp

Проблема такая:

Нужно найти угол между точками 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();
}

    

Ответы

Ответ 1

Вступление

Итак, начать стоит с того, что Вы поставили некорректное условие, так как угол - 


  геометрическая фигура, образованная двумя лучами (сторонами угла), выходящими из
одной точки (которая называется вершиной угла). 


В свою очередь луч - 


  часть прямой, состоящая из данной точки и всех точек, лежащих по одну сторону от
неё. Любая точка на прямой разделяет прямую на два луча.


В свою же очередь одна единственная прямая проходит через 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), как все сломается! 
Чтобы решить проблему, Вам необходимо знать, в какую сторону движется автомобиль.
Я не знаю деталей Вашей реализации, так что могу предложить кэшировать предыдущую точку,
в которой был автомобиль, после чего уже передвигать его на новую. Тем самым Вы спокойно
в любой момент времени найдете вектор скорости машины и примените его в расписанном
выше алгоритме


Ответ 2

Считаю, что дано:   

S: точка, координаты машины  
D: вектор направления машины  
T: целевая точка  


Тогда целевой вектор 

V = T - S 
  //или в компонентах:
V.X = T.X - S.X
V.Y = T.Y - S.Y


Угол между текущим направлением машины и целевым вектором

A = atan2(D x V, D * V)  //векторное и скалярное произведение векторов 
  //или в компонентах:
A = atan2(D.X * V.Y - D.Y * V.X, D.X * V.X + D.Y * V.Y)


Результат - угол в радианах в диапазоне -Pi..Pi, на который нужно повернуть машину,
чтобы мгновенный вектор её направления указывал на цель.   

Знак угла соответствует левому или правому повороту.

(физику (инерцию и т.д.), как я понял, не учитываем)