Python 两个旋转物体之间的角度距离

这可能是一个微不足道的问题，但我找不到一个我满意的解决方案

我想计算两个旋转物体之间的距离角，辐射度，它们的x，y坐标是给定的。 假设对象在xc=0.0、yc=0.0、r=0.0的圆上旋转。 我可以使用函数“atan2”分别检索点1和点2、a1和a2的相关角度

我编写了一个小函数，将atan2结果转换为平滑函数0:2pi

如果a1=0.3和a2=0.2，a1-a2工作正常。 然而，当第一个物体完成第一圈，而第二个物体仍然落后时，如何适应这种情况？例如a1=0.1和a2=6.2

编辑

我想出了一个例子：

from math import *
import random


def estimate_angular_velocity(last_measurement, previous_measurement, old_angular_vel, circle_params, iter, h=0.5):
    def angle(meas, circle):
        x, y = meas
        xc, yc, r = circle
        calc_angle = math.atan2(y - yc, x - xc)
        if calc_angle < 0:
            return (2 * pi + calc_angle) % (2 * pi)
        else:
            return calc_angle % (2 * pi)

    def angle_distance(first_angle, second_angle):
        if abs(first_angle - second_angle) < abs(first_angle + (2*pi - second_angle)):
            error_modulus = abs(first_angle - second_angle) % (2 * pi)
            error_sign = math.copysign(1.0, first_angle - second_angle)
        else:
            error_modulus = abs(first_angle + (2*pi - second_angle)) % (2 * pi)
            error_sign = -math.copysign(1.0, first_angle - second_angle)
        return error_sign * error_modulus

    if old_angular_vel is None:
        return angle(last_measurement, circle_params) - angle(previous_measurement, circle_params)
    else:
        old_angle = angle(previous_measurement, circle_params)
        exp_angle = (old_angle + old_angular_vel) % (2 * pi)
        new_angle = angle(last_measurement, circle_params)
        error = angle_distance(new_angle, exp_angle)
        new_ang_velocity = old_angular_vel + h * error
        print "iter: %f, old: %f, exp: %f, new: %f, est_ang_vel: %f, err: %f" % (iter, old_angle, exp_angle, new_angle, new_ang_velocity, error)
        return new_ang_velocity


true_speed = -pi / 2.7
print "true speed: ", true_speed
est_speed = 0.0
est_speed2 = 0.0
r = 10.0
for i in range(1, 40):
    old_angle = true_speed * (i-1) + random.gauss(0.0, 0.1)
    new_angle = true_speed * i + random.gauss(0.0, 0.1)
    old_meas = (r * cos(old_angle), r * sin(old_angle))
    meas = (r * cos(new_angle), r * sin(new_angle))
    est_speed2 = estimate_angular_velocity(meas, old_meas, est_speed2, (0, 0, 9.9), i)

我想知道这是不是太复杂了，我错过了一些基本的trig技巧来做到这一点，而没有太多的代码膨胀

按照惯例，如果a1在前面，则角度为正，否则为负

---

这更多的是一个估计问题，而不是某种速率，没有重叠，xc=0.0，yc=0.0，r=0.0是点，不是圆。在后一种情况下，a1和a2之间的角距离是多少？你怎么知道a2不在a1之前，而在a1之前？@martineau-谢谢你的关注。1 xc=0.0，yc=0.0，r=0.0实际上是定义圆的参数：圆心和半径。2如果a2与a1相连，则函数应返回负角度。半径为零的圆为点。在a1==0.1和a2==6.2的情况下，你怎么知道a1在a2前面而不是后面？@martineu：是的，当然，我的意思是1xC=0.0，yc=0.0，r>0.0，抱歉。如果你只知道每个物体的角度位置，那么你想要的信息根本就不可用。另一方面，如果物体在移动，有东西在移动它们，理论上，至少这个东西应该能够确定总的角运动，或者帮助计算它。