Python math.cos() Examples

The following are code examples for showing how to use math.cos(). They are from open source Python projects. You can vote up the examples you like or vote down the ones you don't like.

Example 1
Project: building-boundary   Author: Geodan   File: bounding_box.py    MIT License 7 votes vote down vote up
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) 
Example 2
Project: L.E.S.M.A   Author: NatanaelAntonioli   File: L.E.S.M.A. - Fabrica de Noobs Speedtest.py    Apache License 2.0 7 votes vote down vote up
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 
Example 3
Project: unicorn-hat-hd   Author: pimoroni   File: demo.py    MIT License 6 votes vote down vote up
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 
Example 4
Project: unicorn-hat-hd   Author: pimoroni   File: demo.py    MIT License 6 votes vote down vote up
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 
Example 5
Project: AboveTustin   Author: kevinabrandon   File: geomath.py    MIT License 6 votes vote down vote up
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 
Example 6
Project: Kaggle-Statoil-Challenge   Author: adodd202   File: utils.py    MIT License 6 votes vote down vote up
def sgdr(period, batch_idx):
    # returns normalised anytime sgdr schedule given period and batch_idx
    # best performing settings reported in paper are T_0 = 10, T_mult=2
    # so always use T_mult=2
    batch_idx = float(batch_idx)
    restart_period = period
    while batch_idx / restart_period > 1.:
        batch_idx = batch_idx - restart_period
        restart_period = restart_period * 2.

    radians = math.pi * (batch_idx / restart_period)
    return 0.5 * (1.0 + math.cos(radians))


# def adjust_learning_rate(optimizer, epoch):
#     global lr
#     """Sets the learning rate to the initial LR decayed by 10 every 30 epochs"""
#     lr = lr * (0.01 ** (epoch // 10))
#     for param_group in optimizer.state_dict()['param_groups']:
#         param_group['lr'] = lr 
Example 7
Project: streetview_objectmapping   Author: vlkryl   File: objectmapping.py    MIT License 6 votes vote down vote up
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) 
Example 8
Project: mesh_heal   Author: tkeskita   File: op_clean_mesh.py    GNU General Public License v3.0 6 votes vote down vote up
def triangulate_twists(ob):
    """Triangulate Twisted Faces (Mesh Heal)"""

    import bmesh
    import math
    bpy.ops.object.mode_set(mode = 'EDIT')
    bm = bmesh.from_edit_mesh(ob.data)

    twistfaces = []

    # Maximum absolute cosine of angle between face normal vector and
    # center-to-corner vector
    angle = bpy.context.scene.mesh_heal.max_abs_twist_angle
    max_abs_cos_alpha = math.cos((90.0 - angle) / 90.0 * math.pi / 2.0)

    # Find all twisted faces
    for f in bm.faces:
        norvec = f.normal
        co = f.calc_center_median()
        for v in f.verts:
            vertvec = v.co - co
            vertvec.normalize()
            abs_cos_alpha = abs(vertvec @ norvec)
            l.debug("face %d abs_cos_alpha %f" % (f.index, abs_cos_alpha))
            if abs_cos_alpha > max_abs_cos_alpha:
                if f not in twistfaces:
                    twistfaces.append(f)
                    f.select = True

    # Triangulate twisted faces
    bmesh.ops.triangulate(bm, faces=twistfaces)

    bmesh.update_edit_mesh(ob.data, True)
    bm.free()
    return len(twistfaces) 
Example 9
Project: helloworld   Author: pip-uninstaller-python   File: 3D折线.py    GNU General Public License v2.0 6 votes vote down vote up
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 
Example 10
Project: soccer-matlab   Author: utra-robosoccer   File: minitaur_trotting_env.py    BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _gen_signal(self, t, phase):
    """Generates a sinusoidal reference leg trajectory.

    The foot (leg tip) will move in a ellipse specified by extension and swing
    amplitude.

    Args:
      t: Current time in simulation.
      phase: The phase offset for the periodic trajectory.

    Returns:
      The desired leg extension and swing angle at the current time.
    """
    period = 1 / self._step_frequency
    extension = self._extension_amplitude * math.cos(
        2 * math.pi / period * t + phase)
    swing = self._swing_amplitude * math.sin(2 * math.pi / period * t + phase)
    return extension, swing 
Example 11
Project: soccer-matlab   Author: utra-robosoccer   File: minitaur_alternating_legs_env.py    BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _signal(self, t):
    initial_pose = np.array([
        INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS,
        INIT_EXTENSION_POS, INIT_EXTENSION_POS, INIT_EXTENSION_POS,
        INIT_EXTENSION_POS
    ])
    amplitude = STEP_AMPLITUDE
    period = STEP_PERIOD
    extension = amplitude * (-1.0 + math.cos(2 * math.pi / period * t))
    ith_leg = int(t / period) % 2
    first_leg = np.array([0, 0, 0, 0, 0, extension, extension, 0])
    second_leg = np.array([0, 0, 0, 0, extension, 0, 0, extension])
    if ith_leg:
      signal = initial_pose + second_leg
    else:
      signal = initial_pose + first_leg
    return signal 
Example 12
Project: pepperon.ai   Author: JonWiggins   File: utils.py    MIT License 6 votes vote down vote up
def random_unit_vector(dimensions, seed=None):
    """
    Returns a random unit vector in the given number of dimensions
    Created using Gausian Random vars

    :param dimensions: desired dimensions
    :param seed: nullable, random var see

    :return: random unit vecotor
    """
    raw = []
    magnitude = 0
    if seed:
        random.seed(seed)
        
    for count in range(dimensions):
        uniform1 = random.uniform(0, 1)
        uniform2 = random.uniform(0, 1)
        toadd = math.sqrt(-2 * math.log(uniform1)) * math.cos(2 * math.pi * uniform2)
        magnitude += (toadd ** 2)
        raw.append(toadd)
    
    magnitude = math.sqrt(magnitude)
    return [element / magnitude for element in raw] 
Example 13
Project: robosuite   Author: StanfordVL   File: transform_utils.py    MIT License 6 votes vote down vote up
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,
    ) 
Example 14
Project: kitti-object-eval-python   Author: traveller59   File: rotate_iou.py    MIT License 6 votes vote down vote up
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 
Example 15
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 6 votes vote down vote up
def tangent_point(circle, circle_radius, point, angle_sign=1):
    """Circle tangent passing through point, angle sign + if clockwise else - """

    circle2d, point2d = a2(circle), a2(point)

    circle_distance = dist2d(circle2d, point2d) + 1e-9

    relative_angle = math.acos(Range(circle_radius / circle_distance, 1))

    point_angle = angle(point2d, circle2d)

    tangent_angle = (point_angle - relative_angle * sign(angle_sign))

    tangentx = math.cos(tangent_angle) * circle_radius + circle2d[0]
    tangenty = math.sin(tangent_angle) * circle_radius + circle2d[1]

    return a2([tangentx, tangenty]) 
Example 16
Project: unicorn-hat-hd   Author: pimoroni   File: demo.py    MIT License 5 votes vote down vote up
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 
Example 17
Project: unicorn-hat-hd   Author: pimoroni   File: demo.py    MIT License 5 votes vote down vote up
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 
Example 18
Project: unicorn-hat-hd   Author: pimoroni   File: demo.py    MIT License 5 votes vote down vote up
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) 
Example 19
Project: chainer-openai-transformer-lm   Author: soskek   File: opt.py    MIT License 5 votes vote down vote up
def warmup_cosine(x, warmup=0.002):
    s = 1 if x <= warmup else 0
    return s * (x / warmup) + (1 - s) * (0.5 * (1 + math.cos(math.pi * x))) 
Example 20
Project: tmx2map   Author: joshuaskelly   File: mathhelper.py    MIT License 5 votes vote down vote up
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)
        )) 
Example 21
Project: tmx2map   Author: joshuaskelly   File: mathhelper.py    MIT License 5 votes vote down vote up
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]) 
Example 22
Project: client   Author: Scorched-Moon   File: ani.py    GNU General Public License v3.0 5 votes vote down vote up
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 
Example 23
Project: pyblish-win   Author: pyblish   File: turtle.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
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) 
Example 24
Project: pyblish-win   Author: pyblish   File: test_math.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
def testCos(self):
        self.assertRaises(TypeError, math.cos)
        self.ftest('cos(-pi/2)', math.cos(-math.pi/2), 0)
        self.ftest('cos(0)', math.cos(0), 1)
        self.ftest('cos(pi/2)', math.cos(math.pi/2), 0)
        self.ftest('cos(pi)', math.cos(math.pi), -1)
        try:
            self.assertTrue(math.isnan(math.cos(INF)))
            self.assertTrue(math.isnan(math.cos(NINF)))
        except ValueError:
            self.assertRaises(ValueError, math.cos, INF)
            self.assertRaises(ValueError, math.cos, NINF)
        self.assertTrue(math.isnan(math.cos(NAN))) 
Example 25
Project: UR5_Controller   Author: tsinghua-rll   File: API.py    MIT License 5 votes vote down vote up
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) 
Example 26
Project: UR5_Controller   Author: tsinghua-rll   File: quaternion.py    MIT License 5 votes vote down vote up
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) 
Example 27
Project: UR5_Controller   Author: tsinghua-rll   File: quaternion.py    MIT License 5 votes vote down vote up
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]])) 
Example 28
Project: CozmoCommander   Author: cozmobotics   File: CozmoCommander.py    Apache License 2.0 5 votes vote down vote up
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 
Example 29
Project: mtrl-auto-uav   Author: brunapearson   File: utils.py    MIT License 5 votes vote down vote up
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 
Example 30
Project: kuaa   Author: rafaelwerneck   File: application.py    GNU General Public License v3.0 5 votes vote down vote up
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) 
Example 31
Project: CSL_Hamburg_Noise   Author: CityScope   File: CityScopeTable.py    GNU General Public License v3.0 5 votes vote down vote up
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) 
Example 32
Project: CSL_Hamburg_Noise   Author: CityScope   File: GridCell.py    GNU General Public License v3.0 5 votes vote down vote up
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) 
Example 33
Project: nek-type-a   Author: ecopoesis   File: coord_plane.py    Apache License 2.0 5 votes vote down vote up
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])) 
Example 34
Project: nek-type-a   Author: ecopoesis   File: body.py    Apache License 2.0 5 votes vote down vote up
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))) 
Example 35
Project: nek-type-a   Author: ecopoesis   File: body.py    Apache License 2.0 5 votes vote down vote up
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))) 
Example 36
Project: look_around   Author: iqbalhusen   File: haversine.py    MIT License 5 votes vote down vote up
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 
Example 37
Project: spqrel_tools   Author: LCAS   File: utils.py    MIT License 5 votes vote down vote up
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 
Example 38
Project: core   Author: lifemapper   File: colorpalette.py    GNU General Public License v3.0 5 votes vote down vote up
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]) 
Example 39
Project: Perspective   Author: TypesettingTools   File: perspective.py    MIT License 5 votes vote down vote up
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 
Example 40
Project: Perspective   Author: TypesettingTools   File: perspective.py    MIT License 5 votes vote down vote up
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 
Example 41
Project: Perspective   Author: TypesettingTools   File: perspective.py    MIT License 5 votes vote down vote up
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 
Example 42
Project: dynamic-training-with-apache-mxnet-on-aws   Author: awslabs   File: train_resnet.py    Apache License 2.0 5 votes vote down vote up
def __call__(self, num_update):
        if num_update <= self.max_update:
            self.base_lr = self.final_lr + (self.base_lr_orig - self.final_lr) * \
                (1 + cos(pi * (num_update - self.warmup_steps) / self.max_steps)) / 2
        return self.base_lr 
Example 43
Project: dynamic-training-with-apache-mxnet-on-aws   Author: awslabs   File: lr_scheduler.py    Apache License 2.0 5 votes vote down vote up
def __call__(self, num_update):
        if num_update < self.warmup_steps:
            return self.get_warmup_lr(num_update)
        if num_update <= self.max_update:
            self.base_lr = self.final_lr + (self.base_lr_orig - self.final_lr) * \
                (1 + cos(pi * (num_update - self.warmup_steps) / self.max_steps)) / 2
        return self.base_lr 
Example 44
Project: FCOS_GluonCV   Author: DetectionTeamUCAS   File: test_lr_scheduler.py    Apache License 2.0 5 votes vote down vote up
def test_composed_method():
    N = 1000
    constant = LRScheduler('constant', base_lr=0, target_lr=1, niters=N)
    linear = LRScheduler('linear', base_lr=1, target_lr=2, niters=N)
    cosine = LRScheduler('cosine', base_lr=3, target_lr=1, niters=N)
    poly = LRScheduler('poly', base_lr=1, target_lr=0, niters=N, power=2)
    # components with niters=0 will be ignored
    null_cosine = LRScheduler('cosine', base_lr=3, target_lr=1, niters=0)
    null_poly = LRScheduler('cosine', base_lr=3, target_lr=1, niters=0)
    step = LRScheduler('step', base_lr=1, target_lr=0, niters=N,
                       step_iter=[100, 500], step_factor=0.1)
    arr = LRSequential([constant, null_cosine, linear, cosine, null_poly, poly, step])
    # constant
    for i in range(N):
        compare(arr, i, 0)
    # linear
    for i in range(N, 2*N):
        expect_linear = 2 + (1 - 2) * (1 - (i - N) / (N - 1))
        compare(arr, i, expect_linear)
    # cosine
    for i in range(2*N, 3*N):
        expect_cosine = 1 + (3 - 1) * ((1 + cos(pi * (i - 2*N) / (N - 1))) / 2)
        compare(arr, i, expect_cosine)
    # poly
    for i in range(3*N, 4*N):
        expect_poly = 0 + (1 - 0) * (pow(1 - (i - 3*N) / (N - 1), 2))
        compare(arr, i, expect_poly)
    for i in range(4*N, 5*N):
        if i - 4*N < 100:
            expect_step = 1
        elif i - 4*N < 500:
            expect_step = 0.1
        else:
            expect_step = 0.01
        compare(arr, i, expect_step)
    # out-of-bound index
    compare(arr, 10*N, 0.01)
    compare(arr, -1, 0) 
Example 45
Project: FCOS_GluonCV   Author: DetectionTeamUCAS   File: lr_scheduler.py    Apache License 2.0 5 votes vote down vote up
def update(self, num_update):
        N = self.niters - 1
        T = num_update - self.offset
        T = min(max(0, T), N)

        if self.mode == 'constant':
            factor = 0
        elif self.mode == 'linear':
            factor = 1 - T / N
        elif self.mode == 'poly':
            factor = pow(1 - T / N, self.power)
        elif self.mode == 'cosine':
            factor = (1 + cos(pi * T / N)) / 2
        elif self.mode == 'step':
            if self.step is not None:
                count = sum([1 for s in self.step if s <= T])
                factor = pow(self.step_factor, count)
            else:
                factor = 1
        else:
            raise NotImplementedError

        if self.mode == 'step':
            self.learning_rate = self.base_lr * factor
        else:
            self.learning_rate = self.target_lr + (self.base_lr - self.target_lr) * factor 
Example 46
Project: soccer-matlab   Author: utra-robosoccer   File: minitaur_ball_gym_env.py    BSD 2-Clause "Simplified" License 5 votes vote down vote up
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() 
Example 47
Project: soccer-matlab   Author: utra-robosoccer   File: minitaur_push_randomizer.py    BSD 2-Clause "Simplified" License 5 votes vote down vote up
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) 
Example 48
Project: soccer-matlab   Author: utra-robosoccer   File: racecarGymEnv.py    BSD 2-Clause "Simplified" License 5 votes vote down vote up
def _reset(self):
    self._p.resetSimulation()
    #p.setPhysicsEngineParameter(numSolverIterations=300)
    self._p.setTimeStep(self._timeStep)
    #self._p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
    stadiumobjects = self._p.loadSDF(os.path.join(self._urdfRoot,"stadium.sdf"))
    #move the stadium objects slightly above 0
    #for i in stadiumobjects:
    #	pos,orn = self._p.getBasePositionAndOrientation(i)
    #	newpos = [pos[0],pos[1],pos[2]-0.1]
    #	self._p.resetBasePositionAndOrientation(i,newpos,orn)

    dist = 5 +2.*random.random()
    ang = 2.*3.1415925438*random.random()

    ballx = dist * math.sin(ang)
    bally = dist * math.cos(ang)
    ballz = 1

    self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2.urdf"),[ballx,bally,ballz])
    self._p.setGravity(0,0,-10)
    self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
    self._envStepCounter = 0
    for i in range(100):
      self._p.stepSimulation()
    self._observation = self.getExtendedObservation()
    return np.array(self._observation) 
Example 49
Project: o2g   Author: hiposfer   File: gtfs_dummy.py    MIT License 5 votes vote down vote up
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))
    r = 6371  # Radius of earth in kilometers. Use 3956 for miles
    return c * r 
Example 50
Project: deep-nn-car   Author: scope-lab-vu   File: PluggableFeatures.py    MIT License 5 votes vote down vote up
def updatePositionAndHeading(self,deltaX,deltaTheta):
        if (deltaX>0.0):
            self.totalDistanceCovered = deltaX + self.totalDistanceCovered
            self.heading = self.heading + deltaTheta
            self.heading = self.heading%360
            headingInRadians = math.radians(self.heading) # converts degrees to radians
            self.x = self.x + deltaX*math.cos(headingInRadians)
            self.y = self.y + deltaX*math.sin(headingInRadians) 
Example 51
Project: nxt-sketcher   Author: simondolle   File: printer.py    MIT License 5 votes vote down vote up
def get_xy(alpha, beta, structure_settings):

    r = structure_settings.r  # short arm length (attached to the rotative axis)
    a = structure_settings.a  # long arm length
    s = structure_settings.s  # pen distance

    xa = structure_settings.xa #left short arm x
    xb = structure_settings.xb #right short arm x


    # d is the first short arm extremity
    xd = xa - r * math.sin(alpha)
    yd = r * math.cos(alpha)

    # e is the first short arm extremity
    xe = xb - r * math.sin(beta)
    ye = r * math.cos(beta)

    de = compute_distance(xd, yd, xe, ye)

    #theta is the angle formed by de and the left long arm
    cos_theta = de/float(2 * a)
    cos_theta = min(cos_theta, 1.0)
    cos_theta = max(cos_theta, -1.0)
    theta = math.acos(cos_theta)

    #gamma is the angle formed by an horizontal axis and de
    tan_gamma = (ye-yd)/float(xe-xd)
    gamma = math.atan(tan_gamma)

    #lambda is the angle formed by an horizontal axis and the left long arm
    lam = theta + gamma
    xt = xd + a * math.cos(lam) - s * math.sin(lam)
    yt = yd + a * math.sin(lam) + s * math.cos(lam)

    return xt, yt 
Example 52
Project: nxt-sketcher   Author: simondolle   File: printer.py    MIT License 5 votes vote down vote up
def change_referential(x, y, angle):
    angle = angle * degrees_to_radians
    return (x * math.cos(angle)  + y * math.sin(angle), -x * math.sin(angle) + y * math.cos(angle)) 
Example 53
Project: Pytorch-Project-Template   Author: moemen95   File: train_utils.py    MIT License 5 votes vote down vote up
def adjust_learning_rate(optimizer, epoch, config, batch=None, nBatch=None, method='cosine'):
    if method == 'cosine':
        T_total = config.max_epoch * nBatch
        T_cur = (epoch % config.max_epoch) * nBatch + batch
        lr = 0.5 * config.learning_rate * (1 + math.cos(math.pi * T_cur / T_total))
    else:
        """Sets the learning rate to the initial LR decayed by 10 every 30 epochs"""
        lr = config.learning_rate * (0.1 ** (epoch // 30))
    for param_group in optimizer.param_groups:
        param_group['lr'] = lr
    return lr 
Example 54
Project: pirsclockfull   Author: jdgwarren   File: pirsclockfull.py    GNU General Public License v3.0 5 votes vote down vote up
def paraeqsmx(smx):
    return xclockpos-(int(secradius*(math.cos(math.radians((smx)+90))))) 
Example 55
Project: pirsclockfull   Author: jdgwarren   File: pirsclockfull.py    GNU General Public License v3.0 5 votes vote down vote up
def paraeqshx(shx):
    return xclockpos-(int(hradius*(math.cos(math.radians((shx)+90))))) 
Example 56
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __exp__(self):
        exp_x = math.exp(self.real)
        return Complex(exp_x * math.cos(self.imag), exp_x * math.sin(self.imag)) 
Example 57
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __sin__(self):
        return Complex(math.sin(self.real) * math.cosh(self.imag), math.cos(self.real) * math.sinh(self.imag)) 
Example 58
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __cos__(self):
        return Complex(math.cos(self.real) * math.cosh(self.imag), -math.sin(self.real) * math.sinh(self.imag)) 
Example 59
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __sinh__(self):
        return Complex(math.sinh(self.real) * math.cos(self.imag), math.cosh(self.real) * math.sin(self.imag)) 
Example 60
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __cosh__(self):
        return Complex(math.cosh(self.real) * math.cos(self.imag), math.sinh(self.real) * math.sin(self.imag)) 
Example 61
Project: polish-sentence-evaluation   Author: sdadas   File: feature.py    GNU General Public License v3.0 5 votes vote down vote up
def _get_encoding(self, pos: int):
        res = self.encodings.get(pos)
        size = self.parent.dim()
        if res is None:
            values = [float(pos) / math.pow(10000.0, 2.0*math.floor(0.5*float(i)) / 512.0) for i in range(size)]
            values = [math.sin(val) if i % 2 == 0 else math.cos(val) for i, val in enumerate(values)]
            res = np.array(values)
            res /= 10
            self.encodings[pos] = res
        return res 
Example 62
Project: MeteorTracker   Author: heidtn   File: match_events.py    MIT License 5 votes vote down vote up
def distanceBetweenCoords(lat1, lon1, lat2, lon2):
    """
    This uses the haversine formula to calculate the great-circle distance
    between two points.

    Parameters
    ----------
    lat1 : float
        The latitude of the first point
    lon1 : float
        The longitude of the first point
    lat2 : float
        The latitude of the second point
    lon2 : float
        The longitude of the second point
    """
    earthRadius = 6371.0  # earths radius in km
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    deltaPhi = math.radians(lat2 - lat1)
    deltaLambda = math.radians(lon2 - lon1)

    a = math.sin(deltaPhi/2.0)**2 + \
                 math.cos(phi1)*math.cos(phi2)*(math.sin(deltaLambda/2.0)**2)

    c = 2.0*math.atan2(math.sqrt(a), math.sqrt(1 - a))
    d = earthRadius*c

    return d 
Example 63
Project: RLBotPack   Author: RLBot   File: penguin_bot.py    MIT License 5 votes vote down vote up
def align_to(self, rot):
		v = Vec3(self.x, self.y, self.z)
		
		# Apply roll
		v.set(v.x, math.cos(rot.roll) * v.y + math.sin(rot.roll) * v.z, math.cos(rot.roll) * v.z - math.sin(rot.roll) * v.y)
		
		# Apply pitch
		v.set(math.cos(-rot.pitch) * v.x + math.sin(-rot.pitch) * v.z, v.y, math.cos(-rot.pitch) * v.z - math.sin(-rot.pitch) * v.x)
		
		# Apply yaw
		v.set(math.cos(-rot.yaw) * v.x + math.sin(-rot.yaw) * v.y, math.cos(-rot.yaw) * v.y - math.sin(-rot.yaw) * v.x, v.z)
		
		return v
	
	# Inverse of align_to 
Example 64
Project: RLBotPack   Author: RLBot   File: penguin_bot.py    MIT License 5 votes vote down vote up
def align_from(self, rot):
		v = Vec3(self.x, self.y, self.z)
		
		# Apply yaw
		v.set(math.cos(rot.yaw) * v.x + math.sin(rot.yaw) * v.y, math.cos(rot.yaw) * v.y - math.sin(rot.yaw) * v.x, v.z)
		
		# Apply pitch
		v.set(math.cos(rot.pitch) * v.x + math.sin(rot.pitch) * v.z, v.y, math.cos(rot.pitch) * v.z - math.sin(rot.pitch) * v.x)
		
		# Apply roll
		v.set(v.x, math.cos(-rot.roll) * v.y + math.sin(-rot.roll) * v.z, math.cos(-rot.roll) * v.z - math.sin(-rot.roll) * v.y)
		
		return v 
Example 65
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def cartesian(r, a, i):
    """Converts from spherical to cartesian coordinates."""
    x = r * math.sin(i) * math.cos(a)
    y = r * math.sin(i) * math.sin(a)
    z = r * math.cos(i)
    return np.array([x, y, z]) 
Example 66
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def rotate2D(x, y, ang):
    """Rotates a 2d vector clockwise by an angle in radians."""
    x2 = x * math.cos(ang) - y * math.sin(ang)
    y2 = y * math.cos(ang) + x * math.sin(ang)
    return x2, y2 
Example 67
Project: AboveTustin   Author: kevinabrandon   File: geomath.py    MIT License 4 votes vote down vote up
def bearing(pointA, pointB):
	"""
	Calculates the bearing between two points.

	Found here: https://gist.github.com/jeromer/2005586

	The formulae used is the following:
	    θ = atan2(sin(Δlong).cos(lat2),
	              cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))

	:Parameters:
	  - `pointA: The tuple representing the latitude/longitude for the
	    first point. Latitude and longitude must be in decimal degrees
	  - `pointB: The tuple representing the latitude/longitude for the
	    second point. Latitude and longitude must be in decimal degrees

	:Returns:
	  The bearing in degrees

	:Returns Type:
	  float
	"""
	if (type(pointA) != tuple) or (type(pointB) != tuple):
		raise TypeError("Only tuples are supported as arguments")

	lat1 = math.radians(pointA[0])
	lat2 = math.radians(pointB[0])

	diffLong = math.radians(pointB[1] - pointA[1])

	x = math.sin(diffLong) * math.cos(lat2)
	y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) 
		* math.cos(lat2) * math.cos(diffLong))

	initial_bearing = math.atan2(x, y)

	# Now we have the initial bearing but math.atan2 return values
	# from -180° to + 180° which is not what we want for a compass bearing
	# The solution is to normalize the initial bearing as shown below
	initial_bearing = math.degrees(initial_bearing)
	compass_bearing = (initial_bearing + 360) % 360

	return compass_bearing 
Example 68
Project: pyblish-win   Author: pyblish   File: turtle.py    GNU Lesser General Public License v3.0 4 votes vote down vote up
def _drawturtle(self):
        """Manages the correct rendering of the turtle with respect to
        its shape, resizemode, stretch and tilt etc."""
        screen = self.screen
        shape = screen._shapes[self.turtle.shapeIndex]
        ttype = shape._type
        titem = self.turtle._item
        if self._shown and screen._updatecounter == 0 and screen._tracing > 0:
            self._hidden_from_screen = False
            tshape = shape._data
            if ttype == "polygon":
                if self._resizemode == "noresize":
                    w = 1
                    shape = tshape
                else:
                    if self._resizemode == "auto":
                        lx = ly = max(1, self._pensize/5.0)
                        w = self._pensize
                        tiltangle = 0
                    elif self._resizemode == "user":
                        lx, ly = self._stretchfactor
                        w = self._outlinewidth
                        tiltangle = self._tilt
                    shape = [(lx*x, ly*y) for (x, y) in tshape]
                    t0, t1 = math.sin(tiltangle), math.cos(tiltangle)
                    shape = [(t1*x+t0*y, -t0*x+t1*y) for (x, y) in shape]
                shape = self._polytrafo(shape)
                fc, oc = self._fillcolor, self._pencolor
                screen._drawpoly(titem, shape, fill=fc, outline=oc,
                                                      width=w, top=True)
            elif ttype == "image":
                screen._drawimage(titem, self._position, tshape)
            elif ttype == "compound":
                lx, ly = self._stretchfactor
                w = self._outlinewidth
                for item, (poly, fc, oc) in zip(titem, tshape):
                    poly = [(lx*x, ly*y) for (x, y) in poly]
                    poly = self._polytrafo(poly)
                    screen._drawpoly(item, poly, fill=self._cc(fc),
                                     outline=self._cc(oc), width=w, top=True)
        else:
            if self._hidden_from_screen:
                return
            if ttype == "polygon":
                screen._drawpoly(titem, ((0, 0), (0, 0), (0, 0)), "", "")
            elif ttype == "image":
                screen._drawimage(titem, self._position,
                                          screen._shapes["blank"]._data)
            elif ttype == "compound":
                for item in titem:
                    screen._drawpoly(item, ((0, 0), (0, 0), (0, 0)), "", "")
            self._hidden_from_screen = True

##############################  stamp stuff  ############################### 
Example 69
Project: pyblish-win   Author: pyblish   File: turtle.py    GNU Lesser General Public License v3.0 4 votes vote down vote up
def stamp(self):
        """Stamp a copy of the turtleshape onto the canvas and return its id.

        No argument.

        Stamp a copy of the turtle shape onto the canvas at the current
        turtle position. Return a stamp_id for that stamp, which can be
        used to delete it by calling clearstamp(stamp_id).

        Example (for a Turtle instance named turtle):
        >>> turtle.color("blue")
        >>> turtle.stamp()
        13
        >>> turtle.fd(50)
        """
        screen = self.screen
        shape = screen._shapes[self.turtle.shapeIndex]
        ttype = shape._type
        tshape = shape._data
        if ttype == "polygon":
            stitem = screen._createpoly()
            if self._resizemode == "noresize":
                w = 1
                shape = tshape
            else:
                if self._resizemode == "auto":
                    lx = ly = max(1, self._pensize/5.0)
                    w = self._pensize
                    tiltangle = 0
                elif self._resizemode == "user":
                    lx, ly = self._stretchfactor
                    w = self._outlinewidth
                    tiltangle = self._tilt
                shape = [(lx*x, ly*y) for (x, y) in tshape]
                t0, t1 = math.sin(tiltangle), math.cos(tiltangle)
                shape = [(t1*x+t0*y, -t0*x+t1*y) for (x, y) in shape]
            shape = self._polytrafo(shape)
            fc, oc = self._fillcolor, self._pencolor
            screen._drawpoly(stitem, shape, fill=fc, outline=oc,
                                                  width=w, top=True)
        elif ttype == "image":
            stitem = screen._createimage("")
            screen._drawimage(stitem, self._position, tshape)
        elif ttype == "compound":
            stitem = []
            for element in tshape:
                item = screen._createpoly()
                stitem.append(item)
            stitem = tuple(stitem)
            lx, ly = self._stretchfactor
            w = self._outlinewidth
            for item, (poly, fc, oc) in zip(stitem, tshape):
                poly = [(lx*x, ly*y) for (x, y) in poly]
                poly = self._polytrafo(poly)
                screen._drawpoly(item, poly, fill=self._cc(fc),
                                 outline=self._cc(oc), width=w, top=True)
        self.stampItems.append(stitem)
        self.undobuffer.push(("stamp", stitem))
        return stitem 
Example 70
Project: pyblish-win   Author: pyblish   File: random.py    GNU Lesser General Public License v3.0 4 votes vote down vote up
def vonmisesvariate(self, mu, kappa):
        """Circular data distribution.

        mu is the mean angle, expressed in radians between 0 and 2*pi, and
        kappa is the concentration parameter, which must be greater than or
        equal to zero.  If kappa is equal to zero, this distribution reduces
        to a uniform random angle over the range 0 to 2*pi.

        """
        # mu:    mean angle (in radians between 0 and 2*pi)
        # kappa: concentration parameter kappa (>= 0)
        # if kappa = 0 generate uniform random angle

        # Based upon an algorithm published in: Fisher, N.I.,
        # "Statistical Analysis of Circular Data", Cambridge
        # University Press, 1993.

        # Thanks to Magnus Kessler for a correction to the
        # implementation of step 4.

        random = self.random
        if kappa <= 1e-6:
            return TWOPI * random()

        s = 0.5 / kappa
        r = s + _sqrt(1.0 + s * s)

        while 1:
            u1 = random()
            z = _cos(_pi * u1)

            d = z / (r + z)
            u2 = random()
            if u2 < 1.0 - d * d or u2 <= (1.0 - d) * _exp(d):
                break

        q = 1.0 / r
        f = (q + z) / (1.0 + q * z)
        u3 = random()
        if u3 > 0.5:
            theta = (mu + _acos(f)) % TWOPI
        else:
            theta = (mu - _acos(f)) % TWOPI

        return theta

## -------------------- gamma distribution -------------------- 
Example 71
Project: pyblish-win   Author: pyblish   File: random.py    GNU Lesser General Public License v3.0 4 votes vote down vote up
def gauss(self, mu, sigma):
        """Gaussian distribution.

        mu is the mean, and sigma is the standard deviation.  This is
        slightly faster than the normalvariate() function.

        Not thread-safe without a lock around calls.

        """

        # When x and y are two variables from [0, 1), uniformly
        # distributed, then
        #
        #    cos(2*pi*x)*sqrt(-2*log(1-y))
        #    sin(2*pi*x)*sqrt(-2*log(1-y))
        #
        # are two *independent* variables with normal distribution
        # (mu = 0, sigma = 1).
        # (Lambert Meertens)
        # (corrected version; bug discovered by Mike Miller, fixed by LM)

        # Multithreading note: When two threads call this function
        # simultaneously, it is possible that they will receive the
        # same return value.  The window is very small though.  To
        # avoid this, you have to use a lock around all calls.  (I
        # didn't want to slow this down in the serial case by using a
        # lock here.)

        random = self.random
        z = self.gauss_next
        self.gauss_next = None
        if z is None:
            x2pi = random() * TWOPI
            g2rad = _sqrt(-2.0 * _log(1.0 - random()))
            z = _cos(x2pi) * g2rad
            self.gauss_next = _sin(x2pi) * g2rad

        return mu + z*sigma

## -------------------- beta --------------------
## See
## http://mail.python.org/pipermail/python-bugs-list/2001-January/003752.html
## for Ivan Frohne's insightful analysis of why the original implementation:
##
##    def betavariate(self, alpha, beta):
##        # Discrete Event Simulation in C, pp 87-88.
##
##        y = self.expovariate(alpha)
##        z = self.expovariate(1.0/beta)
##        return z/(y+z)
##
## was dead wrong, and how it probably got that way. 
Example 72
Project: CozmoCommander   Author: cozmobotics   File: CozmoCommander.py    Apache License 2.0 4 votes vote down vote up
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False):
    # Final adjustement to properly face the charger.
    # The position can be adjusted several times if 
    # the precision is critical, i.e. when climbing
    # back onto the charger.  
    global RobotGlobal
    robot = RobotGlobal
    global PI

    while(True):
        # 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)
	    vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]]
	    # Angle of vector going from robot's origin to target's position
	    theta_t = math.atan2(vect[1],vect[0])

	    debug (2,'CHECK: Adjusting position')
	    # Face the target position
	    angle = clip_angle((theta_t-r_zRot))
	    robot.turn_in_place(radians(angle)).wait_for_completed()
	    # Drive toward the target position
	    robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed()
	    # Face the charger
	    angle = clip_angle((c_zRot-theta_t))
	    robot.turn_in_place(radians(angle)).wait_for_completed()

        # In case the robot does not need to climb onto the charger
	    if not critical:
	        break
	    elif(check_tol(charger,dist_charger)):
	    	debug (2,'CHECK: Robot aligned relativ to the charger.')
	    	break
    return 
Example 73
Project: spqrel_tools   Author: LCAS   File: turntosoundsource.py    MIT License 4 votes vote down vote up
def onSoundLocalized(self, motion, value):
        """
        :param motion: motion service
        :param value: output of sounddetected
                        [ [time(sec), time(usec)],
                        [azimuth(rad), elevation(rad), confidence, energy],
                        [Head Position[6D]] in FRAME_TORSO
                        [Head Position[6D]] in FRAME_ROBOT
                        ]
        :return:
        """

        confidence = value[1][2]
        # print confidence

        if confidence > 0.5:
            sound_azimuth = value[1][0]
            sound_elevation = value[1][1]
            x = math.sin(sound_elevation) * math.cos(sound_azimuth)
            y = math.sin(sound_elevation) * math.sin(sound_azimuth)
            z = math.cos(sound_elevation)
            head_pitch = value[2][4]
            head_yaw = value[2][5]
            azimuth = sound_azimuth + head_yaw
            elevation = sound_elevation + head_pitch
            turn = 0
            if azimuth > self.HEAD_YAW_MAX:
                turn = azimuth
                azimuth = 0.
            if azimuth < self.HEAD_YAW_MIN:
                turn = azimuth
                azimuth = 0.
            if elevation > self.HEAD_PITCH_MAX:
                elevation = self.HEAD_PITCH_MAX
            if elevation < self.HEAD_PITCH_MIN:
                elevation = self.HEAD_PITCH_MIN
            target_angles = [azimuth, 0]  # [azimuth, elevation]
            print "Current Head Yaw: ", head_yaw, "Current Head Pitch", head_pitch
            print "Sound Detected Azimuth: ", sound_azimuth, "Sound Detected Elevation: ", sound_elevation
            print "Sound Detected Coordinate: ", [x, y, z]
            print "Target Head Yaw: ", azimuth, "Target Head Pitch: ", elevation
            print "Turn: ", turn
            print "------------------------------------------------------------------"
            motion.angleInterpolationWithSpeed(self.NAMES, target_angles, self.MAX_SPEED_FRACTION)
            if math.fabs(turn) > 0.01:
                motion.moveTo(0, 0, turn)
            # print a 
Example 74
Project: FCOS_GluonCV   Author: DetectionTeamUCAS   File: test_lr_scheduler.py    Apache License 2.0 4 votes vote down vote up
def test_single_method():
    N = 1000
    constant = LRScheduler('constant', base_lr=0, target_lr=1, niters=N)
    linear = LRScheduler('linear', base_lr=1, target_lr=2, niters=N)
    cosine = LRScheduler('cosine', base_lr=3, target_lr=1, niters=N)
    poly = LRScheduler('poly', base_lr=1, target_lr=0, niters=N, power=2)
    step = LRScheduler('step', base_lr=1, target_lr=0, niters=N,
                       step_iter=[100, 500], step_factor=0.1)
    step2 = LRScheduler('step', base_lr=1, target_lr=0,
                        nepochs=2, iters_per_epoch=N/2,
                        step_iter=[100, 500], step_factor=0.1)
    step3 = LRScheduler('step', base_lr=1, target_lr=0,
                        nepochs=100, iters_per_epoch=N/100,
                        step_epoch=[10, 50], step_factor=0.1)

    # Test numerical value
    for i in range(N):
        compare(constant, i, 0)

        expect_linear = 2 + (1 - 2) * (1 - i / (N - 1))
        compare(linear, i, expect_linear)

        expect_cosine = 1 + (3 - 1) * ((1 + cos(pi * i / (N-1))) / 2)
        compare(cosine, i, expect_cosine)

        expect_poly = 0 + (1 - 0) * (pow(1 - i / (N-1), 2))
        compare(poly, i, expect_poly)

        if i < 100:
            expect_step = 1
        elif i < 500:
            expect_step = 0.1
        else:
            expect_step = 0.01
        compare(step, i, expect_step)
        compare(step2, i, expect_step)
        compare(step3, i, expect_step)

    # Test out-of-range updates
    for i in range(10):
        constant.update(i - 3)
        linear.update(i - 3)
        cosine.update(i - 3)
        poly.update(i - 3) 
Example 75
Project: FarWest1789   Author: ginoingras   File: FW1789_V001.py    GNU General Public License v3.0 4 votes vote down vote up
def moveElipse(self):
		"""move elipse to new position, use angle from -180 to 0 to 180
		return self.launched"""

		if (self.angle > math.pi) or (self.angle < 0):
			#some more delay to explode
			self.counter += 1
			if (self.counter >= 30):
				fenetre.blit(explode4, (self.rect[0]-30, 500))
			if (self.counter >= 20) and (self.counter < 30):
				fenetre.blit(explode3, (self.rect[0]-30, 500))
			if (self.counter >= 10) and (self.counter < 20):
				fenetre.blit(explode2, (self.rect[0]-30, 500))
			# delay to chech if shoot caravan
			if self.counter < 10:
				fenetre.blit(explode1, (self.rect[0]-30, 500))
			
				yeah = pygame.sprite.spritecollide(self, groupCaravans, dokill=True)
				for idx, cara in enumerate(yeah):
					yeah[idx].life = 0
					mire1.score += 100
					S_Bomb.play() # caravan shooted
					S_Bomb1.stop()
					self.counter = 10 # pass to big explosion animation

				if self.counter == 9:
					S_Bomb1.play() # caravan not shooted
					self.launched = False
			
			if self.counter > 40:
				self.launched = False

		else:
			if self.elipse_fwrw:
				self.angle += VG.ParGrenadeSpeed #0.03
			else:
				self.angle -= VG.ParGrenadeSpeed #0.03
				
			self.rot_center(self.imageOri, self.rectOri, math.degrees(float(self.angle*2)))

			self.movx = math.cos(self.angle)*self.elipse_rayon
			self.movy = math.sin(self.angle)*550
			
			if self.elipse_fwrw:
				self.rect.x = self.elipse_center - self.movx
				self.rect.y = 550 - self.movy
			else:
				self.rect.x = self.elipse_center - self.movx
				self.rect.y = 550 - self.movy

			fenetre.blit(self.image, self.rect)

		return self.launched 
Example 76
Project: sic   Author: Yanixos   File: random.py    GNU General Public License v3.0 4 votes vote down vote up
def vonmisesvariate(self, mu, kappa):
        """Circular data distribution.

        mu is the mean angle, expressed in radians between 0 and 2*pi, and
        kappa is the concentration parameter, which must be greater than or
        equal to zero.  If kappa is equal to zero, this distribution reduces
        to a uniform random angle over the range 0 to 2*pi.

        """
        # mu:    mean angle (in radians between 0 and 2*pi)
        # kappa: concentration parameter kappa (>= 0)
        # if kappa = 0 generate uniform random angle

        # Based upon an algorithm published in: Fisher, N.I.,
        # "Statistical Analysis of Circular Data", Cambridge
        # University Press, 1993.

        # Thanks to Magnus Kessler for a correction to the
        # implementation of step 4.

        random = self.random
        if kappa <= 1e-6:
            return TWOPI * random()

        s = 0.5 / kappa
        r = s + _sqrt(1.0 + s * s)

        while 1:
            u1 = random()
            z = _cos(_pi * u1)

            d = z / (r + z)
            u2 = random()
            if u2 < 1.0 - d * d or u2 <= (1.0 - d) * _exp(d):
                break

        q = 1.0 / r
        f = (q + z) / (1.0 + q * z)
        u3 = random()
        if u3 > 0.5:
            theta = (mu + _acos(f)) % TWOPI
        else:
            theta = (mu - _acos(f)) % TWOPI

        return theta

## -------------------- gamma distribution -------------------- 
Example 77
Project: sic   Author: Yanixos   File: random.py    GNU General Public License v3.0 4 votes vote down vote up
def gauss(self, mu, sigma):
        """Gaussian distribution.

        mu is the mean, and sigma is the standard deviation.  This is
        slightly faster than the normalvariate() function.

        Not thread-safe without a lock around calls.

        """

        # When x and y are two variables from [0, 1), uniformly
        # distributed, then
        #
        #    cos(2*pi*x)*sqrt(-2*log(1-y))
        #    sin(2*pi*x)*sqrt(-2*log(1-y))
        #
        # are two *independent* variables with normal distribution
        # (mu = 0, sigma = 1).
        # (Lambert Meertens)
        # (corrected version; bug discovered by Mike Miller, fixed by LM)

        # Multithreading note: When two threads call this function
        # simultaneously, it is possible that they will receive the
        # same return value.  The window is very small though.  To
        # avoid this, you have to use a lock around all calls.  (I
        # didn't want to slow this down in the serial case by using a
        # lock here.)

        random = self.random
        z = self.gauss_next
        self.gauss_next = None
        if z is None:
            x2pi = random() * TWOPI
            g2rad = _sqrt(-2.0 * _log(1.0 - random()))
            z = _cos(x2pi) * g2rad
            self.gauss_next = _sin(x2pi) * g2rad

        return mu + z*sigma

## -------------------- beta --------------------
## See
## http://mail.python.org/pipermail/python-bugs-list/2001-January/003752.html
## for Ivan Frohne's insightful analysis of why the original implementation:
##
##    def betavariate(self, alpha, beta):
##        # Discrete Event Simulation in C, pp 87-88.
##
##        y = self.expovariate(alpha)
##        z = self.expovariate(1.0/beta)
##        return z/(y+z)
##
## was dead wrong, and how it probably got that way. 
Example 78
Project: robosuite   Author: StanfordVL   File: transform_utils.py    MIT License 4 votes vote down vote up
def rotation_matrix(angle, direction, point=None):
    """
    Returns matrix to rotate about axis defined by point and direction.

    Examples:

        >>> angle = (random.random() - 0.5) * (2*math.pi)
        >>> direc = numpy.random.random(3) - 0.5
        >>> point = numpy.random.random(3) - 0.5
        >>> R0 = rotation_matrix(angle, direc, point)
        >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
        >>> is_same_transform(R0, R1)
        True
        >>> R0 = rotation_matrix(angle, direc, point)
        >>> R1 = rotation_matrix(-angle, -direc, point)
        >>> is_same_transform(R0, R1)
        True
        >>> I = numpy.identity(4, numpy.float32)
        >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
        True
        >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2,
        ...                                                direc, point)))
        True

    """
    sina = math.sin(angle)
    cosa = math.cos(angle)
    direction = unit_vector(direction[:3])
    # rotation matrix around unit vector
    R = np.array(
        ((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float32
    )
    R += np.outer(direction, direction) * (1.0 - cosa)
    direction *= sina
    R += np.array(
        (
            (0.0, -direction[2], direction[1]),
            (direction[2], 0.0, -direction[0]),
            (-direction[1], direction[0], 0.0),
        ),
        dtype=np.float32,
    )
    M = np.identity(4)
    M[:3, :3] = R
    if point is not None:
        # rotation not around origin
        point = np.array(point[:3], dtype=np.float32, copy=False)
        M[:3, 3] = point - np.dot(R, point)
    return M 
Example 79
Project: iceaddr   Author: sveinbjornt   File: add_placename_data.py    BSD 3-Clause "New" or "Revised" License 4 votes vote down vote up
def isnet93_to_wgs84(xx, yy):
    x = xx
    y = yy
    a = 6378137.0
    f = 1 / 298.257222101
    lat1 = 64.25
    lat2 = 65.75
    latc = 65.00
    lonc = 19.00
    eps = 0.00000000001

    def fx(p):
        return a * math.cos(p / rho) / math.sqrt(1 - math.pow(e * math.sin(p / rho), 2))

    def f1(p):
        return math.log((1 - p) / (1 + p))

    def f2(p):
        return f1(p) - e * f1(e * p)

    def f3(p):
        return pol1 * math.exp((f2(math.sin(p / rho)) - f2sin1) * sint / 2)

    rho = 45 / math.atan2(1.0, 1.0)
    e = math.sqrt(f * (2 - f))
    dum = f2(math.sin(lat1 / rho)) - f2(math.sin(lat2 / rho))
    sint = 2 * (math.log(fx(lat1)) - math.log(fx(lat2))) / dum
    f2sin1 = f2(math.sin(lat1 / rho))
    pol1 = fx(lat1) / sint
    polc = f3(latc) + 500000.0
    peq = (
        a
        * math.cos(latc / rho)
        / (sint * math.exp(sint * math.log((45 - latc / 2) / rho)))
    )
    pol = math.sqrt(math.pow(x - 500000, 2) + math.pow(polc - y, 2))
    lat = 90 - 2 * rho * math.atan(math.exp(math.log(pol / peq) / sint))
    lon = 0
    fact = rho * math.cos(lat / rho) / sint / pol
    fact = rho * math.cos(lat / rho) / sint / pol
    delta = 1.0
    while math.fabs(delta) > eps:
        delta = (f3(lat) - pol) * fact
        lat += delta
    lon = -(lonc + rho * math.atan((500000 - x) / (polc - y)) / sint)

    return {"lat": round(lat, 7), "lng": round(lon, 7)} 
Example 80
Project: mpu   Author: MartinThoma   File: __init__.py    MIT License 4 votes vote down vote up
def haversine_distance(origin, destination):
    """
    Calculate the Haversine distance.

    Parameters
    ----------
    origin : tuple of float
        (lat, long)
    destination : tuple of float
        (lat, long)

    Returns
    -------
    distance_in_km : float

    Examples
    --------
    >>> munich = (48.1372, 11.5756)
    >>> berlin = (52.5186, 13.4083)
    >>> round(haversine_distance(munich, berlin), 1)
    504.2

    >>> new_york_city = (40.712777777778, -74.005833333333)  # NYC
    >>> round(haversine_distance(berlin, new_york_city), 1)
    6385.3
    """
    lat1, lon1 = origin
    lat2, lon2 = destination
    if not (-90.0 <= lat1 <= 90):
        raise ValueError("lat1={:2.2f}, but must be in [-90,+90]".format(lat1))
    if not (-90.0 <= lat2 <= 90):
        raise ValueError("lat2={:2.2f}, but must be in [-90,+90]".format(lat2))
    if not (-180.0 <= lon1 <= 180):
        raise ValueError("lon1={:2.2f}, but must be in [-180,+180]"
                         .format(lat1))
    if not (-180.0 <= lon2 <= 180):
        raise ValueError("lon1={:2.2f}, but must be in [-180,+180]"
                         .format(lat1))
    radius = 6371  # km

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

    return d