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