isincos.py

#!/usr/bin/env python
import math

OUTBITS = 16
BITS_PER_CYCLE = 16
BITS_PER_QUARTER = BITS_PER_CYCLE - 2
BITS = 8
ONE = 1 << BITS
HALF = ONE / 2
C = 70710 * ONE / 100000
FINAL_SHIFT = OUTBITS - BITS


def isincos(phasein):
    # modulo phase into quarter, convert to float 0..1
    modphase = (phasein & (1 << BITS_PER_QUARTER) - 1) * ONE / (1 << BITS_PER_QUARTER)

    # extract quarter bits
    quarter = phasein & (3 << BITS_PER_QUARTER)

    # recognize quarter
    if quarter == 0:
        # first quarter, angle = 0 .. pi/2
        ops = (+1, -1, +1, -1, +1, +1, -1)
    elif quarter == 1 << BITS_PER_QUARTER:
        # second quarter, angle pi/2 .. pi
        ops = (-1, +1, +1, -1, +1, +1, -1)
    elif quarter == 2 << BITS_PER_QUARTER:
        # third quarter, angle pi .. 1.5pi
        ops = (+1, -1, -1, +1, -1, -1, +1)
    else:
        # fourth quarter, angle 1.5pi .. 2pi
        ops = (+1, -1, +1, -1, +1, -1, +1)

    x = ops[0] * modphase + ops[1] * HALF
    temp = ((ops[2] * 2 * ONE + ops[3] * 4 * C) * x / ONE) * x / ONE + ops[4] * C
    sinout = temp + ops[5] * x
    cosout = temp + ops[6] * x

    return sinout << FINAL_SHIFT, cosout << FINAL_SHIFT


def main():
    units = 65536 / 360.0
    angles = xrange(0, 360, 1)
    tpl = '{}: real={}, expected={}, diff={}'

    def _check(angle, visible=False):
        sinout, cosout = isincos(int(angle * units))
        sinreal, cosreal = sinout / 65536.0, cosout / 65536.0
        angle_radian = angle * math.pi / 180
        sinexp, cosexp = math.sin(angle_radian), math.cos(angle_radian)
        if visible:
            print(angle, sinout, cosout, sinreal, cosreal)
            print(tpl.format('SIN', sinreal, sinexp,
                             math.fabs(sinreal) - math.fabs(sinexp)))
            print(tpl.format('COS', cosreal, cosexp,
                             math.fabs(cosreal) - math.fabs(cosexp)))
        assert math.fabs(sinreal) - math.fabs(sinexp) < 0.05
        assert math.fabs(cosreal) - math.fabs(cosexp) < 0.05

    for angle in angles:
        _check(angle)

if __name__ == '__main__':
    main()