Python math.sin() Examples

def swirl(x, y, step):
    x -= (u_width / 2)
    y -= (u_height / 2)
    dist = math.sqrt(pow(x, 2) + pow(y, 2)) / 2.0
    angle = (step / 10.0) + (dist * 1.5)
    s = math.sin(angle)
    c = math.cos(angle)
    xs = x * c - y * s
    ys = x * s + y * c
    r = abs(xs + ys)
    r = r * 12.0
    r -= 20
    return (r, r + (s * 130), r + (c * 130))


# roto-zooming checker board 

def checker(x, y, step):
    x -= (u_width / 2)
    y -= (u_height / 2)
    angle = (step / 10.0)
    s = math.sin(angle)
    c = math.cos(angle)
    xs = x * c - y * s
    ys = x * s + y * c
    xs -= math.sin(step / 200.0) * 40.0
    ys -= math.cos(step / 200.0) * 40.0
    scale = step % 20
    scale /= 20
    scale = (math.sin(step / 50.0) / 8.0) + 0.25
    xs *= scale
    ys *= scale
    xo = abs(xs) - int(abs(xs))
    yo = abs(ys) - int(abs(ys))
    v = 0 if (math.floor(xs) + math.floor(ys)) % 2 else 1 if xo > .1 and yo > .1 else .5
    r, g, b = hue_to_rgb[step % 255]
    return (r * (v * 255), g * (v * 255), b * (v * 255))


# weeee waaaah 

def distance(pointA, pointB):
	"""
	Calculate the great circle distance between two points 
	on the earth (specified in decimal degrees)

	http://stackoverflow.com/questions/15736995/how-can-i-quickly-estimate-the-distance-between-two-latitude-longitude-points
	"""
	# convert decimal degrees to radians 
	lon1, lat1, lon2, lat2 = map(math.radians, [pointA[1], pointA[0], pointB[1], pointB[0]])

	# haversine formula 
	dlon = lon2 - lon1 
	dlat = lat2 - lat1 
	a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
	c = 2 * math.asin(math.sqrt(a)) 
	r = 3956  # Radius of earth in miles. Use 6371 for kilometers
	return c * r 

def gamma(z, sqrt2pi=(2.0*pi)**0.5):
    # Reflection to right half of complex plane
    if z < 0.5:
        return pi / sin(pi*z) / gamma(1.0-z)
    # Lanczos approximation with g=7
    az = z + (7.0 - 0.5)
    return az ** (z-0.5) / exp(az) * sqrt2pi * fsum([
        0.9999999999995183,
        676.5203681218835 / z,
        -1259.139216722289 / (z+1.0),
        771.3234287757674 / (z+2.0),
        -176.6150291498386 / (z+3.0),
        12.50734324009056 / (z+4.0),
        -0.1385710331296526 / (z+5.0),
        0.9934937113930748e-05 / (z+6.0),
        0.1659470187408462e-06 / (z+7.0),
    ]) 

def rotate_points(points, angle):
    """
    Rotate points in a coordinate system using a rotation matrix based on
    an angle.

    Parameters
    ----------
    points : (Mx2) array
        The coordinates of the points.
    angle : float
        The angle by which the points will be rotated (in radians).

    Returns
    -------
    points_rotated : (Mx2) array
        The coordinates of the rotated points.
    """
    # Compute rotation matrix
    rot_matrix = np.array(((math.cos(angle), -math.sin(angle)),
                           (math.sin(angle), math.cos(angle))))
    # Apply rotation matrix to the points
    points_rotated = np.dot(points, rot_matrix)

    return np.array(points_rotated) 

def distance(origin, destination):
	"""Determine distance between 2 sets of [lat,lon] in km"""

	lat1, lon1 = origin
	lat2, lon2 = destination
	radius = 6371  # km

	dlat = math.radians(lat2 - lat1)
	dlon = math.radians(lon2 - lon1)
	a = (math.sin(dlat / 2) * math.sin(dlat / 2) +
		 math.cos(math.radians(lat1)) *
		 math.cos(math.radians(lat2)) * math.sin(dlon / 2) *
		 math.sin(dlon / 2))
	c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
	d = radius * c

	return d 

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)) 
    m = 6367000. * c
    return m

# calculating the intersection  point between two rays (specified each by camera position and depth-estimated object location) 

def line_3d():
    data=[]
    for t in range(0,25000):
        _t=t/1000
        x=(1+0.25*math.cos(75*_t))*math.cos(_t)
        y=(1+0.25*math.cos(75*_t))*math.sin(_t)
        z=_t+2.0*math.sin(75*_t)
        data.append([x,y,z])
    c=(
        Line3D()
        .add(
            '',
            data,
            xaxis3d_opts=op.Axis3DOpts(Faker.clock,type_='value'),
            yaxis3d_opts=op.Axis3DOpts(Faker.week_en,type_='value'),
            grid3d_opts=op.Grid3DOpts(width=100,height=100,depth=100,rotate_speed=100,is_rotate=True),
        )
       .set_global_opts(
        visualmap_opts=op.VisualMapOpts(
            max_=30,min_=0,range_color=Faker.visual_color
        ),
        title_opts=op.TitleOpts(title='line3d'),
    )
    )
    return c 

def simplex2D_subjective():
	print("Displaying 2D simplex output")
	N = 100
	pmap = [[combined(simplex2D, x / N, y / N) for x in range(N)] for y in range(N)]

	plt.subplot(221)
	plt.title("6 octaves")
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(222)
	plt.title("1 octave")
	pmap = [[combined(simplex2D, 6 * x / N, 6 * y / N, octaves=1) for x in range(N)] for y in range(N)]
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(223)
	plt.title("4 octaves, marbled")
	mpmap = [[sin(1.6 * 2 * pi * combined(simplex2D, 6 * x / N, 6 * y / N, octaves=4)) for x in range(N)] for y in range(N)]
	plt.imshow(mpmap, cmap='plasma', interpolation='nearest')

	plt.subplot(224)
	plt.title("15 octaves, crinkled")
	cpmap = [[combined(simplex2D, 10 * x / N, 10 * y / N, octaves=15) for x in range(N)] for y in range(N)]
	plt.imshow(cpmap, cmap='plasma', interpolation='nearest')

	plt.show() 

def white2D_subjective():
	print("Displaying 2D white output")
	N = 100
	pmap = [[combined(white, x / N, y / N) for x in range(N)] for y in range(N)]

	plt.subplot(221)
	plt.title("6 octaves")
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(222)
	plt.title("1 octave")
	pmap = [[combined(white, 6 * x / N, 6 * y / N, octaves=1) for x in range(N)] for y in range(N)]
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(223)
	plt.title("4 octaves, marbled")
	mpmap = [[sin(1.6 * 2 * pi * combined(white, 6 * x / N, 6 * y / N, octaves=4)) for x in range(N)] for y in range(N)]
	plt.imshow(mpmap, cmap='plasma', interpolation='nearest')

	plt.subplot(224)
	plt.title("15 octaves, crinkled")
	cpmap = [[combined(white, 10 * x / N, 10 * y / N, octaves=15) for x in range(N)] for y in range(N)]
	plt.imshow(cpmap, cmap='plasma', interpolation='nearest')

	plt.show() 

def perlin2D_subjective():
	print("Displaying 2D perlin output")
	N = 100
	pmap = [[combined(perlin2D, x / N, y / N) for x in range(N)] for y in range(N)]

	plt.subplot(221)
	plt.title("6 octaves")
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(222)
	plt.title("1 octave")
	pmap = [[combined(perlin2D, 6 * x / N, 6 * y / N, octaves=1) for x in range(N)] for y in range(N)]
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(223)
	plt.title("4 octaves, marbled")
	mpmap = [[sin(1.6 * 2 * pi * combined(perlin2D, 6 * x / N, 6 * y / N, octaves=4)) for x in range(N)] for y in range(N)]
	plt.imshow(mpmap, cmap='plasma', interpolation='nearest')

	plt.subplot(224)
	plt.title("15 octaves, crinkled")
	cpmap = [[combined(perlin2D, 10 * x / N, 10 * y / N, octaves=15) for x in range(N)] for y in range(N)]
	plt.imshow(cpmap, cmap='plasma', interpolation='nearest')

	plt.show() 

def opensimplex2D_subjective():
	print("Displaying 2D opensimplex output")
	N = 100
	pmap = [[combined(opensimplex2D, x / N, y / N) for x in range(N)] for y in range(N)]

	plt.subplot(221)
	plt.title("6 octaves")
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(222)
	plt.title("1 octave")
	pmap = [[combined(opensimplex2D, 6 * x / N, 6 * y / N, octaves=1) for x in range(N)] for y in range(N)]
	plt.imshow(pmap, cmap='plasma', interpolation='nearest')

	plt.subplot(223)
	plt.title("4 octaves, marbled")
	mpmap = [[sin(1.6 * 2 * pi * combined(opensimplex2D, 6 * x / N, 6 * y / N, octaves=4)) for x in range(N)] for y in range(N)]
	plt.imshow(mpmap, cmap='plasma', interpolation='nearest')

	plt.subplot(224)
	plt.title("15 octaves, crinkled")
	cpmap = [[combined(opensimplex2D, 10 * x / N, 10 * y / N, octaves=15) for x in range(N)] for y in range(N)]
	plt.imshow(cpmap, cmap='plasma', interpolation='nearest')

	plt.show() 

def random_quat(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_quat()
    >>> np.allclose(1.0, vector_norm(q))
    True
    >>> q = random_quat(np.random.random(3))
    >>> q.shape
    (4,)
    """
    if rand is None:
        rand = np.random.rand(3)
    else:
        assert len(rand) == 3
    r1 = np.sqrt(1.0 - rand[0])
    r2 = np.sqrt(rand[0])
    pi2 = math.pi * 2.0
    t1 = pi2 * rand[1]
    t2 = pi2 * rand[2]
    return np.array(
        (np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2, np.cos(t2) * r2),
        dtype=np.float32,
    ) 

def rbbox_to_corners(corners, rbbox):
    # generate clockwise corners and rotate it clockwise
    angle = rbbox[4]
    a_cos = math.cos(angle)
    a_sin = math.sin(angle)
    center_x = rbbox[0]
    center_y = rbbox[1]
    x_d = rbbox[2]
    y_d = rbbox[3]
    corners_x = cuda.local.array((4, ), dtype=numba.float32)
    corners_y = cuda.local.array((4, ), dtype=numba.float32)
    corners_x[0] = -x_d / 2
    corners_x[1] = -x_d / 2
    corners_x[2] = x_d / 2
    corners_x[3] = x_d / 2
    corners_y[0] = -y_d / 2
    corners_y[1] = y_d / 2
    corners_y[2] = y_d / 2
    corners_y[3] = -y_d / 2
    for i in range(4):
        corners[2 *
                i] = a_cos * corners_x[i] + a_sin * corners_y[i] + center_x
        corners[2 * i
                + 1] = -a_sin * corners_x[i] + a_cos * corners_y[i] + center_y 

def blues_and_twos(x, y, step):
    x -= (u_width / 2)
    y -= (u_height / 2)
    scale = math.sin(step / 6.0) / 1.5
    r = math.sin((x * scale) / 1.0) + math.cos((y * scale) / 1.0)
    b = math.sin(x * scale / 2.0) + math.cos(y * scale / 2.0)
    g = r - .8
    g = 0 if g < 0 else g
    b -= r
    b /= 1.4
    return (r * 255, (b + g) * 255, g * 255)


# rainbow search spotlights 

def rainbow_search(x, y, step):
    xs = math.sin((step) / 100.0) * 20.0
    ys = math.cos((step) / 100.0) * 20.0
    scale = ((math.sin(step / 60.0) + 1.0) / 5.0) + 0.2
    r = math.sin((x + xs) * scale) + math.cos((y + xs) * scale)
    g = math.sin((x + xs) * scale) + math.cos((y + ys) * scale)
    b = math.sin((x + ys) * scale) + math.cos((y + ys) * scale)
    return (r * 255, g * 255, b * 255)


# zoom tunnel 

def tunnel(x, y, step):
    speed = step / 100.0
    x -= (u_width / 2)
    y -= (u_height / 2)
    xo = math.sin(step / 27.0) * 2
    yo = math.cos(step / 18.0) * 2
    x += xo
    y += yo
    if y == 0:
        if x < 0:
            angle = -(math.pi / 2)
        else:
            angle = (math.pi / 2)
    else:
        angle = math.atan(x / y)
    if y > 0:
        angle += math.pi
    angle /= 2 * math.pi  # convert angle to 0...1 range
    hyp = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
    shade = hyp / 2.1
    shade = 1 if shade > 1 else shade
    angle += speed
    depth = speed + (hyp / 10)
    col1 = hue_to_rgb[step % 255]
    col1 = (col1[0] * 0.8, col1[1] * 0.8, col1[2] * 0.8)
    col2 = hue_to_rgb[step % 255]
    col2 = (col2[0] * 0.3, col2[1] * 0.3, col2[2] * 0.3)
    col = col1 if int(abs(angle * 6.0)) % 2 == 0 else col2
    td = .3 if int(abs(depth * 3.0)) % 2 == 0 else 0
    col = (col[0] + td, col[1] + td, col[2] + td)
    col = (col[0] * shade, col[1] * shade, col[2] * shade)
    return (col[0] * 255, col[1] * 255, col[2] * 255) 

def rotation_matrix(degrees):
        angle = math.radians(degrees)
        return numpy.array((
            (math.cos(angle), -math.sin(angle), 0.0, 0.0),
            (math.sin(angle), math.cos(angle), 0.0, 0.0),
            (0.0, 0.0, 1.0, 0.0),
            (0.0, 0.0, 0.0, 1.0)
        )) 

def vector_from_angle(degrees):
    """Returns a unit vector in the xy-plane rotated by the given degrees from
    the positive x-axis"""
    radians = math.radians(degrees)
    z_rot_matrix = numpy.identity(4)
    z_rot_matrix[0, 0] = math.cos(radians)
    z_rot_matrix[0, 1] = -math.sin(radians)
    z_rot_matrix[1, 0] = math.sin(radians)
    z_rot_matrix[1, 1] = math.cos(radians)

    return tuple(numpy.dot(z_rot_matrix, (1, 0, 0, 0))[:3]) 

def image_rotate(tv,name,img,shape,angles,diff=0):
    """Rotate an image and put it into tv.images
    
    Arguments:
        tv -- vid to load into
        name -- prefix name to give the images
        image -- image to load anis from
        shape -- shape fimage (usually a subset of 0,0,w,h) used for collision detection
        angles -- a list of angles to render in degrees
        diff -- a number to add to the angles, to correct for source image not actually being at 0 degrees

    """
    w1,h1 = img.get_width(),img.get_height()
    shape = pygame.Rect(shape)
    ps = shape.topleft,shape.topright,shape.bottomleft,shape.bottomright
    for a in angles:
        img2 = pygame.transform.rotate(img,a+diff)
        w2,h2 = img2.get_width(),img2.get_height()
        minx,miny,maxx,maxy = 1024,1024,0,0
        for x,y in ps:
            x,y = x-w1/2,y-h1/2
            a2 = math.radians(a+diff)
            #NOTE: the + and - are switched from the normal formula because of
            #the weird way that pygame does the angle...
            x2 = x*math.cos(a2) + y*math.sin(a2) 
            y2 = y*math.cos(a2) - x*math.sin(a2)
            x2,y2 = x2+w2/2,y2+h2/2
            minx = min(minx,x2)
            miny = min(miny,y2)
            maxx = max(maxx,x2)
            maxy = max(maxy,y2)
        r = pygame.Rect(minx,miny,maxx-minx,maxy-miny)
        #((ww-w)/2,(hh-h)/2,w,h)
        tv.images["%s.%d"%(name,a)] = img2,r 

def rotate(self, angle):
        """rotate self counterclockwise by angle
        """
        perp = Vec2D(-self[1], self[0])
        angle = angle * math.pi / 180.0
        c, s = math.cos(angle), math.sin(angle)
        return Vec2D(self[0]*c+perp[0]*s, self[1]*c+perp[1]*s) 

def testSin(self):
        self.assertRaises(TypeError, math.sin)
        self.ftest('sin(0)', math.sin(0), 0)
        self.ftest('sin(pi/2)', math.sin(math.pi/2), 1)
        self.ftest('sin(-pi/2)', math.sin(-math.pi/2), -1)
        try:
            self.assertTrue(math.isnan(math.sin(INF)))
            self.assertTrue(math.isnan(math.sin(NINF)))
        except ValueError:
            self.assertRaises(ValueError, math.sin, INF)
            self.assertRaises(ValueError, math.sin, NINF)
        self.assertTrue(math.isnan(math.sin(NAN))) 

def from_rad_axis_to_q(r):
        """
        :param q: rad in format (x-y-z) * r
        :return: quaternion in w-i-j-k format
        """
        norm_axis = math.sqrt(r[0] ** 2 + r[1] ** 2 + r[2] ** 2)
        if norm_axis < 1e-5:
            return np.asarray((1.0, 0., 0., 0.), dtype=np.float32)
        else:
            return np.asarray((math.cos(0.5 * norm_axis),
                               math.sin(0.5 * norm_axis) * r[0] / norm_axis,
                               math.sin(0.5 * norm_axis) * r[1] / norm_axis,
                               math.sin(0.5 * norm_axis) * r[2] / norm_axis), dtype=np.float32) 

def to_angle_axis(q0):
    rad = math.atan2(np.linalg.norm(q0[1:]), q0[0])
    sin_x = math.sin(rad)
    if abs(sin_x) < 1e-5:
        return np.array([sin_x * 2.0, q0[1], q0[2], q0[3]], dtype=np.float32)
    return np.array([rad * 2.0, q0[1] / sin_x, q0[2] / sin_x, q0[3] / sin_x], dtype=np.float32) 

def from_angle_axis(a):
    return np.asarray((math.cos(0.5 * a[0]),
                       math.sin(0.5 * a[0]) * a[1],
                       math.sin(0.5 * a[0]) * a[2],
                       math.sin(0.5 * a[0]) * a[3]), dtype=np.float32) 

def from_euler(rpy, order="321"):
    qx = (math.cos(rpy[0] / 2.), math.sin(rpy[0] / 2.), 0., 0.)
    qy = (math.cos(rpy[1] / 2.), 0., math.sin(rpy[1] / 2.), 0.)
    qz = (math.cos(rpy[2] / 2.), 0., 0., math.sin(rpy[2] / 2.))
    l = (qx, qy, qz)
    order = (ord(order[0]) - ord('1'), ord(order[1]) - ord('1'), ord(order[2]) - ord('1'))
    return normalize(qmul(qmul(l[order[0]], l[order[1]]), l[order[2]])) 

def check_tol(charger: cozmo.objects.Charger,dist_charger=40):
    # Check if the position tolerance in front of the charger is respected
    global RobotGlobal
    robot = RobotGlobal
    global PI

    distance_tol = 5 # mm, tolerance for placement error
    angle_tol = 5*PI/180 # rad, tolerance for orientation error

    try: 
        charger = robot.world.wait_for_observed_charger(timeout=2,include_existing=True)
    except:
        debug (1,'WARNING: Cannot see the charger to verify the position.')

    # Calculate positions
    r_coord = [0,0,0]
    c_coord = [0,0,0]
    # Coordonates of robot and charger
    r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise
    r_coord[1] = robot.pose.position.y
    r_coord[2] = robot.pose.position.z
    r_zRot = robot.pose_angle.radians # .degrees or .radians
    c_coord[0] = charger.pose.position.x
    c_coord[1] = charger.pose.position.y
    c_coord[2] = charger.pose.position.z
    c_zRot = charger.pose.rotation.angle_z.radians

    # Create target position 
    # dist_charger in mm, distance if front of charger
    c_coord[0] -=  dist_charger*math.cos(c_zRot)
    c_coord[1] -=  dist_charger*math.sin(c_zRot)

    # Direction and distance to target position (in front of charger)
    distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2)

    if(distance < distance_tol and math.fabs(r_zRot-c_zRot) < angle_tol):
    	return 1
    else: 
    	return 0 

def to_quaternion(pitch, roll, yaw):
    t0 = math.cos(yaw * 0.5)
    t1 = math.sin(yaw * 0.5)
    t2 = math.cos(roll * 0.5)
    t3 = math.sin(roll * 0.5)
    t4 = math.cos(pitch * 0.5)
    t5 = math.sin(pitch * 0.5)

    q = Quaternionr()
    q.w_val = t0 * t2 * t4 + t1 * t3 * t5 #w
    q.x_val = t0 * t3 * t4 - t1 * t2 * t5 #x
    q.y_val = t0 * t2 * t5 + t1 * t3 * t4 #y
    q.z_val = t1 * t2 * t4 - t0 * t3 * t5 #z
    return q 

def play_tone(frequency, amplitude, duration, fs, stream):
    N = int(fs / frequency)
    T = int(frequency * duration)  # repeat for T cycles
    dt = 1.0 / fs
    # 1 cycle
    tone = (amplitude * math.sin(2 * math.pi * frequency * n * dt)
            for n in xrange(N))
    # todo: get the format from the stream; this assumes Float32
    data = ''.join(struct.pack('f', samp) for samp in tone)
    for n in xrange(T):
        stream.write(data) 

def show_block_wheel(self):
        """Shows the block wheel for the user to choose a block to add in the
        experiment."""
        # If first time showing the wheel, simply remove the background guide
        # text
        if self._textID:
            self.canvas.delete(self._textID)
            self._textID = None

        self._afterjob = None
        self._wheel_visible = True

        # Gray out everything else
        for block in self._experiment_blocks:
            block.gray_out()
            if block._context_visible:
                block._context_box.set_invisible()
                block._context_visible = False

        self.canvas.configure(bg='#444444')

        # Display the wheel
        total_blocks = len(config.APPLICATION_BLOCKS)
        for i in xrange(total_blocks):
            teta = math.radians(i * (float(360) / total_blocks))
            self._wheel_blocks[i].set_visible()
            self._wheel_blocks[i].position(self._button_press_position['x'] -
                                           (config.BLOCK_SIZE / 2) +
                                           (config.WHEEL_RADIUS *
                                            math.sin(teta)),
                                           self._button_press_position['y'] -
                                           (config.BLOCK_SIZE / 2) -
                                           (config.WHEEL_RADIUS *
                                            math.cos(teta)))
            tk.Misc.lift(self._wheel_blocks[i]._frame) 

def get_flipped_origin(self, origin):
        table_width = self.get_table_column_count() * self.table_cell_size
        table_height = self.get_table_row_count() * self.table_cell_size
        diagonal = math.sqrt(pow(table_width, 2) + pow(table_height, 2))
        diagonal_angle = math.degrees(math.atan(table_width/table_height))
        angle_diagonal_to_x_axis = diagonal_angle - self.get_table_rotation()
        delta_x = math.sin(math.radians(angle_diagonal_to_x_axis)) * diagonal
        delta_y = math.cos(math.radians(angle_diagonal_to_x_axis)) * diagonal
        flipped_x = origin.x - delta_x
        flipped_y = origin.y + delta_y

        return Point(flipped_x, flipped_y) 

def get_cell_corner(self, angle):
        if (angle % 90 == 0):
            distance = self.get_cell_size()
        elif (angle % 45 == 0):
            distance = self.get_cell_size() * math.sqrt(2)
        else:
            raise Exception('The angle does not correspond to a corner in a square. Given angle: {}'.format(angle))

        # direction by table rotation and angle of the corner
        bearing = angle + self.get_table_rotation()

        corner_x = self.get_origin().x + distance * math.sin(math.radians(bearing))
        corner_y = self.get_origin().y + distance * math.cos(math.radians(bearing))

        return Point(corner_x, corner_y) 

def preCalculateMarkers(self):
        if self.specialMarkers is None:
            self.specialMarkers = {}
            fade = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
                255, 255, 255, 255, 255, 255, 255, 252, 246, 240, 234, 228, 223,
                217, 211, 205, 199, 193, 188, 182, 176, 170, 164, 159, 153, 147,
                141, 135, 130, 124, 118, 112, 106, 101, 95, 89, 83, 77, 71, 66,
                60, 54, 48, 42, 37, 31, 25]
            self.markerHeight = len(fade)

            # pre-calculate animated "active" marker
            animation = []
            for iCycle in range(10):
                n = len(fade)
                nh = n//2
                rgba = []
                rgba_r = []
                c1 = 110, 110, 110 # basic color
                c2 = 170, 175, 200 # second color
                for iFade,f in enumerate(fade):
                    x = min(0, -(iFade - nh))
                    a = sin(pi*(4*(x / float(n))**2 - iCycle / 10.))**2
                    rgb = ''.join([chr(int(c1[i]*(1-a)+c2[i]*a)) for i in 0,1,2]) + chr(f)
                    rgba.append(rgb*self.markerWidth)
                    x = -min(0, (iFade - nh))
                    a = sin(pi*(4*(x / float(n))**2 + iCycle / 10.))**2
                    rgb = ''.join([chr(int(c1[i]*(1-a)+c2[i]*a)) for i in 0,1,2])  + chr(fade[n-1-iFade])
                    rgba_r.append(rgb*self.markerWidth)
                rgb = tuple([(int(c1[i]*(1-a)+c2[i]*a)) for i in 0,1,2])
                rgba = ''.join(rgba)
                rgba_r = ''.join(rgba_r)
                animation.append((rgb, rgba, rgba_r)) 

def plane(self):
        normal = self.normal()
        return cq.Plane(self.origin, (math.cos(math.radians(self.z_rot)), math.sin(math.radians(self.z_rot)), 0), (normal[0], normal[1], normal[2])) 

def right_big_corner():
    return right_x-big_corner+(big_corner*math.sin(math.radians(45))), \
           -y-wrist+big_corner-(big_corner*math.cos(math.radians(45))) 

def left_big_corner():
    return -left_x+big_corner-(big_corner*math.sin(math.radians(45))), \
           -y-wrist+big_corner-(big_corner*math.cos(math.radians(45))) 

def distance(origin, destination):
    """
    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, origin + destination)

    # 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)) 
    r = 6371  # Radius of earth in kilometers. Use 3956 for miles
    return c * r 

def point2world(memory_service, point):
    
    point_world = []
    try:
        robot_pose = memory_service.getData(robotPoseKey)
        #print robot_pose
        s = math.sin(robot_pose[2])
        c = math.cos(robot_pose[2])
        point_world = [robot_pose[0] + c*point[0]-s*point[1],
                       robot_pose[1] + s*point[0]+c*point[1]];
    except:
        print "Cannot read Robot Pose. Is NAOqiLocalizer running?"

    return point_world 

def prettyscale(self):
    """
    Makes a palette of appealing colors (to me anyway) for a continuous gradient.
    The colors and intensities are intended to maximize percieved separation
    of values across the range.
    """
    import math
    self.pal = []
    a = 0
    rscl = [0.0, -1.0, math.pi/(self.n*0.8)]
    gscl = [0.3, 0.7, math.pi/(self.n*1.0)]
    bscl = [0.0, 1.0, math.pi/(self.n*1.5)]
    for i in range(0,self.n+1):
      r = rscl[0] + rscl[1] * math.cos(i*rscl[2])
      g = gscl[0] + gscl[1] * math.sin(i*gscl[2])
      b = bscl[0] + bscl[1] * math.cos(i*bscl[2])
      if r < 0: r = 0
      elif r > 1.0: r = 1.0
      if g < 0: g = 0
      elif g > 1.0: g = 1.0
      if b < 0: b = 0
      elif b > 1.0: b = 1.0
      if i == self.alpha:
        a = 0
      else:
        a = 255
      self.pal.append([int(r*255), int(g*255), int(b*255), a]) 

def rot_y(self, a):
        rot_v = Point(self.x*math.cos(a) - self.z * math.sin(a),
                      self.y,
                      self.x*math.sin(a) + self.z * math.cos(a))
        return rot_v 

def rot_x(self, a):
        rot_v = Point(self.x, 
                        self.y*math.cos(a) + self.z * math.sin(a),
                       -self.y*math.sin(a) + self.z * math.cos(a))
        return rot_v 

def rot_z(self, a):
        rot_v = Point(self.x*math.cos(a) + self.y * math.sin(a),
                       -self.x*math.sin(a) + self.y * math.cos(a),
                        self.z)
        return rot_v 

def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
  nMotors = 8
  speed = 0.35
  for jthMotor in range(nMotors):
    joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57
  return joint_values 

def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
  speed = 0.35
  phaseDiff = params[2]
  a0 = math.sin(i * speed) * params[0] + 1.57
  a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57
  a2 = math.sin(i * speed + params[3]) * params[0] + 1.57
  a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57
  a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57
  a5 = math.sin(i * speed + params[4]) * params[0] + 1.57
  a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57
  a7 = math.sin(i * speed + params[5]) * params[0] + 1.57
  joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
  return joint_values 

def evaluate_desired_motorAngle_hop(i, params):
  amplitude = params[0]
  speed = params[1]
  a1 = math.sin(i*speed)*amplitude+1.57
  a2 = math.sin(i*speed+3.14)*amplitude+1.57
  joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2]
  return joint_values 

def SinePolicyExample():
  """An example of minitaur walking with a sine gait."""
  env = minitaur_randomize_terrain_gym_env.MinitaurRandomizeTerrainGymEnv(
      render=True,
      motor_velocity_limit=np.inf,
      pd_control_enabled=True,
      on_rack=False)
  sum_reward = 0
  steps = 200
  amplitude_1_bound = 0.5
  amplitude_2_bound = 0.5
  speed = 40

  for step_counter in xrange(steps):
    time_step = 0.01
    t = step_counter * time_step

    amplitude1 = amplitude_1_bound
    amplitude2 = amplitude_2_bound
    steering_amplitude = 0
    if t < 10:
      steering_amplitude = 0.5
    elif t < 20:
      steering_amplitude = -0.5
    else:
      steering_amplitude = 0

    # Applying asymmetrical sine gaits to different legs can steer the minitaur.
    a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude)
    a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude)
    a3 = math.sin(t * speed) * amplitude2
    a4 = math.sin(t * speed + math.pi) * amplitude2
    action = [a1, a2, a2, a1, a3, a4, a4, a3]
    _, reward, _, _ = env.step(action)
    sum_reward += reward 

def StandUpExample():
  """An example that the minitaur stands up."""
  steps = 1000
  environment = minitaur_stand_gym_env.MinitaurStandGymEnv(
      render=True,
      motor_velocity_limit=np.inf)
  action = [0.5]
  _, _, done, _ = environment.step(action)
  for t in range(steps):
    # A policy that oscillates between -1 and 1
    action = [math.sin(t * math.pi * 0.01)]
    _, _, done, _ = environment.step(action)
    if done:
      break 

def _reset(self):
    self._ball_id = 0
    super(MinitaurBallGymEnv, self)._reset()
    self._init_ball_theta = random.uniform(-INIT_BALL_ANGLE, INIT_BALL_ANGLE)
    self._init_ball_distance = INIT_BALL_DISTANCE
    self._ball_pos = [self._init_ball_distance *
                      math.cos(self._init_ball_theta),
                      self._init_ball_distance *
                      math.sin(self._init_ball_theta), 1]
    self._ball_id = self._pybullet_client.loadURDF(
        "%s/sphere_with_restitution.urdf" % self._urdf_root, self._ball_pos)
    return self._get_observation() 

def _apply_steering_to_locomotion(self, action):
    # A hardcoded feedforward walking controller based on sine functions.
    amplitude_swing = 0.5
    amplitude_extension = 0.5
    speed = 200
    steering_amplitude = 0.5 * action[0]
    t = self.minitaur.GetTimeSinceReset()
    a1 = math.sin(t * speed) * (amplitude_swing + steering_amplitude)
    a2 = math.sin(t * speed + math.pi) * (amplitude_swing - steering_amplitude)
    a3 = math.sin(t * speed) * amplitude_extension
    a4 = math.sin(t * speed + math.pi) * amplitude_extension
    action = [a1, a2, a2, a1, a3, a4, a4, a3]
    return action 

def randomize_step(self, env):
    """Randomizes simulation steps.

    Will be called at every timestep. May add random forces/torques to Minitaur.

    Args:
      env: The Minitaur gym environment to be randomized.
    """
    base_link_ids = env.minitaur.chassis_link_ids
    if env.env_step_counter % self._perturbation_interval_steps == 0:
      self._applied_link_id = base_link_ids[np.random.randint(
          0, len(base_link_ids))]
      horizontal_force_magnitude = np.random.uniform(
          self._horizontal_force_bound[0], self._horizontal_force_bound[1])
      theta = np.random.uniform(0, 2 * math.pi)
      vertical_force_magnitude = np.random.uniform(
          self._vertical_force_bound[0], self._vertical_force_bound[1])
      self._applied_force = horizontal_force_magnitude * np.array(
          [math.cos(theta), math.sin(theta), 0]) + np.array(
              [0, 0, -vertical_force_magnitude])

    if (env.env_step_counter % self._perturbation_interval_steps <
        self._perturbation_duration_steps) and (env.env_step_counter >=
                                                self._perturbation_start_step):
      env.pybullet_client.applyExternalForce(
          objectUniqueId=env.minitaur.quadruped,
          linkIndex=self._applied_link_id,
          forceObj=self._applied_force,
          posObj=[0.0, 0.0, 0.0],
          flags=env.pybullet_client.LINK_FRAME)