Esempio n. 1
0
def calc_heading(start_e, start_n, end_e, end_n):
    '''Calculate the equivalent heading in degrees'''
    # https://stackoverflow.com/a/5060108
    dist_e = end_e - start_e
    dist_n = end_n - start_n
    angle = degrees(arctan2(dist_n, dist_e))
    return ((90. - angle) % 360.)
Esempio n. 2
0
    def update(self):
        """move ship"""
        if self.moving_right:
            self.centerx += self.settings.ship_speed
        if self.moving_left:
            self.centerx -= self.settings.ship_speed
        if self.moving_up:
            self.centery -= self.settings.ship_speed
        if self.moving_down:
            self.centery += self.settings.ship_speed

        #off screen check
        if self.centerx >= self.settings.screen_size[0]:
            self.centerx -= self.settings.ship_speed
        if self.centerx <= 0:
            self.centerx += self.settings.ship_speed
        if self.centery >= self.settings.screen_size[1]:
            self.centery -= self.settings.ship_speed
        if self.centery <= 0:
            self.centery += self.settings.ship_speed

        #rotation
        x1 = self.centerx
        x2 = float(self.mouse_pos[0])
        y1 = self.settings.screen_size[1] - self.centery
        y2 = self.settings.screen_size[1] - float(self.mouse_pos[1])
        angle = (arctan2((y2 - y1), (x2 - x1)) * 180.0 / 3.1416) - 90
        self.image = pygame.transform.rotozoom(self.original_image, angle, 1)
        self.rect = self.image.get_rect()
        self.rect.centerx = self.centerx
        self.rect.centery = self.centery
Esempio n. 3
0
def enu2aer(e, n, u, deg=True):
    # type: (float, float, float, bool) -> tuple
    """
    ENU to Azimuth, Elevation, Range

    Parameters
    ----------

    e : float
        ENU East coordinate (meters)
    n : float
        ENU North coordinate (meters)
    u : float
        ENU Up coordinate (meters)
    deg : bool, optional
        degrees input/output  (False: radians in/out)

    Results
    -------

    azimuth : float
        azimuth to rarget
    elevation : float
        elevation to target
    srange : float
        slant range [meters]
    """
    # 1 millimeter precision for singularity

    if abs(e) < 1e-3:
        e = 0.
    if abs(n) < 1e-3:
        n = 0.
    if abs(u) < 1e-3:
        u = 0.

    r = hypot(e, n)
    slantRange = hypot(r, u)
    elev = arctan2(u, r)
    az = arctan2(e, n) % tau

    if deg:
        az = degrees(az)
        elev = degrees(elev)

    return az, elev, slantRange
Esempio n. 4
0
def enu2aer(e, n, u, deg=True):
    """
    ENU to Azimuth, Elevation, Range

    Parameters
    ----------

    e : float
        ENU East coordinate (meters)
    n : float
        ENU North coordinate (meters)
    u : float
        ENU Up coordinate (meters)
    deg : bool, optional
        degrees input/output  (False: radians in/out)

    Results
    -------

    azimuth : float
        azimuth to rarget
    elevation : float
        elevation to target
    srange : float
        slant range [meters]
    """
    # 1 millimeter precision for singularity

    if abs(e) < 1e-3:
        e = 0.
    if abs(n) < 1e-3:
        n = 0.
    if abs(u) < 1e-3:
        u = 0.

    r = hypot(e, n)
    slantRange = hypot(r, u)
    elev = arctan2(u, r)
    az = arctan2(e, n) % tau

    if deg:
        az = degrees(az)
        elev = degrees(elev)

    return az, elev, slantRange
Esempio n. 5
0
def compute_euler_angles(matrix):
    "computes the euler angles for a 3x3 rotation matrix"
    if abs(matrix[2, 0]) == 1:
        phi = 0  #in this case, phi value can be arbitrary
        if matrix[2, 0] == -1:
            theta = pi / 2
            psi = arctan2(matrix[0, 1], matrix[0, 2])
            return theta, psi, phi
        else:
            theta = -1 * pi / 2
            psi = arctan2((-1 * matrix[0, 1]), (-1 * matrix[0, 2]))
            return theta, psi, phi
    else:
        theta_1 = -1 * arcsin(matrix[2, 0])
        theta_2 = pi - theta_1
        psi_1, psi_2 = compute_psi(matrix, theta_1, theta_2)
        phi_1, phi_2 = compute_phi(matrix, theta_1, theta_2)
        return theta_1, psi_1, phi_1
Esempio n. 6
0
    def get_next_waypoint_index(self, x, y, theta):
        index = self.get_closest_waypoint_index(x, y)
        point = self.data[index]
        heading = math.arctan2(point[1] - y, point[0] - x)
        angle = abs(theta - heading)
        angle = min(2 * math.pi - angle, angle)

        if angle > (math.pi / 2.0):
            index += 1
            if index == len(self.data):
                index = 0
        return index
Esempio n. 7
0
 def do_steer(self, x_hat):
     if self.waypoints:
         cwp = self.waypoints[0]
         x, y, theta = x_hat
         cpos = Point(x, y)
         dist = distance(cpos, cwp)
         dx = cwp.x - cpos.x
         dy = cwp.y - cpos.y
         t_angle = math.arctan2(dy, dx)
         dangle = clipangle(t_angle - theta)
         u = 0.1 * np.matrix([3.0, 4.0 * dangle]).transpose()
         if dist < 0.7:
             print('At waypoint!', self.waypoints.pop(0))
     else:
         u = np.matrix([[0.0], [0.0]])
     self.cmd_vel.publish(Twist())
     return u
Esempio n. 8
0
 def do_steer(self, x_hat):
     if self.waypoints:
         cwp = self.waypoints[0]
         x, y, theta = x_hat
         cpos = Point(x, y)
         dist = distance(cpos, cwp)
         dx = cwp.x - cpos.x
         dy = cwp.y - cpos.y
         t_angle = math.arctan2(dy, dx)
         dangle = clipangle(t_angle - theta)
         u = 0.1*np.matrix([3.0, 4.0 * dangle]).transpose()
         if dist < 0.7:
             print('At waypoint!', self.waypoints.pop(0))
     else:
         u = np.matrix([[0.0], [0.0]])
     self.cmd_vel.publish(Twist())
     return u
Esempio n. 9
0
 def __init__(self, C, P, theta):
     """
     Parameters
     ----------
     C : 2D tuple
         Center coordinate.
     P : 2D tuple
         Starting point coordinate
     theta: float
         Angle in radians of the arc
     """
     self.C = np.array(C)
     self.P = np.array(P)
     self.theta = theta
     self.ray = self.P-self.C
     self.r = np.linalg.norm(self.P-self.C)
     self.arc_length = self.r * theta
     self.angle = np.arctan2(self.ray[1],self.ray[0]) #first y then x
Esempio n. 10
0
 def __init__(self, C, P, theta):
     """
     Parameters
     ----------
     C : 2D tuple
         Center coordinate.
     P : 2D tuple
         Starting point coordinate
     theta: float
         Angle in radians of the arc
     """
     self.C = np.array(C)
     self.P = np.array(P)
     self.theta = theta
     self.ray = self.P - self.C
     self.r = np.linalg.norm(self.P - self.C)
     self.arc_length = self.r * theta
     self.angle = np.arctan2(self.ray[1], self.ray[0])  #first y then x
Esempio n. 11
0
def getEllipses(src, nsigs=[1.], **kwargs):
    from matplotlib.patches import Ellipse

    xc = src.getX()
    yc = src.getY()
    x2 = src.getIxx()
    y2 = src.getIyy()
    xy = src.getIxy()
    # SExtractor manual v2.5, pg 29.
    a2 = (x2 + y2)/2. + math.sqrt(((x2 - y2)/2.)**2 + xy**2)
    b2 = (x2 + y2)/2. - math.sqrt(((x2 - y2)/2.)**2 + xy**2)
    theta = math.rad2deg(math.arctan2(2.*xy, (x2 - y2)) / 2.)
    a = math.sqrt(a2)
    b = math.sqrt(b2)
    ells = []
    for nsig in nsigs:
        ells.append(Ellipse([xc,yc], 2.*a*nsig, 2.*b*nsig, angle=theta, **kwargs))
    return ells
Esempio n. 12
0
def getEllipses(src, nsigs=[1.], **kwargs):
    from matplotlib.patches import Ellipse

    xc = src.getX()
    yc = src.getY()
    x2 = src.getIxx()
    y2 = src.getIyy()
    xy = src.getIxy()
    # SExtractor manual v2.5, pg 29.
    a2 = (x2 + y2)/2. + math.sqrt(((x2 - y2)/2.)**2 + xy**2)
    b2 = (x2 + y2)/2. - math.sqrt(((x2 - y2)/2.)**2 + xy**2)
    theta = math.rad2deg(math.arctan2(2.*xy, (x2 - y2)) / 2.)
    a = math.sqrt(a2)
    b = math.sqrt(b2)
    ells = []
    for nsig in nsigs:
        ells.append(Ellipse([xc,yc], 2.*a*nsig, 2.*b*nsig, angle=theta, **kwargs))
    return ells
Esempio n. 13
0
def steer(pose):

  # check for proximity to the target
  dx = target.x - pose.x
  dy = target.y - pose.y

  # if the car is inside a 1.5 meter radius of the target
  # then we have "captured" the target
  if math.pow(dx, 2) + math.pow(dy, 2) < 2.25:
    capturedTarget()

  current_heading = pose.theta
  desired_heading = math.arctan2( dy, dx )

  # p component
  pTurn = ( desired_heading - current_heading ) * p

  turn(pTurn)
  return
Esempio n. 14
0
def steer(pose):

    # check for proximity to the target
    dx = target.x - pose.x
    dy = target.y - pose.y

    # if the car is inside a 1.5 meter radius of the target
    # then we have "captured" the target
    if math.pow(dx, 2) + math.pow(dy, 2) < 2.25:
        capturedTarget()

    current_heading = pose.theta
    desired_heading = math.arctan2(dy, dx)

    # p component
    pTurn = (desired_heading - current_heading) * p

    turn(pTurn)
    return
Esempio n. 15
0
    def get_distance(self, point):
        """
        single point distance function
        """
        x, y, z = point

        px = math.sqrt(x**2 + y**2)
        py = math.arctan2(y, x) * self.polar
        pz = z

        d = 0
        if self.TPMStype == 0:  # 'Gyroid':
            d = math.sin(px) * math.cos(py) + math.sin(py) * math.cos(
                pz) + math.sin(pz) * math.cos(px)
        elif self.TPMStype == 1:  # 'SchwartzP':
            d = math.cos(px) + math.cos(py) + math.cos(pz)
        elif self.TPMStype == 2:  # 'Diamond':
            d = (math.sin(px) * math.sin(py) * math.sin(pz) +
                 math.sin(px) * math.cos(py) * math.cos(pz) +
                 math.cos(px) * math.sin(py) * math.cos(pz) +
                 math.cos(px) * math.cos(py) * math.sin(pz))
        elif self.TPMStype == 3:  # 'Neovius':
            d = (3 * math.cos(px) + math.cos(py) + math.cos(pz) +
                 4 * math.cos(px) * math.cos(py) * math.cos(pz))
        elif self.TPMStype == 4:  # 'Lidinoid':
            d = (0.5 * (math.sin(2 * px) * math.cos(py) * math.sin(pz) +
                        math.sin(2 * py) * math.cos(py) * math.sin(px) +
                        math.sin(2 * pz) * math.cos(px) * math.sin(pz)) - 0.5 *
                 (math.cos(2 * px) * math.cos(2 * py) + math.cos(2 * py) *
                  math.cos(2 * pz) + math.cos(2 * pz) * math.cos(2 * px)) +
                 0.15)
        elif self.TPMStype == 5:  # 'FischerKoch':
            d = (math.cos(2 * px) * math.sin(py) * math.cos(pz) +
                 math.cos(2 * py) * math.sin(pz) * math.cos(px) +
                 math.cos(2 * pz) * math.sin(px) * math.cos(py))
        return d
Esempio n. 16
0
def OSGB36toWGS84(E, N):
    #E, N are the British national grid coordinates - eastings and northings
    a, b = 6377563.396, 6356256.909  #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
    F0 = 0.9996012717  #scale factor on the central meridian
    lat0 = 49 * pi / 180  #Latitude of true origin (radians)
    lon0 = -2 * pi / 180  #Longtitude of true origin and central meridian (radians)
    N0, E0 = -100000, 400000  #Northing & easting of true origin (m)
    e2 = 1 - (b * b) / (a * a)  #eccentricity squared
    n = (a - b) / (a + b)

    lat, M = lat0, 0

    while N - N0 - M >= 0.00001:
        lat = (N - N0 - M) / (a * F0) + lat
        M1 = (1 + n + (5. / 4) * n**2 + (5. / 4) * n**3) * (lat - lat0)
        M2 = (3 * n + 3 * n**2 +
              (21. / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
        M3 = ((15. / 8) * n**2 +
              (15. / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
        M4 = (35. / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))
        M = b * F0 * (M1 - M2 + M3 - M4)

    nu = a * F0 / sqrt(1 - e2 * sin(lat)**2)

    rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
    eta2 = nu / rho - 1

    secLat = 1. / cos(lat)
    VII = tan(lat) / (2 * rho * nu)
    VIII = tan(lat) / (24 * rho * nu**3) * (5 + 3 * tan(lat)**2 + eta2 -
                                            9 * tan(lat)**2 * eta2)
    IX = tan(lat) / (720 * rho * nu**5) * (61 + 90 * tan(lat)**2 +
                                           45 * tan(lat)**4)
    X = secLat / nu
    XI = secLat / (6 * nu**3) * (nu / rho + 2 * tan(lat)**2)
    XII = secLat / (120 * nu**5) * (5 + 28 * tan(lat)**2 + 24 * tan(lat)**4)
    XIIA = secLat / (5040 * nu**7) * (61 + 662 * tan(lat)**2 +
                                      1320 * tan(lat)**4 + 720 * tan(lat)**6)
    dE = E - E0

    lat_1 = lat - VII * dE**2 + VIII * dE**4 - IX * dE**6
    lon_1 = lon0 + X * dE - XI * dE**3 + XII * dE**5 - XIIA * dE**7

    H = 0
    x_1 = (nu / F0 + H) * cos(lat_1) * cos(lon_1)
    y_1 = (nu / F0 + H) * cos(lat_1) * sin(lon_1)
    z_1 = ((1 - e2) * nu / F0 + H) * sin(lat_1)

    s = -20.4894 * 10**-6
    tx, ty, tz = 446.448, -125.157, +542.060
    rxs, rys, rzs = 0.1502, 0.2470, 0.8421
    rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
        180 * 3600.), rzs * pi / (180 * 3600.)
    x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
    y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
    z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1

    a_2, b_2 = 6378137.000, 6356752.3141
    e2_2 = 1 - (b_2 * b_2) / (a_2 * a_2)
    p = sqrt(x_2**2 + y_2**2)

    lat = arctan2(z_2, (p * (1 - e2_2)))
    latold = 2 * pi
    while abs(lat - latold) > 10**-16:
        lat, latold = latold, lat
        nu_2 = a_2 / sqrt(1 - e2_2 * sin(latold)**2)
        lat = arctan2(z_2 + e2_2 * nu_2 * sin(latold), p)

    lon = arctan2(y_2, x_2)
    H = p / cos(lat) - nu_2

    lat = lat * 180 / pi
    lon = lon * 180 / pi

    return lat, lon
Esempio n. 17
0
def WGS84toOSGB36(lat, lon):
    #First convert to radians
    #These are on the wrong ellipsoid currently: GRS80. (Denoted by _1)
    lat_1 = lat*pi/180
    lon_1 = lon*pi/180

    #Want to convert to the Airy 1830 ellipsoid, which has the following:
    a_1, b_1 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2_1 = 1- (b_1*b_1)/(a_1*a_1)   #The eccentricity of the GRS80 ellipsoid
    nu_1 = a_1/sqrt(1-e2_1*sin(lat_1)**2)

    #First convert to cartesian from spherical polar coordinates
    H = 0 #Third spherical coord.
    x_1 = (nu_1 + H)*cos(lat_1)*cos(lon_1)
    y_1 = (nu_1+ H)*cos(lat_1)*sin(lon_1)
    z_1 = ((1-e2_1)*nu_1 +H)*sin(lat_1)

    #Perform Helmut transform (to go between GRS80 (_1) and Airy 1830 (_2))
    s = 20.4894*10**-6 #The scale factor -1
    tx, ty, tz = -446.448, 125.157, -542.060 #The translations along x,y,z axes respectively
    rxs,rys,rzs = -0.1502, -0.2470, -0.8421  #The rotations along x,y,z respectively, in seconds
    rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
    x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
    y_2 = ty + (rz)*x_1  + (1+s)*y_1 + (-rx)*z_1
    z_2 = tz + (-ry)*x_1 + (rx)*y_1 +  (1+s)*z_1

    #Back to spherical polar coordinates from cartesian
    #Need some of the characteristics of the new ellipsoid
    a, b = 6377563.396, 6356256.909 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2 = 1- (b*b)/(a*a)   #The eccentricity of the Airy 1830 ellipsoid
    p = sqrt(x_2**2 + y_2**2)

    #Lat is obtained by an iterative proceedure:
    lat = arctan2(z_2,(p*(1-e2))) #Initial value
    latold = 2*pi
    while abs(lat - latold)>10**-16:
        lat, latold = latold, lat
        nu = a/sqrt(1-e2*sin(latold)**2)
        lat = arctan2(z_2+e2*nu*sin(latold), p)

    #Lon and height are then pretty easy
    lon = arctan2(y_2,x_2)
    H = p/cos(lat) - nu

    #E, N are the British national grid coordinates - eastings and northings
    F0 = 0.9996012717                   #scale factor on the central meridian
    lat0 = 49*pi/180                    #Latitude of true origin (radians)
    lon0 = -2*pi/180                    #Longtitude of true origin and central meridian (radians)
    N0, E0 = -100000, 400000            #Northing & easting of true origin (m)
    n = (a-b)/(a+b)

    #meridional radius of curvature
    rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
    eta2 = nu*F0/rho-1

    M1 = (1 + n + (5/4)*n**2 + (5/4)*n**3) * (lat-lat0)
    M2 = (3*n + 3*n**2 + (21/8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
    M3 = ((15/8)*n**2 + (15/8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
    M4 = (35/24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))

    #meridional arc
    M = b * F0 * (M1 - M2 + M3 - M4)          

    I = M + N0
    II = nu*F0*sin(lat)*cos(lat)/2
    III = nu*F0*sin(lat)*cos(lat)**3*(5- tan(lat)**2 + 9*eta2)/24
    IIIA = nu*F0*sin(lat)*cos(lat)**5*(61- 58*tan(lat)**2 + tan(lat)**4)/720
    IV = nu*F0*cos(lat)
    V = nu*F0*cos(lat)**3*(nu/rho - tan(lat)**2)/6
    VI = nu*F0*cos(lat)**5*(5 - 18* tan(lat)**2 + tan(lat)**4 + 14*eta2 - 58*eta2*tan(lat)**2)/120

    N = I + II*(lon-lon0)**2 + III*(lon- lon0)**4 + IIIA*(lon-lon0)**6
    E = E0 + IV*(lon-lon0) + V*(lon- lon0)**3 + VI*(lon- lon0)**5 

    #Job's a good'n.
    return E,N
Esempio n. 18
0
def ecef2geodetic(x, y, z, ell=None, deg=True):
    """
    convert ECEF (meters) to geodetic coordinates

    Parameters
    ----------
    x : float
        target x ECEF coordinate (meters)
    y : float
        target y ECEF coordinate (meters)
    z : float
        target z ECEF coordinate (meters)
    ell : Ellipsoid, optional
          reference ellipsoid
    deg : bool, optional
          degrees input/output  (False: radians in/out)

    Returns
    -------
    lat : float
           target geodetic latitude
    lon : float
           target geodetic longitude
    h : float
         target altitude above geodetic ellipsoid (meters)

    based on:
    You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
    Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
    """
    if ell is None:
        ell = Ellipsoid()

    r = sqrt(x**2 + y**2 + z**2)

    E = sqrt(ell.semimajor_axis**2 - ell.semiminor_axis**2)

    # eqn. 4a
    u = sqrt(0.5 * (r**2 - E**2) +
             0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))

    Q = hypot(x, y)

    huE = hypot(u, E)

    # eqn. 4b
    Beta = arctan(huE / u * z / hypot(x, y))

    # eqn. 13
    eps = ((ell.semiminor_axis * u - ell.semimajor_axis * huE + E**2) *
           sin(Beta)) / (ell.semimajor_axis * huE * 1 / cos(Beta) -
                         E**2 * cos(Beta))

    Beta += eps
    # %% final output
    lat = arctan(ell.semimajor_axis / ell.semiminor_axis * tan(Beta))

    lon = arctan2(y, x)

    # eqn. 7
    alt = hypot(z - ell.semiminor_axis * sin(Beta),
                Q - ell.semimajor_axis * cos(Beta))

    # inside ellipsoid?
    inside = x**2 / ell.semimajor_axis**2 + y**2 / ell.semimajor_axis**2 + z**2 / ell.semiminor_axis**2 < 1
    if inside:
        alt = -alt

    if deg:
        lat = degrees(lat)
        lon = degrees(lon)

    return lat, lon, alt
Esempio n. 19
0
def OSGB36toWGS84(E,N):
    #E, N are the British national grid coordinates - eastings and northings
    a, b = 6377563.396, 6356256.909     #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
    F0 = 0.9996012717                   #scale factor on the central meridian
    lat0 = 49*pi/180                    #Latitude of true origin (radians)
    lon0 = -2*pi/180                    #Longtitude of true origin and central meridian (radians)
    N0, E0 = -100000, 400000            #Northing & easting of true origin (m)
    e2 = 1 - (b*b)/(a*a)                #eccentricity squared
    n = (a-b)/(a+b)

    lat,M = lat0, 0

    while N-N0-M >= 0.00001:
        lat = (N-N0-M)/(a*F0) + lat;
        M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
        M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
        M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
        M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
        M = b * F0 * (M1 - M2 + M3 - M4)

    nu = a*F0/sqrt(1-e2*sin(lat)**2)

    rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
    eta2 = nu/rho-1

    secLat = 1./cos(lat)
    VII = tan(lat)/(2*rho*nu)
    VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
    IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
    X = secLat/nu
    XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
    XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
    XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
    dE = E-E0

    lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
    lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7

    H = 0
    x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
    y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
    z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)

    s = -20.4894*10**-6
    tx, ty, tz = 446.448, -125.157, + 542.060
    rxs,rys,rzs = 0.1502,  0.2470,  0.8421
    rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.)
    x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
    y_2 = ty + (rz)*x_1  + (1+s)*y_1 + (-rx)*z_1
    z_2 = tz + (-ry)*x_1 + (rx)*y_1 +  (1+s)*z_1

    a_2, b_2 =6378137.000, 6356752.3141
    e2_2 = 1- (b_2*b_2)/(a_2*a_2)
    p = sqrt(x_2**2 + y_2**2)

    lat = arctan2(z_2,(p*(1-e2_2)))
    latold = 2*pi
    while abs(lat - latold)>10**-16:
        lat, latold = latold, lat
        nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
        lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)

    lon = arctan2(y_2,x_2)
    H = p/cos(lat) - nu_2

    lat = lat*180/pi
    lon = lon*180/pi

    return lat, lon
Esempio n. 20
0
def ecef2geodetic(x, y, z, ell=None, deg=True):
    """
    convert ECEF (meters) to geodetic coordinates

    Parameters
    ----------
    x : float
        target x ECEF coordinate (meters)
    y : float
        target y ECEF coordinate (meters)
    z : float
        target z ECEF coordinate (meters)
    ell : Ellipsoid, optional
          reference ellipsoid
    deg : bool, optional
          degrees input/output  (False: radians in/out)

    Returns
    -------
    lat : float
           target geodetic latitude
    lon : float
           target geodetic longitude
    h : float
         target altitude above geodetic ellipsoid (meters)

    based on:
    You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
    Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
    """
    if ell is None:
        ell = Ellipsoid()

    r = sqrt(x**2 + y**2 + z**2)

    E = sqrt(ell.semimajor_axis**2 - ell.semiminor_axis**2)

    # eqn. 4a
    u = sqrt(0.5 * (r**2 - E**2) + 0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))

    Q = hypot(x, y)

    huE = hypot(u, E)

    # eqn. 4b
    Beta = arctan(huE / u * z / hypot(x, y))

    # eqn. 13
    eps = ((ell.semiminor_axis * u - ell.semimajor_axis * huE + E**2) * sin(Beta)) / (ell.semimajor_axis * huE * 1 / cos(Beta) - E**2 * cos(Beta))

    Beta += eps
# %% final output
    lat = arctan(ell.semimajor_axis / ell.semiminor_axis * tan(Beta))

    lon = arctan2(y, x)

    # eqn. 7
    alt = hypot(z - ell.semiminor_axis * sin(Beta),
                Q - ell.semimajor_axis * cos(Beta))

    # inside ellipsoid?
    inside = x**2 / ell.semimajor_axis**2 + y**2 / ell.semimajor_axis**2 + z**2 / ell.semiminor_axis**2 < 1
    if inside:
        alt = -alt

    if deg:
        lat = degrees(lat)
        lon = degrees(lon)

    return lat, lon, alt
Esempio n. 21
0
def avg_lyapunov():
    """Return the average lyapunov exponent for each table type, over a number of trajectories, for
       both angular and radial separation. For section 3.3."""

    radius = lambda ba, bb: sqrt((ba.pos[0] - bb.pos[0])**2 +
                                 (ba.pos[1] - bb.pos[1])**2)
    angle = lambda b1, b2: abs(
        arctan2(b1.pos[1], b1.pos[0]) - arctan2(b2.pos[1], b2.pos[0]))

    x_range = [0.1 * i for i in range(6)
               ]  # Range of x-coordinates to start at (y = 0 always).
    v_range = [[1, 5], [2, 4], [3, 3], [4, 2], [5, 1]]  # Range of velocities.

    rect_lyap_rad, ell_lyap_rad, stad_lyap_rad = [], [], [
    ]  # Lists of computed radial Lyapunov exponents for tables.
    rect_lyap_ang, ell_lyap_ang, stad_lyap_ang = [], [], [
    ]  # Lists of computed angular Lyapunov exponents for tables.
    rect, ell, stad = Rectangle(2, 1), Ellipse(2, 1), Stadium(2, 1)  # Tables.

    for x in x_range:
        for v in v_range:
            # Offset y position by 0.1 between balls to act as slight difference in initial conditions.
            rect_lyap_rad.append(
                lyapunov(radius,
                         rect,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))
            rect_lyap_ang.append(
                lyapunov(angle,
                         rect,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))

            ell_lyap_rad.append(
                lyapunov(radius,
                         ell,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))
            ell_lyap_ang.append(
                lyapunov(angle,
                         ell,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))

            stad_lyap_rad.append(
                lyapunov(radius,
                         stad,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))
            stad_lyap_ang.append(
                lyapunov(angle,
                         stad,
                         Ball([x, 0], v),
                         Ball([x, 0.1], v),
                         n=2000,
                         show=False))

    print(
        f"Rectangle radial separation Lyapunov exponent: {mean(rect_lyap_rad)} +/- {std(rect_lyap_rad)}"
    )
    print(
        f"Ellipse radial separation Lyapunov exponent: {mean(ell_lyap_rad)} +/- {std(ell_lyap_rad)}"
    )
    print(
        f"Stadium radial separation Lyapunov exponent: {mean(stad_lyap_rad)} +/- {std(stad_lyap_rad)}"
    )

    print(
        f"Rectangle angular separation Lyapunov exponent: {mean(rect_lyap_ang)} +/- {std(rect_lyap_ang)}"
    )
    print(
        f"Ellipse angular separation Lyapunov exponent: {mean(ell_lyap_ang)} +/- {std(ell_lyap_ang)}"
    )
    print(
        f"Stadium angular separation Lyapunov exponent: {mean(stad_lyap_ang)} +/- {std(stad_lyap_ang)}"
    )
Esempio n. 22
0
def wgs2osgb(lat, lon = None): # derived from WGS84toOSGB36() by Hannah Fry
    '''
    convert WGS84 latitude, longitude coordinates to OSGB36 numeric coordinates
    arguments are either pair of floats, or a double list/tuple of floats
    return double tuple of east, west integers
    does not check validity of argument values
    '''
    # check for single list or tuple, or 2 arguments
    if lon is None: # assume lat is list or tuple
        lon = lat[1]    # order matters!
        lat = lat[0]

    # convert to radians
    # these are on the wrong ellipsoid currently: GRS80. (Denoted by _G)
    lat_G = lat*pi/180
    lon_G = lon*pi/180

    nu_G = A_G/sqrt(1-E2_G*sin(lat_G)**2)

    # convert to cartesian from spherical polar coordinates
    x_G = (nu_G + H)*cos(lat_G)*cos(lon_G)
    y_G = (nu_G + H)*cos(lat_G)*sin(lon_G)
    z_G = ((1 - E2_G)*nu_G + H)*sin(lat_G)

    # perform Helmut transform (to go from GRS80 (_G) to Airy 1830 (_A))
    x_A = TX_GA + (1+S)*x_G + (-RZ_GA)*y_G + (RY_GA)*z_G
    y_A = TY_GA + (RZ_GA)*x_G+ (1 + S)*y_G + (-RX_GA)*z_G
    z_A = TZ_GA + (-RY_GA)*x_G + (RX_GA)*y_G +(1 + S)*z_G

    # back to spherical polar coordinates from cartesian
    # need some of the characteristics of the new ellipsoid
    p_A = sqrt(x_A**2 + y_A**2)

    # latitude is obtained by iteration
    lat = arctan2(z_A,(p_A*(1 - E2_A)))   # initial value
    latold = 2*pi
    while abs(lat - latold)>10**-16:
        lat, latold = latold, lat
        nu_A = A_A/sqrt(1 - E2_A*sin(latold)**2)
        lat = arctan2(z_A + E2_A*nu_A*sin(latold), p_A)

    # longitude and height are then pretty easy
    lon = arctan2(y_A,x_A)
    h = p_A/cos(lat) - nu_A

    # east, north are the UK National Grid coordinates - eastings and northings
    # meridional radius of curvature
    rho = A_A*F0*(1 - E2_A)*(1 - E2_A*sin(lat)**2)**(-1.5)
    eta2 = nu_A*F0/rho-1

    m1 = (1 + N_A + (5/4)*N_A**2 + (5/4)*N_A**3) * (lat-LAT0)
    m2 = (3*N_A + 3*N_A**2 + (21/8)*N_A**3) * sin(lat - LAT0) * cos(lat + LAT0)
    m3 = ((15/8)*N_A**2 + (15/8)*N_A**3) * sin(2*(lat - LAT0)) * cos(2*(lat + LAT0))
    m4 = (35/24)*N_A**3 * sin(3*(lat - LAT0)) * cos(3*(lat + LAT0))

    # meridional arc
    m = B_A * F0 * (m1 - m2 + m3 - m4)

    i = m + N0
    ii = nu_A*F0*sin(lat)*cos(lat)/2
    iii = nu_A*F0*sin(lat)*cos(lat)**3*(5- tan(lat)**2 + 9*eta2)/24
    iiia = nu_A*F0*sin(lat)*cos(lat)**5*(61- 58*tan(lat)**2 + tan(lat)**4)/720
    iv = nu_A*F0*cos(lat)
    v = nu_A*F0*cos(lat)**3*(nu_A/rho - tan(lat)**2)/6
    vi = nu_A*F0*cos(lat)**5*(5 - 18* tan(lat)**2 + tan(lat)**4 + 14*eta2 - 58*eta2*tan(lat)**2)/120

    north = i + ii*(lon - LON0)**2 + iii*(lon - LON0)**4 + iiia*(lon - LON0)**6
    east = E0 + iv*(lon - LON0) + v*(lon - LON0)**3 + vi*(lon - LON0)**5 

    # round down to nearest metre and return as integer value double tuple
    return (int(east), int(north))
Esempio n. 23
0
def osgb2wgs(east, north = None):   # derived from OSGB36toWGS84() by Hannah Fry
    '''
    convert OSGB36 numeric coordinates to WGS lat, lon coordinates
    arguments are either a pair of integers, or a NGR
    return double tuple of lat, lon floats to precision of 5dp
    '''
    
    # check for single tuple or double argument
    if north is None:   # assume NGR
        (east, north) = ngr2osgb(east)
    east, north = int(east), int(north)

    # Initialise the iterative variables
    lat, m = LAT0, 0

    while north - N0 - m >= 0.00001: #Accurate to 0.01mm
        lat = (north - N0 - m)/(A_A*F0) + lat;
        m1 = (1 + N_A + (5./4)*N_A**2 + (5./4)*N_A**3) * (lat - LAT0)
        m2 = (3*N_A + 3*N_A**2 + (21./8)*N_A**3) * sin(lat - LAT0) * cos(lat + LAT0)
        m3 = ((15./8)*N_A**2 + (15./8)*N_A**3) * sin(2*(lat - LAT0)) * cos(2*(lat + LAT0))
        m4 = (35./24)*N_A**3 * sin(3*(lat - LAT0)) * cos(3*(lat + LAT0))
        #meridional arc
        m = B_A * F0 * (m1 - m2 + m3 - m4)          

    # transverse radius of curvature
    nu_A = A_A*F0/sqrt(1 - E2_A*sin(lat)**2)

    # meridional radius of curvature
    rho = A_A*F0*(1 - E2_A)*(1 - E2_A*sin(lat)**2)**(-1.5)
    eta2 = nu_A/rho-1

    secLat = 1./cos(lat)
    vii = tan(lat)/(2*rho*nu_A)
    viii = tan(lat)/(24*rho*nu_A**3)*(5+3*tan(lat)**2+eta2 - 9*tan(lat)**2*eta2)
    ix = tan(lat)/(720*rho*nu_A**5)*(61 + 90*tan(lat)**2 + 45*tan(lat)**4)
    x = secLat/nu_A
    xi = secLat/(6*nu_A**3)*(nu_A/rho+2*tan(lat)**2)
    xii = secLat/(120*nu_A**5)*(5+28*tan(lat)**2 + 24*tan(lat)**4)
    xiia = secLat/(5040*nu_A**7)*(61 + 662*tan(lat)**2 + 1320*tan(lat)**4 + 720*tan(lat)**6)
    dE = east - E0

    # these are on the wrong ellipsoid currently: Airy1830 (denoted by _A)
    lat_A = lat - vii*dE**2 + viii*dE**4 - ix*dE**6
    lon_A = LON0 + x*dE - xi*dE**3 + xii*dE**5 - xiia*dE**7

    # convert to the GRS80 ellipsoid (denoted by _G). 
    # first convert to cartesian from spherical polar coordinates
    H = 0   # third spherical coord. 
    x_A = (nu_A/F0 + H)*cos(lat_A)*cos(lon_A)
    y_A = (nu_A/F0+ H)*cos(lat_A)*sin(lon_A)
    z_A = ((1 - E2_A)*nu_A/F0 + H)*sin(lat_A)

    # Perform Helmut transform (to go from Airy 1830 to GRS80)
    x_G = TX_AG + (1 + S)*x_A + (-RZ_AG)*y_A + (RY_AG)*z_A
    y_G = TY_AG + (RZ_AG)*x_A  + (1 + S)*y_A + (-RX_AG)*z_A
    z_G = TZ_AG + (-RY_AG)*x_A + (RX_AG)*y_A + (1 + S)*z_A

    # back to spherical polar coordinates from cartesian
    # need a characteristic of the new ellipsoid    
    p_G = sqrt(x_G**2 + y_G**2)

    # lat is obtained by an iterative procedure:   
    lat = arctan2(z_G,(p_G*(1 - E2_G))) #Initial value
    latold = 2*pi
    while abs(lat - latold)>10**-16: 
        lat, latold = latold, lat
        nu_G = A_G/sqrt(1 - E2_G*sin(latold)**2)
        lat = arctan2(z_G + E2_G*nu_G*sin(latold), p_G)

    #Lon and height are then pretty easy
    lon = arctan2(y_G, x_G)
    H = p_G/cos(lat) - nu_G

    #Convert to degrees & returns floats double tuple
    lat = lat*180/pi
    lon = lon*180/pi
    return (round(lat, 5), round(lon, 5))
Esempio n. 24
0
def WGS84toOSGB36(lat, lon):
    '''Hannah's code to convert WGS84 Lat and Lon to OS Eastings and Northings'''

    #First convert to radians
    #These are on the wrong ellipsoid currently: GRS80. (Denoted by _1)
    lat_1 = lat * pi / 180
    lon_1 = lon * pi / 180

    #Want to convert to the Airy 1830 ellipsoid, which has the following:
    a_1, b_1 = 6378137.000, 6356752.3141  #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2_1 = 1 - (b_1 * b_1) / (a_1 * a_1
                              )  #The eccentricity of the GRS80 ellipsoid
    nu_1 = a_1 / sqrt(1 - e2_1 * sin(lat_1)**2)

    #First convert to cartesian from spherical polar coordinates
    H = 0  #Third spherical coord.
    x_1 = (nu_1 + H) * cos(lat_1) * cos(lon_1)
    y_1 = (nu_1 + H) * cos(lat_1) * sin(lon_1)
    z_1 = ((1 - e2_1) * nu_1 + H) * sin(lat_1)

    #Perform Helmut transform (to go between GRS80 (_1) and Airy 1830 (_2))
    s = 20.4894 * 10**-6  #The scale factor -1
    tx, ty, tz = -446.448, 125.157, -542.060  #The translations along x,y,z axes respectively
    rxs, rys, rzs = -0.1502, -0.2470, -0.8421  #The rotations along x,y,z respectively, in seconds
    rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
        180 * 3600.), rzs * pi / (180 * 3600.)  #In radians
    x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
    y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
    z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1

    #Back to spherical polar coordinates from cartesian
    #Need some of the characteristics of the new ellipsoid
    a, b = 6377563.396, 6356256.909  #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2 = 1 - (b * b) / (a * a)  #The eccentricity of the Airy 1830 ellipsoid
    p = sqrt(x_2**2 + y_2**2)

    #Lat is obtained by an iterative proceedure:
    lat = arctan2(z_2, (p * (1 - e2)))  #Initial value
    latold = 2 * pi
    while abs(lat - latold) > 10**-16:
        lat, latold = latold, lat
        nu = a / sqrt(1 - e2 * sin(latold)**2)
        lat = arctan2(z_2 + e2 * nu * sin(latold), p)

    #Lon and height are then pretty easy
    lon = arctan2(y_2, x_2)
    H = p / cos(lat) - nu

    #E, N are the British national grid coordinates - eastings and northings
    F0 = 0.9996012717  #scale factor on the central meridian
    lat0 = 49 * pi / 180  #Latitude of true origin (radians)
    lon0 = -2 * pi / 180  #Longtitude of true origin and central meridian (radians)
    N0, E0 = -100000, 400000  #Northing & easting of true origin (m)
    n = (a - b) / (a + b)

    #meridional radius of curvature
    rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
    eta2 = nu * F0 / rho - 1

    M1 = (1 + n + (5 / 4) * n**2 + (5 / 4) * n**3) * (lat - lat0)
    M2 = (3 * n + 3 * n**2 +
          (21 / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
    M3 = ((15 / 8) * n**2 +
          (15 / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
    M4 = (35 / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))

    #meridional arc
    M = b * F0 * (M1 - M2 + M3 - M4)

    I = M + N0
    II = nu * F0 * sin(lat) * cos(lat) / 2
    III = nu * F0 * sin(lat) * cos(lat)**3 * (5 - tan(lat)**2 + 9 * eta2) / 24
    IIIA = nu * F0 * sin(lat) * cos(lat)**5 * (61 - 58 * tan(lat)**2 +
                                               tan(lat)**4) / 720
    IV = nu * F0 * cos(lat)
    V = nu * F0 * cos(lat)**3 * (nu / rho - tan(lat)**2) / 6
    VI = nu * F0 * cos(lat)**5 * (5 - 18 * tan(lat)**2 + tan(lat)**4 +
                                  14 * eta2 - 58 * eta2 * tan(lat)**2) / 120

    N = I + II * (lon - lon0)**2 + III * (lon - lon0)**4 + IIIA * (lon -
                                                                   lon0)**6
    E = E0 + IV * (lon - lon0) + V * (lon - lon0)**3 + VI * (lon - lon0)**5

    #Job's a good'n.
    return E, N
Esempio n. 25
0
 def GetTheta(self):
     return math.degrees(math.arctan2(self.GetDeltaY(), self.GetDeltaX()))
Esempio n. 26
0
 def from_cartesian(cls, x, y):
     magnitude = math.sqrt(math.hypot(x, y))
     direction = math.arctan2(y, x)
     return cls(magnitude, direction)
Esempio n. 27
0
def OSGB36toWGS84(E, N):
    # E, N are the British national grid coordinates - eastings and northings
    a = 6377563.396
    b = 6356256.909
    # The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
    F0 = 0.9996012717
    lat0 = 49 * pi / 180
    lon0 = -2 * pi / 180
    N0, E0 = -100000, 400000
    e2 = 1 - (b * b) / (a * a)
    n = (a - b) / (a + b)

    # Initialise the iterative variables
    lat, M = lat0, 0

    while N - N0 - M >= 0.00001:
        lat = (N - N0 - M) / (a * F0) + lat
        M1 = (1 + n + (5. / 4) * n**2 + (5. / 4) * n**3) * (lat - lat0)
        M2 = (3 * n + 3 * n**2 +
              (21. / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
        M3 = ((15. / 8) * n**2 +
              (15. / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
        M4 = (35. / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))
        # meridional arc
        M = b * F0 * (M1 - M2 + M3 - M4)

    # transverse radius of curvature
    nu = a * F0 / sqrt(1 - e2 * sin(lat)**2)

    # meridional radius of curvature
    rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
    eta2 = nu / rho - 1

    secLat = 1. / cos(lat)
    VII = tan(lat) / (2 * rho * nu)
    VIII = tan(lat) / (24 * rho * nu**3) * (5 + 3 * tan(lat)**2 + eta2 -
                                            9 * tan(lat)**2 * eta2)
    IX = tan(lat) / (720 * rho * nu**5) * (61 + 90 * tan(lat)**2 +
                                           45 * tan(lat)**4)
    X = secLat / nu
    XI = secLat / (6 * nu**3) * (nu / rho + 2 * tan(lat)**2)
    XII = secLat / (120 * nu**5) * (5 + 28 * tan(lat)**2 + 24 * tan(lat)**4)
    XIIA = secLat / (5040 * nu**7) * (61 + 662 * tan(lat)**2 +
                                      1320 * tan(lat)**4 + 720 * tan(lat)**6)
    dE = E - E0

    # These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
    lat_1 = lat - VII * dE**2 + VIII * dE**4 - IX * dE**6
    lon_1 = lon0 + X * dE - XI * dE**3 + XII * dE**5 - XIIA * dE**7

    # Want to convert to the GRS80 ellipsoid.
    # First convert to cartesian from spherical polar coordinates
    H = 0
    x_1 = (nu / F0 + H) * cos(lat_1) * cos(lon_1)
    y_1 = (nu / F0 + H) * cos(lat_1) * sin(lon_1)
    z_1 = ((1 - e2) * nu / F0 + H) * sin(lat_1)

    # Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
    s = -20.4894 * 10**-6
    tx, ty, tz = 446.448, -125.157, +542.060
    rxs, rys, rzs = 0.1502, 0.2470, 0.8421
    rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
        180 * 3600.), rzs * pi / (180 * 3600.)
    x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
    y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
    z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1

    # Back to spherical polar coordinates from cartesian
    # Need some of the characteristics of the new ellipsoid
    a_2, b_2 = 6378137.000, 6356752.3141
    e2_2 = 1 - (b_2 * b_2) / (a_2 * a_2)
    p = sqrt(x_2**2 + y_2**2)

    # Lat is obtained by an iterative proceedure:
    lat = arctan2(z_2, (p * (1 - e2_2)))
    latold = 2 * pi
    while abs(lat - latold) > 10**-16:
        lat, latold = latold, lat
        nu_2 = a_2 / sqrt(1 - e2_2 * sin(latold)**2)
        lat = arctan2(z_2 + e2_2 * nu_2 * sin(latold), p)

    # Lon and height are then pretty easy
    lon = arctan2(y_2, x_2)
    H = p / cos(lat) - nu_2

    # Convert to degrees
    lat = lat * 180 / pi
    lon = lon * 180 / pi

    # Job's a good'n.
    return lat, lon
Esempio n. 28
0
def OSGB36toWGS84_(E,N):

    '''
    From http://hannahfry.co.uk/2012/02/01/converting-british-national-grid-to-latitude-and-longitude-ii/
    '''
    #E, N are the British national grid coordinates - eastings and northings
    a, b = 6377563.396, 6356256.909     #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
    F0 = 0.9996012717                   #scale factor on the central meridian
    lat0 = 49*pi/180                    #Latitude of true origin (radians)
    lon0 = -2*pi/180                    #Longtitude of true origin and central meridian (radians)
    N0, E0 = -100000, 400000            #Northing & easting of true origin (m)
    e2 = 1 - (b*b)/(a*a)                #eccentricity squared
    n = (a-b)/(a+b)

    #Initialise the iterative variables
    lat,M = lat0, 0

    while N-N0-M >= 0.00001: #Accurate to 0.01mm
        lat = (N-N0-M)/(a*F0) + lat;
        M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
        M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
        M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
        M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
        #meridional arc
        M = b * F0 * (M1 - M2 + M3 - M4)          

    #transverse radius of curvature
    nu = a*F0/sqrt(1-e2*sin(lat)**2)

    #meridional radius of curvature
    rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
    eta2 = nu/rho-1

    secLat = 1./cos(lat)
    VII = tan(lat)/(2*rho*nu)
    VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
    IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
    X = secLat/nu
    XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
    XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
    XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
    dE = E-E0

    #These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
    lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
    lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7

    #Want to convert to the GRS80 ellipsoid. 
    #First convert to cartesian from spherical polar coordinates
    H = 0 #Third spherical coord. 
    x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
    y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
    z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)

    #Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
    s = -20.4894*10**-6 #The scale factor -1
    tx, ty, tz = 446.448, -125.157, + 542.060 #The translations along x,y,z axes respectively
    rxs,rys,rzs = 0.1502,  0.2470,  0.8421  #The rotations along x,y,z respectively, in seconds
    rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
    x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
    y_2 = ty + (rz)*x_1  + (1+s)*y_1 + (-rx)*z_1
    z_2 = tz + (-ry)*x_1 + (rx)*y_1 +  (1+s)*z_1

    #Back to spherical polar coordinates from cartesian
    #Need some of the characteristics of the new ellipsoid    
    a_2, b_2 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2_2 = 1- (b_2*b_2)/(a_2*a_2)   #The eccentricity of the GRS80 ellipsoid
    p = sqrt(x_2**2 + y_2**2)

    #Lat is obtained by an iterative proceedure:   
    lat = arctan2(z_2,(p*(1-e2_2))) #Initial value
    latold = 2*pi
    while abs(lat - latold)>10**-16: 
        lat, latold = latold, lat
        nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
        lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)

    #Lon and height are then pretty easy
    lon = arctan2(y_2,x_2)
    H = p/cos(lat) - nu_2

    #Uncomment this line if you want to print the results
    #print [(lat-lat_1)*180/pi, (lon - lon_1)*180/pi]

    #Convert to degrees
    lat = lat*180/pi
    lon = lon*180/pi

    #Job's a good'n. 
    return lat, lon
def OSIRISH36toWGS84(E,N):
    #E, N are the British national grid coordinates - eastings and northings
    a, b = 6377,340.189                 #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
    F0 = 1.000035                       #scale factor on the central meridian
    lat0 = 53*pi/180                    #Latitude of true origin (radians)
    lon0 = -8*pi/180                    #Longtitude of true origin and central meridian (radians)
    N0, E0 = 250000 , 200000            #Northing & easting of true origin (m)
    e2 = 1 - (b*b)/(a*a)                #eccentricity squared
    n = (a-b)/(a+b)

    #Initialise the iterative variables
    lat,M = lat0, 0

    while N-N0-M >= 0.00001: #Accurate to 0.01mm
        lat = (N-N0-M)/(a*F0) + lat;
        M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
        M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
        M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
        M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
        #meridional arc
        M = b * F0 * (M1 - M2 + M3 - M4)          

    #transverse radius of curvature
    nu = a*F0/sqrt(1-e2*sin(lat)**2)

    #meridional radius of curvature
    rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
    eta2 = nu/rho-1

    secLat = 1./cos(lat)
    VII = tan(lat)/(2*rho*nu)
    VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
    IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
    X = secLat/nu
    XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
    XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
    XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
    dE = E-E0

    #These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
    lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
    lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7

    #Want to convert to the GRS80 ellipsoid. 
    #First convert to cartesian from spherical polar coordinates
    H = 0 #Third spherical coord. 
    x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
    y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
    z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)

    #Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
    s = -20.4894*10**-6 #The scale factor -1
    tx, ty, tz = 446.448, -125.157, + 542.060 #The translations along x,y,z axes respectively
    rxs,rys,rzs = 0.1502,  0.2470,  0.8421  #The rotations along x,y,z respectively, in seconds
    rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
    x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
    y_2 = ty + (rz)*x_1  + (1+s)*y_1 + (-rx)*z_1
    z_2 = tz + (-ry)*x_1 + (rx)*y_1 +  (1+s)*z_1

    #Back to spherical polar coordinates from cartesian
    #Need some of the characteristics of the new ellipsoid    
    a_2, b_2 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
    e2_2 = 1- (b_2*b_2)/(a_2*a_2)   #The eccentricity of the GRS80 ellipsoid
    p = sqrt(x_2**2 + y_2**2)

    #Lat is obtained by an iterative proceedure:   
    lat = arctan2(z_2,(p*(1-e2_2))) #Initial value
    latold = 2*pi
    while abs(lat - latold)>10**-16: 
        lat, latold = latold, lat
        nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
        lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)

    #Lon and height are then pretty easy
    lon = arctan2(y_2,x_2)
    H = p/cos(lat) - nu_2

    #Uncomment this line if you want to print the results
    #print [(lat-lat_1)*180/pi, (lon - lon_1)*180/pi]

    #Convert to degrees
    lat = lat*180/pi
    lon = lon*180/pi

    #Job's a good'n. 
    return lat, lon
Esempio n. 30
0
    def edge_probability(self, u, v):
        """ Probability of edge in the similarity graph based on geo-damped Lin similarity.

        Edge probability consists of two independent contributions:
        - probability induced by graphical distance between agents
        - probability induced by similarity between agents

        Parameters
        ----------
        u, v : int
            Indices of vertices
        """

        # Compute contribution of geo-attributes to the edge probability.

        # Get vectors with (lon,lat)-pairs of all locations for 2 nodes.
        geo_attrs_u = self.geo_attrs[u]
        geo_attrs_v = self.geo_attrs[v]
        # Compute minimum geo-distance between locations of a and b
        # NOTE: For the moment, it selects the closest distance for
        #       matching types of locations (e.g., between 2 households,
        #       but not between household of `a` and workplace of `b`)
        min_dist = numpy.PINF
        for i in xrange(0, len(geo_attrs_u), 2):
            dlon = geo_attrs_u[i] - geo_attrs_v[i]
            # TODO: precompute sines and cosines before computing individual edge probabilities
            lat1, lat2 = geo_attrs_u[i + 1], geo_attrs_v[i + 1]
            cos_lat1, cos_lat2, cos_dlon = numpy.cos(lat1), numpy.cos(
                lat2), numpy.cos(dlon)
            sin_lat1, sin_lat2, sin_dlon = numpy.sin(lat1), numpy.sin(
                lat2), numpy.sin(dlon)
            y = sqrt((cos_lat2 * sin_dlon)**2 +
                     (cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon)**2)
            x = sin_lat1 * sin_lat2 + cos_lat1 * cos_lat2 * cos_dlon
            # TODO: clarify about negative distances
            min_dist = min(min_dist, arctan2(y, x))

        if self.damping > 0.:
            # Scale distance by half-similarity scale to make it adimensional and damp by a factor 2.
            prob_geo = (1. + self.geo_scaling * min_dist)**(-self.damping)
        else:
            # Compute geo-similarity with exponential damping.
            prob_geo = 2**(-self.geo_scaling * min_dist)
        # logging.debug( "minimum distance rho({0},{1})={2}; geo-induced probability Pr({0},{1})={3}".\
        #                format(a, b, min_dist, prob_geo) )

        # If probability induced by the geo-attributes is smaller than a certain lower bound threshold,
        # the Lin similarity contribution is disregarded.
        if prob_geo <= self.similarity_threshold:
            return 0.

        # Compute contribution of non-geographic attributes to the edge probability.
        # This contribution is based on Lin similarity metric.
        # Lin similarity handles categorical ('c') and ordinal ('o') attributes
        # (while geographic ('g') attributes are subject of geo-damping).
        # In order to reduce calculation time, code below evaluates
        # the similarity between node `a` and node `b` on the sampled attributes.

        # First, find the frequency of agents sharing all attributes shared by the two analysed nodes.
        # The idea is that the lower the number of agents sharing the same attribute,
        # the more information this shared attribute contains about the two agents.
        # Since attributes are dependent, one must estimate the probability (get frequency)
        # of observing all different attributes at the same time. If they were independent,
        # one might handle attributes independently by computing their contributions separately
        # and summing up.

        attrs_u, attrs_v = self.nongeo_attrs[u], self.nongeo_attrs[v]
        # Select nodes as similar if categorical attributes are the same to `a` and `b`
        # If vertices share categorical attribute, the number of agents sharing this attribute is considered.
        # TODO: ensure that we do not have to cut of by categorical_attrs
        similar_nodes = True  # start with all sampled items as similar
        for sample, attr_u, attr_v in islice(
                izip(self.sampled_nongeo_attrs, attrs_u, attrs_v),
                self.num_categorical):
            if attr_u == attr_v:
                # filter out indices of samples with the same attribute
                similar_nodes &= sample == attr_u
        # logging.debug( "similar categorical attributes in the sample: {0} out of {1}".\
        #                format(numpy.sum(similar_nodes) if not isinstance(similar_nodes, bool) else "all",
        #                       self.sample_size) )

        # Select nodes as similar if ordinal attributes are between values for `a` and `b`.
        # If vertices do not share an attribute and the attribute is ordinal,
        # the number of agents sharing attributes between the two values is considered.
        for sample, attr_u, attr_v in islice(
                izip(self.sampled_nongeo_attrs, attrs_u, attrs_v),
                self.num_categorical, self.num_categorical + self.num_ordinal):
            attr_min, attr_max = min(attr_u, attr_v), max(attr_u, attr_v)
            # filter out indices of samples with the attribute in the range of values between `a` and `b`
            similar_nodes &= (attr_min <= sample) & (sample <= attr_max)

        num_similar = numpy.sum(similar_nodes) if not isinstance(
            similar_nodes, bool) else self.sample_size
        # logging.debug( "similar attributes in the sample after ordinal attributes filtering: {0} out of {1}".\
        #                format(num_similar, self.sample_size) )

        # If the superposition is zero on the sample, then there is no agents with the same characteristics.
        # The probability of finding something with the same feature of both `a` and `b` is really small.
        if num_similar == 0:
            return prob_geo  # TODO: consult why not zero

        # Second, find the frequency of agents sharing all attributes with each analysed node separately.
        # TODO: clarify whether we need to take geo-filtering into account as in original script
        # NOTE: `numpy.sum` performs better than `sum` on numpy-arrays
        num_equal_u = numpy.sum(
            self.sampled_vertex_attrs == self.vertex_attrs[u])
        num_equal_v = numpy.sum(
            self.sampled_vertex_attrs == self.vertex_attrs[v])
        # logging.debug( "similar attributes (in the sample) to the 1st vertex: {0}, to the 2nd vertex: {1}".\
        #                format(num_equal_u, num_equal_v) )

        # Compute Lin similarity (use inverses of frequencies estimated above)
        num_sample = float(self.sample_size)
        num_total = len(self)
        prob_lin = log2(num_sample / num_similar)
        if num_equal_u == 0:  # there is no agents as `a`
            # We make assumption that `a` is the only one with this characteristic in the whole dataset.
            if num_equal_v == 0:  # the same thing for `b`
                prob_lin /= log2(num_total)
            else:  # `a` is unique, but `b` has similar vertices (agents) in the dataset (population)
                prob_lin /= 0.5 * log2(num_sample * num_total / num_equal_v)
        elif num_equal_v == 0:  # `b` is unique, but `a` has similar vertices (agents) in the dataset (population)
            prob_lin /= (log2(num_sample * num_total / num_equal_u))
        else:  # both `a` and `b` have similar vertices (agents) in the dataset (population)
            prob_lin /= log2(num_sample * num_sample /
                             (num_equal_u * num_equal_v))

        return prob_geo * prob_lin