Python math.sin() Examples

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 interpolate(self, other, this_weight):
        q0, q1 = np.roll(self.q, shift=1), np.roll(other.q, shift=1)
        u = 1 - this_weight
        assert(u >= 0 and u <= 1)
        cos_omega = np.dot(q0, q1)

        if cos_omega < 0:
            result = -q0[:]
            cos_omega = -cos_omega
        else:
            result = q0[:]

        cos_omega = min(cos_omega, 1)

        omega = math.acos(cos_omega)
        sin_omega = math.sin(omega)
        a = math.sin((1-u) * omega)/ sin_omega
        b = math.sin(u * omega) / sin_omega

        if abs(sin_omega) < 1e-6:
            # direct linear interpolation for numerically unstable regions
            result = result * this_weight + q1 * u
            result /= math.sqrt(np.dot(result, result))
        else:
            result = result*a + q1*b
        return Quaternion(np.roll(result, shift=-1))

    # To conversions 

def __init__(self):
        # Angle at which to fail the episode
        self.theta_threshold_radians = 12 * 2 * math.pi / 360
        self.x_threshold = 2.4

        # Initializing Course : predfined Oval Course
        # ToDo: ????????????
        Rad = 190.0
        Poly = 16
        self.Course = Walls(240, 50, 640-(50+Rad),50)
        for i in range(1, Poly):
            self.Course.addPoint(Rad*math.cos(-np.pi/2.0 + np.pi*i/Poly)+640-(50+Rad), 
                                Rad*math.sin(-np.pi/2.0 + np.pi*i/Poly)+50+Rad)
        self.Course.addPoint(240, 50+Rad*2)
        for i in range(1, Poly):
            self.Course.addPoint(Rad*math.cos(np.pi/2.0 + np.pi*i/Poly)+(50+Rad), 
                                Rad*math.sin(np.pi/2.0 + np.pi*i/Poly)+50+Rad)
        self.Course.addPoint(240,50)
        
        # Outr Boundary Box
        self.BBox = Walls(640, 479, 0, 479)
        self.BBox.addPoint(0,0)
        self.BBox.addPoint(640,0)
        self.BBox.addPoint(640,479)
        
        # Mono Sensor Line Follower 
        self.A = Agent((640, 480), 240, 49)

        # Action Space : left wheel speed, right wheel speed
        # Observation Space : Detect Line (True, False)
        self.action_space = spaces.Box( np.array([-1.,-1.]), np.array([+1.,+1.])) 
        self.observation_space = spaces.Discrete(1)

        self._seed()
        self.reset()
        self.viewer = None

        self.steps_beyond_done = None

        self._configure() 

def lonlat_to_pixel(self, lonlat, zoom):
        "Converts a longitude, latitude coordinate pair for the given zoom level."
        # Setting up, unpacking the longitude, latitude values and getting the
        # number of pixels for the given zoom level.
        lon, lat = self.get_lon_lat(lonlat)
        npix = self._npix[zoom]

        # Calculating the pixel x coordinate by multiplying the longitude value
        # with the number of degrees/pixel at the given zoom level.
        px_x = round(npix + (lon * self._degpp[zoom]))

        # Creating the factor, and ensuring that 1 or -1 is not passed in as the
        # base to the logarithm.  Here's why:
        #  if fac = -1, we'll get log(0) which is undefined;
        #  if fac =  1, our logarithm base will be divided by 0, also undefined.
        fac = min(max(sin(DTOR * lat), -0.9999), 0.9999)

        # Calculating the pixel y coordinate.
        px_y = round(npix + (0.5 * log((1 + fac) / (1 - fac)) * (-1.0 * self._radpp[zoom])))

        # Returning the pixel x, y to the caller of the function.
        return (px_x, px_y) 

def sinox(x):
	'''sin(x)/x'''
	if(abs(x) > 1e-2):
		return math.sin(x) / x
	else:
		return 1 - x*x / 6 + x*x*x*x / 120 

def sinox3(x):		
	'''(x-sin(x))/(x*x*x)'''
	if (abs(x) > 1e-2):			
		return (x - math.sin(x)) / (x*x*x)			
	else:			
		return 1./6. - x*x / 120. + x*x*x*x / 5040. 

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 shape_ratio(self, shape, ratio):
        """Calculate shape ratio as defined in paper"""
        if shape == 1:  # spherical
            result = 0.2 + 0.8 * sin(pi * ratio)
        elif shape == 2:  # hemispherical
            result = 0.2 + 0.8 * sin(0.5 * pi * ratio)
        elif shape == 3:  # cylindrical
            result = 1.0
        elif shape == 4:  # tapered cylindrical
            result = 0.5 + 0.5 * ratio
        elif shape == 5:  # flame
            if ratio <= 0.7:
                result = ratio / 0.7
            else:
                result = (1.0 - ratio) / 0.3
        elif shape == 6:  # inverse conical
            result = 1.0 - 0.8 * ratio
        elif shape == 7:  # tend flame
            if ratio <= 0.7:
                result = 0.5 + 0.5 * ratio / 0.7
            else:
                result = 0.5 + 0.5 * (1.0 - ratio) / 0.3
        elif shape == 8:  # envelope
            if ratio < 0 or ratio > 1:
                result = 0.0
            elif ratio < 1 - self.param.prune_width_peak:
                result = pow(ratio / (1 - self.param.prune_width_peak),
                             self.param.prune_power_high)
            else:
                result = pow((1 - ratio) / (1 - self.param.prune_width_peak),
                             self.param.prune_power_low)
        else:  # conical (0)
            result = 0.2 + 0.8 * ratio
        return result 

def q_prod(sym):
    """Production rule for Q"""
    prop_off = sym.parameters["t"] / __t_max__
    if prop_off < 1:
        res = [LSymbol("!", {"w": 0.85 + 0.15 * sin(sym.parameters["t"])}),
               LSymbol("^", {"a": random() - 0.65})]
        if prop_off > __p_max__:
            d_ang = 1 / (1 - __p_max__) * (1 - prop_off) * 110 + 15
            res.extend([LSymbol("!", {"w": 0.1})])
            for ind in range(int(random() * 2 + 5)):
                r_ang = sym.parameters["t"] * 10 + ind * (random() * 50 + 40)
                e_d_ang = d_ang * (random() * 0.4 + 0.8)
                res.extend([LSymbol("/", {"a": r_ang}),
                            LSymbol("&", {"a": e_d_ang}),
                            LSymbol("["),
                            LSymbol("A"),
                            LSymbol("]"),
                            LSymbol("^", {"a": e_d_ang}),
                            LSymbol("\\", {"a": r_ang})],)
            res.append(LSymbol("F", {"l": 0.05}))
        else:
            res.append(LSymbol("F", {"l": 0.15}))
        res.append(LSymbol("Q", {"t": sym.parameters["t"] + __d_t__}))
    else:
        res = [LSymbol("!", {"w": 0}),
               LSymbol("F", {"l": 0.15})]
    return res 

def calculate_cut_off(self):
        """
        Calculate cut-off radius as Rc = L/2 from a given MOF object.
        """
        width_a = self.ucv / (self.uc_size[1] * self.uc_size[2] / math.sin(math.radians(self.uc_angle[0])))
        width_b = self.ucv / (self.uc_size[0] * self.uc_size[2] / math.sin(math.radians(self.uc_angle[1])))
        width_c = self.ucv / (self.uc_size[0] * self.uc_size[1] / math.sin(math.radians(self.uc_angle[2])))
        self.cut_off = min(width_a / 2, width_b / 2, width_c / 2) 

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 test_send():
    btl = BluetoothLogger(fields=["speed", "rpm", "soc"], password=password)
    btl.start()

    # generate some data and send it
    x = range(1000)
    y = map(lambda v: sin(v * pi / 45) * 5000 + 5000, x)
    speeds = cycle(y)
    print("Sending dummy data")
    while 1:
        try:
            row = map(str, [round(next(speeds), 2), 5000, 50])
            btl.send(",".join(row))
            time.sleep(1)
        except KeyboardInterrupt:
            print("Terminating")
            break
    btl.join() 

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 to_angle_axis(self):
        """ Return axis-angle representation """
        q = np.roll(self.q, shift=1)
        halftheta = math.acos(q[0])
        if abs(halftheta) < 1e-12:
            return 0, np.array((0, 0, 1))
        else:
            theta = halftheta * 2
            axis = np.array(q[1:4]) / math.sin(halftheta)
            return theta, axis 

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 quaternion_about_axis(angle, axis):
    """Return quaternion for rotation about axis.

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

    """
    quaternion = numpy.zeros((4, ), dtype=numpy.float64)
    quaternion[:3] = axis[:3]
    qlen = vector_norm(quaternion)
    if qlen > _EPS:
        quaternion *= math.sin(angle/2.0) / qlen
    quaternion[3] = math.cos(angle/2.0)
    return quaternion 

def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
    """Return spherical linear interpolation between two quaternions.

    >>> q0 = random_quaternion()
    >>> q1 = random_quaternion()
    >>> q = quaternion_slerp(q0, q1, 0.0)
    >>> numpy.allclose(q, q0)
    True
    >>> q = quaternion_slerp(q0, q1, 1.0, 1)
    >>> numpy.allclose(q, q1)
    True
    >>> q = quaternion_slerp(q0, q1, 0.5)
    >>> angle = math.acos(numpy.dot(q0, q))
    >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \
        numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle)
    True

    """
    q0 = unit_vector(quat0[:4])
    q1 = unit_vector(quat1[:4])
    if fraction == 0.0:
        return q0
    elif fraction == 1.0:
        return q1
    d = numpy.dot(q0, q1)
    if abs(abs(d) - 1.0) < _EPS:
        return q0
    if shortestpath and d < 0.0:
        # invert rotation
        d = -d
        q1 *= -1.0
    angle = math.acos(d) + spin * math.pi
    if abs(angle) < _EPS:
        return q0
    isin = 1.0 / math.sin(angle)
    q0 *= math.sin((1.0 - fraction) * angle) * isin
    q1 *= math.sin(fraction * angle) * isin
    q0 += q1
    return q0 

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 sine(middle, pos):
    return (sin((-pi / 2.0) + pi * linear(middle, pos)) + 1.0) / 2.0 

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 setup(self):
		pl = Plot(self.size, [-4, 4, -1.5, 1.5]).config(bg="white")
		pl.series(sin, param=[-pi, pi, 2 * self.width - 1], marker=("blue", 4))
		self.bg = pl.snapshot()
		robo = Robot(["#ff5050", "#ffd428"])
		self["Traci"] = self.bindBrain(robo).config(width=60, pos=self.center) 

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