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)
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
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)
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;
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
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)
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)
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)
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)
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
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
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
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()
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
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 )
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)
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
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])))
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
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))
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)))
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)))
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)
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)
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
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
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))
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)
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'}
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
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
# 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
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
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))))
def degrees(x): return math.degrees(x)
def get_angle(self): if (self.get_length_sqrd() == 0): return 0 return math.degrees(math.atan2(self.y, self.x))
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)
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))
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
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)
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)
def latitude(y): lat_rad = math.atan(math.sinh(math.pi - 2 * math.pi * y / factor)) return math.degrees(lat_rad)
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]
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