def distance_on_unit_sphere(self,lat1, long1, lat2, long2):
        """
        from john d cook website http://www.johndcook.com/blog/python_longitude_latitude/
    
        :arg lat1: latitude value in degrees for clSite
        :arg lon1: longitude value in degrees for clSite
        :arg lat2: latitude value in degrees for zipcode
        :arg lon2: longitude value in degrees for zipcode
        :var all: see link for further explanation
        """
        degrees_to_radians = math.pi/180.0
             
        # phi = 90 - latitude
        phi1 = (90.0 - lat1)*degrees_to_radians
        phi2 = (90.0 - lat2)*degrees_to_radians
                 
        # theta = longitude
        theta1 = long1*degrees_to_radians
        theta2 = long2*degrees_to_radians
    
        cos = (math.sin(phi1)*math.sin(phi2)*math.cos(theta1 - theta2) + math.cos(phi1)*math.cos(phi2))
        arc = math.acos( cos )
 
        #To get the distance in miles, multiply by 3960. 
        #To get the distance in kilometers, multiply by 6373.
        arc = arc * 3960 #miles
        return arc 

def show_visual(sec = False, radius = 39):
    c = Canvas(2*radius+1, 2*radius+1)
    x = 0
    while True:
        t = time.localtime()

        c.draw_circle(radius,radius,radius,1)
        c.draw_circle(radius,radius,radius-1,1)
        for i in xrange(12):
            dx = math.sin(2.0*math.pi*i/12)
            dy = math.cos(2.0*math.pi*i/12)
            if i%3 == 0:
                c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.8*dx*radius), radius-int(0.8*dy*radius),1)
            else:
                c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.9*dx*radius), radius-int(0.9*dy*radius),1)

        G = [((t[3]%12), 12, 0.4, 2), (t[4], 60, 0.6, 3), (t[5], 60, 0.8, 4)]
        for gnomon in G:
            dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1])
            dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1])

            c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), gnomon[3])
            c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), gnomon[3])

        c.render(width, height)
        time.sleep(1)

        for gnomon in G:
            dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1])
            dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1])

            c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), 0)
            c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), 0)

def AngularDistance(RA1, DEC1, RA2, DEC2):

	try:
		RA1 = float(RA1)
		DEC1 = float(DEC1)
		RA2 = float(RA2)
		DEC2 = float(DEC2)

		# Converting everything to radians
		ra1rad = RA1 * math.pi/180.
		ra2rad = RA2 * math.pi/180.
		dec1rad = DEC1 * math.pi/180.
		dec2rad = DEC2 * math.pi/180.

		# Calculate scalar product for determination of angular separation      
		x=math.cos(ra1rad)*math.cos(dec1rad)*math.cos(ra2rad)*math.cos(dec2rad)
		y=math.sin(ra1rad)*math.cos(dec1rad)*math.sin(ra2rad)*math.cos(dec2rad)
		z=math.sin(dec1rad)*math.sin(dec2rad)
		rad=math.acos(x+y+z)

		# Use Pythargoras approximation if rad < 1 arcsec
		if rad<0.000004848:
			rad=math.sqrt((math.cos(dec1rad)*(ra1rad-ra2rad))**2+(dec1rad-dec2rad)**2)
			pass

		# Angular separation in degrees
		Angle = rad*180/math.pi

		return Angle

	except Exception, message:
		print message

		return float('nan')

def gps_distance_between(point_a, point_b):
    """
    Calculate the orthodromic distance between two GPS readings.

    point_a and point_b can be either of the two:
    - tuples in the form (latitude, longitude).
    - instances of the class logwork.Signal
    The result is in metres.

    ATTENTION: since latitude is given before longitude, if we are using the
    X and Y representation, then we must pass in (Y, X) and *not* (X, Y)

    Computed with the Haversine formula
    (http://en.wikipedia.org/wiki/Haversine_formula)
    """
    if hasattr(point_a, "latitude"):
        a_lat, a_lon = math.radians(point_a.latitude), math.radians(point_a.longitude)
    else:
        a_lat, a_lon = math.radians(point_a[0]), math.radians(point_a[1])
    if hasattr(point_b, "latitude"):
        b_lat, b_lon = math.radians(point_b.latitude), math.radians(point_b.longitude)
    else:
        b_lat, b_lon = math.radians(point_b[0]), math.radians(point_b[1])
    d_lat = b_lat - a_lat
    d_lon = b_lon - a_lon
    a = math.sin(d_lat / 2.0) ** 2 + math.cos(a_lat) * math.cos(b_lat) * math.sin(d_lon / 2.0) ** 2
    c = 2 * math.asin(math.sqrt(a))
    return EARTH_RADIUS * c * 1000

def vorbiswindow(j, K):
    if j < 0:
        return 0
    elif j >= K:
        return 0
    z = sin(pi / K * (j + 0.5))
    return sin(pi * 0.5 * z * z)

 def __init__(self, lb, lb_length, up_angle, dn_angle, hb, hb_length):
     # define the name
     # (there is only one launchbar element) --> isn't it ?
     name = 'YASim_Launchbar'
     
     # Calculate points for the mesh
     
     # here in the original script hb = hb - lb
     # --> seems to be tuple - vector, that is not working
     # assuming: (this step is necessary to get from global to local coordinates !!)
     hb = hb - Vector(lb)
     
     lb_tip = ORIGIN + lb_length * math.cos(dn_angle * DEG2RAD) * X - lb_length * math.sin(dn_angle * DEG2RAD) * Z
     hb_tip = hb - hb_length * math.cos(dn_angle * DEG2RAD) * X - hb_length * math.sin(dn_angle * DEG2RAD) * Z
     
     # create the mesh: launchbar and holdback extended position
     lb_obj = mesh_create(name, lb, [ORIGIN, lb_tip, hb, hb_tip, lb_tip+0.05*Y, lb_tip-0.05*Y, hb_tip+0.05*Y, hb_tip-0.05*Y],
                                 [(0,1),(0,2),(2,3),(4,5),(6,7)], [])
     
     # set the created object active !!!!!!!
     bpy.context.scene.objects.active = lb_obj
     
     # draw dashed lines for the retracted position
     # get the active mesh
     mesh = bpy.context.object.data
     
     lb_up = lb_length * math.cos(up_angle * DEG2RAD) * X - lb_length * math.sin(up_angle * DEG2RAD) * Z
     hb_up = hb - hb_length * math.cos(up_angle * DEG2RAD) * X - hb_length * math.sin(up_angle * DEG2RAD) * Z
     
     draw_dashed_line(mesh, ORIGIN, lb_up)
     draw_dashed_line(mesh, hb, hb_up)
     
     # set material
     Item.set_material('grey2', (0.3,0.3,0.3), 1)

    def execute(self, context):

        A = 6.283185307179586476925286766559 / 3

        verts = [(sin(A * 1), 0.0, cos(A * 1)),
                 (sin(A * 2), 0.0, cos(A * 2)),
                 (sin(A * 3), 0.0, cos(A * 3)),
                 ]

        faces = [(0, 1, 2)]

        mesh = bpy.data.meshes.new("Cube")

        bm = bmesh.new()

        for v_co in verts:
            bm.verts.new(v_co)

        for f_idx in faces:
            bm.faces.new([bm.verts[i] for i in f_idx])

        bm.to_mesh(mesh)
        mesh.update()

        object_utils.object_data_add(context, mesh)

        return{'FINISHED'}

    def __init__(self, scale=1.0):
        self.translation = Vector3()
        self.rotation = Vector3()
        self.initialHeight = Vector3(0, 0, scale*StewartPlatformMath.SCALE_INITIAL_HEIGHT)
        self.baseJoint = []
        self.platformJoint = []
        self.q = []
        self.l = []
        self.alpha = []
        self.baseRadius = scale*StewartPlatformMath.SCALE_BASE_RADIUS
        self.platformRadius = scale*StewartPlatformMath.SCALE_PLATFORM_RADIUS
        self.hornLength = scale*StewartPlatformMath.SCALE_HORN_LENGTH
        self.legLength = scale*StewartPlatformMath.SCALE_LEG_LENGTH;

        for angle in self.baseAngles:
            mx = self.baseRadius*cos(radians(angle))
            my = self.baseRadius*sin(radians(angle))
            self.baseJoint.append(Vector3(mx, my))

        for angle in self.platformAngles:
            mx = self.platformRadius*cos(radians(angle))
            my = self.platformRadius*sin(radians(angle))
            self.platformJoint.append(Vector3(mx, my))

        self.q = [Vector3()]*len(self.platformAngles)
        self.l = [Vector3()]*len(self.platformAngles)
        self.alpha = [0]*len(self.beta)

    def refresh(self, matrix):
        matrix.fade(0.995)

        y0 = matrix.height/2
        x0 = matrix.width/2

        if self.angle >= pi:
            x0 -= 1

        if self.angle > (0.5*pi) and self.angle < (1.5*pi):
            y0 -= 1

        x1 = int(self.x0 + self.radius * sin(self.angle-self.astep))
        y1 = int(self.y0 + self.radius * cos(self.angle+self.astep))

        x2 = int(self.x0 + self.radius * sin(self.angle))
        y2 = int(self.y0 + self.radius * cos(self.angle))

        matrix.drawPoly(
            [(self.x0, self.y0), (x1, y1), (x2, y2)],
            hsvToRgb(self.hue)
        )

        self.hue = fmod(self.hue+self.hstep, 1.0)
        self.angle += self.astep

def distance(origin, destination):
    """ 
    Calculates both distance and bearing
    """
    lat1, lon1 = origin
    lat2, lon2 = destination
    if lat1>1000:
        (lat1,lon1)=dm2dd(lat1,lon1)
        (lat2,lon2)=dm2dd(lat2,lon2)
        print('converted to from ddmm to dd.ddd')
    radius = 6371 # km
    

    dlat = math.radians(lat2-lat1)
    dlon = math.radians(lon2-lon1)
    a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) \
        * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    d = radius * c
    
    def calcBearing(lat1, lon1, lat2, lon2):
       dLon = lon2 - lon1
       y = math.sin(dLon) * math.cos(lat2)
       x = math.cos(lat1) * math.sin(lat2) \
           - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
       return math.atan2(y, x)
       
    bear= math.degrees(calcBearing(lat1, lon1, lat2, lon2))  
    return d,bear

    def sumVectors(self, vectors):
        """ sum all vectors (including targetvector)"""
        endObstacleVector = (0,0)

        ##generate endvector of obstacles
        #sum obstaclevectors
        for vector in vectors:
            vectorX = math.sin(math.radians(vector[1])) * vector[0] # x-position
            vectorY = math.cos(math.radians(vector[1])) * vector[0] # y-position
            endObstacleVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
        #mean obstaclevectors
        if len(vectors) > 0:
            endObstacleVector = (endObstacleVector[0]/len(vectors), endObstacleVector[1]/len(vectors))

        #add targetvector
        targetVector = self.target
        if targetVector != 0 and targetVector != None:
            vectorX = math.sin(math.radians(targetVector[1])) * targetVector[0] # x-position
            vectorY = math.cos(math.radians(targetVector[1])) * targetVector[0] # y-position
            endVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
            #endVector = (endVector[0]/2, endVector[1]/2)
        else:
            endVector = endObstacleVector


        return endVector

    def update_location(self, delta_encoder_count_1, delta_encoder_count_2):
        """
        Update the robot's location

        @rtype : DifferentialDriveRobotLocation
        @return: Updated location
        @param delta_encoder_count_1: Count of wheel 1's encoder since last update
        @param delta_encoder_count_2: Count of wheel 2's encoder since last update
        @type delta_encoder_count_1: int
        @type delta_encoder_count_2: int
        """
        dfr = delta_encoder_count_2 * 2 * math.pi / self.robot_parameters.steps_per_revolution
        dfl = delta_encoder_count_1 * 2 * math.pi / self.robot_parameters.steps_per_revolution

        ds = (dfr + dfl) * self.robot_parameters.wheel_radius / 2
        dz = (dfr - dfl) * self.robot_parameters.wheel_radius / self.robot_parameters.wheel_distance

        self.location.x_position += ds * math.cos(self.location.z_position + dz / 2)
        self.location.y_position += ds * math.sin(self.location.z_position + dz / 2)
        self.location.z_position += dz

        self.globalLocation.x_position += ds * math.cos(self.globalLocation.z_position + dz / 2)
        self.globalLocation.y_position += ds * math.sin(self.globalLocation.z_position + dz / 2)
        self.globalLocation.z_position += dz

        return self.location, self.globalLocation

def distance(xlat, xlon, ylat, ylon):
    dlon = ylon - xlon
    dlat = ylat - xlat
    a = sin(dlat / 2) ** 2 + cos(xlat) * cos(ylat) * sin(dlon / 2) ** 2
    c = 2 * atan2(sqrt(a), sqrt(1 - a))
    distance = R * c
    return distance

    def calculate_initial_compass_bearing(self, pointA, pointB):
        """
        Calculates direction between two points.
        Code based on compassbearing.py module
        https://gist.github.com/jeromer/2005586

        pointA: latitude/longitude for first point (decimal degrees)
        pointB: latitude/longitude for second point (decimal degrees)
    
        Return: direction heading in degrees (0-360 degrees, with 90 = North)
        """

        if (type(pointA) != tuple) or (type(pointB) != tuple):
            raise TypeError("Only tuples are supported as arguments")
    
        lat1 = math.radians(pointA[0])
        lat2 = math.radians(pointB[0])
    
        diffLong = math.radians(pointB[1] - pointA[1])
    
        # Direction angle (-180 to +180 degrees):
        # θ = atan2(sin(Δlong).cos(lat2),cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))

        x = math.sin(diffLong) * math.cos(lat2)
        y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) * math.cos(lat2) * math.cos(diffLong))
    
        initial_bearing = math.atan2(x, y)
    
        # Direction calculation requires to normalize direction angle (0 - 360)
        initial_bearing = math.degrees(initial_bearing)
        compass_bearing = (initial_bearing + 360) % 360
    
        return compass_bearing

	def calc_point_distance( self, p1, p2 ) :
		x1    = float( p1['x'] )
		y1    = float( p1['y'] )
		lat1  = x1 * math.pi / 180.0
		long1 = y1 * math.pi / 180.0
		sinl1 = math.sin( lat1 )
		cosl1 = math.cos( lat1 )
		
		x2    = float( p2['x'] )
		y2    = float( p2['y'] )
		lat2  = x2 * math.pi / 180.0
		long2 = y2 * math.pi / 180.0
		sinl2 = math.sin( lat2 )
		cosl2 = math.cos( lat2 )
		
		dl    = long2 - long1
		sindl = math.sin( dl )
		cosdl = math.cos( dl )
		
		a = cosl2 * sindl
		b = cosl1 * sinl2 - sinl1 * cosl2 * cosdl
		y = math.sqrt( a*a + b*b )
		x = sinl1 * sinl2 + cosl1 * cosl2 * cosdl
		d = math.atan2( y, x ) * 6372795 # радиус Земли
		
		#print( "d=%d" % ( d ) )
		return d

    def update(self):
        if self.turning_right:
            self.rot -= 5
        if self.turning_left:
            self.rot += 5

        a = [0.0,0.0]
        if self.boost_endtime > rabbyt.get_time():
            f = 3*(self.boost_endtime - rabbyt.get_time())/self.boost_length
            a[0] += cos(radians(self.boost_rot))*f
            a[1] += sin(radians(self.boost_rot))*f
            self.create_boost_particle()

        if self.accelerating:
            a[0] += cos(radians(self.rot))*.9
            a[1] += sin(radians(self.rot))*.9
            self.create_dust_particle(self.dust_r)
            self.create_dust_particle(self.dust_l)

        ff = .9 # Friction Factor

        self.velocity[0] *= ff
        self.velocity[1] *= ff

        self.velocity[0] += a[0]
        self.velocity[1] += a[1]

        self.x += self.velocity[0]
        self.y += self.velocity[1]

 def __init__(self,pos,direction,d_range):
     self.surface = pygame.surface.Surface((50,50))
     self.surface.fill((0,250,200))
     self.rect = pygame.rect.Rect(pos[0],pos[1],50,50)
     self.dir = direction
     self.dist = 0
     self.range = d_range
     speed = 2
     dx = 0
     dy = 0
     if self.dir>0:
         if self.dir>90:
             dy = speed*math.sin(180-self.dir)
             dx = speed*math.cos(180-self.dir)
         else:
             dy = speed*math.sin(self.dir)
             dx = speed*math.cos(self.dir)
     else:
         if self.dir<-90:
             dy = speed*math.sin(180+self.dir)
             dx = speed*math.cos(180+self.dir)
         else:
             dy = speed*math.sin(self.dir)
             dx = speed*math.cos(self.dir)
     self.dx = dx
     self.dy = dy

    def __init__(self, matrix=None, scale=None, rotation=None,
                 translation=None):
        params = any(param is not None
                     for param in (scale, rotation, translation))

        if params and matrix is not None:
            raise ValueError("You cannot specify the transformation matrix and"
                             " the implicit parameters at the same time.")
        elif matrix is not None:
            if matrix.shape != (3, 3):
                raise ValueError("Invalid shape of transformation matrix.")
            self._matrix = matrix
        elif params:
            if scale is None:
                scale = 1
            if rotation is None:
                rotation = 0
            if translation is None:
                translation = (0, 0)

            self._matrix = np.array([
                [math.cos(rotation), - math.sin(rotation), 0],
                [math.sin(rotation),   math.cos(rotation), 0],
                [                 0,                    0, 1]
            ])
            self._matrix[0:2, 0:2] *= scale
            self._matrix[0:2, 2] = translation
        else:
            # default to an identity transform
            self._matrix = np.eye(3)

	def cart2tether_actual(sz,xyz):
		#convert a cartesian goal to tether length goals. assumes the tethers go to points on the outside edge of the end effector.
		#returns a more precise estimate of tether length, but one that is inadmissible to the get_xyz_pos function
		#goal : 1x3 array [x,y,z]
			# L = 1x4 array [L0,L1,L2,L3] spiral zipper and each tether length
		#finds the axis-angle rotation matrix from the column's vertical pose.
		theta = sz.angle_between([0,0,sz.L[0]], xyz)
		k = np.cross([0,0,sz.L[0]],xyz)

		if np.linalg.norm(k) != 0: #ensures k is a unit vector where its norm == 1
			k = k/np.linalg.norm(k)

		Xk = k[0]
		Yk = k[1]
		Zk = k[2]
		v = 1 - m.cos(theta)

		R = np.array( [[m.cos(theta) + (Xk**2*v)   , (Xk*Yk*v) - (Zk*m.sin(theta)), (Xk*Zk*v) + Yk*m.sin(theta)],\
					   [((Yk*Xk*v) + Zk*m.sin(theta)), m.cos(theta) + (Yk**2*v)     , (Yk*Zk*v) - Xk*m.sin(theta)],\
					   [(Zk*Xk*v) - Yk*m.sin(theta), (Zk*Yk*v) + Xk*m.sin(theta)  , m.cos(theta) + (Zk**2*v)   ]])


		#calculates position vector of the column tether attachment points in the world frame
		OB1 = xyz + np.dot(R,sz.ef[0])  
		OB2 = xyz + np.dot(R,sz.ef[1])		
		OB3 = xyz + np.dot(R,sz.ef[2])

		L0 = m.sqrt((xyz[0]**2+xyz[1]**2+xyz[2]**2)) # should just be sz.L[0] if not there is a math mistake
		L1 = np.linalg.norm(OB1 - sz.p[0])
		L2 = np.linalg.norm(OB2 - sz.p[1])
		L3 = np.linalg.norm(OB3 - sz.p[2])
		L = [L0,L1,L2,L3]
		return L

def flange(typeofFlange):

    #variables
    points = [] #collection
    nP = 100
    nSin = 10


    #conditional assignment

    if (typeofFlange == "floppy"):
        t = 3
        r = 15
        amp = 10
        
    if (typeofFlange == "extrafloppy"):
        t = 3
        r = 30
        amp = 20

#iteration to calculate points
    for i in rs.frange(0, nP, 1):
        x = r * math.sin(i*360/(nP-1))
        y = r * math.cos(i*360/(nP-1))
        z = amp*math.sin(i*nSin*360/(nP-1))
#Feature
        point = rs.AddPoint([x,y,z])
        points.append(point)