Example #1
0
	def getZD(self, md, lat, decl, umd):
		'''Calculates Regiomontan zenith distance '''

		zd = 0.0
		if md == 90.0:
			zd = 90.0-math.degrees(math.atan(math.sin(math.fabs(math.radians(lat))))*math.tan(math.radians(decl)))
		elif md < 90.0:
			A = math.degrees(math.atan(math.cos(math.radians(lat))*math.tan(math.radians(md))))
			B = math.degrees(math.atan(math.tan(math.fabs(math.radians(lat)))*math.cos(math.radians(md))))

			C = 0.0
			if (decl < 0 and lat < 0) or (decl >= 0 and lat >= 0):
				if umd:
					C = B-math.fabs(decl)
				else:
					C = B+math.fabs(decl)
			elif (decl < 0 and lat > 0) or (decl > 0 and lat < 0):
				if umd:
					C = B+math.fabs(decl)
				else:
					C = B-math.fabs(decl)

			F = math.degrees(math.atan(math.sin(math.fabs(math.radians(lat)))*math.sin(math.radians(md))*math.tan(math.radians(C)))) #C and F can be negative
			zd = A+F

		return zd
    def show_particles(self, particles):
        self.update_cnt += 1
        if UPDATE_EVERY > 0 and self.update_cnt % UPDATE_EVERY != 1:
            return

        # turtle.clearstamps()
        turtle.shape('tri')

        draw_cnt = 0
        px = {}
        for p in particles:
            draw_cnt += 1
            if DRAW_EVERY == 0 or draw_cnt % DRAW_EVERY == 1:
                # Keep track of which positions already have something
                # drawn to speed up display rendering
                scaled_x1 = int(p.x1 * self.one_px)
                scaled_y1 = int(p.y1 * self.one_px)
                scaled_xy1 = scaled_x1 * 10000 + scaled_y1
                if not scaled_xy1 in px:
                    px[scaled_xy1] = 1
                    turtle.setposition(*p.xy1)
                    turtle.setheading(math.degrees(p.h))
                    turtle.color("Red")
                    turtle.stamp()

                    turtle.setposition(*p.xy2)
                    turtle.setheading(math.degrees(p.h))
                    turtle.color("Blue")
                    turtle.stamp()
    def lookToXY(self, x, y):
        self.tracker.target.x = x
        self.tracker.target.y = y
        heading = self.tracker.brain.loc.h
        robx = self.tracker.brain.loc.x
        roby = self.tracker.brain.loc.y
        yaw = math.degrees(math.atan((y-roby)/(x-robx)))
        if robx > x:
            if roby < y:
                yaw = 180 + yaw
            else:
                yaw = -180 + yaw
        yaw = yaw - heading
        curYaw  = degrees(self.tracker.brain.interface.joints.head_yaw)
        maxSpeed = 2.0

        # Set motion message fields
        command = self.tracker.brain.interface.headMotionCommand
        command.type = command.CommandType.POS_HEAD_COMMAND

        command.pos_command.head_yaw = yaw
        command.pos_command.head_pitch = constants.FIXED_PITCH_VALUE
        command.pos_command.max_speed_yaw = maxSpeed
        command.pos_command.max_speed_pitch = maxSpeed

        command.timestamp = int(self.tracker.brain.time * 1000)
Example #4
0
	def iterateRegio(self, pl, rwa, rwd, robl, rpoh, lon):
		
		okGa = okGd = True

		if pl.speculums[0][Planet.PMP] < 90.0 or (pl.speculums[0][Planet.PMP] >= 270.0 and pl.speculums[0][Planet.PMP] < 360.0):
			Ga = math.degrees(math.cos(rwa)*math.cos(robl)-math.sin(robl)*math.tan(rpoh))
			if Ga != 0.0:
				Fa = math.degrees(math.atan(math.sin(rwa)/(math.cos(rwa)*math.cos(robl)-math.sin(robl)*math.tan(rpoh))))

				if Fa >= 0.0 and Ga > 0.0:
					lon = Fa
				elif Fa < 0.0 and Ga > 0.0:
					lon = Fa+360.0
				elif Ga < 0.0:
					lon = Fa+180.0
			else:
				okGa = False
		else:
			Gd = math.degrees(math.cos(rwd)*math.cos(robl)+math.sin(robl)*math.tan(rpoh))
			if Gd != 0.0:
				Fd = math.degrees(math.atan(math.sin(rwd)/(math.cos(rwd)*math.cos(robl)+math.sin(robl)*math.tan(rpoh))))

				if Fd >= 0.0 and Gd > 0.0:
					lon = Fd
				elif Fd < 0.0 and Gd > 0.0:
					lon = Fd+360.0
				elif Gd < 0.0:
					lon = Fd+180.0
			else:
				okGd = False

		return okGa, okGd, lon
Example #5
0
def strokeOrientation(inStroke):
    "Input: List inPoints.  Returns the Angle of Orientation of a set of points (in degrees) where 0 is horizontal"

    cen = inStroke.Center
    inPoints = inStroke.Points
    pArea = area(inPoints)

    if (len(inPoints) <= 1):
        print "Warning: trying to get the Angle of Orientation of one or fewer points."
        return 0.0

    if pArea == 0:  #Perfect line        
        orientX = inPoints[-1].X - inPoints[0].X
        orientY = inPoints[-1].Y - inPoints[0].Y
        orientMag = vectorLength(orientX, orientY)
        if orientMag == 0:
            return 0.0
        orientX = orientX / orientMag
        orientY = orientY / orientMag
        return math.degrees( math.acos(orientX) )

    moment11 = momentOfOrder(cen, inPoints, 1, 1)
    if moment11 == 0:
        return 0    #There is no Moment of order 1,1.  We'd get a divide by zero.  Orientation is undefined; just return zero.

    angle = (.5 * math.atan((momentOfOrder(cen, inPoints, 0, 2) - momentOfOrder(cen, inPoints, 2, 0)) / (2 * moment11))) \
            + sign(moment11) * math.pi / 4

    return math.degrees(angle)
Example #6
0
		def ecef_to_lla(ecef):
			#earths's radius in meters
			a = 6378137 
			#eccentricity
			e = 8.1819190842622e-2  
			asq = math.pow(a,2)
			esq = math.pow(e,2)
			x = ecef[0]
			y = ecef[1]
			z = ecef[2]
			b = math.sqrt( asq * (1-esq) )
			bsq = math.pow(b,2)
			ep = math.sqrt( (asq - bsq)/bsq)
			p = math.sqrt( math.pow(x,2) + math.pow(y,2) )
			th = math.atan2(a*z, b*p)
			lon = math.atan2(y,x)
			lat = math.atan2( (z + math.pow(ep,2)*b*math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) )
			N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) )
			alt = p / math.cos(lat) - N
			#mod lat to keep it between 0 and 2 pi
			lon = lon % (2*math.pi)
			#changing radians to degrees
			lat = math.degrees(lat)
			lon = math.degrees(lon)
			#normalizing angle
			lat = normalizeAngle(lat)
			lon = normalizeAngle(lon)
			ret = (lat, lon)
			return ret;
Example #7
0
def ene2mots(energy, mat=None, hkl=None, r=None, alpha=None, pp=False):
    """calculates the real positions of the motors for a given energy (kev)

    Returns a list with positions: [atheh1, axeh1, dtheh1, dxeh1, dyeh1]
    """
    if mat is None:
        mat = CRYST_MAT
    if hkl is None:
        hkl = CRYST_HKL
    if r is None:
        r = CRYST_R
    if alpha is None:
        alpha = CRYST_ALPHA
    rthetab = theta_b(energy, get_dspacing(mat, hkl))
    ralpha = math.radians(alpha)
    p0 = r * math.sin(rthetab + ralpha)
    q0 = r * math.sin(rthetab - ralpha)

    atheh1 = math.degrees(rthetab)
    axeh1 = p0
    dtheh1 = 2 * math.degrees(rthetab)
    dxeh1 = p0 + q0 * math.cos(2 * rthetab)
    dyeh1 = q0 * math.sin(2 * rthetab)

    _mot_list = [atheh1, axeh1, dtheh1, dxeh1, dyeh1]

    if pp:
        # pretty print (= for humans)
        _tmpl_head = "MOT: {0:=^10} {1:=^10} {2:=^10} {3:=^10} {4:=^10}"
        _tmpl_data = "POS: {0:^ 10.4f} {1:^ 10.4f} {2:^ 10.4f} {3:^ 10.4f} {4:^ 10.4f}"
        print(_tmpl_head.format('ath', 'ax', 'dth', 'dx', 'dy'))
        print(_tmpl_data.format(*_mot_list))
    else:
        return _mot_list
Example #8
0
   def wmplugin_exec(self, m):
	
	axes = [None, None, None, None]


	self.acc = [self.NEW_AMOUNT*(new-zero)/(one-zero) + self.OLD_AMOUNT*old
	       for old,new,zero,one in zip(self.acc,m,self.acc_zero,self.acc_one)]
	a = math.sqrt(sum(map(lambda x: x**2, self.acc)))

	roll = math.atan(self.acc[cwiid.X]/self.acc[cwiid.Z])
	if self.acc[cwiid.Z] <= 0:
		if self.acc[cwiid.X] > 0: roll += math.pi
		else: roll -= math.pi

	pitch = math.atan(self.acc[cwiid.Y]/self.acc[cwiid.Z]*math.cos(roll))

	axes[0] = int(roll  * 1000 * self.Roll_Scale)
	axes[1] = int(pitch * 1000 * self.Pitch_Scale)

	if (a > 0.85) and (a < 1.15):
		if (math.fabs(roll)*(180/math.pi) > 10) and \
		   (math.fabs(pitch)*(180/math.pi) < 80):
			axes[2] = int(roll * 5 * self.X_Scale)

		if (math.fabs(pitch)*(180/math.pi) > 10):
			axes[3] = int(pitch * 10 * self.Y_Scale)

	return math.degrees(pitch), math.degrees(roll)
Example #9
0
    def getValues(self):        
        accx = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_X_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_X_LSB))
        accy = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Y_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Y_LSB))
        accz = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Z_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Z_LSB))

        gyrox = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_X_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_X_LSB))
        gyroy = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_LSB))
        gyroz = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_LSB))

        rate_gyrox = gyrox*self.GYRO_ADD
        rate_gyroy = gyroy*self.GYRO_ADD
        rate_gyroz = gyroz*self.GYRO_ADD

        if (not self.FIRST):
            self.FIRST = True
            self.gyroXangle = rate_gyrox*self.DT
            self.gyroYangle = rate_gyroy*self.DT
            self.gyroZangle = rate_gyroz*self.DT
        else:
            self.gyroXangle += rate_gyrox*self.DT
            self.gyroYangle += rate_gyroy*self.DT
            self.gyroZangle += rate_gyroz*self.DT

        roll = int(round(math.degrees(math.atan2(accx, accz))))
        pitch = int(round(math.degrees(math.atan2(accy, accz))))

        print "Przechylenie: ", int(round(roll,0)), " Pochylenie: ", int(round(pitch,0))

        self.FILTR_X = self.MULTIPLY*(roll)+(1-self.MULTIPLY)*self.gyroXangle
        self.FILTR_Y = self.MULTIPLY*(pitch)+(1-self.MULTIPLY)*self.gyroYangle

        print "Filtr przechylenie: ", int(round(self.FILTR_X,0)), " Filtr pochylenie: ", int(round(self.FILTR_Y,0))

        return str(roll)+';'+str(pitch)
Example #10
0
    def to_geographic(self, x, y):

        # Retrieve the locals.
        A, E, PI4, PI2, P0, M0, X0, Y0, P1, P2, m1, m2, t1, t2, t0, n, F, rho0 = self._locals

        # Subtract the false northing/easting.
        x = x - X0
        y = y - Y0

        # Calculate the Longitude.
        lon = math.atan2(x, rho0 - y) / n + M0

        # Estimate the Latitude.
        rho = math.sqrt(math.pow(x, 2.0) + math.pow(rho0 - y, 2.0))
        t = math.pow(rho / (A * F), 1.0 / n)
        lat = PI2 - (2.0 * math.atan2(t, 1.0))

        # Substitute the estimate into the iterative calculation
        # that converges on the correct Latitude value.
        while True:
            lat1 = lat
            es = E * math.sin(lat1)
            lat = PI2 - 2.0 * math.atan2(t * math.pow((1.0 - es) / (1.0 + es), E / 2.0), 1.0)
            if math.fabs(lat - lat1) < 2.0e-9:
                break

        # Return lat/lon in degrees.
        return math.degrees(lat), math.degrees(lon)
Example #11
0
    def _calculate_position(self, radius, icon_size, index, children_count,
                            width, height, sin=math.sin, cos=math.cos):
        # tweak cos and sin in order to make the 'ring' into an equilateral
        # triangle.

        def cos_d(d):
            while d < -90:
                d += 360
            if d <= 30:
                return (d + 90) / 120.
            if d <= 90:
                return (90 - d) / 60.
            # mirror around 90
            return -cos_d(180 - d)

        sqrt_3 = math.sqrt(3)

        def sin_d(d):
            while d < -90:
                d += 360
            if d <= 30:
                return ((d + 90) / 120.) * sqrt_3 - 1
            if d <= 90:
                return sqrt_3 - 1
            # mirror around 90
            return sin_d(180 - d)

        cos = lambda r: cos_d(math.degrees(r))
        sin = lambda r: sin_d(math.degrees(r))

        return RingLayout._calculate_position(self, radius, icon_size, index,
                                              children_count, width, height,
                                              sin=sin, cos=cos)
Example #12
0
def toKepler(u, which = 'Pueyo', mass = 1, referenceTime = None):
    """
    """
    if which == 'Pueyo':
        res = np.zeros(6)
        res[1] = u[1]
        res[5] = u[5]
        
        res[0] = semimajoraxis(math.exp(u[0]), starMass = mass)
        res[2] = math.degrees(math.acos(u[2]))
        res[3] = np.mod((u[3]-u[4])*0.5,360)
        res[4] = np.mod((u[3]+u[4])*0.5,360)
        return res
    elif which == 'alternative':
        res = np.zeros(6)
        res[1] = u[1]
        res[5] = u[5]
        
        res[0] = semimajoraxis(math.exp(u[0]), starMass = mass)
        res[2] = math.degrees(math.acos(u[2]))
        res[3] = u[3]
        res[4] = u[4]
        return res        
    elif which == 'Chauvin':
        stat = StatisticsMCMC()
        res = stat.xFROMu(u,referenceTime,mass)    
        return res
    
    return None
Example #13
0
 def gazePix(self,anglex,angley):
     """Converts gaze angle to monitor pixel"""
     alphax = math.degrees(math.atan(self.ledx/self.monitordistance))
     pixx = self.deg2pix(anglex-alphax)
     alphay = math.degrees(math.atan(self.ledy/self.monitordistance))
     pixy = self.deg2pix(angley-alphay)
     return pixx,pixy
Example #14
0
def plot_planets(ax, plot, date, hill_sphere=False):
#     # only add the planets that would actually fall in this plot
#     plot_polygon = Polygon.Polygon(((plot[0],plot[2]),
#                                    (plot[0],plot[3]),
#                                    (plot[1],plot[3]),
#                                    (plot[0],plot[3]),
#                                    (plot[0],plot[2])))
# #    print plot_polygon
    mass = {"Sun":1.989*10**30, "Mars":639*10**21, "Jupiter":1.898*10**27, "Saturn":568.3*10**24, "Uranus":86.81*10**24, "Neptune":102.4*10**24}  # kg
    for planet in [ephem.Mars(), ephem.Jupiter(), ephem.Saturn(), ephem.Uranus(), ephem.Neptune()]:
        planet.compute(ephem.date(date))
        pos = (math.degrees(planet.ra), math.degrees(planet.dec))
#        if plot_polygon.isInside(math.degrees(planet.ra), math.degrees(planet.dec)):
        ax.scatter(pos[0], pos[1],
                 marker='o',
                 s=30,
                 facecolor='#E47833',
                 edgecolor='#E47833')
        ax.annotate(planet.name, (pos[0]+.4, pos[1]+0.15)) #(pos[0]+.9, pos[1]+0.5))  # offset to make it readable

        if hill_sphere:
            print planet.name, planet.sun_distance, mass[planet.name],
            hs_radius = (planet.sun_distance*ephem.meters_per_au)*((mass[planet.name]/3*mass['Sun'])**(1/3.))
            angular_size = planet.earth_distance*hs_radius  # FIXME
            print 'Hill sphere', hs_radius, hs_radius/ephem.meters_per_au, angular_size
            ax.add_patch(plt.Circle(pos, radius=angular_size, fill=False))

    return ax
Example #15
0
def draw(x,y,count,isGrant,val,acceptCount):
	List_Of_Turtles = list()
	p = list()
	screen = turtle.getscreen()
	screen.setup( width = 2000, height = 2000, startx = None, starty = None) 
	for i in range(count):
	    screen.tracer(10)
	    List_Of_Turtles.append(turtle.Turtle())
	    #print "here",i,acceptCount,count
	    List_Of_Turtles[i].color("red")
	    if i >= acceptCount-1 and acceptCount!= 0:
		List_Of_Turtles[i].shape("square")
	    	List_Of_Turtles[i].color("green")
	    List_Of_Turtles[i].speed(1)
	    List_Of_Turtles[i].width(4)
	    angle = math.atan((y[i]-x[i])/300.0)
	    p.append(math.sqrt(90000+(x[i]-y[i])*(x[i]-y[i])))
	    if val == 200:
		Position_Set(List_Of_Turtles[i],val-275,200-x[i])
	    else:
		Position_Set(List_Of_Turtles[i],val,200-x[i])
	    if isGrant == 1:
		    List_Of_Turtles[i].right(180-math.degrees(angle)+90)
	    else:
		    List_Of_Turtles[i].right(math.degrees(angle)+90)
	    screen.update()
    
	for i in xrange(100):
		j=0
		for t in List_Of_Turtles:
			t.down()
			if (i*(i+1))/2 < p[j]:
				t.forward(i)
			j=j+1
		screen.update()
Example #16
0
def uproj_tmerc(x, y):
    easting = x - x0
    northing = y
    # Meridional Arc
    M = northing / k0
    mu = M / (a * (1.0 - e * e / 4.0 - 3.0 * e * e * e * e / 64.0 -
        5.0 * e * e * e * e * e * e / 256.0))

    # Footprint latitude
    fp = mu + J1 * math.sin(2.0 * mu) + J2 * math.sin(4.0 * mu) + \
            J3 * math.sin(6.0 * mu) + J4 * math.sin(8.0 * mu)
    C1 = ep2 * math.cos(fp) * math.cos(fp)
    T1 = math.tan(fp) * math.tan(fp)
    # Radius of curvature in meridian plane
    R1 = a * (1.0 - e * e) / \
            math.pow(1.0 - e * e * math.sin(fp) * math.sin(fp), 1.5)
    # Radius of curvature perpendicular to meridian plane
    N1 = a / math.sqrt(1.0 - e * e * math.sin(fp) * math.sin(fp))
    D = easting / (N1 * k0)

    Q1 = N1 * math.tan(fp) / R1
    Q2 = D * D / 2.0
    Q3 = (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - 9.0 * ep2) * \
            D * D * D * D / 24.0
    Q4 = (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - 3.0 * C1 * C1 -
            252.0 * ep2) * D * D * D * D * D * D / 720.0
    Q5 = D
    Q6 = (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0
    Q7 = (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + 8.0 * ep2 +
            24.0 * T1 * T1) * D * D * D * D * D / 120.0

    lat = fp - Q1 * (Q2 - Q3 + Q4)
    lon = math.radians(long0) + (Q5 - Q6 + Q7) / math.cos(fp)

    return ( math.degrees(lat), math.degrees(lon) )
def nearby(lat,lng,radius):
    radius = float(radius) / 1000
    lat = float(lat)
    lng = float(lng)

    maxLat = lat + degrees(radius/EARTH_RADIUS)
    minLat = lat - degrees(radius/EARTH_RADIUS)

    maxLng = lng + degrees(radius/EARTH_RADIUS/cos(radians(lat)))
    minLng = lng - degrees(radius/EARTH_RADIUS/cos(radians(lat)))

    lat = radians(lat)
    lng = radians(lng)


    rawSet = Spot.objects.raw('Select id, name, lat, lng, \
            acos(sin(%s)*sin(radians(lat)) + cos(%s)*cos(radians(lat))*cos(radians(lng)-%s)) * %s As D \
        From ( \
            Select id, name, lat, lng \
            From spots_spot \
            Where lat Between %s And %s \
              And lng Between %s And %s \
        ) As FirstCut \
        Where acos(sin(%s)*sin(radians(lat)) + cos(%s)*cos(radians(lat))*cos(radians(lng)-%s)) * %s < %s \
        Order by D', [lat,lat,lng,EARTH_RADIUS,minLat,maxLat,minLng,maxLng,lat,lat,lng,EARTH_RADIUS,radius])
    return rawSet
Example #18
0
    def goToObject(self, first, second, *args ):
        
        if cmds.nodeType( first ) == 'joint':
            
            jo = cmds.getAttr( first+'.jo' )[0]
            mpxTransform = mpx.MPxTransformationMatrix()
            rotVector = om.MVector( math.radians( jo[0] ), math.radians( jo[1] ), math.radians( jo[2] ) ) 
            mpxTransform.rotateTo( om.MEulerRotation( rotVector ) )
            joMtx = mpxTransform.asMatrix()
            
            fMtx = om.MMatrix()
            fPMtx = om.MMatrix()
            fMtxList = cmds.getAttr( first+'.wm' )
            fPMtxList = cmds.getAttr( first+'.pm' )
            sMtx = om.MMatrix()
            sMtxList = cmds.getAttr( second+'.wm' )
            
            om.MScriptUtil.createMatrixFromList( fMtxList, fMtx )
            om.MScriptUtil.createMatrixFromList( fPMtxList, fPMtx )
            om.MScriptUtil.createMatrixFromList( sMtxList, sMtx )
            
            sMtxPose = [ sMtx(3,0), sMtx(3,1), sMtx(3,2) ]
            
            rMtx = sMtx*(joMtx*fPMtx).inverse()
            rTransform = mpx.MPxTransformationMatrix( rMtx )
            rVector = rTransform.eulerRotation().asVector()
            
            rot = [ math.degrees( rVector.x ), math.degrees( rVector.y ), math.degrees( rVector.z ) ]

            cmds.setAttr( first+'.r', *rot )
            cmds.move( sMtxPose[0], sMtxPose[1], sMtxPose[2], first, ws=1 )

        else:
            rigbase.goToSamePosition( first, second )
Example #19
0
    def rotateSprite(self, angle, checkcoll = False): 
#            self.facing = math.degrees(-angle) - 90
#            center = self.rect.center
#            rotate = pygame.transform.rotate
#            self.image = rotate(self.originalImage, self.facing)
#            self.rect = self.image.get_rect(center=center)
        if checkcoll is False:
            self.facing = math.degrees(-angle) - 90
            center = self.rect.center
            rotate = pygame.transform.rotate
            self.image = rotate(self.originalImage, self.facing)
            self.rect = self.image.get_rect(center=center)
        #Rotate back if collide
        elif checkcoll is True:
            originalfacing = self.facing
            self.facing = math.degrees(-angle) - 90
            center = self.rect.center
            rotate = pygame.transform.rotate
            self.image = rotate(self.originalImage, self.facing)
            self.rect = self.image.get_rect(center=center)
            for b in BG:
                if self.rect.colliderect(b):
                    if pygame.sprite.collide_mask(self, b) != None:
                        self.facing = originalfacing
                        self.image = rotate(self.originalImage, originalfacing)
                        self.rect = self.image.get_rect(center=center)      
Example #20
0
def select_destination(origin='',
                       angle='',
                       radius='',
                       access_type='personal',
                       config_path='config/'):
    """
    Given a distance and polar angle, calculate the geocode of a destination point from the origin.
    """
    if origin == '':
        raise Exception('origin cannot be blank.')
    if angle == '':
        raise Exception('angle cannot be blank.')
    if radius == '':
        raise Exception('radius cannot be blank.')

    if isinstance(origin, str):
        origin_geocode = geocode_address(origin, access_type, config_path)
    elif isinstance(origin, list) and len(origin) == 2:
        origin_geocode = origin
    else:
        raise Exception('origin should be a list [lat, lng] or a string address.')

    # Find the location on a sphere a distance 'radius' along a bearing 'angle' from origin
    # This uses haversines rather than simple Pythagorean distance in Euclidean space
    #   because spheres are more complicated than planes.
    r = 3963.1676  # Radius of the Earth in miles
    bearing = radians(angle)  # Bearing in radians converted from angle in degrees
    lat1 = radians(origin_geocode[0])
    lng1 = radians(origin_geocode[1])
    lat2 = asin(sin(lat1) * cos(radius / r) + cos(lat1) * sin(radius / r) * cos(bearing))
    lng2 = lng1 + atan2(sin(bearing) * sin(radius / r) * cos(lat1), cos(radius / r) - sin(lat1) * sin(lat2))
    lat2 = degrees(lat2)
    lng2 = degrees(lng2)
    return [lat2, lng2]
    def trouverInfoRobot(self, contourGauche, contourDroit):
        centreGauche = self.trouverCentre(contourGauche)
        centreDroit = self.trouverCentre(contourDroit)
        centreRobot = (int(round((centreDroit[0]+centreGauche[0])/2)), int(round((centreDroit[1]+centreGauche[1])/2)))
        deltaX = centreDroit[0]-centreGauche[0]
        deltaY = -1*(centreDroit[1]-centreGauche[1])
        if not deltaX == 0:
            pente = deltaY/deltaX

        if deltaY == 0 and deltaX < 0:
            angle = 180
        elif deltaY == 0 and deltaX > 0:
            angle = 0
        elif deltaX == 0 and deltaY > 0:
            angle = 90
        elif deltaX == 0 and deltaY < 0:
            angle = 270
        elif deltaX > 0 and deltaY > 0:
            angle = int(round(math.degrees(math.atan(pente))))
        elif deltaX > 0 and deltaY < 0:
            angle = 360 + int(round(math.degrees(math.atan(pente))))
        elif deltaX < 0:
            angle = 180 + int(round(math.degrees(math.atan(pente))))

        angle += 90
        if angle >= 360:
            angle -= 360

        return centreRobot, angle
Example #22
0
  def pitch_roll(self, px, pz):
    """works out the pitch (rx) and roll (rz) to apply to an object
    on the surface of the map at this point

    * returns a tuple (pitch, roll) in degrees

    Arguments:
      *px*
        x location
      *pz*
        z location
    """
    px -= self.unif[0]
    pz -= self.unif[2]
    halfw = self.width/2.0
    halfd = self.depth/2.0
    dx = self.width/self.ix
    dz = self.depth/self.iy
    x0 = int(math.floor((halfw + px)/dx + 0.5))
    if x0 < 0: x0 = 0
    if x0 > self.ix-1: x0 = self.ix-1
    z0 = int(math.floor((halfd + pz)/dz + 0.5))
    if z0 < 0: z0 = 0
    if z0 > self.iy-1: z0 = self.iy-1
    normp = array(self.buf[0].array_buffer[z0*self.ix + x0,3:6])
    # slight simplification to working out cross products as dirctn always 0,0,1
    #sidev = cross(normp, dirctn)
    sidev = array([normp[1], -normp[0], 0.0])
    sidev = sidev / sqrt(sidev.dot(sidev))
    #forwd = cross(sidev, normp)
    forwd = array([-normp[2]*normp[0], -normp[2]*normp[1],
                  normp[0]*normp[0] + normp[1]*normp[1]])
    forwd = forwd / sqrt(forwd.dot(forwd))
    return (degrees(asin(-forwd[1])), degrees(atan2(sidev[1], normp[1])))
Example #23
0
    def move_angle(self, ang, angvel=None, blocking=True):
        ''' move to angle (radians)
        '''
        if angvel == None:
            angvel = self.settings['max_speed']

        if angvel > self.settings['max_speed']:
            print 'lib_robotis.move_angle: angvel too high - %.2f deg/s' % (math.degrees(angvel))
            print 'lib_robotis.ignoring move command.'
            return

        if ang > self.settings['max_ang'] or ang < self.settings['min_ang']:
            print 'lib_robotis.move_angle: angle out of range- ', math.degrees(ang)
            print 'lib_robotis.ignoring move command.'
            return
        
        self.set_angvel(angvel)

        if self.settings['flipped']:
            ang = ang * -1.0
        enc_tics = int(round( ang / self.settings['rad_per_enc'] ))
        enc_tics += self.settings['home_encoder']
        self.move_to_encoder( enc_tics )

        if blocking == True:
            while(self.is_moving()):
                continue
Example #24
0
	def compute(self):
		self.e.compute(datetime.datetime.utcnow())
		self.long  = math.degrees(float(self.e.sublong))
		self.lat   = math.degrees(float(self.e.sublat))
		self.x     = (self.long * 128/45) + 512
		self.y     = (self.lat * 128/45) + 256 + self.yoffset
		self.label = pyglet.text.Label(self.e.name, x=7,y=0, anchor_y="center", color=(255,255,255,255))
Example #25
0
	def init_line(self):
		self.lines, self.vline_list, current_line = [], [], []
		for x in xrange(-total,total):
			temp = datetime.datetime.utcnow() + datetime.timedelta(seconds=interval*x)
			self.e.compute(temp)
			x = (math.degrees(float(self.e.sublong)) * 128/45) + 512
			y = (math.degrees(float(self.e.sublat)) * 128/45) + 256 + self.yoffset
			if len(current_line) > 1:
				# TO AVOID LINE FROM LEFT TO RIGHT
				temp_x, temp_y = current_line[-2], current_line[-1]
				if temp_x - x > 600:
					# From right edge to left edge
					current_line.extend((x+1024,y))
					self.lines.append(current_line)
					current_line = []
					current_line.extend((temp_x-1024,temp_y))
				elif temp_x - x < -600:
					# From left edge to right edge
					current_line.extend((x-1024,y))
					self.lines.append(current_line)
					current_line = []
					current_line.extend((temp_x+1024,temp_y))
			current_line.extend((x,y))
		self.lines.append(current_line)
		for x in self.lines:
			self.vline_list.append(pyglet.graphics.vertex_list(len(x)/2, ("v2f", x)))
Example #26
0
 def initialize(self):
     pos_std = self.parameter("pos_std", default=0.05)
     if isinstance(pos_std, dict):
         self._pos_std_dev = pos_std
     else:
         self._pos_std_dev = {'x': float(pos_std), 'y': float(pos_std), 'z': float(pos_std)}
     rot_std = self.parameter("rot_std", default=radians(5))
     if isinstance(rot_std, dict):
         self._rot_std_dev = rot_std
     else:
         self._rot_std_dev = {'roll': float(rot_std), 'pitch': float(rot_std), 'yaw': float(rot_std)}
     self._2D = bool(self.parameter("_2D", default=False))
     if self._2D:
         logger.info("Noise modifier standard deviations: x:%.4f, y:%.4f, yaw:%.3f deg",
                     self._pos_std_dev.get('x', 0),
                     self._pos_std_dev.get('y', 0),
                     degrees(self._rot_std_dev.get('yaw', 0)))
     else:
         logger.info("Noise modifier standard deviations: x:%.4f, y:%.4f, z:%.4f, "
                     "roll:%.3f deg, pitch:%.3f deg, yaw:%.3f deg",
                     self._pos_std_dev.get('x', 0),
                     self._pos_std_dev.get('y', 0),
                     self._pos_std_dev.get('z', 0),
                     degrees(self._rot_std_dev.get('roll', 0)),
                     degrees(self._rot_std_dev.get('pitch', 0)),
                     degrees(self._rot_std_dev.get('yaw', 0)))
Example #27
0
    def connectionRound(self, first, last, pen, close):
        angle_1 = radians(degrees(self.prevAngle)+90)
        angle_2 = radians(degrees(self.currentAngle)+90)

        tempFirst = first - self.pointClass(cos(angle_1), sin(angle_1)) * self.miterLimit
        tempLast = last + self.pointClass(cos(angle_2), sin(angle_2)) * self.miterLimit

        newPoint = interSect((first, tempFirst), (last, tempLast))
        if newPoint is None:
            pen.lineTo(last)
            return
        #print "(%s, %s)," % (newPoint.x, newPoint.y)
        distance1 = newPoint.distance(first)
        distance2 = newPoint.distance(last)
        #print distance1, distance2
        if roundFloat(distance1) > self.miterLimit + self.contrast:
            distance1 = self.miterLimit + tempFirst.distance(tempLast) * .7
        if roundFloat(distance2) > self.miterLimit + self.contrast:
            distance2 = self.miterLimit + tempFirst.distance(tempLast) * .7

        distance1 *= self.magicCurve
        distance2 *= self.magicCurve

        bcp1 = first - self.pointClass(cos(angle_1), sin(angle_1)) * distance1
        bcp2 = last + self.pointClass(cos(angle_2), sin(angle_2)) * distance2
        pen.curveTo(bcp1, bcp2, last)
Example #28
0
def generate_wind(nodes, cells, prevailing_wind_bearing=270, average_wind_speed=7.0):
    import noise
    import math
    import numpy as np
    for n in nodes:
        wind_bearing = (int(round(180.0 * noise.pnoise3(n.x, n.y, n.z))) + 360) % 360  # in degrees
        wind_strength = (8 * noise.pnoise3(n.x, n.y, n.z)) + 16.0  # kmph

        # move the generated random wind bearing towards the prevailing wind a little
        wind_vector = np.add(np.array([wind_strength * math.cos(math.radians(90 - wind_bearing)),
                                       wind_strength * math.cos(math.radians(wind_bearing))]),
                             np.array([average_wind_speed * math.cos(math.radians(90 - prevailing_wind_bearing)),
                                       average_wind_speed * math.cos(math.radians(prevailing_wind_bearing))]))

        wind_strength = math.sqrt(math.pow(wind_vector[0], 2) + math.pow(wind_vector[1], 2)) #  sqrt((x2-x1)^2 + (y2-y1)^2) = vector magnitude
        wind_bearing = int(round(math.degrees(math.atan(wind_vector[1]/wind_vector[0])))) # tan-1((y2-y1)/x2-x1)) = vector bearing

        n.set_feature(['wind', 'bearing'], wind_bearing)
        n.set_feature(['wind', 'strength'], wind_strength)
        n.set_feature(['wind', 'vector', 'x'], wind_vector[0])
        n.set_feature(['wind', 'vector', 'y'], wind_vector[1])
        n.wind = Wind(wind_bearing, wind_strength, wind_vector)
    for c in cells:
        wind_vector = np.sum(np.array([n.wind.vector for n in c.nodes]), axis=0)
        wind_strength = math.sqrt(math.pow(wind_vector[0], 2) + math.pow(wind_vector[1], 2))  #  sqrt((x2-x1)^2 + (y2-y1)^2) = vector magnitude
        wind_bearing = int(round(math.degrees(math.atan(wind_vector[1]/wind_vector[0]))))  # tan-1((y2-y1)/x2-x1)) = vector bearing

        c.wind = Wind(wind_bearing, wind_strength, wind_vector)
Example #29
0
def get_location(tle, now=None, lat=None, lng=None):
    """Compute the current location of the ISS"""
    now = now or datetime.datetime.utcnow()
    lat = lat or 37.7701
    lng = lng or -122.4664

    satellite = ephem.readtle(str(tle[0]), str(tle[1]), str(tle[2]))

    # Compute for current location
    observer = ephem.Observer()
    observer.lat = lat
    observer.lon = lng
    observer.elevation = 0
    observer.date = now
    satellite.compute(observer)
    lon = degrees(satellite.sublong)
    lat = degrees(satellite.sublat)

    # Return the relevant timestamp and data
    data = {'position': {'latitude': lat,
                         'longitude': lon},
            'visible': float(repr(satellite.alt)) > 0 and float(repr(satellite.alt)) < math.pi,
            'altitude': satellite.alt,
            'azimuth': satellite.az,
            'range': satellite.range,
            'velocity': satellite.range_velocity,
            'name': satellite.name}
    return data
Example #30
0
def followCentre(data,desired_trajectory):
	global alpha

	a = getRange(data,130)
	b = getRange(data,179.9)
	swing = math.radians(50)
	print "center distances: ", a, b
	alpha = -math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
	print "Alpha left",math.degrees(alpha)
	curr_dist1 = b*math.cos(alpha)
	future_dist1 = curr_dist1-car_length*math.sin(alpha)



	a = getRange(data,50)
	b = getRange(data,0)
	swing = math.radians(50)
	alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
	print "Alpha right",math.degrees(alpha)
	curr_dist2 = b*math.cos(alpha)

	future_dist2 = curr_dist2+car_length*math.sin(alpha)

	desired_trajectory = (future_dist1 + future_dist2)/2

	print "dist 1 : ",future_dist1
	print "dist 2 : ",future_dist2
	# print "dist : ",future_dist
	error = future_dist1 - future_dist2
	print "Error : ",error
	return error, curr_dist2-curr_dist1
Example #31
0
 def sim_location(self):
     """Return current simulator location."""
     m = self.mav.recv_match(type='SIMSTATE', blocking=True)
     return mavutil.location(m.lat * 1.0e-7, m.lng * 1.0e-7, 0,
                             math.degrees(m.yaw))
Example #32
0
def DangerDanger(dmd,rci,rce):
    """
    La función sirve alerta al sujeto de si existe un objeto qu ele obstruya
    
    @inputs md - es matriz de los datos a usar para conocer la cercania de algun objeto
            rci - radio del circulo interno
            rce - radio del circulo externo
    
    @outputs I - intensidad de vibración
             NV - numéro de motor    
    """
    
    # lmd = len(md)
    I = 0
    NM = 0    
    # for nnn in range(0,lmd):
    dx = dmd[0]
    dy = dmd[1]
    dz = dmd[2]
    dd = np.sqrt(((dx)**2)+((dy)**2)+((dz)**2))
    # angulo = -1
    
    die = rce-rci
    
    if (dd <= rce):
        # Decisiones dentro del Radio externo
        angulo = math.atan2(dy,dx)
        angulo = math.degrees(angulo)
        # Angls.append(angulo)
        
        if ((dd > (die+rci))and(dd <= rce)):
            I = 70
        elif ((dd > rci)and(dd <= (rci+die))):
            I = 50
                    
        if ((angulo >= 150)and(angulo <= 180)):
            NM = 5          
        if ((angulo <= 30)and(angulo >= 0)):
            NM = 1
        if ((angulo > 30)and(angulo < 80)):
            NM = 2
        if ((angulo > 100)and(angulo < 150)):
            NM = 4
        if ((angulo >= 80)and(angulo <= 100)):
            NM = 3
            
    if (dd <= rci):
        # Decisiones dentro del radio intero
        angulo = math.atan2(dy,dx)
        angulo = math.degrees(angulo)
        # Angls.append(angulo)
        if (dd <= rci/2):
            I = 100
        elif ((dd > rci/2)and(dd <= rci)):
            I = 80
        
        # if (angulo >= 120):
        #     print("obstaculo a la izquierda")
            
        # if (angulo <= 60):
        #     print("obstaculo a la derecha")
        # if ((angulo > 60)and(angulo < 120)):
        #     print("Obstaculo enfrente")
        
        if ((angulo >= 150)and(angulo <= 180)):
            NM = 5              
        if ((angulo <= 30)and(angulo >= 0)):
            NM = 1
        if ((angulo > 30)and(angulo < 80)):
            NM = 2
        if ((angulo > 100)and(angulo < 150)):
            NM = 4
        if ((angulo >= 80)and(angulo <= 100)):
            NM = 3

            
    return(I,NM)
Example #33
0
    def invoke(self, context, event):
        self.current_area = context.area
        prefs = get_addon_prefs()
        self.hud = prefs.canvas_use_hud
        self.use_view_center = prefs.canvas_use_view_center
        self.angle = 0.0
        self.in_cam = context.region_data.view_perspective == 'CAMERA'

        ## store ratio for view rotate correction

        # CORRECT UI OVERLAP FROM HEADER TOOLBAR
        regs = context.area.regions
        if context.preferences.system.use_region_overlap:
            w = context.area.width
            # minus tool header
            h = context.area.height - regs[0].height
        else:
            # minus tool leftbar + sidebar right
            w = context.area.width - regs[2].width - regs[3].width
            # minus tool header + header
            h = context.area.height - regs[0].height - regs[1].height

        self.ratio = h / w
        self.ratio_inv = w / h

        if self.in_cam:
            # Get camera from scene
            self.cam = bpy.context.scene.camera

            #return if one element is locked (else bypass location)
            if self.cam.lock_rotation[:] != (False, False, False):
                self.report({'WARNING'}, 'Camera rotation is locked')
                return {'CANCELLED'}

            if self.use_view_center:
                self.center = mathutils.Vector((w / 2, h / 2))
            else:
                self.center = self.get_center_view(context, self.cam)

            # store original rotation mode
            self.org_rotation_mode = self.cam.rotation_mode
            # set to euler to works with quaternions, restored at finish
            self.cam.rotation_mode = 'XYZ'
            # store camera matrix world
            self.cam_matrix = self.cam.matrix_world.copy()
            # self.cam_init_euler = self.cam.rotation_euler.copy()

            ## initialize current view_offset in camera
            self.view_cam_offset = mathutils.Vector(
                context.space_data.region_3d.view_camera_offset)

        else:
            self.center = mathutils.Vector((w / 2, h / 2))
            # self.center = mathutils.Vector((context.area.width/2, context.area.height/2))

            # store current rotation
            self._rotation = context.space_data.region_3d.view_rotation.copy()

        # Get current mouse coordination
        self.pos_current = mathutils.Vector(
            (event.mouse_region_x, event.mouse_region_y))

        self.initial_pos = self.pos_current  # for draw debug, else no need
        # Calculate initial vector
        self.vector_initial = self.pos_current - self.center
        self.vector_initial.normalize()

        # Initializes the current vector with the same initial vector.
        self.vector_current = self.vector_initial.copy()

        #Snap keys
        self.snap_ctrl = not prefs.use_ctrl
        self.snap_shift = not prefs.use_shift
        self.snap_alt = not prefs.use_alt
        # round to closer degree and convert back to radians
        self.snap_step = math.radians(round(math.degrees(prefs.rc_angle_step)))

        self.timer = time()
        args = (self, context)
        if self.hud:
            self._handle = bpy.types.SpaceView3D.draw_handler_add(
                draw_callback_px, args, 'WINDOW', 'POST_PIXEL')
        context.window_manager.modal_handler_add(self)
        return {'RUNNING_MODAL'}
Example #34
0
    def modal(self, context, event):
        if event.type in {'MOUSEMOVE'}:  #,'INBETWEEN_MOUSEMOVE'
            # Get current mouse coordination (region)
            self.pos_current = mathutils.Vector(
                (event.mouse_region_x, event.mouse_region_y))
            # Get current vector
            self.vector_current = (self.pos_current - self.center).normalized()
            # Calculates the angle between initial and current vectors
            self.angle = self.vector_initial.angle_signed(
                self.vector_current)  #radian
            # print (math.degrees(self.angle), self.vector_initial, self.vector_current)

            ## handle snap key
            snap = False
            if self.snap_ctrl and event.ctrl:
                snap = True
            if self.snap_shift and event.shift:
                snap = True
            if self.snap_alt and event.alt:
                snap = True
            ## Snapping to specific degrees angle
            if snap:
                self.angle = step_value(self.angle, self.snap_step)

            if self.in_cam:
                self.cam.matrix_world = self.cam_matrix
                self.cam.rotation_euler.rotate_axis("Z", self.angle)

                if self.use_view_center:
                    ## apply inverse rotation on view offset
                    self.set_cam_view_offset_from_angle(context, self.angle)

            else:  # free view
                context.space_data.region_3d.view_rotation = self._rotation
                rot = context.space_data.region_3d.view_rotation
                rot = rot.to_euler()
                rot.rotate_axis("Z", self.angle)
                context.space_data.region_3d.view_rotation = rot.to_quaternion(
                )

        if event.type in {'RIGHTMOUSE', 'LEFTMOUSE', 'MIDDLEMOUSE'
                          } and event.value == 'RELEASE':
            # Trigger reset : Less than 150ms and less than 2 degrees move
            if time() - self.timer < 0.15 and abs(math.degrees(
                    self.angle)) < 2:
                # self.report({'INFO'}, 'Reset')
                aim = context.space_data.region_3d.view_rotation @ mathutils.Vector(
                    (0.0, 0.0, 1.0))  # view vector
                z_up_quat = aim.to_track_quat('Z', 'Y')  # track Z, up Y
                if self.in_cam:

                    q = self.cam.matrix_world.to_quaternion(
                    )  # store current rotation

                    if self.cam.parent:
                        q = self.cam.parent.matrix_world.inverted(
                        ).to_quaternion() @ q
                        cam_quat = self.cam.parent.matrix_world.inverted(
                        ).to_quaternion() @ z_up_quat
                    else:
                        cam_quat = z_up_quat
                    self.cam.rotation_euler = cam_quat.to_euler('XYZ')

                    # get diff angle (might be better way to get view axis rot diff)
                    diff_angle = q.rotation_difference(cam_quat).to_euler(
                        'ZXY').z
                    # print('diff_angle: ', math.degrees(diff_angle))
                    self.set_cam_view_offset_from_angle(context, diff_angle)

                else:
                    context.space_data.region_3d.view_rotation = z_up_quat
            self.execute(context)
            return {'FINISHED'}

        if event.type == 'ESC':  #Cancel
            self.execute(context)
            if self.in_cam:
                self.cam.matrix_world = self.cam_matrix
                context.space_data.region_3d.view_camera_offset = self.view_cam_offset
            else:
                context.space_data.region_3d.view_rotation = self._rotation
            return {'CANCELLED'}

        return {'RUNNING_MODAL'}
        def __to_degrees(rad: float) -> tuple:
            degrees = math.degrees(rad)
            minutes = (degrees % 1) * 60
            seconds = (minutes % 1) * 60

            return degrees - minutes, minutes - seconds, seconds
Example #36
0
        xmin = xvalues[item]
    if yvalues[item] < ymin:
        ymin = yvalues[item]
print(xmin, ymin, xmax, ymax)
xaverage = sumx / n
yaverage = sumy / n
slope = (sumxy - n * xaverage * yaverage) / (sumxsquared - n * xaverage**2)
#Draw regression line
wn = turtle.Screen()
wn.setworldcoordinates((xmin - 10), (ymin - 10), (xmax + 10), (ymax + 10))
t = turtle.Turtle()
t.penup()
t.setpos(xaverage, yaverage)  #put turtle at known point on line
t.pencolor("red")
t.pendown()
angular_slope = math.degrees(math.atan(slope))
print(angular_slope)
t.left(angular_slope)
t.forward(50)
t.left(180)
t.forward(100)
t.penup()
t.pencolor("black")
t.shape("circle")
for i in range(n):  #plot points
    t.goto(xvalues[i], yvalues[i])
    print(xvalues[i], yvalues[i], t.pos())
    t.pendown()
    t.stamp()
    t.penup()
    t.goto(0, 0)
def angleBetween(ve1, ve2):
    # Find angle between two vectors in degrees
    return math.degrees(ve1.getAngle(ve2))
def vectorDotAndTheta(dimensions, v0Info, v1Info, blank=False, thetaName='θ'):
    if (blank):
        result = None

        v0Name = v0Info
        v1Name = v1Info

        v0Elems = ', '.join([BLANK_D] * dimensions)
        v1Elems = ', '.join([BLANK_D] * dimensions)
        dotAdd = ' + '.join([BLANK_D + '×' + BLANK_D] * dimensions)
        dotProds = ' + '.join([BLANK_SD] * dimensions)
        dotSum = BLANK_SSD
        eNV0 = BLANK_FLT
        eNV1 = BLANK_FLT
        eNV0NV1 = BLANK_FLT
        eCos = BLANK_FLT2
        eTheta = BLANK_FLT
        eThetaDeg = BLANK_FLT

    else:
        (v0Name, v0, nv0) = v0Info
        (v1Name, v1, nv1) = v1Info

        rDotProds = [a * b for a, b in zip(v0, v1)]
        rDotSum = sum(rDotProds)

        # θ is the angle between v0 and v1
        cosTheta = rDotSum / (nv0 * nv1)  # cos θ ≡ v0·v1 / ( ‖v0‖*‖v1‖ )
        theta = math.acos(cosTheta)  # Get θ from cos θ.
        thetaDeg = math.degrees(theta)  # Get θ in degrees.

        v0Elems = ', '.join([EFF_D % e for e in v0])
        v1Elems = ', '.join([EFF_D % e for e in v1])
        dotAdd = ' + '.join([(EFF_D % a) + '×' + (EFF_D % b)
                             for a, b in zip(v0, v1)])
        dotProds = ' + '.join([EFF_SD % e for e in rDotProds])
        dotSum = EFF_SSD % rDotSum
        eNV0 = EFF_FLT % nv0
        eNV1 = EFF_FLT % nv1
        eNV0NV1 = EFF_FLT % (nv0 * nv1)
        eCos = EFF_FLT2 % cosTheta
        eTheta = EFF_FLT % theta
        eThetaDeg = EFF_FLT % thetaDeg

        result = (cosTheta)

    #----------
    PAD1 = ' ' * (2)
    PAD2 = ' ' * (len(v0Name) + len(v1Name) + 1 + 2)
    PAD3 = ' ' * (2)
    PAD4 = ' ' * (len(thetaName) + 4 + 2)
    PAD5 = ' ' * (len(thetaName) + (3 if len(thetaName) > 1 else 5))

    print('%s·%s%s≡ [ %s ]ᵀ · [ %s ]ᵀ' %
          (v0Name, v1Name, PAD1, v0Elems, v1Elems))
    print('%s= %s' % (PAD2, dotAdd))
    print('%s= %s = %s' % (PAD2, dotProds, dotSum))

    print(
        'cos %s%s≡ (%s·%s)/(‖%s‖×‖%s‖) = %s/(%s×%s)' %
        (thetaName, PAD3, v0Name, v1Name, v0Name, v1Name, dotSum, eNV0, eNV1))

    print('%s= %s/%s = %s' % (PAD4, dotSum, eNV0NV1, eCos))
    print('%s%s= arccos( %s ) = %s rad or %s°' %
          (thetaName, PAD5, eCos, eTheta, eThetaDeg))

    return result
Example #39
0
    # im = toimage(new_image)
    # im.save("slantised.png")
    verticalProjection(new_image)
    # Show the image
    showImage(thin_image, new_image)
    # cv2.imshow("afterslant", new_image)
    # cv2.waitKey(0)


imageName = sys.argv[1]
image = cv2.imread(imageName)  # Image path

# Make copy of the image so that original image is not lost
thin_image = image.copy()
row, col = thin_image.shape
print(row, col)
xi = findxi(thin_image, row, col)
xm = findxm(thin_image, row, col)
print(xi)
print(xm)
val = (1.5 * (xm + xi)) / (xm - xi)
print(val)
# math.degrees to convert radians to degrees
theta = math.degrees(math.atan(val))
print(theta)
minusTheta = math.degrees(math.atan(-val))
print(minusTheta)
verticalProjection(image)
removeSlant(image, theta, row, col)
removeSlant(image, minusTheta, row, col)
def crossProduct(v0Info, v1Info, blank=False, showVectors=False):
    (v0Name, v0) = v0Info
    (v1Name, v1) = v1Info

    if (blank):
        basisChars = ['_'] * 3
        basisNames = '  '.join(['_' * ELEM_DIGITS] * 3)

        # If this problem is being printed _without_ the vector problems
        # also, we have to print the vectors so the user knows them.
        # Otherwise the entire problem is blank.
        v0Str = [BLANK_D] * 3
        v1Str = [BLANK_D] * 3

        if (showVectors):
            v0Elems = ', '.join([EFF_D % e for e in v0])
            v1Elems = ', '.join([EFF_D % e for e in v1])

        else:
            v0Elems = ', '.join(v0Str)
            v1Elems = ', '.join(v1Str)

        M0ProdsStr = [BLANK_SD] * 2
        M1ProdsStr = [BLANK_SD] * 2
        M2ProdsStr = [BLANK_SD] * 2

        detValsStr = [BLANK_SSD] * 3
        crossProdStr = [BLANK_SSD] * 3
        crPrdSqStr = [BLANK_4SD] * 3
        nsqCrPrdStr = BLANK_4SD
        nCrPrdStr = BLANK_FLT

        nV0Str = BLANK_FLT
        nV1Str = BLANK_FLT
        #nV0nV1Str   = BLANK_FLT
        sinThetaStr = BLANK_FLT2
        thetaStr = BLANK_FLT
        thetaDegStr = BLANK_FLT

        result = None

    else:
        fmt = '^%ds' % (ELEM_DIGITS)
        basisChars = ['i', 'j', 'k']
        basisNames = '  '.join(format(b, fmt) for b in basisChars)

        v0Str = [EFF_D % e for e in v0]
        v1Str = [EFF_D % e for e in v1]

        v0Elems = ', '.join(v0Str)
        v1Elems = ', '.join(v1Str)

        nV0 = math.sqrt(sum([e * e for e in v0]))
        nV0Str = EFF_FLT % nV0
        nV1 = math.sqrt(sum([e * e for e in v1]))
        nV1Str = EFF_FLT % nV1

        nV0nV1 = nV0 * nV1
        #nV0nV1Str = EFF_FLT % nV0nV1

        M0Prods = [v0[1] * v1[2], v0[2] * v1[1]]
        M1Prods = [v0[0] * v1[2], v0[2] * v1[0]]
        M2Prods = [v0[0] * v1[1], v0[1] * v1[0]]

        detVals = [
            M0Prods[0] - M0Prods[1], M1Prods[0] - M1Prods[1],
            M2Prods[0] - M2Prods[1]
        ]
        crossProd = [detVals[0], -detVals[1], detVals[2]]
        crPrdSq = [e * e for e in crossProd]
        nsqCrPrd = sum(crPrdSq)
        nCrPrd = math.sqrt(nsqCrPrd)

        sinTheta = nCrPrd / nV0nV1

        # We can't get θ this easily because in the range [ 0..180 ],
        # sin θ is ambiguous, going from 0 → 1 → 0.  Therefore, an
        # angle θ cannot be distinguished from its supplement (180-θ).
        # We get an accurate θ here by using the dot product
        # relation θ = arccos( (v0·v1)/(‖v0‖×‖v1‖) ).
        #theta     = math.asin( sinTheta )

        dotProd = sum([a * b for a, b in zip(v0, v1)])
        cosTheta = dotProd / (nV0 * nV1)
        theta = math.acos(cosTheta)

        sinThetaStr = EFF_FLT2 % sinTheta
        thetaStr = EFF_FLT % theta
        thetaDegStr = EFF_FLT % (math.degrees(theta))

        M0ProdsStr = [EFF_SD % e for e in M0Prods]
        M1ProdsStr = [EFF_SD % e for e in M1Prods]
        M2ProdsStr = [EFF_SD % e for e in M2Prods]

        detValsStr = [EFF_SSD % e for e in detVals]
        crossProdStr = [EFF_SSD % e for e in crossProd]
        crPrdSqStr = [EFF_4SD % e for e in crPrdSq]
        nsqCrPrdStr = EFF_4SD % (nsqCrPrd)
        nCrPrdStr = EFF_FLT % (nCrPrd)

        result = (crossProd)

    v0MElems = '  '.join(v0Str)
    v1MElems = '  '.join(v1Str)

    M0Str = ['  '.join([v0Str[1], v0Str[2]]), '  '.join([v1Str[1], v1Str[2]])]
    M1Str = ['  '.join([v0Str[0], v0Str[2]]), '  '.join([v1Str[0], v1Str[2]])]
    M2Str = ['  '.join([v0Str[0], v0Str[1]]), '  '.join([v1Str[0], v1Str[1]])]

    #----------
    nameStr = '%s×%s' % (v0Name, v1Name)
    frontStrA = '%s = [ %s ]ᵀ×[ %s ]ᵀ' % (nameStr, v0Elems, v1Elems)
    frontStrB = '‖%s‖' % (nameStr)
    PAD1 = ' ' * (len(frontStrA) + 7)
    PAD2 = ' ' * (len(nameStr))
    PAD3 = ' ' * (len(nameStr) + 9)
    PAD4 = ' ' * (len(frontStrB))

    print('%s| %s |' % (PAD1, basisNames))
    print('%s ≡ det | %s |' % (frontStrA, v0MElems))
    print('%s| %s |' % (PAD1, v1MElems))

    print('%s| %s |         | %s |         | %s |' %
          (PAD3, M0Str[0], M1Str[0], M2Str[0]))
    print('%s = %s·det | %s | - %s·det | %s | + %s·det | %s |' %
          (PAD2, basisChars[0], M0Str[1], basisChars[1], M1Str[1],
           basisChars[2], M2Str[1]))

    print('%s = %s(%s·%s - %s·%s)-%s(%s·%s - %s·%s)+%s(%s·%s - %s·%s)' %
          (PAD2, basisChars[0], v0Str[1], v1Str[2], v0Str[2], v1Str[1],
           basisChars[1], v0Str[0], v1Str[2], v0Str[2], v1Str[0],
           basisChars[2], v0Str[0], v1Str[1], v0Str[1], v1Str[0]))

    print('%s = %s(  %s - %s)  -%s(  %s - %s)  +%s(  %s - %s)' %
          (PAD2, basisChars[0], M0ProdsStr[0], M0ProdsStr[1], basisChars[1],
           M1ProdsStr[0], M1ProdsStr[1], basisChars[2], M2ProdsStr[0],
           M2ProdsStr[1]))

    print('%s = %s(%s)           -%s(%s)           +%s(%s)' %
          (PAD2, basisChars[0], detValsStr[0], basisChars[1], detValsStr[1],
           basisChars[2], detValsStr[2]))

    print('%s = [ %s ]ᵀ' % (PAD2, ', '.join(crossProdStr)))

    #----------
    print()

    print('%s = ‖[ %s ]ᵀ‖' % (frontStrB, ', '.join(crossProdStr)))

    print('%s = √( %s )' % (PAD4, ' + '.join([e + '^2'
                                              for e in crossProdStr])))

    print('%s = √( %s )' % (PAD4, ' + '.join(crPrdSqStr)))

    print('%s = √( %s )' % (PAD4, nsqCrPrdStr))

    print('%s = %s' % (PAD4, nCrPrdStr))

    #----------
    print()

    print('%s = ‖%s‖×‖%s‖×sin θ' % (frontStrB, v0Name, v1Name))
    print('%s = %s × %s × sin( %s rad or %s° )' %
          (PAD4, nV0Str, nV1Str, thetaStr, thetaDegStr))
    print('%s = %s × %s ×  %s' % (PAD4, nV0Str, nV1Str, sinThetaStr))
    print('%s = %s          ∎' % (PAD4, nCrPrdStr))

    # One can't compute the angle between two vectors using the
    # cross product because sin is ambiguous in the range [ 0..180 ].
    # This means that if the angle is 90 < θ < 180, it will
    # instead be computed as its supplement, 180-θ.
    # To get an accurate θ, use θ = arccos( v0·v1 / ( ‖v0‖*‖v1‖ ) ).
    # The dot product preserves the quadrant as it will return a
    # value in the range [-1 .. 1].
    #print( 'sin θ = %s / (‖%s‖×‖%s‖)' % ( frontStrB, v0Name, v1Name ) )
    #print( '      = %s / ( %s · %s )' % ( nCrPrdStr, nV0Str, nV1Str ) )
    #print( '      = %s / %s = %s' % ( nCrPrdStr, nV0nV1Str, sinThetaStr ) )
    #print( 'θ     = arcsin( %s ) = %s rad or %s°' % ( sinThetaStr, thetaStr, thetaDegStr ) )

    #----------
    return result
Example #41
0
def rotate_to_horizon(line):
    angle = math.atan2(line.coords[1][1], line.coords[1][0])
    angle = math.degrees(angle)
    line = affinity.rotate(line, -angle, origin=(0, 0))
    return line
Example #42
0
def click(event):
    global scvalue
    text = event.widget.cget("text")
    if text == "=":
        if scvalue.get().isdigit():
            value = int(scvalue.get())

        # elif ((scvalue.get()).count("pow") > 0):

        else:
            try:
                value = eval(screen.get())

            except Exception as e:
                print(e)
                value = "Error"

        scvalue.set(value)
        screen.update()

    elif text == "C":
        scvalue.set("")
        screen.update()

    elif text == "x^2":
        if scvalue.get().isdigit():
            val = int(scvalue.get())

            avi = math.pow(val, 2)
            scvalue.set(avi)
            screen.update()
        else:
            try:
                value = eval(screen.get())
                val = int(value)
            except Exception as e:
                print(e)
                value = "Error"
            avi = math.pow(val, 2)
            scvalue.set(avi)
            screen.update()

    elif text == "x^3":
        if scvalue.get().isdigit():
            val = int(scvalue.get())

            avi = math.pow(val, 3)
            scvalue.set(avi)
            screen.update()
        else:
            try:
                value = eval(screen.get())
                val = int(value)
            except Exception as e:
                print(e)
                value = "Error"
            avi = math.pow(val, 3)
            scvalue.set(avi)
            screen.update()

    elif text == "sqrt":
        value = eval(screen.get())
        nw = math.sqrt(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "sin":
        value = eval(screen.get())
        value1 = math.radians(value)
        nw = math.sin(value1)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "cos":
        value = eval(screen.get())
        value1 = math.radians(value)
        nw = math.cos(value1)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "tan":
        value = eval(screen.get())
        value1 = math.radians(value)
        nw = math.tan(value1)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "n!":
        value = eval(screen.get())
        nw = math.factorial(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "pi":
        value = scvalue.get()
        v = "3.141592653589"
        nw = value + v
        scvalue.set(nw)
        screen.update()

    elif text == "cut":
        value = scvalue.get()
        l = len(value)
        nw = value[:l - 1]
        scvalue.set(nw)
        screen.update()

    elif text == "log":
        value = eval(screen.get())
        nw = math.log10(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "exp":
        value = eval(screen.get())
        nw = math.exp(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "e":
        value = scvalue.get()
        v = "2.718281"
        nw = value + v
        scvalue.set(nw)
        screen.update()

    elif text == "log 2":
        value = eval(screen.get())
        nw = math.log2(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "round":
        value = eval(screen.get())
        nw = round(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "isin":
        value = eval(screen.get())
        nw = math.degrees(math.asin(value))
        # print(nw)
        scvalue.set(nw)
        screen.update()
    elif text == "icos":
        value = eval(screen.get())
        nw = math.degrees(math.acos(value))
        # print(nw)
        scvalue.set(nw)
        screen.update()
    elif text == "itan":
        value = eval(screen.get())
        nw = math.degrees(math.atan(value))
        # print(nw)
        scvalue.set(nw)
        screen.update()
    elif text == "deg":
        value = eval(screen.get())
        nw = math.degrees(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()
    elif text == "abs":
        value = eval(screen.get())
        nw = math.fabs(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()
    elif text == "gamma":
        value = eval(screen.get())
        nw = math.gamma(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    elif text == "trunc":
        value = eval(screen.get())
        nw = math.trunc(value)
        # print(nw)
        scvalue.set(nw)
        screen.update()

    else:
        scvalue.set(scvalue.get() + text)
        screen.update()
    RAdeg = 15 * RA
    RArad = math.radians(RAdeg)

    #l = sqrt(x^2 + y^2) , x^2 + y^2 + z^2 = 1
    l = math.sqrt(1 - z * z)

    x = l * math.cos(RArad)
    y = l * math.sin(RArad)

    v = vector(x, y, z)
    return v


b = toXYZ(195.27 / 15., 40.02)
n = toXYZ(162.93 / 15., 41.31)

print n, b
print math.degrees(arccos(dot(n, b)))
print mag(b - n) * 6371
print b - n
ntob = b - n

ntoasteroid = 149597871 * .3 / 6371. * n
btoasteroid = ntoasteroid + ntob
print ntoasteroid
print btoasteroid

print degrees(
    arccos(
        dot(ntoasteroid, btoasteroid) / (mag(ntoasteroid) * mag(btoasteroid))))
Example #44
0
def degrees(x):
    return math.degrees(x)
Example #45
0
 def get_angle(self):
     if (self.get_length_sqrd() == 0):
         return 0
     return math.degrees(math.atan2(self.y, self.x))
Example #46
0
import math
AB = int(input())
BC = int(input())

print(str(int(round(math.degrees(math.atan2(AB, BC))))) + '°')
def rotate_rectangle(img_name,img, contour):

    shape= {}
    shape['id'] = img_name
# for c in contour:
    c = contour
    
    area = cv2.contourArea(c)
    x,y,w,h = cv2.boundingRect(c)
    ratiowh  =  min(float(w/h),float(h/w))
    shape['ratiowh'] = ratiowh

    ratioarea = float(area/(w*h))
    shape['ratioarea'] = ratioarea

    epsilon = 0.01 * cv2.arcLength(c, True)
    approx = cv2.approxPolyDP(c, epsilon, True)

    approxlen = len(approx)
    shape['approxlen'] = approxlen


    #  the original num set to be -1 to be different no operation
    num_angle = 0
    num_angle90 = -1
    num_angle80 = -1
    num_angle70 = -1

    mask = np.zeros(img.shape, np.uint8)
    cv2.drawContours(mask, [approx], -1, (255, 255, 255), -1)
    cv2.drawContours(img, [approx], -1, (255, 255, 255), 2)
    # mask = np.concatenate((mask, mask, mask), axis=-1)
    gray = cv2.cvtColor(mask, cv2.COLOR_BGR2GRAY)
    contour_list = []
    ret, thresh = cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY)
    _,contours, hierarchy = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    # get the list of contours
    for points in contours[0]:
        x, y = points.ravel()
        contour_list.append([x, y])
    corners = cv2.goodFeaturesToTrack(gray, 50, 0.01, 10)
    corners = np.int0(corners)
    for i in corners:
        x, y = i.ravel()
        #  decide whether the corner is on the contours
        if (cv2.pointPolygonTest(contours[0], (x, y), True) == 0):
            center_index = contour_list.index([x, y])
            length = len(contour_list)
            # get the point three before, and ignore the end point
            a_index = center_index - 5
            b_index = center_index + 5
            if ((a_index > 0) & (b_index > 0) & (a_index < length)& (b_index < length)):
                xa, ya = contour_list[a_index]
                xb, yb = contour_list[b_index]
                # print(x , y)
                # print(xa, ya)
                a = math.sqrt((x - xa) * (x - xa) + (y - ya) * (y - ya))
                b = math.sqrt((x - xb) * (x - xb) + (y - yb) * (y - yb))
                c = math.sqrt((xa - xb) * (xa - xb) + (ya - yb) * (ya - yb))
                if ((a > 0) & (b > 0)):
                    if(((a * a + b * b - c * c) / (2 * a * b))<1) & (((a * a + b * b - c * c) / (2 * a * b) >-1)):
                        angle = math.degrees(math.acos((a * a + b * b - c * c) / (2 * a * b)))
                        num_angle =num_angle +1
                        # print(angle)
                        if (angle < 90):
                            num_angle90 = num_angle90 + 1
                        if (angle < 80):
                            num_angle80 = num_angle80 + 1
                        if (angle < 70):
                            num_angle70 = num_angle70 + 1
        cv2.circle(img, (x, y), 5, 255, -1)

    shape['numangle'] = num_angle
    shape['numangle90'] = num_angle90
    shape['numangle80'] = num_angle80
    shape['numangle70'] = num_angle70

    return(shape)
Example #48
0
 def get_angle_between(self, other):
     cross = self.x*other[1] - self.y*other[0]
     dot = self.x*other[0] + self.y*other[1]
     return math.degrees(math.atan2(cross, dot))
Example #49
0
g = -9.81 #meters/s^2
dt = .001 #seconds
distance_uncertainty = .333333 #meters
goal_y_min = 3 #meters
goal_y_max = 3.5 #meters
goal_y_avg = .5*goal_y_min+.5*goal_y_max #meters

while(1):
	if cv.waitKey(1) & 0xFF == ord('q'):
		break

	goal_x = cv.getTrackbarPos("goal_x", 'params')/15 #meters
	
	#theta = cv.getTrackbarPos("theta", 'params') #degrees
	theta = math.degrees(math.atan(2*goal_y_avg/goal_x)) #degrees

	#v_0 =  cv.getTrackbarPos("v_0", 'params')/10.0 #meters/s
	v_0 = math.sqrt((2*-g*goal_x/math.sin(math.radians(2*theta)))) #meters/s
	
	print("goal distance:", goal_x, theta, v_0)

	time = []
	x_pos = []
	y_pos = []
	x_vel = []
	y_vel = []
	
	xvel = v_0*math.cos(math.radians(theta)) #meters/s
	xpos = 0 #meters
	yvel = v_0*math.sin(math.radians(theta)) #meters/s
Example #50
0
  def update(self, sm, pm, CP, VM):

    v_ego = sm['carState'].vEgo
    angle_steers = sm['carState'].steeringAngle
    active = sm['controlsState'].active

    angle_offset = sm['liveParameters'].angleOffset

    # Run MPC
    self.angle_steers_des_prev = self.angle_steers_des_mpc

    # Update vehicle model
    x = max(sm['liveParameters'].stiffnessFactor, 0.1)
    sr = max(sm['liveParameters'].steerRatio, 0.1)
    VM.update_params(x, sr)

    curvature_factor = VM.curvature_factor(v_ego)

    # Get steerRatio and steerRateCost from kegman.json every x seconds
    self.mpc_frame += 1
    if self.mpc_frame % 500 == 0:
      # live tuning through /data/openpilot/tune.py overrides interface.py settings
      kegman = kegman_conf()
      if kegman.conf['tuneGernby'] == "1":
        self.steerRateCost = float(kegman.conf['steerRateCost'])
        if self.steerRateCost != self.steerRateCost_prev:
          self.setup_mpc()
          self.steerRateCost_prev = self.steerRateCost

        self.sR = [float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))]
        self.sRBP = [float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1'])]
        self.sR_time = int(float(kegman.conf['sR_time'])) * 100

      self.mpc_frame = 0

    if v_ego > 11.111:
      # boost steerRatio by boost amount if desired steer angle is high
      self.steerRatio_new = interp(abs(angle_steers), self.sRBP, self.sR)

      self.sR_delay_counter += 1
      if self.sR_delay_counter % self.sR_time != 0:
        if self.steerRatio_new > self.steerRatio:
          self.steerRatio = self.steerRatio_new
      else:
        self.steerRatio = self.steerRatio_new
        self.sR_delay_counter = 0
    else:
      self.steerRatio = self.sR[0]

    #print("steerRatio = ", self.steerRatio)

    self.LP.parse_model(sm['model'])

    # Lane change logic
    one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
    below_lane_change_speed = v_ego < self.alc_min_speed

    if sm['carState'].leftBlinker:
      self.lane_change_direction = LaneChangeDirection.left
    elif sm['carState'].rightBlinker:
      self.lane_change_direction = LaneChangeDirection.right

    if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled):
      self.lane_change_state = LaneChangeState.off
      self.lane_change_direction = LaneChangeDirection.none
    else:
      torque_applied = sm['carState'].steeringPressed and \
                       ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                        (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))

      blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
                            (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))

      lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob

      # State transitions
      # off

      if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
        self.lane_change_state = LaneChangeState.preLaneChange
        self.lane_change_ll_prob = 1.0

      # pre
      elif self.lane_change_state == LaneChangeState.preLaneChange:
        if not one_blinker or below_lane_change_speed:
          self.lane_change_state = LaneChangeState.off
        elif torque_applied and not blindspot_detected:
          self.lane_change_state = LaneChangeState.laneChangeStarting

      # starting
      elif self.lane_change_state == LaneChangeState.laneChangeStarting:
        # fade out over .5s
        self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
        # 98% certainty
        if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
          self.lane_change_state = LaneChangeState.laneChangeFinishing

      # finishing
      elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
        # fade in laneline over 1s
        self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
        if one_blinker and self.lane_change_ll_prob > 0.99:
          self.lane_change_state = LaneChangeState.preLaneChange
        elif self.lane_change_ll_prob > 0.99:
          self.lane_change_state = LaneChangeState.off

    if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
      self.lane_change_timer = 0.0
    else:
      self.lane_change_timer += DT_MDL

    self.prev_one_blinker = one_blinker

    desire = DESIRES[self.lane_change_direction][self.lane_change_state]

    # Turn off lanes during lane change
    if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
      self.LP.l_prob *= self.lane_change_ll_prob
      self.LP.r_prob *= self.lane_change_ll_prob
      self.libmpc.init_weights(MPC_COST_LAT.PATH / 3.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
    else:
      self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)

    self.LP.update_d_poly(v_ego)


    # TODO: Check for active, override, and saturation
    # if active:
    #   self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
    #   self.path_offset_i = clip(self.path_offset_i, -0.5,  0.5)
    #   self.LP.d_poly[3] += self.path_offset_i
    # else:
    #   self.path_offset_i = 0.0

    # account for actuation delay
    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)

    v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
    self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
                        list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
                        self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)

    # reset to current steer angle if not active or overriding
    if active:
      delta_desired = self.mpc_solution[0].delta[1]
      rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
    else:
      delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
      rate_desired = 0.0

    self.cur_state[0].delta = delta_desired

    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)

    #  Check for infeasable MPC solution
    mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
    t = sec_since_boot()
    if mpc_nans:
      self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steerRateCost)
      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR

      if t > self.last_cloudlog_t + 5.0:
        self.last_cloudlog_t = t
        cloudlog.warning("Lateral mpc - nan: True")

    if self.mpc_solution[0].cost > 20000. or mpc_nans:   # TODO: find a better way to detect when MPC did not converge
      self.solution_invalid_cnt += 1
    else:
      self.solution_invalid_cnt = 0
    plan_solution_valid = self.solution_invalid_cnt < 2

    plan_send = messaging.new_message('pathPlan')
    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
    plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
    plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
    plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
    plan_send.pathPlan.lProb = float(self.LP.l_prob)
    plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
    plan_send.pathPlan.rProb = float(self.LP.r_prob)

    plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
    plan_send.pathPlan.rateSteers = float(rate_desired)
    plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
    plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
    plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)

    plan_send.pathPlan.desire = desire
    plan_send.pathPlan.laneChangeState = self.lane_change_state
    plan_send.pathPlan.laneChangeDirection = self.lane_change_direction

    pm.send('pathPlan', plan_send)

    if LOG_MPC:
      dat = messaging.new_message('liveMpc')
      dat.liveMpc.x = list(self.mpc_solution[0].x)
      dat.liveMpc.y = list(self.mpc_solution[0].y)
      dat.liveMpc.psi = list(self.mpc_solution[0].psi)
      dat.liveMpc.delta = list(self.mpc_solution[0].delta)
      dat.liveMpc.cost = self.mpc_solution[0].cost
      pm.send('liveMpc', dat)
Example #51
0
import svgwrite
from astropy import units as u
from astropy.io import fits
from astropy.wcs import WCS
from astropy.coordinates import SkyCoord

hdu = fits.open(wcs_fits)[0]
w = WCS(hdu.header, fix=False)
image_w = hdu.header['IMAGEW']
image_h = hdu.header['IMAGEH']

sky_left = w.pixel_to_world(0, 0)
sky_right = w.pixel_to_world(image_w, 0)
sky_right2 = SkyCoord(ra=sky_right.ra, dec=sky_left.dec)
x2, y2 = w.world_to_pixel(sky_right2)
image_tilt = math.degrees(math.atan(y2 / x2))
print('tilt=', image_tilt)
scales = w.proj_plane_pixel_scales()
px_scale = (scales[0].to_value(unit=u.deg) +
            scales[1].to_value(unit=u.deg)) / 2
print('scale=', px_scale)

drw = svgwrite.Drawing(out_file, size=(image_w, image_h))
drw.add(drw.style(style_sheet))

#TBD: select embed or link by option.
embed_image = True
if embed_image:
    with open(image_file, 'rb') as f:
        mime = ''
        if image_file.upper().endswith('.JPG'):
 def update_rotation(self, x, y):
     self.angle_start = 270 + (math.degrees(
         math.atan2((x - self.canvas_width / 2), (y - self.canvas_height / 2))) + .5 * self.angle_length)
     self.canvas.itemconfig(self.arc_id, start=self.angle_start)
Example #53
0
 def latitude(y):
     lat_rad = math.atan(math.sinh(math.pi - 2 * math.pi * y / factor))
     return math.degrees(lat_rad)
Example #54
0
 def onPushButton1(self):
     from math import pi, degrees
     import frameCmd
     try:
         obj = self.Selection.getSelection()[0]
         self.labName.setText(obj.Label)
         self.labBaseVal.setText(
             str("P = %.1f,%.1f,%.1f" % tuple(obj.Placement.Base)))
         self.labRotAng.setText(
             str("%.2f " % (degrees(obj.Placement.Rotation.Angle))))
         ax = obj.Placement.Rotation.Axis
         self.labRotAx.setText(
             str("v = (%(x).2f,%(y).2f,%(z).2f)" % {
                 'x': ax.x,
                 'y': ax.y,
                 'z': ax.z
             }))
         shapes = [
             y for x in self.Selection.getSelectionEx()
             for y in x.SubObjects if hasattr(y, 'ShapeType')
         ]
         if len(shapes) == 1:
             sub = shapes[0]
             if sub.ShapeType == 'Edge':
                 if sub.curvatureAt(0) == 0:
                     self.labSubObj.setText(sub.ShapeType +
                                            ':\tL = %.1f mm' % sub.Length)
                 else:
                     x, y, z = sub.centerOfCurvatureAt(0)
                     d = 2 / sub.curvatureAt(0)
                     self.labSubObj.setText(
                         sub.ShapeType +
                         ':\tD = %.1f mm\n\tC = %.1f,%.1f,%.1f' %
                         (d, x, y, z))
             elif sub.ShapeType == 'Face':
                 self.labSubObj.setText(sub.ShapeType +
                                        ':\tA = %.1f mm2' % sub.Area)
             elif sub.ShapeType == 'Vertex':
                 self.labSubObj.setText(
                     sub.ShapeType + ': pos = (%(x).1f,%(y).1f,%(z).1f)' % {
                         'x': sub.X,
                         'y': sub.Y,
                         'z': sub.Z
                     })
         elif len(shapes) > 1:
             self.labSubObj.setText(shapes[0].ShapeType + ' to ' +
                                    shapes[1].ShapeType +
                                    ': distance = %.1f mm' %
                                    (shapes[0].distToShape(shapes[1])[0]))
         else:
             self.labSubObj.setText(' ')
         if len(frameCmd.beams()) == 1:
             b = frameCmd.beams()[0]
             self.labBeam.setText(b.Label + ":\tL=%.2f" % (b.Height))
             self.labProfile.setText("Profile: " + b.Profile)
         elif len(frameCmd.beams()) > 1:
             b1, b2 = frameCmd.beams()[:2]
             self.labBeam.setText(b1.Label + "^" + b2.Label + ": %.2f" % (
                 degrees(frameCmd.beamAx(b1).getAngle(frameCmd.beamAx(b2))))
                                  )
             self.labProfile.setText("")
         else:
             self.labBeam.setText("")
             self.labProfile.setText("")
     except:
         pass
                vis = frame.copy()
                flowVectorLength = []
                flowVectorLength_x = []
                flowVectorLength_y = []
                flowAngle = []
                flowVectorLength_H = []
                flowVectorLength_xH = []
                flowVectorLength_yH = []
                flowAngle_H = []
                for (x0, y0), (x1, y1), (xT, yT), good in zip(p0[:, 0], p1[:, 0], pT[:, 0], st[:, 0]):
                    if good:
                        cv2.line(vis, (x0, y0), (x1, y1), (0, 128, 0))
                        flowVectorLength_H.append(math.sqrt((x1 - xT) ** 2 + (y1 - yT) ** 2)) # calculate flow vector length (speed)
                        flowVectorLength_xH.append(x1 - xT) # calculate the x vector
                        flowVectorLength_yH.append(y1 - yT) # calculate the y vector
                        flowAngle_H.append(math.degrees(math.atan2((y1 - yT), (x1 - xT)))) # calculate flow vector angle (direction)
                        flowVectorLength.append(math.sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2)) # calculate flow vector length (speed)
                        flowVectorLength_x.append(x1 - x0) # calculate the x vector
                        flowVectorLength_y.append(y1 - y0) # calculate the y vector
                        flowAngle.append(math.degrees(math.atan2((y1 - y0), (x1 - x0)))) # calculate flow vector angle (direction)
                    vis = cv2.circle(vis, (x1, y1), 2, (red, green)[good], -1)
                    #vis = cv2.circle(vis, (x0, y0), 2, (red, green)[good], -1)
                    vis = cv2.circle(vis, (xT, yT), 2, (255, 0, 0), -1)

                # get distance, and store in polar format
                x = p1[:, 0, 0] - p0[:, 0, 0]
                y = p1[:, 0, 1] - p0[:, 0, 1]
                mag, ang = cv2.cartToPolar(x, y, angleInDegrees=False)
                im2Reg = cv2.warpPerspective(im2, h, (frame_width, frame_height), flags=cv2.INTER_LINEAR + cv2.WARP_FILL_OUTLIERS)

                x = p1[:, 0, 0] - pT[:, 0, 0]
Example #56
0
def alt_angle(plane):
    angle = math.atan2(plane.get('GAlt', 0)*FEET_PER_KILOMETRE,
                       plane.get('Dst', 0))
    return math.degrees(angle)
from sympy import *


def grad_ascent(x0, f1x, x):
    epsilon = 1e-6
    step_size = 1e-4
    x_old = x0
    x_new = x_old + step_size * f1x.subs({x: x_old}).evalf()
    while abs(x_old - x_new) > epsilon:
        x_old = x_new
        x_new = x_old + step_size * f1x.subs({x: x_old}).evalf()
    return x_new


def find_max_theta(R, theta):
    R1theta = Derivative(R, theta).doit()
    theta0 = 1e-3
    theta_max = grad_ascent(theta0, R1theta, theta)
    return theta_max


if __name__ == '__main__':
    g = 9.8
    u = 25
    theta = Symbol('theta')
    R = u**2 * sin(2 * theta) / g

    theta_max = find_max_theta(R, theta)
    print('Theta : {0}'.format(math.degrees(theta_max)))
    print('Maximun Range : {0}'.format(R.subs({theta: theta_max})))
    def run(self):
        self.create.start()
        self.create.safe()

        self.create.drive_direct(0, 0)
        self.arm.open_gripper()
        self.time.sleep(4)

        # request sensors
        self.create.start_stream([
            create2.Sensor.LeftEncoderCounts,
            create2.Sensor.RightEncoderCounts,
        ])

        start = self.create.sim_get_position()
        self.create.drive_direct(10, 10)
        self.time.sleep(2)
        start = self.create.sim_get_position()

        # Start all particles in robot's start location
        # Note: Please change start theta here if changing orientation of mobile robot
        start_theta = math.radians(0)
        self.pf.set_particles(start, start_theta)
        self.visualize()

        # Tell Create To Go to Arm
        # Note: Please change Arms Coordinate Position here when changing start position of arm
        arm_postion = (1.6001, 3.3999)
        arm_position_in_pixel_coordinates = self.get_arm_position_in_pixels(
            arm_postion)
        self.go_to_arm(arm_position_in_pixel_coordinates)
        self.time.sleep(5)

        # Move slightly forward
        # Note: Depending on arm location, robot will face rear side to arm then move slightly forward
        self.move_slightly_forward(arm_postion)

        # Bring arm down and pick up cup then place it on shelf
        # Note: Change shelf number here (currently placing on shelf number 4)
        self.arm.go_to(0, math.radians(0))

        theta1_i = 0
        theta2_i = 0

        self.arm.open_gripper()
        self.time.sleep(5)
        [theta1_i, theta2_i] = self.inverse_kinematics(0.98, 0.17, theta1_i,
                                                       theta2_i)
        print(math.degrees(theta1_i), math.degrees(theta2_i),
              'theta 1 and 2 initial')
        self.arm.close_gripper()
        self.time.sleep(10)

        [theta1_i, theta2_i] = self.inverse_kinematics(.9, 0.4, theta1_i,
                                                       theta2_i)
        joint_1_rotate = 0
        for i in range(0, 50):
            self.arm.go_to(0, math.radians(joint_1_rotate))
            joint_1_rotate = joint_1_rotate - 2
            self.time.sleep(.5)

        shelf_number = 4

        if shelf_number == 1:
            self.inverse_kinematics(.9, 0.2, theta1_i, theta2_i)
            for i in range(0, 15):
                self.arm.go_to(0, math.radians(joint_1_rotate))
                joint_1_rotate = joint_1_rotate - 2
                self.time.sleep(.5)
        elif shelf_number == 2:
            self.inverse_kinematics(.9, 0.55, theta1_i, theta2_i)
            for i in range(0, 15):
                self.arm.go_to(0, math.radians(joint_1_rotate))
                joint_1_rotate = joint_1_rotate - 2
                self.time.sleep(.5)
        elif shelf_number == 3:
            self.inverse_kinematics(.9, 0.9, theta1_i, theta2_i)

            joint_6_rotation = 0
            for j in range(0, 5):
                self.arm.go_to(5, math.radians(joint_6_rotation))
                joint_6_rotation = joint_6_rotation + 2
                self.time.sleep(.5)

            for i in range(0, 15):
                self.arm.go_to(0, math.radians(joint_1_rotate))
                joint_1_rotate = joint_1_rotate - 2
                self.time.sleep(.5)
        elif shelf_number == 4:
            self.inverse_kinematics(.45, 1.27, theta1_i, theta2_i)
            joint_6_rotation = 0
            for i in range(0, 15):
                self.arm.go_to(5, math.radians(joint_6_rotation))
                joint_6_rotation = joint_6_rotation + 2
                self.time.sleep(.5)

            for j in range(0, 40):
                self.arm.go_to(0, math.radians(joint_1_rotate))
                joint_1_rotate = joint_1_rotate - 2
                self.time.sleep(.5)
        else:
            print('not valid')

        self.arm.open_gripper()
        self.time.sleep(10)
def LatitudeLongitudeAtDistanceBearing(StartLatitudeLongitudeDegrees, DistanceMeters, BearingDegrees):
    '''
    compute latitude and longitude of a point at a given distance and on a radial from a given point
    
    Destination point given distance and bearing from start point
    Given a start point, initial bearing, and distance, 
    this will calculate the destination point 
    and final bearing travelling along a (shortest distance) great circle arc.
    '''
    assert (isinstance(StartLatitudeLongitudeDegrees, list)) and (len(StartLatitudeLongitudeDegrees) == 2)
    
    assert (isinstance(StartLatitudeLongitudeDegrees[0], float))
    startLatitudeDegrees = StartLatitudeLongitudeDegrees[0]
    assert (startLatitudeDegrees <= 90.0) and (-90.0 <= startLatitudeDegrees)
        
    assert (isinstance(StartLatitudeLongitudeDegrees[1], float))
    startLongitudeDegrees = StartLatitudeLongitudeDegrees[1]
    assert (startLongitudeDegrees <= 180.0) and  (-180.0 <= startLongitudeDegrees)
        
    assert (isinstance(BearingDegrees, float))
    assert (BearingDegrees >= -360.0) and (BearingDegrees <= 360.0)
        
    start_latitude_radians = math.radians(StartLatitudeLongitudeDegrees[0])
    start_longitude_radians = math.radians(StartLatitudeLongitudeDegrees[1])
    bearing_radians = math.radians(BearingDegrees)
        
    '''
        Formula:     φ2 = asin( sin φ1 ⋅ cos δ + cos φ1 ⋅ sin δ ⋅ cos θ )
                    λ2 = λ1 + atan2( sin θ ⋅ sin δ ⋅ cos φ1, cos δ − sin φ1 ⋅ sin φ2 )
            where     φ is latitude, λ is longitude, 
            θ is the bearing (in radians, clockwise from north), 
            δ is the angular distance (in radians) d/R; d being the distance travelled, R the earth’s radius
            JavaScript:     

            var φ2 = Math.asin( Math.sin(φ1)*Math.cos(d/R) +
                    Math.cos(φ1)*Math.sin(d/R)*Math.cos(brng) );
            var λ2 = λ1 + Math.atan2(Math.sin(brng)*Math.sin(d/R)*Math.cos(φ1),
                         Math.cos(d/R)-Math.sin(φ1)*Math.sin(φ2));
                         
    '''
    earthRadiusMeters = 6378135.0 # earth’s radius in meters
    latitudeRadians = math.asin( math.sin(start_latitude_radians) * math.cos(DistanceMeters/earthRadiusMeters) 
                                     + math.cos(start_latitude_radians) * math.sin(DistanceMeters/earthRadiusMeters) * math.cos(bearing_radians)) 
    deltaLon = math.atan2( math.sin(bearing_radians) * math.sin(DistanceMeters/earthRadiusMeters) * math.cos(start_latitude_radians) , 
                               math.cos(DistanceMeters/earthRadiusMeters) - math.sin(start_latitude_radians) * math.sin(latitudeRadians))
    longitudeRadians = math.fmod ( start_longitude_radians + deltaLon + math.pi, 2 * math.pi ) - math.pi
        #longitudeRadians = start_longitude_radians + deltaLon
    latitudeDegrees = math.degrees(latitudeRadians)
    longitudeDegrees = math.degrees(longitudeRadians)
    #longitudeDegrees = math.fmod(longitudeDegrees + 180.0 , 360.0 ) - 180.0
    longitudeDegrees = ((longitudeDegrees + 180) % 360) - 180
        
    assert (latitudeDegrees <= 90.0) and (-90.0 <= latitudeDegrees)
    if (longitudeDegrees > 180.0) or (longitudeDegrees < -180.0):
        assert (longitudeDegrees <= 180.0) and (-180.0 <= longitudeDegrees)
#     if longitudeDegrees > 180.0:
#         longitudeDegrees = math.fmod(longitudeDegrees - 180.0, 360.0) + 180.0
#     if longitudeDegrees < -180.0:
#         longitudeDegrees = math.fmod(360.0 + longitudeDegrees , 360.0)
        
    return latitudeDegrees, longitudeDegrees
def calculateDepotAngle(x, y, depot_x, depot_y):
    angle = degrees(atan2(y - depot_y, x - depot_x))
    bearing = (90 - angle) % 360
    return bearing