求平均角度

#输入角度(弧度)的数组,返回角度平均值
def avgAngle(angles):
    def getDiff(x1,x2):
                pi  = math.pi
        if x1<0:
            x1 += math.pi*2
        if x2<0:
            x2 += math.pi*2
        diff = x2 - x1 + pi
        if (diff < 0):
            diff = diff + 2*pi  - pi
        elif (diff > 2 *pi):
            diff = diff - 2*pi - pi
        else:
            diff = diff - pi
        return diff
        
    diff = 0
    last = angles[0]
    sum  = angles[0]

    for i in range(len(angles)-1):
        diff = getDiff(angles[i],angles[i+1])
        last += diff
        sum  += last

    return normAngle(sum/len(angles))

# wrapping angle to [-pi .. pi]
def normAngle(angle):
        import math
        angle  = math.fmod( angle  + 5*math.pi, 2*math.pi) - math.pi
        return angle