Python math 模块，cos() 实例源码

def warpImage(src, theta, phi, gamma, scale, fovy):
    halfFovy = fovy * 0.5
    d = math.hypot(src.shape[1], src.shape[0])
    sideLength = scale * d / math.cos(deg2Rad(halfFovy))
    sideLength = np.int32(sideLength)

    M = warpMatrix(src.shape[1], src.shape[0], theta, phi, gamma, scale, fovy)
    dst = cv2.warpPerspective(src, M, (sideLength, sideLength))
    mid_x = mid_y = dst.shape[0] // 2
    target_x = target_y = src.shape[0] // 2
    offset = (target_x % 2)

    if len(dst.shape) == 3:
        dst = dst[mid_y - target_y:mid_y + target_y + offset,
              mid_x - target_x:mid_x + target_x + offset,
              :]
    else:
        dst = dst[mid_y - target_y:mid_y + target_y + offset,
              mid_x - target_x:mid_x + target_x + offset]

    return dst

def earth_distance(lat_lng1, lat_lng2):
    """
    Compute the distance (in km) along earth between two latitude and longitude pairs

    Parameters
    ----------
    lat_lng1: tuple
        the first latitude and longitude pair
    lat_lng2: tuple
        the second latitude and longitude pair

    Returns
    -------
    float
        the distance along earth in km
    """
    lat1, lng1 = [l*pi/180 for l in lat_lng1]
    lat2, lng2 = [l*pi/180 for l in lat_lng2]
    dlat, dlng = lat1-lat2, lng1-lng2
    ds = 2 * asin(sqrt(sin(dlat/2.0) ** 2 + cos(lat1) * cos(lat2) * sin(dlng/2.0) ** 2))
    return 6371.01 * ds  # spherical earth...

def unit_cell_volume(self):
        """
        Calculates unit cell volume of a given MOF object.
        """
        a = self.uc_size[0]
        b = self.uc_size[1]
        c = self.uc_size[2]
        alp = math.radians(self.uc_angle[0])
        bet = math.radians(self.uc_angle[1])
        gam = math.radians(self.uc_angle[2])

        volume = 1 - math.cos(alp)**2 - math.cos(bet)**2 - math.cos(gam)**2
        volume += 2 * math.cos(alp) * math.cos(bet) * math.cos(gam)
        volume = a * b * c * math.sqrt(volume)
        frac_volume = volume / (a * b * c)

        self.ucv = volume
        self.frac_ucv = frac_volume

def neighbor_circle(location, pos, shift=False, factor=1.0):
    pos = pos % 6
    latrad = location[0] * pi / 180
    x_un = factor * safety / earth_Rrect / cos(latrad) * 180 / pi
    y_un = factor * safety / earth_Rrect * 180 / pi
    if not shift:
        y_un = y_un * (3.0 ** 0.5) / 2.0 * HEX_R
        x_un = x_un * HEX_R * 1.5
        yvals = [-2, -1, 1, 2, 1, -1]
        xvals = [0, 1, 1, 0, -1, -1]
    else:
        y_un = y_un * HEX_R * 1.5
        x_un = x_un * (3.0 ** 0.5) / 2.0 * HEX_R
        yvals = [-1, 0, 1, 1, 0, -1]
        xvals = [1, 2, 1, -1, -2, -1]

    newlat = location[0] + y_un * yvals[pos]
    newlng = ((location[1] + x_un * xvals[pos] + 180) % 360) - 180
    return (newlat, newlng)

def cal_distance(s,t):
    #s,t = sorted([s,t])
    #if (s,t) in cdic:
    #    return cdic[(s,t)]
    if s in cdic:
        lat1,lon1 = cdic[s]
    else:
        lat1,lon1 = decode(s)
        cdic[s] = (lat1,lon1)

    if t in cdic:
        lat2,lon2 = cdic[t]
    else:
        lat2,lon2 = decode(t)
        cdic[t] = (lat2,lon2)

    #lat2,lon2 = decode(t)
    dx = abs(lon1 - lon2)  
    dy = abs(lat1 - lat2)  
    b = (lat1 + lat2) / 2.0
    Lx = 6371004.0 * (dx / 57.2958) * cos(b / 57.2958)
    Ly = 6371004.0 * (dy / 57.2958)
    L = (Lx**2 + Ly**2) ** 0.5
    cdic[(s,t)] = L
    return L

def distance(latitude_1, longitude_1, elevation_1, latitude_2, longitude_2, elevation_2,
             haversine=None):
    """ Distance between two points """

    # If points too distant -- compute haversine distance:
    if haversine or (abs(latitude_1 - latitude_2) > .2 or abs(longitude_1 - longitude_2) > .2):
        return haversine_distance(latitude_1, longitude_1, latitude_2, longitude_2)

    coef = math.cos(latitude_1 / 180. * math.pi)
    #pylint: disable=invalid-name
    x = latitude_1 - latitude_2
    y = (longitude_1 - longitude_2) * coef

    distance_2d = math.sqrt(x * x + y * y) * ONE_DEGREE

    if elevation_1 is None or elevation_2 is None or elevation_1 == elevation_2:
        return distance_2d

    return math.sqrt(distance_2d ** 2 + (elevation_1 - elevation_2) ** 2)

def est_tone_phase(sdata,a,f,sr):
    samples = len(sdata)
    points  = 360
    rms = numpy.zeros(points)
    sum_min = numpy.sum(numpy.square(sdata))
    min_index = 0
    for offset in xrange(points):
        sum = 0
        phase = pi*offset/180.0
        for i in xrange(samples):
            diff = (sdata[i] - a*cos(2*pi*i*f/(sr/2.0) + phase))
            sum += diff*diff
        rms[offset] = sum
        if (sum < sum_min):
            sum_min = sum
            min_index = offset
            #print "sum_min",sum_min,' index = ',min_index

    min_phase = pi*(min_index)/180.0
    #print "min for phase sweep is ",sum_min,' at offset ',min_index
    return min_phase

def old_est_tone_phase(sdata,a,f,sr):
    samples = len(sdata)
    points  = 360
    rms = numpy.zeros(points)
    sum_min = numpy.sum(numpy.square(sdata))
    min_index = 0
    for offset in xrange(points):
        sum = 0
        phase = pi*offset/180.0
        for i in xrange(samples):
            diff = (sdata[i] - a*cos(2*pi*i*f/sr + phase))
            sum += diff*diff
        rms[offset] = sum
        if (sum < sum_min):
            sum_min = sum
            min_index = offset
            #print "sum_min",sum_min,' index = ',min_index

    min_phase = pi*(min_index)/180.0
    #print "min for phase sweep is ",sum_min,' at offset ',min_index
    return min_phase

def exp(v):
    hv = 0.5*v
    theta = np.linalg.norm(hv)
    a = trig.sinox(theta)
    b = math.cos(theta)
    return Quaternion(b, a*hv)

def cosox2(x):      
    '''(1-cos(x))/(x*x)'''
    if (abs(x) > 1e-2):         
        return (1. - math.cos(x)) / (x*x)       
    else:           
        return 0.5 - x*x / 24 + x*x*x*x / 720.

def specialFun1(x):     
    '''(x*sin(x) - 2.*(1.-cos(x)))/(x*x*x*x)'''
    if (abs(x) > 1e-2):         
        return (x*math.sin(x) - 2.*(1. - math.cos(x))) / (x*x*x*x)          
    else:           
        return -1./12. + x*x / 180. - x*x*x*x / 6720.

def specialFun2(x):     
    '''(2.*(1.-cos(x)) - x*sin(x))/(2.*x*x*(1.-cos(x)))'''
    if (abs(x) > 1e-2):         
        return (2.*(1. - math.cos(x)) - x*math.sin(x)) / (2.*x*x*(1. - math.cos(x)))            
    else:           
        return 1./12. + x*x / 720. + x*x*x*x / 30240.

def specialFun3(x):     
    '''(-2.*x + 3.*sin(x) - x*cos(x))/(x*x*x*x*x)'''
    if (abs(x) > 1e-2):         
        return (-2.*x + 3.*math.sin(x) - x*math.cos(x)) / (x*x*x*x*x)           
    else:           
        return - 1./60. + x*x / 1260. - x*x*x*x / 60480.

def hexagon_generator(edge_length, offset):
    """Generator for coordinates in a hexagon."""
    x, y = offset
    for angle in range(0, 360, 60):
        x += math.cos(math.radians(angle)) * edge_length
        y += math.sin(math.radians(angle)) * edge_length
        yield x, y

def hexagon_generator(self, edge_length, offset):
        """Generator for coordinates in a hexagon."""
        x, y = offset
        for angle in range(0, 360, 60):
            x += math.cos(math.radians(angle)) * edge_length
            y += math.sin(math.radians(angle)) * edge_length
            yield x, y

def calc_helix_points(turtle, rad, pitch):
    """ calculates required points to produce helix bezier curve with given radius and pitch in direction of turtle"""
    # alpha = radians(90)
    # pit = pitch/(2*pi)
    # a_x = rad*cos(alpha)
    # a_y = rad*sin(alpha)
    # a = pit*alpha*(rad - a_x)*(3*rad - a_x)/(a_y*(4*rad - a_x)*tan(alpha))
    # b_0 = Vector([a_x, -a_y, -alpha*pit])
    # b_1 = Vector([(4*rad - a_x)/3, -(rad - a_x)*(3*rad - a_x)/(3*a_y), -a])
    # b_2 = Vector([(4*rad - a_x)/3, (rad - a_x)*(3*rad - a_x)/(3*a_y), a])
    # b_3 = Vector([a_x, a_y, alpha*pit])
    # axis = Vector([0, 0, 1])

    # simplifies greatly for case inc_angle = 90
    points = [Vector([0, -rad, -pitch / 4]),
              Vector([(4 * rad) / 3, -rad, 0]),
              Vector([(4 * rad) / 3, rad, 0]),
              Vector([0, rad, pitch / 4])]

    # align helix points to turtle direction and randomize rotation around axis
    trf = turtle.dir.to_track_quat('Z', 'Y')
    spin_ang = rand_in_range(0, 2 * pi)
    for p in points:
        p.rotate(Quaternion(Vector([0, 0, 1]), spin_ang))
        p.rotate(trf)

    return points[1] - points[0], points[2] - points[0], points[3] - points[0], turtle.dir.copy()

def points_for_floor_split(self):
        """Calculate Poissonly distributed points for stem start points"""
        array = []
        # calculate approx spacing radius for dummy stem
        self.tree_scale = self.param.g_scale + self.param.g_scale_v
        stem = Stem(0, None)
        stem.length = self.calc_stem_length(stem)
        rad = 2.5 * self.calc_stem_radius(stem)
        # generate points
        for _ in range(self.param.floor_splits + 1):
            point_ok = False
            while not point_ok:
                # distance from center proportional for number of splits, tree scale and stem radius
                dis = sqrt(rand_in_range(0, 1) * self.param.floor_splits / 2.5 * self.param.g_scale * self.param.ratio)
                # angle random in circle
                theta = rand_in_range(0, 2 * pi)
                pos = Vector([dis * cos(theta), dis * sin(theta), 0])
                # test point against those already in array to ensure it will not intersect
                point_m_ok = True
                for point in array:
                    if (point[0] - pos).magnitude < rad:
                        point_m_ok = False
                        break
                if point_m_ok:
                    point_ok = True
                    array.append((pos, theta))
        return array

def pbc_parameters(self):
        """
        Calculates constants used in periodic boundary conditions.
        """
        uc_cos = [math.cos(math.radians(a)) for a in self.uc_angle]
        uc_sin = [math.sin(math.radians(a)) for a in self.uc_angle]
        a, b, c = self.uc_size
        v = self.frac_ucv

        xf1 = 1 / a
        xf2 = - uc_cos[2] / (a * uc_sin[2])
        xf3 = (uc_cos[0] * uc_cos[2] - uc_cos[1]) / (a * v * uc_sin[2])
        yf1 = 1 / (b * uc_sin[2])
        yf2 = (uc_cos[1] * uc_cos[2] - uc_cos[0]) / (b * v * uc_sin[2])
        zf1 = uc_sin[2] / (c * v)
        self.to_frac = [xf1, xf2, xf3, yf1, yf2, zf1]

        xc1 = a
        xc2 = b * uc_cos[2]
        xc3 = c * uc_cos[1]
        yc1 = b * uc_sin[2]
        yc2 = c * (uc_cos[0] - uc_cos[1] * uc_cos[2]) / uc_sin[2]
        zc1 = c * v / uc_sin[2]
        self.to_car = [xc1, xc2, xc3, yc1, yc2, zc1]

def uc_vectors(cls, uc_size, uc_angle):
        """
        Calculate unit cell vectors for given unit cell size and angles
        """
        a = uc_size[0]
        b = uc_size[1]
        c = uc_size[2]
        alpha = math.radians(uc_angle[0])
        beta = math.radians(uc_angle[1])
        gamma = math.radians(uc_angle[2])

        x_v = [a, 0, 0]
        y_v = [b * math.cos(gamma), b * math.sin(gamma), 0]
        z_v = [0.0] * 3
        z_v[0] = c * math.cos(beta)
        z_v[1] = (c * b * math.cos(alpha) - y_v[0] * z_v[0]) / y_v[1]
        z_v[2] = math.sqrt(c * c - z_v[0] * z_v[0] - z_v[1] * z_v[1])
        uc_vectors = [x_v, y_v, z_v]
        return uc_vectors

def earth_Rreal(latrad):
    return (1.0 / (((cos(latrad)) / earth_Rmax) ** 2 + ((sin(latrad)) / earth_Rmin) ** 2)) ** 0.5

def get_distance(location1, location2):
    lat1, lng1 = location1
    lat2, lng2 = location2

    lat1, lng1, lat2, lng2 = map(radians, (lat1, lng1, lat2, lng2))

    d = sin(0.5*(lat2 - lat1)) ** 2 + cos(lat1) * cos(lat2) * sin(0.5*(lng2 - lng1)) ** 2
    return 2 * earth_Rrect * asin(sqrt(d))

def init_grid(self):
        grid_all = []
        lats = self.init_lats()
        c = 2 * pi / (3 ** 0.5 * self.r_sight * self.safety) * self.earth_R

        even_lng = True

        strip_amount = int(ceil(c))
        grid_all.append((0, strip_amount, even_lng))
        ind_lat = 2

        while ind_lat < len(lats):
            amount = int(ceil(c * cos(lats[ind_lat])))
            if amount < strip_amount - (sin(lats[ind_lat]*2)*self.param_shift+self.param_stretch):
                ind_lat -= 1
                strip_amount = int(ceil(c * cos(lats[ind_lat])))
            else:
                even_lng = not even_lng

            if ind_lat + 1 < len(lats):
                lat = lats[ind_lat + 1] * 180 / pi
                grid_all.append((lat, strip_amount, even_lng))
            ind_lat += 3

        grid_all.append((90.0, 1, True))  # pole

        return grid_all

def dist_cmp(self, location1, location2):
        return sin(0.5 * (location2[0] - location1[0])) ** 2 + cos(location2[0]) * cos(location1[0]) * sin(0.5 * (location2[1] - location1[1])) ** 2

def getEarthRadius(latrad):
    return (1.0 / (((math.cos(latrad)) / EARTH_Rmax) ** 2 + ((math.sin(latrad)) / EARTH_Rmin) ** 2)) ** (1.0 / 2)

def getCircularBounds(fitCloud=None,width=64,height=64,smoothing=0.01):
    circumference = 2*(width+height)

    if not fitCloud is None:
        cx = np.mean(fitCloud[:,0])
        cy = np.mean(fitCloud[:,1])
        r = 0.5* max( np.max(fitCloud[:,0])- np.min(fitCloud[:,0]),np.max(fitCloud[:,1])- np.min(fitCloud[:,1]))
    else:
        r = circumference /(2.0*math.pi)
        cx = cy = r
    perimeterPoints = np.zeros((circumference,2),dtype=float)
    for i in range(circumference):
        angle = (2.0*math.pi)*float(i) / circumference - math.pi * 0.5 
        perimeterPoints[i][0] = cx + r * math.cos(angle)
        perimeterPoints[i][1] = cy + r * math.sin(angle)


    bounds = {'top':perimeterPoints[0:width],
              'right':perimeterPoints[width-1:width+height-1],
              'bottom':perimeterPoints[width+height-2:2*width+height-2],
              'left':perimeterPoints[2*width+height-3:]}

    bounds['s_top'],u = interpolate.splprep([bounds['top'][:,0], bounds['top'][:,1]],s=smoothing)
    bounds['s_right'],u = interpolate.splprep([bounds['right'][:,0],bounds['right'][:,1]],s=smoothing)
    bounds['s_bottom'],u = interpolate.splprep([bounds['bottom'][:,0],bounds['bottom'][:,1]],s=smoothing)
    bounds['s_left'],u = interpolate.splprep([bounds['left'][:,0],bounds['left'][:,1]],s=smoothing)


    return bounds

def geometry(self, frame=0):
        t = frame / self.frames
        Rot = Matrix.Rotation(0.5*pi, 4, 'Y')
        bm = bmesh.new()

        for i in range(self.n):
            t0 = i / self.n
            r0, theta = t0*self.r0, i*goldenAngle - frame*goldenAngle + t*self.offset

            x = r0*cos(theta)
            y = r0*sin(theta)
            z = self.h0/2 - (self.h0 / (self.r0*self.r0))*r0*r0
            p0 = Vector((x, y, z))

            T0, N0, B0 = getTNBfromVector(p0)
            M0 = Matrix([T0, B0, N0]).to_4x4().transposed()

            for j in range(self.m):
                t1 = j / self.m
                t2 = 0.4 + 0.6*t0
                r1, theta = t2*t1*self.r1, j*goldenAngle #- frame*goldenAngle + t*self.offset

                x = r1*cos(theta)
                y = r1*sin(theta)
                z = self.h1 - (self.h1 / (self.r1*self.r1))*r1*r1
                p1 = Vector((x, y, z))
                T1, N1, B1 = getTNBfromVector(p1)
                M1 = Matrix([T1, B1, N1]).to_4x4().transposed()

                p = p0 + M0*p1
                r2 = t2*t1*self.r2

                T = Matrix.Translation(p)
                bmesh.ops.create_cone(bm,
                                cap_ends=True, segments=6,
                                diameter1=r2, diameter2=r2,
                                depth=0.1*r2, matrix=T*M0*M1*Rot)
        return bm

def torusSurface(r0, r1):
    def surface(u, v):
        point = ((r0 + r1*cos(TAU*v))*cos(TAU*u), \
                 (r0 + r1*cos(TAU*v))*sin(TAU*u), \
                  r1*sin(TAU*v))
        return point
    return surface

# Create an object from a surface parameterization

def rainbowLights(r=5, n=100, freq=2, energy=0.1):
    for i in range(n):
        t = float(i)/float(n)
        pos = (r*sin(tau*t), r*cos(tau*t), r*sin(freq*tau*t))

        # Create lamp
        bpy.ops.object.add(type='LAMP', location=pos)
        obj = bpy.context.object
        obj.data.type = 'POINT'

        # Apply gamma correction for Blender
        color = tuple(pow(c, 2.2) for c in colorsys.hsv_to_rgb(t, 0.6, 1))

        # Set HSV color and lamp energy
        obj.data.color = color
        obj.data.energy = energy

def radial(cls, r, theta):
        """Provide a radial acceleration.

         Arguments:
             r (float): speed in pixels per second (per second)
             theta (float): angle in degrees (0 = +X axis, 90 = +Y axis)
         """
        radians = math.radians(theta)
        ax = r * math.cos(radians)
        ay = r * math.sin(radians)
        return cls(ax=ax, ay=ay)

def create_matrix(self,parent_joint,parent_joint_matrix):
        # The calculation of the local matrix is an optimized version of
        # local_matrix = T*IPS*R*S if ignore_parent_scale else T*R*S
        # where S, R and T is the scale, rotation and translation matrix
        # respectively and IPS is the inverse parent scale matrix.

        cx = cos(radians(self.rotation_x))
        sx = sin(radians(self.rotation_x))
        cy = cos(radians(self.rotation_y))
        sy = sin(radians(self.rotation_y))
        cz = cos(radians(self.rotation_z))
        sz = sin(radians(self.rotation_z))

        if self.ignore_parent_scale:
            ips_x = 1/parent_joint.scale_x
            ips_y = 1/parent_joint.scale_y
            ips_z = 1/parent_joint.scale_z
        else:
            ips_x = 1
            ips_y = 1
            ips_z = 1

        local_matrix = numpy.empty((3,4),numpy.float32)
        local_matrix[0,0] = cy*cz*self.scale_x*ips_x
        local_matrix[1,0] = cy*sz*self.scale_x*ips_y
        local_matrix[2,0] = -sy*self.scale_x*ips_z
        local_matrix[0,1] = (sx*sy*cz - cx*sz)*self.scale_y*ips_x
        local_matrix[1,1] = (sx*sy*sz + cx*cz)*self.scale_y*ips_y
        local_matrix[2,1] = sx*cy*self.scale_y*ips_z
        local_matrix[0,2] = (cx*sy*cz + sx*sz)*self.scale_z*ips_x
        local_matrix[1,2] = (cx*sy*sz - sx*cz)*self.scale_z*ips_y
        local_matrix[2,2] = cx*cy*self.scale_z*ips_z
        local_matrix[0,3] = self.translation_x
        local_matrix[1,3] = self.translation_y
        local_matrix[2,3] = self.translation_z

        return matrix3x4_multiply(parent_joint_matrix,local_matrix)

def create_matrix(self):
        c = cos(radians(self.rotation))
        s = sin(radians(self.rotation))
        R = numpy.matrix([[c,-s,0],[s,c,0],[0,0,1]])
        S = numpy.matrix([[self.scale_s,0,0],[0,self.scale_t,0],[0,0,1]])
        C = numpy.matrix([[1,0,self.center_s],[0,1,self.center_t],[0,0,1]])
        T = numpy.matrix([[1,0,self.translation_s],[0,1,self.translation_t],[0,0,1]])

        # Only types 0x00, 0x06, 0x07, 0x08 and 0x09 have been tested
        if self.matrix_type in {0x00,0x02,0x0A,0x0B,0x80}:
            P = numpy.matrix([[1,0,0,0],[0,1,0,0],[0,0,0,1]])
        elif self.matrix_type == 0x06:
            P = numpy.matrix([[0.5,0,0,0.5],[0,-0.5,0,0.5],[0,0,0,1]])
        elif self.matrix_type == 0x07:
            P = numpy.matrix([[0.5,0,0.5,0],[0,-0.5,0.5,0],[0,0,1,0]])
        elif self.matrix_type in {0x08,0x09}:
            P = numpy.matrix([[0.5,0,0.5,0],[0,-0.5,0.5,0],[0,0,1,0]])*numpy.matrix(self.projection_matrix)
        else:
            raise ValueError('invalid texture matrix type')

        M = T*C*S*R*C.I*P

        if self.shape == gx.TG_MTX2x4:
            return M[:2,:]
        elif self.shape == gx.TG_MTX3x4:
            return M
        else:
            raise ValueError('invalid texture matrix shape')

def update(self,time):
        scale_x = self.scale_x.interpolate(time)
        scale_y = self.scale_y.interpolate(time)
        scale_z = self.scale_z.interpolate(time)
        rotation_x = self.rotation_x.interpolate(time)
        rotation_y = self.rotation_y.interpolate(time)
        rotation_z = self.rotation_z.interpolate(time)
        translation_x = self.translation_x.interpolate(time)
        translation_y = self.translation_y.interpolate(time)
        translation_z = self.translation_z.interpolate(time)

        cx = cos(radians(rotation_x))
        sx = sin(radians(rotation_x))
        cy = cos(radians(rotation_y))
        sy = sin(radians(rotation_y))
        cz = cos(radians(rotation_z))
        sz = sin(radians(rotation_z))

        R = numpy.matrix([[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,1.0]]) #<-?
        R[0,0] = cy*cz
        R[0,1] = (sx*sy*cz - cx*sz)
        R[0,2] = (cx*sy*cz + sx*sz)
        R[1,0] = cy*sz
        R[1,1] = (sx*sy*sz + cx*cz)
        R[1,2] = (cx*sy*sz - sx*cz)
        R[2,0] = -sy
        R[2,1] = sx*cy
        R[2,2] = cx*cy

        S = numpy.matrix([[scale_x,0,0,0],[0,scale_y,0,0],[0,0,scale_z,0],[0,0,0,1]])
        C = numpy.matrix([[1,0,0,self.center_x],[0,1,0,self.center_y],[0,0,1,self.center_z],[0,0,0,1]])
        T = numpy.matrix([[1,0,0,translation_x],[0,1,0,translation_y],[0,0,1,translation_z],[0,0,0,1]])

        self.texture_matrix[:] = (T*C*S*R*C.I)[:self.row_count,:]

def rotation(axis_x,axis_y,axis_z,angle):
        s = sin(angle/2)
        return Quarternion(cos(angle/2),s*axis_x,s*axis_y,s*axis_z)

def haversine(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)
    """
    # convert decimal degrees to radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
    c = 2 * asin(sqrt(a))
    km = 6367 * c
    return km

def get_motion_vector(self):
        """Returns the current motion vector indicating the velocity of the player.

        Returns
        -------
        vector : tuple of len 3
            Tuple containing the velocity in x, y, and z respectively.
        """
        if any(self.strafe):
            x, y = self.rotation
            strafe = math.degrees(math.atan2(*self.strafe))
            y_angle = math.radians(y)
            x_angle = math.radians(x + strafe)
            if self.flying:
                m = math.cos(y_angle)
                dy = math.sin(y_angle)
                if self.strafe[1]:
                    # Moving left or right.
                    dy = 0.0
                    m = 1
                if self.strafe[0] > 0:
                    # Moving backwards.
                    dy *= -1
                # When you are flying up or down, you have less left and right motion.
                dx = math.cos(x_angle) * m
                dz = math.sin(x_angle) * m
            else:
                dy = 0.0
                dx = math.cos(x_angle)
                dz = math.sin(x_angle)
        else:
            dy = 0.0
            dx = 0.0
            dz = 0.0
        dy += self.strafe_z
        return dx, dy, dz

def get_sight_vector(self):
        """Returns the current line of sight vector indicating the direction the
        player is looking.
        """
        x, y = self.rotation
        # y ranges from -90 to 90, or -pi/2 to pi/2, so m ranges from 0 to 1 and
        # is 1 when looking ahead parallel to the ground and 0 when looking
        # straight up or down.
        m = math.cos(math.radians(y))
        # dy ranges from -1 to 1 and is -1 when looking straight down and 1 when
        # looking straight up.
        dy = math.sin(math.radians(y))
        dx = math.cos(math.radians(x - 90)) * m
        dz = math.sin(math.radians(x - 90)) * m
        return dx, dy, dz

def set_3d(self, size):
        """Configure OpenGL to draw in 3d."""
        width, height = size
        GL.glEnable(GL.GL_DEPTH_TEST)
        GL.glViewport(0, 0, width, height)
        GL.glMatrixMode(GL.GL_PROJECTION)
        GL.glLoadIdentity()
        GL.gluPerspective(65.0, width / float(height), 0.1, 60.0)
        GL.glMatrixMode(GL.GL_MODELVIEW)
        GL.glLoadIdentity()
        x, y = self.player.rotation
        GL.glRotatef(x, 0, 1, 0)
        GL.glRotatef(-y, math.cos(math.radians(x)), 0, math.sin(math.radians(x)))
        x, y, z = self.player.position
        GL.glTranslatef(-x, -y, -z)

def from_angle_axis(cls, theta, axis):
        """ Construct Quaternion from axis-angle representation """
        x, y, z = axis
        norm = math.sqrt(x*x + y*y + z*z)
        if 0 == norm:
            return cls([0, 0, 0, 1])
        t = math.sin(theta/2) / norm;
        return cls([x*t, y*t, z*t, math.cos(theta/2)])

    # Properties

def orthogonalization_matrix(lengths, angles):
    """Return orthogonalization matrix for crystallographic cell coordinates.

    Angles are expected in degrees.

    The de-orthogonalization matrix is the inverse.

    >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.))
    >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
    True
    >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
    >>> numpy.allclose(numpy.sum(O), 43.063229)
    True

    """
    a, b, c = lengths
    angles = numpy.radians(angles)
    sina, sinb, _ = numpy.sin(angles)
    cosa, cosb, cosg = numpy.cos(angles)
    co = (cosa * cosb - cosg) / (sina * sinb)
    return numpy.array((
        ( a*sinb*math.sqrt(1.0-co*co),  0.0,    0.0, 0.0),
        (-a*sinb*co,                    b*sina, 0.0, 0.0),
        ( a*cosb,                       b*cosa, c,   0.0),
        ( 0.0,                          0.0,    0.0, 1.0)),
        dtype=numpy.float64)

def random_quaternion(rand=None):
    """Return uniform random unit quaternion.

    rand: array like or None
        Three independent random variables that are uniformly distributed
        between 0 and 1.

    >>> q = random_quaternion()
    >>> numpy.allclose(1.0, vector_norm(q))
    True
    >>> q = random_quaternion(numpy.random.random(3))
    >>> q.shape
    (4,)

    """
    if rand is None:
        rand = numpy.random.rand(3)
    else:
        assert len(rand) == 3
    r1 = numpy.sqrt(1.0 - rand[0])
    r2 = numpy.sqrt(rand[0])
    pi2 = math.pi * 2.0
    t1 = pi2 * rand[1]
    t2 = pi2 * rand[2]
    return numpy.array((numpy.sin(t1)*r1,
                        numpy.cos(t1)*r1,
                        numpy.sin(t2)*r2,
                        numpy.cos(t2)*r2), dtype=numpy.float64)

def applyLinearTransformToCoords(self, coords, angle, shear_x, shear_y, scale, \
                                   size_in, size_out):
    '''Apply the image transformation specified by three parameters to a list of
    coordinates. The anchor point of the transofrmation is the center of the tile.

    Args:
      x: list of coordinates.
      angle: Angle by which the image is rotated.
      shear_x: Shearing factor along the x-axis by which the image is sheared.
      shear_y: Shearing factor along the x-axis by which the image is sheared.
      scale: Scaling factor by which the image is scaled.

    Returns:
      A list of transformed coordinates.

    '''
    s_in = (size_in, size_in)
    s_out = (size_out, size_out)
    c_in = .5 * np.asarray(s_in, dtype=np.float64).reshape((1, 2))
    c_out = .5 * np.asarray(s_out, dtype=np.float64).reshape((1, 2)) 

    M_rot = np.asarray([[math.cos(angle), -math.sin(angle)], \
                        [math.sin(angle),  math.cos(angle)]])
    M_shear = np.asarray([[1., shear_x], [shear_y, 1.]])
    M = np.dot(M_rot, M_shear)
    M *= scale  # Without translation, it does not matter whether scale is
                # applied first or last.

    coords = coords.astype(np.float64)
    coords -= c_in
    coords = np.dot(M.T, coords.T).T
    coords += c_out
    return np.round(coords).astype(np.int32)


  # tf augmentation methods
  # TODO https://github.com/tensorflow/benchmarks/blob/master/scripts/tf_cnn_benchmarks/preprocessing.py

def rotate(self, angle):
        """Return a new transformation, rotated by 'angle' (radians).

        Example:
            >>> import math
            >>> t = Transform()
            >>> t.rotate(math.pi / 2)
            <Transform [0 1 -1 0 0 0]>
            >>>
        """
        import math
        c = _normSinCos(math.cos(angle))
        s = _normSinCos(math.sin(angle))
        return self.transform((c, s, -s, c, 0, 0))

def img_affine_aug_pipeline_2d(img, op_str='rts', rotate_angle_range=5, translate_range=3, shear_range=3, random_mode=True, probability=0.5):
    if random_mode:
        if random.random() < 0.5:
            return img

    mat = np.identity(3)
    for op in op_str:
        if op == 'r':
            rad = math.radian(((random.random() * 2) - 1) * rotate_angle_range)
            cos = math.cos(rad)
            sin = math.sin(rad)
            rot_mat = np.identity(3)
            rot_mat[0][0] = cos
            rot_mat[0][1] = sin
            rot_mat[1][0] = -sin
            rot_mat[1][1] = cos
            mat = np.dot(mat, rot_mat)
        elif op == 't':
            dx = ((random.random() * 2) - 1) * translate_range
            dy = ((random.random() * 2) - 1) * translate_range
            shift_mat = np.identity(3)
            shift_mat[0][2] = dx
            shift_mat[1][2] = dy
            mat = np.dot(mat, shift_mat)
        elif op == 's':
            dx = ((random.random() * 2) - 1) * shear_range
            dy = ((random.random() * 2) - 1) * shear_range
            shear_mat = np.identity(3)
            shear_mat[0][1] = dx
            shear_mat[1][0] = dy
            mat = np.dot(mat, shear_mat)
        else:
            continue

    affine_mat = np.array([mat[0], mat[1]])
    return apply_affine(img, affine_mat), affine_mat

def checkFront(self):
        "Update the front color sensor"

        # Get sensor position
        pos = delta(self.pos, vec2d(-self.radius, self.angle))

        # Sensor distance to edge of sketch
        sk = self.sketch
        if sk.weight:
            obj = sk
            prox = _distToWall(pos, self.angle, self.sensorWidth, *sk.size)
        else: obj = prox = None

        # Find closest object within sensor width
        u = vec2d(1, self.angle)
        sw = self.sensorWidth * DEG
        for gr in self.sensorObjects(sk):
            if gr is not self and gr.avgColor and hasattr(gr, "rect"):
                dr = delta(gr.rect.center, pos)
                d = hypot(*dr)
                r = gr.radius
                if r >= d:
                    prox = 0
                    obj = gr
                elif prox is None or d - r < prox:
                    minDot = cos(min(sw + asin(r/d), pi / 2))
                    x = (1 - sprod(u, dr) / d) / (1 - minDot)
                    if x < 1:
                        obj = gr
                        prox = (d - r) * (1 - x) + x * sqrt(d*d-r*r)

        # Save data
        self.closestObject = obj
        c = rgba(sk.border if obj is sk
            else obj.avgColor if obj else (0,0,0))
        self.sensorFront = noise(divAlpha(c), self.sensorNoise, 255)
        self.proximity = None if prox is None else round(prox)

def vec2d(r, a, deg=True):
    "2D Polar to Cartesian conversion"
    if deg: a *= DEG
    return r * cos(a), r * sin(a)

def _matrix(rotate=0, scale=1, rev=False):
    "Create a 2x2 matrix (as a 4-tuple) to perform a scale transformation and a rotation"
    sx, sy = (scale, scale) if type(scale) in (float, int) else scale
    if rotate:
        rotate *= DEG
        c, s = cos(rotate), sin(rotate)
    else: c, s = 1, 0
    if rev: # Rotate before scaling
        return sx * c, -sx * s, sy * s, sy * c
    else:   # Scale before rotating
        return sx * c, -sy * s, sx * s, sy * c

def generate_points(width, height):
    amp = 5  # radius fillet

    width += 2
    height += 4
    width = ((width/2) - amp) + 2
    height -= (2*amp)

    pos_list, final_list = [], []

    n_points = 12
    seg_angle = 2 * math.pi / n_points
    for i in range(n_points + 1):
        angle = i * seg_angle
        x = math.cos(angle) * amp
        y = math.sin(angle) * amp
        pos_list.append([x, -y])

    w_list, h_list = [1, -1, -1, 1], [-1, -1, 1, 1]
    slice_list = [[i, i+4] for i in range(0, n_points, 3)]

    for idx, (start, end) in enumerate(slice_list):
        point_array = pos_list[start:end]
        w = width * w_list[idx]
        h = height * h_list[idx]
        final_list += adjust_list(point_array, w, h)

    return final_list

def orthogonalization_matrix(lengths, angles):
    """Return orthogonalization matrix for crystallographic cell coordinates.

    Angles are expected in degrees.

    The de-orthogonalization matrix is the inverse.

    >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
    >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
    True
    >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
    >>> numpy.allclose(numpy.sum(O), 43.063229)
    True

    """
    a, b, c = lengths
    angles = numpy.radians(angles)
    sina, sinb, _ = numpy.sin(angles)
    cosa, cosb, cosg = numpy.cos(angles)
    co = (cosa * cosb - cosg) / (sina * sinb)
    return numpy.array([
        [ a*sinb*math.sqrt(1.0-co*co),  0.0,    0.0, 0.0],
        [-a*sinb*co,                    b*sina, 0.0, 0.0],
        [ a*cosb,                       b*cosa, c,   0.0],
        [ 0.0,                          0.0,    0.0, 1.0]])

def quaternion_about_axis(angle, axis):
    """Return quaternion for rotation about axis.

    >>> q = quaternion_about_axis(0.123, [1, 0, 0])
    >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0])
    True

    """
    q = numpy.array([0.0, axis[0], axis[1], axis[2]])
    qlen = vector_norm(q)
    if qlen > _EPS:
        q *= math.sin(angle/2.0) / qlen
    q[0] = math.cos(angle/2.0)
    return q

def get_object_absolute_coords(self, obj):
        """
        Determines the absolute coordinates of the given object based on the
        agent's current position.  Returns None if the coordinates can't be
        calculated.
        """

        # we can't calculate this without a distance to the object
        if obj.distance is None:
            return None

        # get the components of the vector to the object
        dx = obj.distance * math.cos(obj.direction)
        dy = obj.distance * math.sin(obj.direction)

        # return the point the object is at relative to our current position
        return (self.abs_coords[0] + dx, self.abs_coords[1] + dy)