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

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

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

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

def eci_to_latlon(pos, phi_0=0):
    (x, y, z) = pos
    rg = (x*x + y*y + z*z)**0.5
    z = z/rg
    if abs(z) > 1.0:
        z = int(z)

    lat = degrees(asin(z))
    lon = degrees(atan2(y, x)-phi_0)
    if lon > 180:
        lon -= 360
    elif lon < -180:
        lon += 360
    assert -90 <= lat <= 90
    assert -180 <= lon <= 180
    return lat, lon

def distance(self, loc):
        """
        Calculate the great circle distance between two points
        on the earth (specified in decimal degrees)
        """
        assert type(loc) == type(self)

        # convert decimal degrees to radians
        lon1, lat1, lon2, lat2 = map(radians, [
            self.lon,
            self.lat,
            loc.lon,
            loc.lat,
        ])

        # 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 = 6371000 # Radius of earth in meters.
        return c * r

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 winding_angle(self, path, point):
        wa = 0
        for i in range(len(path)-1):
            p = np.array([path[i].x, path[i].y])
            pn = np.array([path[i+1].x, path[i+1].y])

            vp = p - point
            vpn = pn - point

            vp_norm = sqrt(vp[0]**2 + vp[1]**2)
            vpn_norm = sqrt(vpn[0]**2 + vpn[1]**2)

            assert (vp_norm > 0)
            assert (vpn_norm > 0)

            z = np.cross(vp, vpn)/(vp_norm * vpn_norm)
            z = min(max(z, -1.0), 1.0)
            wa += asin(z)

        return wa

def compute_all_ic_rels_with_threshold(threshold):
    """
    Computes all intercity relations for documents classified with higher probability
    than the given threshold.

    :param threshold: The minimum probability
    :result: A list of dictionaries with city_a, city_b, category and score fields.
    """
    query = '''
        UNWIND [{0}] AS category
        MATCH (a:City)-[:{1}]->(i:Index)<-[:{1}]-(b:City)
        WHERE ID(a) < ID(b) AND a.name <> b.name AND i[category] >= {2}
        MATCH (a)-[r:{3}]->(b)
        WITH a.name AS city_a, a.population AS pop_a, b.name AS city_b,
            b.population AS pop_b, r.total AS total, COUNT(i) AS count, category,
            2 * 6371 * asin(sqrt(haversin(radians(a.latitude - b.latitude)) +
            cos(radians(a.latitude)) * cos(radians(b.latitude)) *
            haversin(radians(a.longitude - b.longitude)))) AS dist
        RETURN city_a, pop_a, city_b, pop_b, dist, total, category, SUM(count) AS score
    '''.format(','.join('"{}"'.format(c) for c in CAT_NO_OTHER), OCCURS_IN, threshold, RELATES_TO)

    return perform_query(query, [], access_mode='read')

def gps_newpos(lat, lon, bearing, distance):
    """Extrapolate latitude/longitude given a heading and distance
    thanks to http://www.movable-type.co.uk/scripts/latlong.html .
    """
    from math import sin, asin, cos, atan2, radians, degrees

    lat1 = radians(lat)
    lon1 = radians(lon)
    brng = radians(bearing)
    dr = distance / radius_of_earth

    lat2 = asin(sin(lat1) * cos(dr) +
                cos(lat1) * sin(dr) * cos(brng))
    lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
                        cos(dr) - sin(lat1) * sin(lat2))
    return (degrees(lat2), degrees(lon2))

def haversine(self, lat1, lon1, lat2, lon2):
        """
        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))

     # 6367 km is the radius of the Earth
        km = 6367 * c
        return km

#   might extend capability to restrict towns to network boundary

def haversine(self, lat1, lon1, lat2, lon2):
        """
        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))

   #     6367 km is the radius of the Earth
        km = 6367 * c
        return km

def haversine(self, lat1, lon1, lat2, lon2):
        """
        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))

   #     6367 km is the radius of the Earth
        km = 6367 * c
        return km

def spherical_distance(a, b):
    lat1 = a[1]
    lon1 = a[0]
    lat2 = b[1]
    lon2 = b[0]
    R = 6371000.0
    rlon1 = lon1 * math.pi / 180.0
    rlon2 = lon2 * math.pi / 180.0
    rlat1 = lat1 * math.pi / 180.0
    rlat2 = lat2 * math.pi / 180.0
    dlon = (rlon1 - rlon2) / 2.0
    dlat = (rlat1 - rlat2) / 2.0
    lat12 = (rlat1 + rlat2) / 2.0
    sindlat = math.sin(dlat)
    sindlon = math.sin(dlon)
    cosdlon = math.cos(dlon)
    coslat12 = math.cos(lat12)
    f = sindlat * sindlat * cosdlon * cosdlon + sindlon * sindlon * coslat12 * coslat12
    f = math.sqrt(f)
    f = math.asin(f) * 2.0 # the angle between the points
    f *= R
    return f

def coords_down_bearing(lat, lon, bearing, distance, body):
        '''
        Takes a latitude, longitude and bearing in degrees, and a
        distance in meters over a given body.  Returns a tuple
        (latitude, longitude) of the point you've calculated.
        '''
        bearing = math.radians(bearing)
        R = body.equatorial_radius
        lat = math.radians(lat)
        lon = math.radians(lon)

        lat2 = math.asin( math.sin(lat)*math.cos(distance/R) +
                     math.cos(lat)*math.sin(distance/R)*math.cos(bearing))

        lon2 = lon + math.atan2(math.sin(bearing)*math.sin(distance/R
                        )*math.cos(lat),math.cos(distance/R)-math.sin(lat
                        )*math.sin(lat2))

        lat2 = math.degrees(lat2)
        lon2 = math.degrees(lon2)
        return (lat2, lon2)

def rpy(self):
        acc = self.acceleration()
        yaw = self.yaw()
        norm = np.linalg.norm(acc)
        # print(acc)
        if norm < 1e-6:
            return (0.0, 0.0, yaw)
        else:
            thrust = acc + np.array([0, 0, 9.81])
            z_body = thrust / np.linalg.norm(thrust)
            x_world = np.array([math.cos(yaw), math.sin(yaw), 0])
            y_body = np.cross(z_body, x_world)
            x_body = np.cross(y_body, z_body)
            pitch = math.asin(-x_body[2])
            roll = math.atan2(y_body[2], z_body[2])
            return (roll, pitch, yaw)

    # "private" methods

def haversine(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)

    Taken from: http://stackoverflow.com/questions/4913349/haversine-formula-in-python-bearing-and-distance-between-two-gps-points
    """
    # convert decimal degrees to radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2

    c = 2 * asin(sqrt(a))
    km = 6367 * c
    return km

def haversine(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)

    Taken from: http://stackoverflow.com/questions/4913349/haversine-formula-in-python-bearing-and-distance-between-two-gps-points
    """
    # convert decimal degrees to radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2

    c = 2 * asin(sqrt(a))
    km = 6367 * c
    return km

def haversine(lat1, lon1, lat2, lon2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)
    """
    # convert decimal degrees to radians
    lat1, lon1, lat2, lon2 = map(radians, [lat1, lon1, lat2, lon2])

    # 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 = 3956 # Radius of earth in miles

    return c * r

def point_from_distance_and_angle(point: CoordinatePoint, distance_in_m: float,
                                  angle_in_degrees: int) -> CoordinatePoint:
    """Calculates a point with the given angle and distance meters away from the given point"""

    lat1 = math.radians(point.latitude)
    lon1 = math.radians(point.longitude)

    distance = distance_in_m / 1000 / 6371
    angle = math.radians(angle_in_degrees)

    lat2 = math.asin(math.sin(lat1) * math.cos(distance) +
                     math.cos(lat1) * math.sin(distance) * math.cos(angle))
    lon2 = lon1 + math.asin((math.sin(distance) /
                             math.cos(lat2)) * math.sin(angle))

    return CoordinatePoint(latitude=math.degrees(lat2), longitude=math.degrees(lon2))

def get_euler(self):
        t = self.x * self.y + self.z * self.w
        if t > 0.4999:
            heading = 2 * math.atan2(self.x, self.w)
            attitude = math.pi / 2
            bank = 0
        elif t < -0.4999:
            heading = -2 * math.atan2(self.x, self.w)
            attitude = -math.pi / 2
            bank = 0
        else:
            sqx = self.x ** 2
            sqy = self.y ** 2
            sqz = self.z ** 2
            heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z,
                                 1 - 2 * sqy - 2 * sqz)
            attitude = math.asin(2 * t)
            bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z,
                              1 - 2 * sqx - 2 * sqz)
        return heading, attitude, bank

def get_cycle(self, lat, lng, rad):
        # unit of radius: meter
        cycle = []
        d = (rad / 1000.0) / 6378.8
        lat1 = (math.pi / 180.0) * lat
        lng1 = (math.pi / 180.0) * lng

        r = [x * 10 for x in range(36)]
        for a in r:
            tc = (math.pi / 180.0) * a
            y = math.asin(
                math.sin(lat1) * math.cos(d) + math.cos(lat1) * math.sin(d) * math.cos(tc))
            dlng = math.atan2(math.sin(
                tc) * math.sin(d) * math.cos(lat1), math.cos(d) - math.sin(lat1) * math.sin(y))
            x = ((lng1 - dlng + math.pi) % (2.0 * math.pi)) - math.pi
            cycle.append(
                (float(y * (180.0 / math.pi)), float(x * (180.0 / math.pi))))
        return cycle

def quat2euler(q):
    x, y, z, w = q.ravel()

    ysqr = y * y

    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    X = atan2(t0, t1)

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y = asin(t2)

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    Z = atan2(t3, t4)

    return np.array([[X], [Y], [Z]])

def getTiltHeading(self):
        magValue=self.getMag()
        accelValue=self.getRealAccel()

        X=self.X
        Y=self.Y
        Z=self.Z

        pitch = math.asin(-accelValue[X])

        print(accelValue[Y],pitch,math.cos(pitch),accelValue[Y]/math.cos(pitch),math.asin(accelValue[Y]/math.cos(pitch)))
        roll = math.asin(accelValue[Y]/math.cos(pitch))

        xh = magValue[X] * math.cos(pitch) + magValue[Z] * math.sin(pitch)
        yh = magValue[X] * math.sin(roll) * math.sin(pitch) + magValue[Y] * math.cos(roll) - magValue[Z] * math.sin(roll) * math.cos(pitch)
        zh = -magValue[X] * (roll) * math.sin(pitch) + magValue[Y] * math.sin(roll) + magValue[Z] * math.cos(roll) * math.cos(pitch)
        heading = 180 * math.atan2(yh, xh)/math.pi

        if (yh >= 0):
            return heading
        else:
            return (360 + heading)

def __ComputeCurved(vpercent, w, vec, via, pts, segs):
    """Compute the curves part points"""

    radius = via[1]/2.0

    # Compute the bezier middle points
    req_angle = asin(vpercent/100.0)
    oppside = tan(req_angle)*(radius-(w/sin(req_angle)))
    length = sqrt(radius*radius + oppside*oppside)
    d = req_angle - acos(radius/length)
    vecBC = [vec[0]*cos(d)+vec[1]*sin(d), -vec[0]*sin(d)+vec[1]*cos(d)]
    pointBC = via[0] + wxPoint(int(vecBC[0] * length), int(vecBC[1] * length))
    d = -d
    vecAE = [vec[0]*cos(d)+vec[1]*sin(d), -vec[0]*sin(d)+vec[1]*cos(d)]
    pointAE = via[0] + wxPoint(int(vecAE[0] * length), int(vecAE[1] * length))

    curve1 = __Bezier(pts[1], pointBC, pts[2], n=segs)
    curve2 = __Bezier(pts[4], pointAE, pts[0], n=segs)

    return curve1 + [pts[3]] + curve2

def haversine(pt_a, pt_b):
    """
    `pt_a` and `pt_b` are tuples with two float numbers each, i.e.
    representing lng/lat pairs:
    (99,100)  (-42, 85)

    The geo distance between the two points is returned in kilometers
    """
    from math import radians, cos, sin, asin, sqrt

    lng_a = radians(float(pt_a[0]))
    lat_a = radians(float(pt_a[1]))

    lng_b = radians(float(pt_b[0]))
    lat_b = radians(float(pt_b[1]))
    # haversine formula
    dlng = lng_b - lng_a
    dlat = lat_b - lat_a
    whatevs = sin(dlat /2 ) ** 2 + cos(lat_a) * cos(lat_b) * sin(dlng / 2) ** 2
    c = 2 * asin(sqrt(whatevs))
    r = 6371 # Radius of earth in kilometers.
    # return the final calculation
    return c * r

def cvt_inert2att_Q_to_angles(Q):
   """Creates a angle tuple from Q, assuming an inertial to attitude Q"""
   V1_body = Vector(1.,0.,0.)
   V1_eci_pt = Q.inv_cnvrt(V1_body)
   coord1  = math.atan2(V1_eci_pt.y,V1_eci_pt.x)
   if coord1 < 0. : coord1 += PI2
   coord2 = math.asin(unit_limit(V1_eci_pt.z))

   V3_body = Vector(0.,0.,1.)
   V3_eci_pt = Q.inv_cnvrt(V3_body)
   NP_eci = Vector(0.,0.,1.)
   V_left = cross(NP_eci,V1_eci_pt)
   V_left = V_left / V_left.length()
   NP_in_plane = cross(V1_eci_pt,V_left)
   x = dot(V3_eci_pt,NP_in_plane)
   y = dot(V3_eci_pt,V_left)
   pa  = math.atan2(y,x)
   if pa < 0. : pa += PI2
   return (coord1,coord2,pa)

def cvt_v2v3_using_body2inertial_Q_to_c1c2pa_tuple(Q,v2,v3):
   """Given Q and v2,v3 gives pos on sky and V3 PA """
   Vp_body = Vector(0.,0.,0.)
   Vp_body.set_xyz_from_angs(v2,v3)
   Vp_eci_pt = Q.cnvrt(Vp_body)
   coord1  = atan2(Vp_eci_pt.y,Vp_eci_pt.x)
   if coord1 < 0. : coord1 += PI2
   coord2 = asin(unit_limit(Vp_eci_pt.z))

   V3_body = Vector(0.,0.,1.)
   V3_eci_pt = Q.cnvrt(V3_body)
   NP_eci = Vector(0.,0.,1.)
   V_left = cross(NP_eci,Vp_eci_pt)
   if V_left.length()>0.:
      V_left = V_left/V_left.length()
   NP_in_plane = cross(Vp_eci_pt,V_left)
   x = dot(V3_eci_pt,NP_in_plane)
   y = dot(V3_eci_pt,V_left)
   pa  = atan2(y,x)
   if pa < 0. : pa += PI2

   return (coord1,coord2,pa)

def cvt_c1c2_using_body2inertial_Q_to_v2v3pa_tuple(Q,coord1,coord2):
   """Given Q and a position, returns v2,v3,V3PA tuple """
   Vp_eci = Vector(1.,0.,0.)
   Vp_eci.set_xyz_from_angs(coord1,coord2)
   Vp_body_pt = Q.inv_cnvrt(Vp_eci)
   v2 = atan2(Vp_body_pt.y,Vp_body_pt.x)
   v3 = asin(unit_limit(Vp_body_pt.z))
   V3_body = Vector(0.,0.,1.)
   V3_eci_pt = self.cnvrt(V3_body)
   NP_eci = Vector(0.,0.,1.)
   V_left = cross(NP_eci,Vp_eci)
   if V_left.length()>0.:
    V_left = V_left / V_left.length()
   NP_in_plane = cross(Vp_eci,V_left)
   x = dot(V3_eci_pt,NP_in_plane)
   y = dot(V3_eci_pt,V_left)
   pa  = atan2(y,x)
   if pa < 0. : pa += PI2
   return (v2,v3,pa)


############################################################

def add_kapla_tower(scn, width, height, length, radius, level_count: int, x=0, y=0, z=0):
    """Create a Kapla tower, return a list of created nodes"""
    tower = []
    level_y = y + height / 2

    for i in range(level_count // 2):
        def fill_ring(r, ring_y, size, r_adjust, y_off):
            step = asin((size * 1.01) / 2 / (r - r_adjust)) * 2
            cube_count = (2 * pi) // step
            error = 2 * pi - step * cube_count
            step += error / cube_count  # distribute error

            a = 0
            while a < (2 * pi - error):
                world = gs.Matrix4.TransformationMatrix(gs.Vector3(cos(a) * r + x, ring_y, sin(a) * r + z), gs.Vector3(0, -a + y_off, 0))
                tower.append(plus.AddPhysicCube(scn, world, width, height, length, 2)[0])
                a += step

        fill_ring(radius - length / 2, level_y, width, length / 2, pi / 2)
        level_y += height
        fill_ring(radius - length + width / 2, level_y, length, width / 2, 0)
        fill_ring(radius - width / 2, level_y, length, width / 2, 0)
        level_y += height

    return tower

def add_kapla_tower(scn, width, height, length, radius, level_count: int, x=0, y=0, z=0):
    """Create a Kapla tower, return a list of created nodes"""
    level_y = y + height / 2

    for i in range(level_count // 2):
        def fill_ring(r, ring_y, size, r_adjust, y_off):
            step = asin((size * 1.01) / 2 / (r - r_adjust)) * 2
            cube_count = (2 * pi) // step
            error = 2 * pi - step * cube_count
            step += error / cube_count  # distribute error

            a = 0
            while a < (2 * pi - error):
                world = gs.Matrix4.TransformationMatrix((cos(a) * r + x, ring_y, sin(a) * r + z), (0, -a + y_off, 0))
                plus.AddPhysicCube(scn, world, width, height, length, 2)
                a += step

        fill_ring(radius - length / 2, level_y, width, length / 2, pi / 2)
        level_y += height
        fill_ring(radius - length + width / 2, level_y, length, width / 2, 0)
        fill_ring(radius - width / 2, level_y, length, width / 2, 0)
        level_y += height

def intersectChordCircle(C, P, chord_length):
    ''' 
    Accepts center of circle, a point on the circle, and chord length.  
    Returns a list of two points on the circle at chord_length distance away from original point
    '''
    C = dPnt(C)
    P = dPnt(P)
    d = chord_length
    r = distance(C, P)
    # point on circle given chordlength & starting point=2*asin(d/2r)
    d_div_2r = d / (2.0 * r)
    angle = 2 * asin(d_div_2r)
    P = []
    P.append(polar(C, r, angle))
    P.append(polar(C, r, - angle))
    return P

#def tangentOnCircleFromPoint(C, r, P):

def getcycle(self,rpoint):
        cycle = []
        lat = rpoint[0]
        lng = rpoint[1]
        rad = rpoint[2] #unit: meter
        d = (rad/1000.0)/6378.8;
        lat1 = (math.pi/180.0)* lat
        lng1 = (math.pi/180.0)* lng

        r = [x*30 for x in range(12)]
        for a in r:
            tc = (math.pi/180.0)*a;
            y = math.asin(math.sin(lat1)*math.cos(d)+math.cos(lat1)*math.sin(d)*math.cos(tc))
            dlng = math.atan2(math.sin(tc)*math.sin(d)*math.cos(lat1),math.cos(d)-math.sin(lat1)*math.sin(y))
            x = ((lng1-dlng+math.pi) % (2.0*math.pi)) - math.pi 
            cycle.append( ( float(y*(180.0/math.pi)),float(x*(180.0/math.pi)) ) )
        return cycle

def lines_len_in_circle(r, font_size=12, letter_width=7.2):
    """Return the amount of chars that fits each line in a circle according to
    its radius *r*

    Doctest:

    .. doctest::

        >>> lines_len_in_circle(20)
        [2, 5, 2]
    """
    lines = 2 * r // font_size
    positions = [
        x + (font_size // 2) * (-1 if x <= 0 else 1)
        for x in text_y(lines)
    ]
    return [
        int(2 * r * cos(asin(y / r)) / letter_width)
        for y in positions
    ]

def get_new_coords(init_loc, distance, bearing):
    """ Given an initial lat/lng, a distance(in kms), and a bearing (degrees),
    this will calculate the resulting lat/lng coordinates.
    """
    R = 6378.1 #km radius of the earth
    bearing = math.radians(bearing)

    init_coords = [math.radians(init_loc[0]), math.radians(init_loc[1])] # convert lat/lng to radians

    new_lat = math.asin( math.sin(init_coords[0])*math.cos(distance/R) +
        math.cos(init_coords[0])*math.sin(distance/R)*math.cos(bearing))

    new_lon = init_coords[1] + math.atan2(math.sin(bearing)*math.sin(distance/R)*math.cos(init_coords[0]),
        math.cos(distance/R)-math.sin(init_coords[0])*math.sin(new_lat))

    return [math.degrees(new_lat), math.degrees(new_lon)]

def calc_pitch():

    """ Calculates pitch value"""

    acc_x = read_acc_x()
    acc_y = read_acc_y()
    acc_z = read_acc_z()

    acc_x_norm = acc_x / math.sqrt(acc_x * acc_x + acc_y * acc_y + acc_z * acc_z)

    try:
        pitch = math.asin(acc_x_norm)
        return pitch

    except Exception as e:
        logging.debug(e)
    return 0

def calc_roll():

    """ Calculates roll value """

    acc_x = read_acc_x()
    acc_y = read_acc_y()
    acc_z = read_acc_z()

    acc_y_norm = acc_y / math.sqrt(acc_x * acc_x + acc_y * acc_y + acc_z * acc_z)

    try:
        roll = -math.asin(acc_y_norm / math.cos(calc_pitch()))
        return roll

    except Exception as e:
        logging.debug(e)
        return 0

def haversine(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)

    Taken from: http://stackoverflow.com/questions/4913349/haversine-formula-in-python-bearing-and-distance-between-two-gps-points
    """
    # convert decimal degrees to radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2

    c = 2 * asin(sqrt(a))
    km = 6367 * c
    return km

def draw_circle(radius_in_meters, center_point, steps=15):
    """
    get a circle shape polygon based on centerPoint and radius

    Keyword arguments:
    point1  -- point one geojson object
    point2  -- point two geojson object

    if(point inside multipoly) return true else false
    """
    steps = steps if steps > 15 else 15
    center = [center_point['coordinates'][1], center_point['coordinates'][0]]
    dist = (radius_in_meters / 1000) / 6371
    # convert meters to radiant
    rad_center = [number2radius(center[0]), number2radius(center[1])]
    # 15 sided circle
    poly = []
    for step in range(0, steps):
        brng = 2 * math.pi * step / steps
        lat = math.asin(math.sin(rad_center[0]) * math.cos(dist) +
                        math.cos(rad_center[0]) * math.sin(dist) * math.cos(brng))
        lng = rad_center[1] + math.atan2(math.sin(brng) * math.sin(dist)
                                         * math.cos(rad_center[0]), math.cos(dist) - math.sin(rad_center[0]) * math.sin(lat))
        poly.append([number2degree(lng), number2degree(lat)])
    return {"type": "Polygon", "coordinates": [poly]}

def destination_point(point, brng, dist):
    """
    Calculate a destination Point base on a base point and a distance

    Keyword arguments:
    pt   -- polygon geojson object
    brng -- an angle in degrees
    dist -- distance in Kilometer between destination and base point

    return destination point object

    """
    dist = float(dist) / 6371  # convert dist to angular distance in radians
    brng = number2radius(brng)

    lon1 = number2radius(point['coordinates'][0])
    lat1 = number2radius(point['coordinates'][1])

    lat2 = math.asin(math.sin(lat1) * math.cos(dist) +
                     math.cos(lat1) * math.sin(dist) * math.cos(brng))
    lon2 = lon1 + math.atan2(math.sin(brng) * math.sin(dist) *
                             math.cos(lat1), math.cos(dist) - math.sin(lat1) * math.sin(lat2))
    lon2 = (lon2 + 3 * math.pi) % (2 * math.pi) - math.pi  # normalise to -180 degree +180 degree

    return {'type': 'Point', 'coordinates': [number2degree(lon2), number2degree(lat2)]}

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(math.radians, [lon1, lat1, lon2, lat2])

    # 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))
    # 6367 km is the radius of the Earth
    km = 6367 * c
    latency = km / 200000
    return latency

def eubik(pos, armid="rgt"):
    """
    compute the euristic waist rotation
    ew = euristic waist

    :param pos: object position
    :return: waistangle in degree

    author: weiwei
    date: 20170410
    """

    anglecomponent1 = 0
    try:
        anglecomponent1 = math.asin(80/np.linalg.norm(pos[0:2]))
    except:
        pass
    waistangle = (math.atan2(pos[1], pos[0]) + anglecomponent1)*180/math.pi
    if armid=="lft":
        waistangle = 180.0-2*math.atan2(pos[0], pos[1])*180.0/math.pi-waistangle
    return waistangle

def eubik(pos, armid="rgt"):
    """
    compute the euristic waist rotation
    ew = euristic waist

    :param pos: object position
    :return: waistangle in degree

    author: weiwei
    date: 20170410
    """

    anglecomponent1 = 0
    try:
        anglecomponent1 = math.asin(80/np.linalg.norm(pos[0:2]))
    except:
        pass
    waistangle = (math.atan2(pos[1], pos[0]) + anglecomponent1)*180/math.pi
    if armid=="lft":
        waistangle = 180.0-2*math.atan2(pos[0], pos[1])*180.0/math.pi-waistangle
    return waistangle

def compute_hdistance(lon1,lat1,lon2,lat2):
    lon1 = float(lon1)
    lat1 = float(lat1)
    lon2 = float(lon2)
    lat2 = float(lat2)

    radLat1 = rad(lat1)
    radLat2 = rad(lat2)
    a = radLat1 - radLat2
    b = rad(lon1) - rad(lon2)
    s = 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(a/2),2) + 
        Math.cos(radLat1) * Math.cos(radLat2) * Math.pow(Math.sin(b/2),2)))
    s = s * 6378.137
    s = round(s * 10000) / 10
    return s


### return odistance ############################

def angular_position(texcoord):
    up = texcoord[0]
    v = texcoord[1]
    # correct for hemisphere
    if up>=0.5:
      u = 2.0*(up-0.5)
    else:
      u = 2.0*up

    # ignore points outside of circles
    if ((u-0.5)*(u-0.5) + (v-0.5)*(v-0.5))>0.25:
      return None, None

    # v: 0..1-> vp: -1..1
    phi = math.asin(2.0*(v-0.5))

    # u = math.cos(phi)*math.cos(theta)
    # u: 0..1 -> upp: -1..1
    u = 1.0-u
    theta = math.acos( 2.0*(u-0.5) / math.cos(phi) )

    if up<0.5:
       theta = theta-math.pi

    return (theta,phi)

def quat2euler(q):
    """
    Returns the euler representation (in degrees) of a quaternion. Note, the
    heading is wrapped between 0-360 degrees.

    In:
        [q0 q1 q2 q3] = [w x y z]
    out:
        (roll, pitch, yaw) in degrees
    """
    q0, q1, q2, q3 = q
    roll = atan2(2.0*q2*q3-2.0*q0*q1, 2.0*q0*q0+2.0*q3*q3-1.0)
    pitch = -asin(2.0*q1*q3+2.0*q0*q2)
    heading = atan2(2.0*q1*q2-2.0*q0*q3, 2.0*q0*q0+2.0*q1*q1-1.0)

    heading = heading if heading <= 2*pi else heading-2*pi
    heading = heading if heading >= 0 else heading+2*pi

    return map(rad2deg, (roll, pitch, heading))

def rotation(self):
        '''Extracts Euler angles of rotation from this matrix.
        This attempts to find alternate rotations in case of gimbal lock,
        but all of the usual problems with Euler angles apply here.
        All Euler angles are in radians.
        '''
        (a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p) = self._v
        rotY = D = math.asin(c)
        C = math.cos(rotY)
        if (abs(C) > 0.005):
            trX = k/C ; trY = -g/C ; rotX = math.atan2(trY, trX)
            trX = a/C ; trY = -b/C ; rotZ = math.atan2(trY, trX)
        else:
            rotX = 0
            trX = f ; trY = e ; rotZ = math.atan2(trY, trX)
        return V(rotX,rotY,rotZ)

def __sub__(self, other):
    """Subtraction.

    Args:
      other: the LatLng which this LatLng is subtracted by.

    Returns:
      the great circle distance between two LatLng objects as computed
      by the Haversine formula.
    """

    assert isinstance(other, LatLng)

    lat_rad = math.radians(self._lat)
    lng_rad = math.radians(self._lng)
    other_lat_rad = math.radians(other.latitude)
    other_lng_rad = math.radians(other.longitude)

    dlat = lat_rad - other_lat_rad
    dlng = lng_rad - other_lng_rad
    a1 = math.sin(dlat / 2)**2
    a2 = math.cos(lat_rad) * math.cos(other_lat_rad) * math.sin(dlng / 2)**2
    return 2 * self._EARTH_RADIUS_METERS * math.asin(math.sqrt(a1 + a2))

def pitch(self):
        return math.asin(2*(self.w*self.y() - self.z()*self.x()))

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

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

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

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

def ondraw(self):

        # Calculate electric force
        sk = self.sketch
        dr = delta(self.pos, sk["blue"].pos)
        r = hypot(*dr) / sk.scale / 100
        F = delta(dr, mag = 8.99e-3 * sk.q1 * sk.q2 / (r * r))

        # Add electric plus gravitational forces 
        F = F[0], F[1] + sk.mass * 9.81e-3

        # Tangential acceleration
        s = sk["string"]
        u = s.u
        t = 1 / sk.frameRate
        F = (F[0] * u[1] - F[1] * u[0]) / (sk.mass / 1000) * (sk.scale / 100) / t ** 2
        ax, ay = F * u[1], -F * u[0]

        # Kinematics
        v1x, v1y = tuple(0.95 * v for v in self.vel)
        v2x = v1x + ax * t
        v2y = v1y + ay * t
        self.vel = v2x, v2y
        x, y = self.pos
        x += (v1x + v2x) * t / 2
        y += (v1y + v2y) * t / 2
        x, y = delta((x,y), s.pos, 20 * sk.scale)
        self.pos = s.pos[0] + x, s.pos[1] + y
        s.__init__(s.pos, self.pos)

        # Protractor
        if s.u[1] > 0:
            a = round(2 * degrees(asin(s.u[0]))) / 2
            a = "Angle = {:.1f}° ".format(abs(a))
        else: a = "Angle = ? "
        sk["angle"].config(data=a)

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

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

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

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

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