计算两个 vector 之间的角度(atan2 不连续问题)

Question

我有两个 vector ，我想计算它们之间的角度。

一个 vector 定义为两个点，即 P1(x1,y1) 和 P2(x2,y2)。另一个 vector 定义为点 P3(x3,y3) 及其相对于 x 轴的航向角(例如 theta)。

我知道我需要获得它们的乘积，但是如何获得两点的第二个 vector ？

实际上我尝试过使用atan2()，但是当角度在pi(+-epsilon)附近时，它会从pi跳转到-pi，这是不希望的，并且我无法简单地通过添加2*pi来修复它角度小于零。在这种情况下，我的模拟会从 +pi 跳到 -pi。

我正在使用C-mex S-functions，但任何伪代码也可以。

非常感谢

Answer 1

从 here 获取全部内容。请阅读那里的讨论以了解该主题。

Assuming a = [x1,y1] and b = [x2,y2] are two vectors with their bases at the origin, the non-negative angle between them measured counterclockwise from a to b is given by

angle = mod(atan2(x1*y2-x2*y1,x1*x2+y1*y2),2*pi);

对于 MATLAB 实现，mod 可以替换为 rem:

angle = rem(atan2(x1*y2-x2*y1,x1*x2+y1*y2),2*pi);

The quantities, x1*y2-x2*y1 and x1*x2+y1*y2 are, respectively, the sine and cosine of the counterclockwise angle from vector a to vector b, multiplied by the product of their norms - that is, their cross product and the dot product restricted to two dimensions. The 'atan2' function then gives the angle between them ranging from -pi to +pi, and the 'mod' operation changes this so as to range from 0 to 2*pi.

请不要使用诸如 theta = acos(dot(a,b)) 之类的公式，因为它在 pi 和 -pi< 附近存在准确性问题.