I am trying to implement an algorithm to calculate the signed change in direction between two vectors. Here, I define the signed change in heading to be the smallest angle that needs to be added or subtracted from the angle of a given vector to meet the angle of another vector.

I have a naive implementation, but I was wondering if there was a better way.

import math

def signum(value):
    if value > 0:
        return 1
    if value < 0:
        return -1
    return 0

def angleOfVector(v):
    return math.atan2(v[1], v[0])

def principalAngleOfVector(v):
    theta = angleOfVector(v)
    if theta < 0:
        theta  = 2 * math.pi
    return theta

def signedChangeInHeading(src, dst):
    theta_src = principalAngleOfVector(src)
    theta_dst = principalAngleOfVector(dst)
    initial_sign = signum(theta_dst-theta_src)
    initial_guess_magnitude = abs(theta_dst-theta_src)
    alternate_guess_magnitude = 2*math.pi-abs(theta_dst-theta_src)
    ret_magnitude = initial_guess_magnitude
    ret_sign = initial_sign
    if alternate_guess_magnitude < ret_magnitude:
        ret_sign = -initial_sign
        ret_magnitude = alternate_guess_magnitude
    return ret_sign * ret_magnitude

CodePudding user response：

I would use atan2(Ax * By - Ay * Bx, Ax * Bx Ay * By).