Обратная Кинематическая ошибка ACos QNAN, INAN

Question

Эй, я получаю возвращаемое значение "Не число" из следующей функции:

void Spider::setAngles(double x,double y, double l1, double l2){
double theta1, theta2;
theta2=acos((pow(x,2)+pow(y,2)-pow(l1,2)-pow(l2,2))/(2*l1*l2));
cout<<theta2* 180/PI<<endl;
theta1=(-((l2*sin(theta2)*x)+(l1+l2*cos(theta2)*y))/((l2*sin(theta2)*y)+    (l1+l2*cos(theta2)*x)))* 180/PI;
cout<<theta1;
}

Я понимаю, что ACos нужно значение аргумента от -1 до 1, но я не могу понять, как это сделать, если, скажем, конечная точка эффектора находится в (15,15) с длинами, равными 2...

Нужно ли все нормализовать? Включая расстояния между суставами и вектор направления из (0,0)->(15,15)

Любая помощь будет принята с благодарностью!

Answer 1

Напомним, что для прямоугольного треугольника:

cos(angle) = Adjacent/Hypotenuse

что означает в вашем коде для theta2 у тебя есть:

Adjacent = x*x + y*y - l1*l1 - l2*l2
Hypotenuse = 2*l1*l2

В лучшем случае это наоборот, и скорее всего просто неправильно, в зависимости от того, что именно вы пытаетесь сделать. Если вы пытаетесь определить угол прямоугольного треугольника с гипотенузой от (0,0) до (x+l1, y+l2), вы должны использовать:

Adjacent = x + l1
Hypotenuse = sqrt((x+l1)*(x+l1) + (y+l2)*(y+l2))

или для треугольника (0,0) - (x-l1, y-l2):

Adjacent = x - l1
Hypotenuse = sqrt((x-l1)*(x-l1) + (y-l1)*(y-l1))

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