def latlon2xy(lat, lon, origin=(37.0, -118.0), az=0, km2deg=111.3195):
    """Project cartesian x,y coordinates to geographical lat,lon.

    Parameters
    -----------
    lat and lon : 2darrays in decimal degrees.

    origin : tuple or list, optional, default: lat=37.0 lon=-118.0
            lat,lon coordinates of the south-west corner of the x,y grid

    az : float or int, optional, default: 0 degrees
            rotation of the grid aroud the vertical (z-axis),
            see the SW4 User Guide for more information.

    km2deg : float, optional, default: 111.3195 km
            how many km to a degree.

    Returns
    --------
    x and y 2darrays."""

    az = np.radians(az)
    x = km2deg * (
        ((lat - origin[0]) +
         (lon - origin[1]) * np.cos(np.radians(lat)) * np.tan(az)) /
        (np.cos(az) * (1 + np.tan(az)**2)))
    y = (km2deg * ((lon - origin[1]) * np.cos(np.radians(lat))) /
         np.cos(az)) - x * np.tan(az)

    return x, y

def calculate_fracture_step_sizes(start_yx, ang):
    """
    Calculate the sizes of steps dx and dy to be used when "drawing" the
    fracture onto the grid.

    Parameters
    ----------
    start_yx : tuple of int
        Starting grid coordinates
    ang : float
        Fracture angle relative to horizontal (radians)

    Returns
    -------
    (dy, dx) : tuple of float
        Step sizes in y and x directions. One will always be unity, and the
        other will always be <1.
    """
    starty, startx = start_yx
    if startx==0:  # frac starts on left side
        dx = 1
        dy = tan(ang)
    else:  # frac starts on bottom side
        dy = 1
        dx = -tan(ang-pi/2)

    return (dy, dx)

    def Reached(self, qdr, dist, flyby):
        # Calculate distance before waypoint where to start the turn
        # Turn radius:      R = V2 tan phi / g
        # Distance to turn: wpturn = R * tan (1/2 delhdg) but max 4 times radius
        # using default bank angle per flight phase
        turnrad = bs.traf.tas * bs.traf.tas /     \
                      (np.maximum(bs.traf.eps, np.tan(bs.traf.bank)) * g0)  \
                       # Turn radius in meters!

        next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr)

        # Avoid circling by checking for flying away
        away     = np.abs(degto180(bs.traf.trk%360. - qdr%360.)) > 90. # difference large than 90

        # Ratio between distance close enough to switch to next wp when flying away
        # When within pro1 nm and flying away: switch also
        proxfact = 1.01 # Turnradius scales this contant , factor => [turnrad]
        incircle = dist<turnrad*proxfact
        circling = away*incircle # [True/False] passed wp,used for flyover as well


        # distance to turn initialisation point [m]
        self.turndist = flyby*np.abs(turnrad *
            np.tan(np.radians(0.5 * np.abs(degto180(qdr%360. - next_qdr%360.)))))


        # Check whether shift based dist is required, set closer than WP turn distance
        swreached = np.where(bs.traf.swlnav * ((dist < self.turndist)+circling))[0]

        # Return True/1.0 for a/c where we have reached waypoint
        return swreached

	def project(self, p, radius, width, height) :
		#print p
		#print width
		#print height
		#print "radius"
		#print radius

		xFOV = 63.38
		yFOV = 48.25
		cx = width /2
		cy = height /2
		fx = cx / np.tan((xFOV/2) * np.pi / 180)
		fy = cy / np.tan((yFOV/2) * np.pi / 180)
		
		toball = np.zeros(3)
		toball[0] = (p[0] - cx) / fx
		toball[1] = -(p[1] - cy) / fy
		toball[2] = 1
		toball = toball / np.linalg.norm(toball) #normalize so we can then multiply by distance
		distance = self.pixel_radius / radius
		toball = toball * distance

		pose = Pose()
		pose.position = Point(toball[0], toball[1], toball[2])
		pose.orientation = Quaternion(0,0,0,1)
		#print "FOUND ORANGE BALL!!!!"
		#print toball
		return pose

    def __init__(self, ra_0, dec_0, dec_1, dec_2):
        """Lambert Conformal conic projection.

        LCC is a conic projection with an origin along the lines connecting
        the poles. It preserves angles, but is not equal-area,
        perspective or equistant.

        Its preferred use of for areas with predominant east-west extent
        at higher latitudes.

        As a conic projection, it depends on two standard parallels, i.e.
        intersections of the cone with the sphere. To minimize scale variations,
        these standard parallels should be chosen as small as possible while
        spanning the range in declinations of the data.

        For details, see Snyder (1987, section 15).

        Args:
            ra_0: RA that maps onto x = 0
            dec_0: Dec that maps onto y = 0
            dec_1: lower standard parallel
            dec_2: upper standard parallel (must not be -dec_1)
        """
        ConicProjection.__init__(self, ra_0, dec_0, dec_1, dec_2)
        # Snyder 1987, eq. 14-1, 14-2 and 15-1 to 15-3.
        self.dec_max = 89.99

        dec_1 *= self.deg2rad
        dec_2 *= self.deg2rad
        self.n = np.log(np.cos(dec_1)/np.cos(dec_2)) / \
        (np.log(np.tan(np.pi/4 + dec_2/2)/np.tan(np.pi/4 + dec_1/2)))
        self.F = np.cos(dec_1)*(np.tan(np.pi/4 + dec_1/2)**self.n)/self.n
        self.rho_0 = self._rho(dec_0)

 def execute(self, fp):
     fp.rack.m = fp.module.Value
     fp.rack.z = fp.teeth
     fp.rack.pressure_angle = fp.pressure_angle.Value * np.pi / 180.
     fp.rack.thickness = fp.thickness.Value
     fp.rack.beta = fp.beta.Value * np.pi / 180.
     fp.rack.head = fp.head
     fp.rack._update()
     pts = fp.rack.points()
     pol = Wire(makePolygon(list(map(fcvec, pts))))
     if fp.beta.Value == 0:
         face = Face(Wire(pol))
         fp.Shape = face.extrude(fcvec([0., 0., fp.height.Value]))
     elif fp.double_helix:
         beta = fp.beta.Value * np.pi / 180.
         pol2 = Part.Wire(pol)
         pol2.translate(fcvec([0., np.tan(beta) * fp.height.Value / 2, fp.height.Value / 2]))
         pol3 = Part.Wire(pol)
         pol3.translate(fcvec([0., 0., fp.height.Value]))
         fp.Shape = makeLoft([pol, pol2, pol3], True, True)
     else:
         beta = fp.beta.Value * np.pi / 180.
         pol2 = Part.Wire(pol)
         pol2.translate(fcvec([0., np.tan(beta) * fp.height.Value, fp.height.Value]))
         fp.Shape = makeLoft([pol, pol2], True)

def build_planar_surface(geometry):
    """
    Builds the planar rupture surface from the openquake.nrmllib.models
    instance
    """
    # Read geometry from wkt
    geom = wkt.loads(geometry.wkt)
    top_left = Point(geom.xy[0][0],
                     geom.xy[1][0],
                     geometry.upper_seismo_depth)
    top_right = Point(geom.xy[0][1],
                      geom.xy[1][1],
                      geometry.upper_seismo_depth)
    strike = top_left.azimuth(top_right)
    dip_dir = (strike + 90.) % 360.
    depth_diff = geometry.lower_seismo_depth - geometry.upper_seismo_depth
    bottom_right = top_right.point_at(
        depth_diff / np.tan(geometry.dip * (np.pi / 180.)),
        depth_diff,
        dip_dir)
    bottom_left = top_left.point_at(
        depth_diff / np.tan(geometry.dip * (np.pi / 180.)),
        depth_diff,
        dip_dir)
    return PlanarSurface(1.0,
                         strike,
                         geometry.dip,
                         top_left,
                         top_right,
                         bottom_right,
                         bottom_left)

def fundamental_geometry_plot_data(par):
    '''Returns the coordinates for line end points of the bicycle fundamental
    geometry.

    Parameters
    ----------
    par : dictionary
        Benchmark bicycle parameters.

    Returns
    -------
    x : ndarray
    z : ndarray

    '''
    d1 = np.cos(par['lam']) * (par['c'] + par['w'] -
                par['rR'] * np.tan(par['lam']))
    d3 = -np.cos(par['lam']) * (par['c'] - par['rF'] *
                np.tan(par['lam']))
    x = np.zeros(4, dtype=object)
    z = np.zeros(4, dtype=object)
    x[0] = 0.
    x[1] = d1 * np.cos(par['lam'])
    x[2] = par['w'] - d3 * np.cos(par['lam'])
    x[3] = par['w']
    z[0] = -par['rR']
    z[1] = -par['rR'] - d1 * np.sin(par['lam'])
    z[2] = -par['rF'] + d3 * np.sin(par['lam'])
    z[3] = -par['rF']

    return x, z

    def get_corner_coordinates(self):
        inc = self.inclination

        llLat, llLon = self.get_ll_anchor()
        urLat, urLon = self.get_ur_anchor()

        if self.orbital_node == 'Ascending':

            ulLat, ulLon = od.ne_to_latlon(
                self.lat_center, self.lon_center,
                self.track_length/2,
                -num.tan(inc*d2r) * self.width/2)
            lrLat, lrLon = od.ne_to_latlon(
                self.lat_center, self.lon_center,
                -self.track_length/2,
                num.tan(inc*d2r) * self.width/2)

        elif self.orbital_node == 'Descending':
            urLat, urLon = od.ne_to_latlon(
                self.lat_center, self.lon_center,
                self.track_length/2,
                num.tan(inc*d2r) * self.width/2)
            llLat, llLon = od.ne_to_latlon(
                self.lat_center, self.lon_center,
                -self.track_length/2,
                -num.tan(inc*d2r) * self.width/2)

        return ((llLat, llLon), (ulLat, ulLon),
                (urLat, urLon), (lrLat, lrLon))

def compute_voroni_area_of_triangle(w1,w2,l1,l2):
    """
    computes the part of the triangle for the Voroni area
    descriped in [1]

            x_i
          +
          |\
          | \
          |  \
        l1|   \l2
          |    \
          |     \
          |w1  w2\
          +-------+
        x_j  l3  x_{j+1}

    """
    # Check if triangle is obtuse in x_i
    if w1+w2 < pi/2.0:
        # Then get Area(T)/2
        return norm(cross(l1,l2))/4.0
    
    if w1 > pi/2.0 or w2 > pi/2:
        # Then get Area(T)/4
        return norm(cross(l1,l2))/8.0
            

    #Else use formula on page 9 in [1]
    return ((1/tan(w1))*inner(l2,l2) + (1/tan(w2))*inner(l1,l1))/8.0

    def draw(self, output="plot.png", label=None):
        """ draw the wave structure """

        print(self.left_pos, self.contact_pos, self.right_pos)

        # each wave has a point on the origin.  The maximum height we
        # want to draw to is self.dx -- we'll do the center and right
        x1 = self.x0 + self.dx * np.tan(np.radians(self.left_pos))
        xc = self.x0 + self.dx * np.tan(np.radians(self.contact_pos))
        x3 = self.x0 + self.dx * np.tan(np.radians(self.right_pos))

        # draw the waves
        if self.left_type == "shock":
            plt.plot([self.x0, x1], [0, self.dx], color="C0", lw=3)
        else:
            for n in range(5):
                x1 = self.x0 + self.dx * np.tan(np.radians(self.left_pos - 2*n))
                plt.plot([self.x0, x1], [0, self.dx], color="C0", lw=1)

        plt.plot([self.x0, xc], [0, self.dx], color="C1")

        if self.right_type == "shock":
            plt.plot([self.x0, x3], [0, self.dx], color="C0", lw=3)
        else:
            for n in range(5):
                x3 = self.x0 + self.dx * np.tan(np.radians(self.right_pos + 2*n))
                plt.plot([self.x0, x3], [0, self.dx], color="C0", lw=1)

 def publish(self, event):
     if self.imgmsg is None:
         return
     now = rospy.Time.now()
     # setup ros message and publish
     with self.lock:
         self.imgmsg.header.stamp = now
         self.imgmsg.header.frame_id = self.frame_id
     self.pub.publish(self.imgmsg)
     if self.publish_info:
         info = CameraInfo()
         info.header.stamp = now
         info.header.frame_id = self.frame_id
         info.width = self.imgmsg.width
         info.height = self.imgmsg.height
         if self.fovx is not None and self.fovy is not None:
             fx = self.imgmsg.width / 2.0 / \
                 np.tan(np.deg2rad(self.fovx / 2.0))
             fy = self.imgmsg.height / 2.0 / \
                 np.tan(np.deg2rad(self.fovy / 2.0))
             cx = self.imgmsg.width / 2.0
             cy = self.imgmsg.height / 2.0
             info.K = np.array([fx, 0, cx,
                                0, fy, cy,
                                0, 0, 1.0])
             info.P = np.array([fx, 0, cx, 0,
                                0, fy, cy, 0,
                                0, 0, 1, 0])
             info.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
         self.pub_info.publish(info)

def cone_to_plane(theta, a_max):
    '''
    convert the cone to plane
    '''
    h = 1./np.tan(a_max)
    sl = h*np.tan(theta)
    return sl

def get_distance_matrix(pan, fov, height, rows):
    """
        画像と同じ行数で一列の配列を返す。
        それぞれの行にはcameraからcenterまでの距離が入ってる。

        src: pan       -> カメラの傾き   [radian]
             fov       -> カメラの視野角 [radian]
             height    -> 地面からカメラまでの高さ [m]
             rows      -> 画像の縦方向のピクセル数 [pixel]

        dst: dis       -> 距離の真値
             dis_ratio -> dis[row] / dis[len(rows)]の値(画像中心からの距離の比)
    """

    # Convert radian -> degree
    pan = 1.0 * pan * np.pi / 180
    fov = 1.0 * fov * np.pi / 180
    dis = []
    dis_ratio = []

    for row in range(rows - 1, -1, -1):
        # dis.append( height * np.tan(1.*row*fov/rows + pan - 1.*fov/2))
        dis_ratio.append(np.tan(1.0 * row * fov / rows + pan - 1.0 * fov / 2) / np.tan(pan))

    return dis_ratio

def nominal_q(sx, sy, az_in, el_in, az_out, el_out, dz):
    nx, ny = sx + dz * tan(az_in), sy + dz * tan(el_in)
    nd = sqrt( (nx-sx)**2 + (ny-sy)**2 + dz**2 )
    #plt.subplot(131); plot(nx/5,ny/5,'G: direct flight beam center')

    px, py = sx + dz * tan(az_out), sy + dz * tan(el_out)
    pd = sqrt( (px-sx)**2 + (py-sy)**2 + dz**2 )
    #plt.subplot(122); plot(px/5,py/5,'G: direct flight scattered beam')

    if 0:
        # Correction to move px,py into the q normal plane.  This is
        # insignificant for small angle scattering.
        nx_hat, ny_hat, nz_hat = (nx-sx)/nd, (ny-sy)/nd, dz/nd
        px_hat, py_hat, pz_hat = (px-sx)/pd, (py-sy)/pd, dz/pd
        d = nd / (px_hat*nx_hat + py_hat*ny_hat + pz_hat*nz_hat)
        px, py = sx + px_hat*d, sy + py_hat*d
    #plt.subplot(122); plot((px)/5,(py)/5,'G: scattered beam on q normal plane')

    # Note: px,py is the location of the scattered neutron relative to the
    # beam center without gravity in detector coordinates, not the qx,qy vector
    # in inverse coordinates.  This allows us to compute the scattered angle at 
    # the sample, returning theta and phi.
    qd = sqrt((px-nx)**2 + (py-ny)**2)
    theta, phi = arctan2(qd, nd)/2, arctan2(py-ny, px-nx)

    return theta, phi

def simulator_alpha(theta, N=100):
    """
    function that samples form the alpha stable distribution
    theta is np.array([alpha, beta, gamma, delta])
    """
    # unpack values
    #	theta = theta.astype(object)
    alpha = theta[0,:]
    beta =  theta[1,:]
    gamma =  theta[2,:]
    delta =  theta[3,:]
    # add random seed
    random_seed = random.randrange(10**9)
    np.random.seed(seed=random_seed)
    # generate w and u for simulating
    #	pdb.set_trace()
    w = nr.exponential(size=N)
    u = nr.uniform(low=-np.pi/2., high=np.pi/2., size=N)
    #	w = w.astype(float)
    #	u = u.astype(float)
    S_a_b = (1.+ beta**2. * np.tan(np.pi*alpha/2.)**2. )**(1/(2.*alpha))
    B_a_b = 1./alpha * np.arctan(beta*np.tan(np.pi*alpha*0.5))
    if alpha == 1.:
        y_bar = 2./np.pi * ((np.pi/2. + beta*u)*np.tan(u)-beta*np.log(np.pi/2. * w *np.cos(u ) / (np.pi/2. + beta*u)   ) )
    else:
        y_bar = S_a_b * ((np.sin(alpha)*(u + B_a_b ) ) / np.cos(u)**(1./alpha)  ) * (np.cos(u-alpha*(u+ B_a_b ))/w) **((1-alpha)/alpha )

    return S1_summary_statistic_alpha(y_bar*gamma+delta, theta)

def omega2rates(phi,theta,input_unit='rad',output_type='ndarray'):
    """
    This function is used to create the transformation matrix to get
    phi_dot, the_dot and psi_dot from given pqr (body rate).
    Input:
    e: Euler Angle in the format of [phi the psi];
    Output:
    R: Transformation matrix, numpy array 3x3
    Programmer:    Adhika Lie
    Created:    	 May 13, 2011
    Last Modified: May 13, 2011
    """
    R = np.zeros((3,3))
    if(input_unit=='deg'):
        phi = np.deg2rad(phi)
        theta = np.deg2rad(theta)
    
    R[0,0] = 1.0
    R[0,1] = np.sin(phi)*np.tan(theta)
    R[0,2] = np.cos(phi)*np.tan(theta)
    
    R[1,0] = 0.0
    R[1,1] = np.cos(phi)
    R[1,2] = -np.sin(phi)
    
    R[2,0] = 0.0
    R[2,1] = np.sin(phi)/np.cos(theta)
    R[2,2] = np.cos(phi)/np.cos(theta)
    
    if(output_type=='matrix'):
        R = np.matrix(R)
    
    return R

def KMatrixFun(N=0, O=0, h=12, day=173, iter=len(X)):
	KMatrix = KMatrixCrown = KMatrixHeight = np.zeros([iter, 2]) 
	KMatrixTot = np.zeros(iter)
	
	for i in xrange(iter):
		KMatrixCrown[i] = ([X[i], Y[i]]+np.dot(RM(np.radians(-(SolAziNBGW(h,day)-210)))/np.tan(SolElvNBGW(h,day)),[-CW[i]/200,HE[i]])-[N, O])/([X[i], Y[i]]+np.dot(RM(np.radians(-(SolAziNBGW(h,day)-210)))/np.tan(SolElvNBGW(h,day)),[-CW[i]/200,HE[i]-CH[i]])-[N, O])
		for j in xrange(2):
			if KMatrixCrown[i, j] <= 0:
				KMatrixCrown[i, j] = 0
			else:
				KMatrixCrown[i, j] = 1
	
		KMatrixHeight[i] = ([X[i], Y[i]]+np.dot(RM(np.radians(-(SolAziNBGW(h,day)-210)))/np.tan(SolElvNBGW(h,day)),[0,HE[i]])-[N, O])/([X[i], Y[i]]+np.dot(RM(np.radians(-(SolAziNBGW(h,day)-210)))/np.tan(SolElvNBGW(h,day)),[0,HE[i]-CH[i]])-[N, O])
		for j in xrange(2):
			if KMatrixHeight[i, j] <= 0:
				KMatrixHeight[i, j] = 0
			else:
				KMatrixHeight[i, j] = 1
			
		KMatrix[i] = KMatrixHeight[i]+KMatrixCrown[i]
		KMatrixTot[i] = KMatrix[i,0]+KMatrix[i,1]
		
	for i in xrange(iter):
		if KMatrixTot[i] <= 0:
			KMatrixTot[i] = 0
		else:
			KMatrixTot[i] = 1
	
	if (iter - KMatrixTot.sum()) <= 0:
		KMatrixTotTot = 1
	else:
		KMatrixTotTot = 0
	
	return KMatrixTotTot

    def profile(self, m, r, r0, t_c, t_i, alpha_w, y0, y1, y2):
        r_ew = m * t_i / 2

        # 1: modifizierter Waelzkreisdurchmesser:
        r_e = r / r0 * r_ew

        # 2: modifizierter Schraegungswinkel:
        alpha = np.arccos(r0 / r * np.cos(alpha_w))

        # 3: winkel phi bei senkrechter stellung eines zahns:
        phi = np.pi / t_i / 2 + (alpha - alpha_w) + (np.tan(alpha_w) - np.tan(alpha))

        # 4: Position des Eingriffspunktes:
        x_c = r_e * np.sin(phi)
        dy = -r_e * np.cos(phi) + r_ew

        # 5: oberer Punkt:
        b = y1 - dy
        a = np.tan(alpha) * b
        x1 = a + x_c

        # 6: unterer Punkt
        d = y2 + dy
        c = np.tan(alpha) * d
        x2 = x_c - c

        r *= np.cos(phi)
        pts = [
            [-x1, r, y0],
            [-x2, r, y0 - y1 - y2],
            [x2, r, y0 - y1 - y2],
            [x1, r, y0]
        ]
        pts.append(pts[0])
        return pts

def accel_to_lin_xz_xy(seg_len,xa,ya,za):

    x=seg_len/np.sqrt(1+(np.tan(np.arctan(za/(np.sqrt(xa**2+ya**2))))**2+(np.tan(np.arctan(ya/(np.sqrt(xa**2+za**2))))**2)))
    xz=x*(za/(np.sqrt(xa**2+ya**2)))
    xy=x*(ya/(np.sqrt(xa**2+za**2)))
    
    return np.round(xz,4),np.round(xy,4)