Follow

Keep Up to Date with the Most Important News

By pressing the Subscribe button, you confirm that you have read and are agreeing to our Privacy Policy and Terms of Use
Contact

Algorithm Improvement: Calculate signed change in heading

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.

enter image description here

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

MEDevel.com: Open-source for Healthcare and Education

Collecting and validating open-source software for healthcare, education, enterprise, development, medical imaging, medical records, and digital pathology.

Visit Medevel

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

>Solution :

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

Add a comment

Leave a Reply

Keep Up to Date with the Most Important News

By pressing the Subscribe button, you confirm that you have read and are agreeing to our Privacy Policy and Terms of Use

Discover more from Dev solutions

Subscribe now to keep reading and get access to the full archive.

Continue reading