Functions for working with angles

angle_helpers.py

import math


def average_angles(angles):
    """Average (mean) of angles

    Return the average of an input sequence of angles. The result is between
    ``0`` and ``2 * math.pi``.

    If the average is not defined (e.g. ``average_angles([0, math.pi]))``,
    a ``ValueError`` is raised.
    """

    x = sum(math.cos(a) for a in angles)
    y = sum(math.sin(a) for a in angles)

    if x == 0 and y == 0:
        raise ValueError(
            "The angle average of the inputs is undefined: %r" % angles)

    # To get outputs from -pi to +pi, delete everything but math.atan2() here.
    return math.fmod(math.atan2(y, x) + 2 * math.pi, 2 * math.pi)


def subtract_angles(lhs, rhs):
    """Return the signed difference between angles lhs and rhs

    Return ``(lhs - rhs)``, the value will be within ``[-math.pi, math.pi)``.

    Both ``lhs`` and ``rhs`` may either be zero-based (within
    ``[0, 2*math.pi]``), or ``-pi``-based (within ``[-math.pi, math.pi]``).
    """

    return math.fmod((lhs - rhs) + math.pi * 3, 2 * math.pi) - math.pi


def is_close(lhs, rhs):
    return abs(lhs - rhs) < 1E-5


def test_average_angles():
    assert is_close(average_angles([-math.pi/4, +math.pi/4]),   0)
    assert is_close(average_angles([-math.pi/4, +math.pi*3/8]), math.pi/16)
    assert is_close(average_angles([-math.pi*3/8, +math.pi/4]), math.pi*31/16)

    assert is_close(average_angles([math.pi*7/4, +math.pi/4]),   0)
    assert is_close(average_angles([math.pi*7/4, +math.pi*3/8]), math.pi/16)
    assert is_close(average_angles([math.pi*13/8, +math.pi/4]),  math.pi*31/16)


def test_subtract_angles():
    assert is_close(subtract_angles(0, math.pi/2),              -math.pi/2)
    assert is_close(subtract_angles(math.pi/2, 0),              +math.pi/2)

    assert is_close(subtract_angles(0, math.pi*3/2),            +math.pi/2)
    assert is_close(subtract_angles(math.pi*3/2, 0),            -math.pi/2)

    assert is_close(subtract_angles(math.pi, math.pi/8),        +math.pi * 7/8)
    assert is_close(subtract_angles(math.pi, math.pi*15/8),     -math.pi * 7/8)

    assert is_close(subtract_angles(math.pi*9/8, math.pi/8),    -math.pi)
    assert is_close(subtract_angles(math.pi*9/8, math.pi*15/8), -math.pi * 6/8)


def test_subtract_angles_bradphelan():
    # Cf. https://gist.github.com/bradphelan/7fe21ad8ebfcb43696b8
    assert is_close(subtract_angles(0.1,           0.2),           -0.1)
    assert is_close(subtract_angles(0.1,           0.2+math.pi*2), -0.1)
    assert is_close(subtract_angles(0.1,           0.2-math.pi*2), -0.1)
    assert is_close(subtract_angles(0.1+math.pi*2, 0.2),           -0.1)
    assert is_close(subtract_angles(0.1-math.pi*2, 0.2),           -0.1)

    assert is_close(subtract_angles(0.2,           0.1),            0.1)
    assert is_close(subtract_angles(0.2,           0.1-math.pi*2),  0.1)
    assert is_close(subtract_angles(0.2,           0.1+math.pi*2),  0.1)
    assert is_close(subtract_angles(0.2+math.pi*2, 0.1),            0.1)
    assert is_close(subtract_angles(0.2-math.pi*2, 0.1),            0.1)