def distance_on_unit_sphere(self,lat1, long1, lat2, long2): """ from john d cook website http://www.johndcook.com/blog/python_longitude_latitude/ :arg lat1: latitude value in degrees for clSite :arg lon1: longitude value in degrees for clSite :arg lat2: latitude value in degrees for zipcode :arg lon2: longitude value in degrees for zipcode :var all: see link for further explanation """ degrees_to_radians = math.pi/180.0 # phi = 90 - latitude phi1 = (90.0 - lat1)*degrees_to_radians phi2 = (90.0 - lat2)*degrees_to_radians # theta = longitude theta1 = long1*degrees_to_radians theta2 = long2*degrees_to_radians cos = (math.sin(phi1)*math.sin(phi2)*math.cos(theta1 - theta2) + math.cos(phi1)*math.cos(phi2)) arc = math.acos( cos ) #To get the distance in miles, multiply by 3960. #To get the distance in kilometers, multiply by 6373. arc = arc * 3960 #miles return arc
def brachistochroneReal(x0, y0, x1, y1, n): dy = y0 - y1 prec=10.0**-12 t1=0.0 t2=2*pi xm=x0 while abs(xm-x1) > prec: tm = (t1+t2)/2 if (1-cos(tm)==0): continue rm = dy / (1 - cos(tm)) xm = x0 + rm * (tm - sin(tm)) if (xm > x1): #pag 258 t2 = tm else: t1 = tm L = [] L2 = [] r=rm for i in xrange(n+1): t=tm*i/n L.append ( [(x0+r*(t-sin(t))), (y0-r*(1-cos(t)))] ) L2.extend ( [(x0+r*(t-sin(t))), (y0-r*(1-cos(t)))] ) return L, L2
def GetSpindownStuff(tc, age, l0, b0, emax0, n0): t = np.logspace(0,math.log10(1000.*age),300) lumt = [] emaxt = [] bt = [] nt = [] n = 0 for i in t: l = l0/math.pow(1.+i/tc,2.) btt = b0*math.sqrt(l/l0)*(1.+0.5*math.sin(0.1*n*3.14)) lumtt = l*(1.+0.5*math.cos(0.1*n*3.14)) emaxtt = emax0*math.pow(l/l0,0.25)*(1.+0.5*math.sin(0.05*n*3.14)) ntt = n0*math.pow(l/l0,0.25)*(1.+0.5*math.cos(0.05*n*3.14)) bt.append([]) bt[n].append(i) bt[n].append(btt) lumt.append([]) lumt[n].append(i) lumt[n].append(lumtt) emaxt.append([]) emaxt[n].append(i) emaxt[n].append(emaxtt) nt.append([]) nt[n].append(i) nt[n].append(ntt) n = n+1 return lumt,bt,emaxt,nt
def calculate_initial_compass_bearing(self, pointA, pointB): """ Calculates direction between two points. Code based on compassbearing.py module https://gist.github.com/jeromer/2005586 pointA: latitude/longitude for first point (decimal degrees) pointB: latitude/longitude for second point (decimal degrees) Return: direction heading in degrees (0-360 degrees, with 90 = North) """ if (type(pointA) != tuple) or (type(pointB) != tuple): raise TypeError("Only tuples are supported as arguments") lat1 = math.radians(pointA[0]) lat2 = math.radians(pointB[0]) diffLong = math.radians(pointB[1] - pointA[1]) # Direction angle (-180 to +180 degrees): # θ = atan2(sin(Δlong).cos(lat2),cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong)) x = math.sin(diffLong) * math.cos(lat2) y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) * math.cos(lat2) * math.cos(diffLong)) initial_bearing = math.atan2(x, y) # Direction calculation requires to normalize direction angle (0 - 360) initial_bearing = math.degrees(initial_bearing) compass_bearing = (initial_bearing + 360) % 360 return compass_bearing
def simplesnr(f,h,i=None,years=1,noisemodel=None,includewd=None): if i == None: h0 = h * math.sqrt(16.0/5.0) # rms average over inclinations else: h0 = h * math.sqrt((1 + math.cos(i)**2)**2 + (2*math.cos(i))**2) return h0 * math.sqrt(years * 365.25*24*3600) / math.sqrt(lisanoise(f,noisemodel,includewd))
def shoot(self, obstMgr): """ The method to shoot the ray to the world. Obstacle manager will tell if the ray hits anything (or the world boundary). If hit, return the distance from the ray's origin to the hit point. """ stepLength = 1.0 isInitiallyInside = obstMgr.isConfigInObstacle( self.mOrigin ); nextCheckPoint = [ self.mOrigin[0] + stepLength*math.cos( self.mTheta ), self.mOrigin[1]+stepLength*math.sin( self.mTheta ) ] i = 1; while( isInitiallyInside == obstMgr.isConfigInObstacle( nextCheckPoint ) and not obstMgr.isOutOfWorld(nextCheckPoint) ): i+=1; nextCheckPoint = [ self.mOrigin[0] + stepLength*i*math.cos( self.mTheta ), self.mOrigin[1]+stepLength*i*math.sin( self.mTheta ) ]; pass; self.mEnd = ( nextCheckPoint[0], nextCheckPoint[1] ) dx = nextCheckPoint[0] - self.mOrigin[0]; dy = nextCheckPoint[1] - self.mOrigin[1]; dist = math.sqrt( dx**2 + dy**2 ); if isInitiallyInside: return dist*(-1); else: return dist;
def show_visual(sec = False, radius = 39): c = Canvas(2*radius+1, 2*radius+1) x = 0 while True: t = time.localtime() c.draw_circle(radius,radius,radius,1) c.draw_circle(radius,radius,radius-1,1) for i in xrange(12): dx = math.sin(2.0*math.pi*i/12) dy = math.cos(2.0*math.pi*i/12) if i%3 == 0: c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.8*dx*radius), radius-int(0.8*dy*radius),1) else: c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.9*dx*radius), radius-int(0.9*dy*radius),1) G = [((t[3]%12), 12, 0.4, 2), (t[4], 60, 0.6, 3), (t[5], 60, 0.8, 4)] for gnomon in G: dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1]) dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1]) c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), gnomon[3]) c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), gnomon[3]) c.render(width, height) time.sleep(1) for gnomon in G: dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1]) dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1]) c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), 0) c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), 0)
def findPointOnSphere(cx, cy, cz, radius, phi, theta): #phi - angle around the pole 0<= phi <= 360 #theta - angle from plan 'up' -90 <= theta <= 90 x = cx + radius * math.cos(math.radians(theta)) * math.cos(math.radians(phi)) z = cz + radius * math.cos(math.radians(theta)) * math.sin(math.radians(phi)) y = cy + radius * math.sin(math.radians(theta)) return int(round(x,0)), int(round(y,0)), int(round(z,0))
def rotate_point(point, center, angle): """ Rotate a point around another point """ angle = math.radians(angle) x = center[0] + (point[0] - center[0]) * math.cos(angle) - (point[1] - center[1]) * math.sin(angle); y = center[1] - (point[0] - center[0]) * math.sin(angle) + (point[1] - center[1]) * math.cos(angle); return (x, y)
def parmeq(G0, G, GB, w0, Dw): beta = math.tan(Dw/2) * math.sqrt(abs(GB**2 - G0**2)) / math.sqrt(abs(G**2 - GB**2)) #global aeq,beq beq = [(G0 + G*beta), -2*G0*math.cos(w0), (G0 - G*beta)] beq= np.array(beq) / (1+beta) aeq = np.array([1, -2*math.cos(w0)/(1+beta), (1-beta)/(1+beta)]) return beq,aeq
def gps_distance_between(point_a, point_b): """ Calculate the orthodromic distance between two GPS readings. point_a and point_b can be either of the two: - tuples in the form (latitude, longitude). - instances of the class logwork.Signal The result is in metres. ATTENTION: since latitude is given before longitude, if we are using the X and Y representation, then we must pass in (Y, X) and *not* (X, Y) Computed with the Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula) """ if hasattr(point_a, "latitude"): a_lat, a_lon = math.radians(point_a.latitude), math.radians(point_a.longitude) else: a_lat, a_lon = math.radians(point_a[0]), math.radians(point_a[1]) if hasattr(point_b, "latitude"): b_lat, b_lon = math.radians(point_b.latitude), math.radians(point_b.longitude) else: b_lat, b_lon = math.radians(point_b[0]), math.radians(point_b[1]) d_lat = b_lat - a_lat d_lon = b_lon - a_lon a = math.sin(d_lat / 2.0) ** 2 + math.cos(a_lat) * math.cos(b_lat) * math.sin(d_lon / 2.0) ** 2 c = 2 * math.asin(math.sqrt(a)) return EARTH_RADIUS * c * 1000
def execute(self, context): A = 6.283185307179586476925286766559 / 3 verts = [(sin(A * 1), 0.0, cos(A * 1)), (sin(A * 2), 0.0, cos(A * 2)), (sin(A * 3), 0.0, cos(A * 3)), ] faces = [(0, 1, 2)] mesh = bpy.data.meshes.new("Cube") bm = bmesh.new() for v_co in verts: bm.verts.new(v_co) for f_idx in faces: bm.faces.new([bm.verts[i] for i in f_idx]) bm.to_mesh(mesh) mesh.update() object_utils.object_data_add(context, mesh) return{'FINISHED'}
def distance_on_unit_sphere(lat1, long1, lat2, long2): # Convert latitude and longitude to # spherical coordinates in radians. degrees_to_radians = math.pi/180.0 # phi = 90 - latitude phi1 = (90.0 - lat1)*degrees_to_radians phi2 = (90.0 - lat2)*degrees_to_radians # theta = longitude theta1 = long1*degrees_to_radians theta2 = long2*degrees_to_radians # Compute spherical distance from spherical coordinates. # For two locations in spherical coordinates # (1, theta, phi) and (1, theta, phi) # cosine( arc length ) = # sin phi sin phi' cos(theta-theta') + cos phi cos phi' # distance = rho * arc length cos = (math.sin(phi1)*math.sin(phi2)*math.cos(theta1 - theta2) + math.cos(phi1)*math.cos(phi2)) arc = math.acos(cos) # Remember to multiply arc by the radius of the earth # in your favorite set of units to get length. return arc * EARTH_RADIUS_MILES
def __init__(self, lb, lb_length, up_angle, dn_angle, hb, hb_length): # define the name # (there is only one launchbar element) --> isn't it ? name = 'YASim_Launchbar' # Calculate points for the mesh # here in the original script hb = hb - lb # --> seems to be tuple - vector, that is not working # assuming: (this step is necessary to get from global to local coordinates !!) hb = hb - Vector(lb) lb_tip = ORIGIN + lb_length * math.cos(dn_angle * DEG2RAD) * X - lb_length * math.sin(dn_angle * DEG2RAD) * Z hb_tip = hb - hb_length * math.cos(dn_angle * DEG2RAD) * X - hb_length * math.sin(dn_angle * DEG2RAD) * Z # create the mesh: launchbar and holdback extended position lb_obj = mesh_create(name, lb, [ORIGIN, lb_tip, hb, hb_tip, lb_tip+0.05*Y, lb_tip-0.05*Y, hb_tip+0.05*Y, hb_tip-0.05*Y], [(0,1),(0,2),(2,3),(4,5),(6,7)], []) # set the created object active !!!!!!! bpy.context.scene.objects.active = lb_obj # draw dashed lines for the retracted position # get the active mesh mesh = bpy.context.object.data lb_up = lb_length * math.cos(up_angle * DEG2RAD) * X - lb_length * math.sin(up_angle * DEG2RAD) * Z hb_up = hb - hb_length * math.cos(up_angle * DEG2RAD) * X - hb_length * math.sin(up_angle * DEG2RAD) * Z draw_dashed_line(mesh, ORIGIN, lb_up) draw_dashed_line(mesh, hb, hb_up) # set material Item.set_material('grey2', (0.3,0.3,0.3), 1)
def cart2tether_actual(sz,xyz): #convert a cartesian goal to tether length goals. assumes the tethers go to points on the outside edge of the end effector. #returns a more precise estimate of tether length, but one that is inadmissible to the get_xyz_pos function #goal : 1x3 array [x,y,z] # L = 1x4 array [L0,L1,L2,L3] spiral zipper and each tether length #finds the axis-angle rotation matrix from the column's vertical pose. theta = sz.angle_between([0,0,sz.L[0]], xyz) k = np.cross([0,0,sz.L[0]],xyz) if np.linalg.norm(k) != 0: #ensures k is a unit vector where its norm == 1 k = k/np.linalg.norm(k) Xk = k[0] Yk = k[1] Zk = k[2] v = 1 - m.cos(theta) R = np.array( [[m.cos(theta) + (Xk**2*v) , (Xk*Yk*v) - (Zk*m.sin(theta)), (Xk*Zk*v) + Yk*m.sin(theta)],\ [((Yk*Xk*v) + Zk*m.sin(theta)), m.cos(theta) + (Yk**2*v) , (Yk*Zk*v) - Xk*m.sin(theta)],\ [(Zk*Xk*v) - Yk*m.sin(theta), (Zk*Yk*v) + Xk*m.sin(theta) , m.cos(theta) + (Zk**2*v) ]]) #calculates position vector of the column tether attachment points in the world frame OB1 = xyz + np.dot(R,sz.ef[0]) OB2 = xyz + np.dot(R,sz.ef[1]) OB3 = xyz + np.dot(R,sz.ef[2]) L0 = m.sqrt((xyz[0]**2+xyz[1]**2+xyz[2]**2)) # should just be sz.L[0] if not there is a math mistake L1 = np.linalg.norm(OB1 - sz.p[0]) L2 = np.linalg.norm(OB2 - sz.p[1]) L3 = np.linalg.norm(OB3 - sz.p[2]) L = [L0,L1,L2,L3] return L
def calc_point_distance( self, p1, p2 ) : x1 = float( p1['x'] ) y1 = float( p1['y'] ) lat1 = x1 * math.pi / 180.0 long1 = y1 * math.pi / 180.0 sinl1 = math.sin( lat1 ) cosl1 = math.cos( lat1 ) x2 = float( p2['x'] ) y2 = float( p2['y'] ) lat2 = x2 * math.pi / 180.0 long2 = y2 * math.pi / 180.0 sinl2 = math.sin( lat2 ) cosl2 = math.cos( lat2 ) dl = long2 - long1 sindl = math.sin( dl ) cosdl = math.cos( dl ) a = cosl2 * sindl b = cosl1 * sinl2 - sinl1 * cosl2 * cosdl y = math.sqrt( a*a + b*b ) x = sinl1 * sinl2 + cosl1 * cosl2 * cosdl d = math.atan2( y, x ) * 6372795 # радиус Земли #print( "d=%d" % ( d ) ) return d
def __init__(self,pos,direction,d_range): self.surface = pygame.surface.Surface((50,50)) self.surface.fill((0,250,200)) self.rect = pygame.rect.Rect(pos[0],pos[1],50,50) self.dir = direction self.dist = 0 self.range = d_range speed = 2 dx = 0 dy = 0 if self.dir>0: if self.dir>90: dy = speed*math.sin(180-self.dir) dx = speed*math.cos(180-self.dir) else: dy = speed*math.sin(self.dir) dx = speed*math.cos(self.dir) else: if self.dir<-90: dy = speed*math.sin(180+self.dir) dx = speed*math.cos(180+self.dir) else: dy = speed*math.sin(self.dir) dx = speed*math.cos(self.dir) self.dx = dx self.dy = dy
def mat_unitary(self): s = [ math.sin(2 * self.at0), math.sin(2 * self.at1), math.sin(2 * self.aq0), math.sin(2 * self.aq1), math.sin(2 * self.aq2), ] c = [ math.cos(2 * self.at0), math.cos(2 * self.at1), math.cos(2 * self.aq0), math.cos(2 * self.aq1), math.cos(2 * self.aq2), ] phi = [ numpy.exp(complex(0, 4 * self.pt1)), numpy.exp(complex(0, 2 * self.pt2)), numpy.exp(complex(0, -2 * self.pt2)), ] U = numpy.array( [ [c[2] * c[3], -phi[0] * s[0] * s[2] * c[3] - phi[1] * c[0] * s[1] * s[3]], [c[2] * s[3], -phi[0] * s[0] * s[2] * s[3] + phi[1] * c[0] * s[1] * c[3]], [s[2] * c[4], phi[0] * s[0] * c[2] * c[4] + phi[2] * c[0] * c[1] * s[4]], [s[2] * s[4], phi[0] * s[0] * c[2] * s[4] - phi[2] * c[0] * c[1] * c[4]], ] ) theta = cmath.phase(U[0, 1]) for i in range(4): U[i, 1] = U[i, 1] * numpy.exp(complex(0, -theta)) return U
def get_geo_distance_from(self, lat2, long2): lat1 = self.geo_latitude long1 = self.geo_longitude if not lat1 or not long1 or not lat2 or not long2: return -1 # Convert latitude and longitude to # spherical coordinates in radians. degrees_to_radians = math.pi / 180.0 # phi = 90 - latitude phi1 = (90.0 - lat1) * degrees_to_radians phi2 = (90.0 - lat2) * degrees_to_radians # theta = longitude theta1 = long1 * degrees_to_radians theta2 = long2 * degrees_to_radians # Compute spherical distance from spherical coordinates. # For two locations in spherical coordinates # (1, theta, phi) and (1, theta, phi) # cosine( arc length ) = # sin phi sin phi' cos(theta-theta') + cos phi cos phi' # distance = rho * arc length cos = (math.sin(phi1) * math.sin(phi2) * math.cos(theta1 - theta2) + math.cos(phi1) * math.cos(phi2)) arc = math.acos(cos) # Remember to multiply arc by the radius of the earth # in your favorite set of units to get length. return float(round(arc * 6371 * 1000))
def paint(self, painter, option, widget): if not self.source or not self.dest: return # Draw the line itself. line = QtCore.QLineF(self.sourcePoint, self.destPoint) if line.length() == 0.0: return painter.setPen(QtGui.QPen(QtCore.Qt.black, 1, QtCore.Qt.SolidLine, QtCore.Qt.RoundCap, QtCore.Qt.RoundJoin)) painter.drawLine(line) # Draw the arrows if there's enough room. angle = math.acos(line.dx() / line.length()) if line.dy() >= 0: angle = Edge.TwoPi - angle sourceArrowP1 = self.sourcePoint + QtCore.QPointF(math.sin(angle + Edge.Pi / 3) * self.arrowSize, math.cos(angle + Edge.Pi / 3) * self.arrowSize) sourceArrowP2 = self.sourcePoint + QtCore.QPointF(math.sin(angle + Edge.Pi - Edge.Pi / 3) * self.arrowSize, math.cos(angle + Edge.Pi - Edge.Pi / 3) * self.arrowSize); destArrowP1 = self.destPoint + QtCore.QPointF(math.sin(angle - Edge.Pi / 3) * self.arrowSize, math.cos(angle - Edge.Pi / 3) * self.arrowSize) destArrowP2 = self.destPoint + QtCore.QPointF(math.sin(angle - Edge.Pi + Edge.Pi / 3) * self.arrowSize, math.cos(angle - Edge.Pi + Edge.Pi / 3) * self.arrowSize) painter.setBrush(QtCore.Qt.black) painter.drawPolygon(QtGui.QPolygonF([line.p1(), sourceArrowP1, sourceArrowP2])) painter.drawPolygon(QtGui.QPolygonF([line.p2(), destArrowP1, destArrowP2]))
def day_length(doy, yr_days, latitude): """ Daylength in hours Eqns come from Leuning A4, A5 and A6, pg. 1196 Reference: ---------- Leuning et al (1995) Plant, Cell and Environment, 18, 1183-1200. Parameters: ----------- doy : int day of year, 1=jan 1 yr_days : int number of days in a year, 365 or 366 latitude : float latitude [degrees] Returns: -------- dayl : float daylength [hrs] """ deg2rad = pi / 180.0 latr = latitude * deg2rad sindec = -sin(23.5 * deg2rad) * cos(2.0 * pi * (doy + 10.0) / yr_days) a = sin(latr) * sindec b = cos(latr) * cos(asin(sindec)) dayl = 12.0 * (1.0 + (2.0 / pi) * asin(a / b)) return dayl
def __init__(self, matrix=None, scale=None, rotation=None, translation=None): params = any(param is not None for param in (scale, rotation, translation)) if params and matrix is not None: raise ValueError("You cannot specify the transformation matrix and" " the implicit parameters at the same time.") elif matrix is not None: if matrix.shape != (3, 3): raise ValueError("Invalid shape of transformation matrix.") self._matrix = matrix elif params: if scale is None: scale = 1 if rotation is None: rotation = 0 if translation is None: translation = (0, 0) self._matrix = np.array([ [math.cos(rotation), - math.sin(rotation), 0], [math.sin(rotation), math.cos(rotation), 0], [ 0, 0, 1] ]) self._matrix[0:2, 0:2] *= scale self._matrix[0:2, 2] = translation else: # default to an identity transform self._matrix = np.eye(3)
def refresh(self, matrix): matrix.fade(0.995) y0 = matrix.height/2 x0 = matrix.width/2 if self.angle >= pi: x0 -= 1 if self.angle > (0.5*pi) and self.angle < (1.5*pi): y0 -= 1 x1 = int(self.x0 + self.radius * sin(self.angle-self.astep)) y1 = int(self.y0 + self.radius * cos(self.angle+self.astep)) x2 = int(self.x0 + self.radius * sin(self.angle)) y2 = int(self.y0 + self.radius * cos(self.angle)) matrix.drawPoly( [(self.x0, self.y0), (x1, y1), (x2, y2)], hsvToRgb(self.hue) ) self.hue = fmod(self.hue+self.hstep, 1.0) self.angle += self.astep
def sumVectors(self, vectors): """ sum all vectors (including targetvector)""" endObstacleVector = (0,0) ##generate endvector of obstacles #sum obstaclevectors for vector in vectors: vectorX = math.sin(math.radians(vector[1])) * vector[0] # x-position vectorY = math.cos(math.radians(vector[1])) * vector[0] # y-position endObstacleVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY) #mean obstaclevectors if len(vectors) > 0: endObstacleVector = (endObstacleVector[0]/len(vectors), endObstacleVector[1]/len(vectors)) #add targetvector targetVector = self.target if targetVector != 0 and targetVector != None: vectorX = math.sin(math.radians(targetVector[1])) * targetVector[0] # x-position vectorY = math.cos(math.radians(targetVector[1])) * targetVector[0] # y-position endVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY) #endVector = (endVector[0]/2, endVector[1]/2) else: endVector = endObstacleVector return endVector
def __init__(self, scale=1.0): self.translation = Vector3() self.rotation = Vector3() self.initialHeight = Vector3(0, 0, scale*StewartPlatformMath.SCALE_INITIAL_HEIGHT) self.baseJoint = [] self.platformJoint = [] self.q = [] self.l = [] self.alpha = [] self.baseRadius = scale*StewartPlatformMath.SCALE_BASE_RADIUS self.platformRadius = scale*StewartPlatformMath.SCALE_PLATFORM_RADIUS self.hornLength = scale*StewartPlatformMath.SCALE_HORN_LENGTH self.legLength = scale*StewartPlatformMath.SCALE_LEG_LENGTH; for angle in self.baseAngles: mx = self.baseRadius*cos(radians(angle)) my = self.baseRadius*sin(radians(angle)) self.baseJoint.append(Vector3(mx, my)) for angle in self.platformAngles: mx = self.platformRadius*cos(radians(angle)) my = self.platformRadius*sin(radians(angle)) self.platformJoint.append(Vector3(mx, my)) self.q = [Vector3()]*len(self.platformAngles) self.l = [Vector3()]*len(self.platformAngles) self.alpha = [0]*len(self.beta)
def update_location(self, delta_encoder_count_1, delta_encoder_count_2): """ Update the robot's location @rtype : DifferentialDriveRobotLocation @return: Updated location @param delta_encoder_count_1: Count of wheel 1's encoder since last update @param delta_encoder_count_2: Count of wheel 2's encoder since last update @type delta_encoder_count_1: int @type delta_encoder_count_2: int """ dfr = delta_encoder_count_2 * 2 * math.pi / self.robot_parameters.steps_per_revolution dfl = delta_encoder_count_1 * 2 * math.pi / self.robot_parameters.steps_per_revolution ds = (dfr + dfl) * self.robot_parameters.wheel_radius / 2 dz = (dfr - dfl) * self.robot_parameters.wheel_radius / self.robot_parameters.wheel_distance self.location.x_position += ds * math.cos(self.location.z_position + dz / 2) self.location.y_position += ds * math.sin(self.location.z_position + dz / 2) self.location.z_position += dz self.globalLocation.x_position += ds * math.cos(self.globalLocation.z_position + dz / 2) self.globalLocation.y_position += ds * math.sin(self.globalLocation.z_position + dz / 2) self.globalLocation.z_position += dz return self.location, self.globalLocation
def distance(origin, destination): """ Calculates both distance and bearing """ lat1, lon1 = origin lat2, lon2 = destination if lat1>1000: (lat1,lon1)=dm2dd(lat1,lon1) (lat2,lon2)=dm2dd(lat2,lon2) print('converted to from ddmm to dd.ddd') radius = 6371 # km dlat = math.radians(lat2-lat1) dlon = math.radians(lon2-lon1) a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) \ * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2) c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) d = radius * c def calcBearing(lat1, lon1, lat2, lon2): dLon = lon2 - lon1 y = math.sin(dLon) * math.cos(lat2) x = math.cos(lat1) * math.sin(lat2) \ - math.sin(lat1) * math.cos(lat2) * math.cos(dLon) return math.atan2(y, x) bear= math.degrees(calcBearing(lat1, lon1, lat2, lon2)) return d,bear
def update(self): if self.turning_right: self.rot -= 5 if self.turning_left: self.rot += 5 a = [0.0,0.0] if self.boost_endtime > rabbyt.get_time(): f = 3*(self.boost_endtime - rabbyt.get_time())/self.boost_length a[0] += cos(radians(self.boost_rot))*f a[1] += sin(radians(self.boost_rot))*f self.create_boost_particle() if self.accelerating: a[0] += cos(radians(self.rot))*.9 a[1] += sin(radians(self.rot))*.9 self.create_dust_particle(self.dust_r) self.create_dust_particle(self.dust_l) ff = .9 # Friction Factor self.velocity[0] *= ff self.velocity[1] *= ff self.velocity[0] += a[0] self.velocity[1] += a[1] self.x += self.velocity[0] self.y += self.velocity[1]
def distance(xlat, xlon, ylat, ylon): dlon = ylon - xlon dlat = ylat - xlat a = sin(dlat / 2) ** 2 + cos(xlat) * cos(ylat) * sin(dlon / 2) ** 2 c = 2 * atan2(sqrt(a), sqrt(1 - a)) distance = R * c return distance
def scale(self): if math.cos(self.rotation) == 0: # sin(self.rotation) == 1 scale = self._matrix[0, 1] else: scale = self._matrix[0, 0] / math.cos(self.rotation) return scale
def Rx(theta): theta = m.radians(theta) return np.array([[1, 0, 0], [0, m.cos(theta), -m.sin(theta)], [0, m.sin(theta), m.cos(theta)]])
def move(x, y, step, angle): nx = x + step * math.cos(angle) ny = y - step * math.sin(angle) return nx, ny
def getTarget(self, angle, distance): new_target_dx = math.cos(math.radians(angle)) * distance new_target_dy = math.sin(math.radians(angle)) * distance return Position(self.x + new_target_dx, self.y + new_target_dy)
def pullBodyModelAtBackIntoRelativeDirection(self, pull_angle, speed_fact): pull_angle_BM = pull_angle + math.atan2(-self.segm[2][-1][1], -self.segm[2][-1][0]) self.pull_back[0] = speed_fact * math.cos(pull_angle_BM) # pull x self.pull_back[1] = speed_fact * math.sin(pull_angle_BM) # pull y
def pullBodyModelAtFrontIntoRelativeDirection(self, pull_angle, speed_fact): pull_angle_BM = pull_angle + math.atan2(self.segm[0][-1][1], self.segm[0][-1][0]) self.pull_front[0] = speed_fact * math.cos(pull_angle_BM) # pull x self.pull_front[1] = speed_fact * math.sin(pull_angle_BM) # pull y
def get_cartesian_coordinates(self): return ((self.back_pos),(self.back_pos[0] + self.length * math.cos(self.angle), self.back_pos[1] + self.length * math.sin(self.angle)))
def cos(a): return m.cos(a)
def Ry_rad(theta): return np.array([[m.cos(theta), 0, m.sin(theta)], [0, 1, 0], [-m.sin(theta), 0, m.cos(theta)]])
def Rz(theta): theta = m.radians(theta) return np.array([[m.cos(theta), -m.sin(theta), 0], [m.sin(theta), m.cos(theta), 0], [0, 0, 1]])
def Rz_rad(theta): return np.array([[m.cos(theta), -m.sin(theta), 0], [m.sin(theta), m.cos(theta), 0], [0, 0, 1]])
def fxy1(x, y): fxy = sin(x) + cos(y) print('fxy1: ' + str(fxy)) return fxy
def Rx_rad(theta): return np.array([[1, 0, 0], [0, m.cos(theta), -m.sin(theta)], [0, m.sin(theta), m.cos(theta)]])
def vxy(self) -> (float, float): theta = self.theta return -self.speed * cos(theta), -self.speed * sin(theta)
def Ry(theta): theta = m.radians(theta) return np.array([[m.cos(theta), 0, m.sin(theta)], [0, 1, 0], [-m.sin(theta), 0, m.cos(theta)]])
def speedDown(self): # self.speed -= SHIP_THRUST_AMOUNT self.velocity.dx += math.sin(math.radians( self.angle)) * SHIP_THRUST_AMOUNT self.velocity.dy -= math.cos(math.radians( self.angle)) * SHIP_THRUST_AMOUNT
def setup_coords_recursive(rootnode, parentnode, start_x, start_y, go_x, go_y, NODE_R, PRIMARY_SPACE, PAIR_SPACE, FLIPPED): cross_x = -go_y cross_y = go_x children_width = len(rootnode.children_) * NODE_R * 2 rootnode.go_x_ = go_x rootnode.go_y_ = go_y if(len(rootnode.children_) == 1) : rootnode.x_ = start_x rootnode.y_ = start_y if(rootnode.children_[0].is_pair_): setup_coords_recursive(rootnode.children_[0], rootnode, start_x + go_x * PRIMARY_SPACE, start_y + (-1 if FLIPPED else 1) * go_y * PRIMARY_SPACE, go_x, go_y, NODE_R, PRIMARY_SPACE, PAIR_SPACE, FLIPPED) elif(rootnode.children_[0].is_pair_ == False and rootnode.children_[0].index_a_ < 0): setup_coords_recursive(rootnode.children_[0], rootnode, start_x, start_y, go_x, go_y, NODE_R, PRIMARY_SPACE, PAIR_SPACE, FLIPPED) else: setup_coords_recursive(rootnode.children_[0], rootnode, start_x + go_x * PRIMARY_SPACE, start_y + (-1 if FLIPPED else 1) * go_y * PRIMARY_SPACE, go_x, go_y, NODE_R, PRIMARY_SPACE, PAIR_SPACE, FLIPPED) elif(len(rootnode.children_) > 1) : npairs = 0 for ii in range(0, len(rootnode.children_)): if(rootnode.children_[ii].is_pair_) : npairs+=1 circle_length = (len(rootnode.children_) + 1) * PRIMARY_SPACE + (npairs + 1) * PAIR_SPACE circle_radius = circle_length / (2 * math.pi) length_walker = PAIR_SPACE / 2.0 if (parentnode == None) : rootnode.x_ = go_x * circle_radius rootnode.y_ = go_y * circle_radius else : rootnode.x_ = parentnode.x_ + go_x * circle_radius rootnode.y_ = parentnode.y_ + (-1 if FLIPPED else 1) * go_y * circle_radius for ii in range(0,len(rootnode.children_)): length_walker += PRIMARY_SPACE if(rootnode.children_[ii].is_pair_) : length_walker += PAIR_SPACE / 2.0 rad_angle = length_walker/circle_length * 2 * math.pi - math.pi / 2.0 child_x = rootnode.x_ + math.cos(rad_angle) * cross_x * circle_radius + math.sin(rad_angle) * go_x * circle_radius child_y = rootnode.y_ + (-1 if FLIPPED else 1) * math.cos(rad_angle) * cross_y * circle_radius + (-1 if FLIPPED else 1) * math.sin(rad_angle) * go_y * circle_radius child_go_x = child_x - rootnode.x_ child_go_y = child_y - rootnode.y_ child_go_len = math.sqrt(child_go_x * child_go_x + child_go_y * child_go_y) setup_coords_recursive(rootnode.children_[ii], rootnode, child_x, child_y, child_go_x / child_go_len, (-1 if FLIPPED else 1) * child_go_y / child_go_len, NODE_R, PRIMARY_SPACE, PAIR_SPACE, FLIPPED) if(rootnode.children_[ii].is_pair_) : length_walker += PAIR_SPACE / 2.0 else : rootnode.x_ = start_x rootnode.y_ = start_y
def rotate(self, angle): x1 = self.x y1 = self.y self.x = x1 * math.cos(angle) - y1 * math.sin(angle) self.y = x1 * math.sin(angle) + y1 * math.cos(angle)
def EnergyLoss(angle): return 2 * m_e / m_T2 * E_0 * (1-math.cos(angle/180. * math.pi))
global endlat, endlong endlat = 13.3477168 endlong = 74.7921555 matchdist() print("Ball search starting") startlat, startlong = pos_update() x = startlat y = startlong way = [] r = 5 val = "NOTFOUND" ang = 60 # plt.plot(x,y,marker='o',markersize=5, color='red') while val != "FOUND": for i in range(0, 361, ang): cx = cos(degrees(i)) * r / 111035 + x cy = sin(degrees(i)) * r / 111035 + y a = [] a.append(cx) a.append(cy) way.append(a) print("way", way) # plt.plot(cx,cy,marker='o',markersize=3, color='green') # plt.draw() # plt.pause(0.001) for i in range(len(way)): endlat = way[i][0] endlong = way[i][1] matchdist() val = getball() if (val == 'FOUND'):
def speedUp(self): # self.speed += SHIP_THRUST_AMOUNT self.velocity.dx -= math.sin(math.radians( self.angle)) * SHIP_THRUST_AMOUNT self.velocity.dy += math.cos(math.radians( self.angle)) * SHIP_THRUST_AMOUNT
def get_lr(self, epoch): if self.max_epoch <= 0: return self.base_lr theta = math.pi * epoch / self.max_epoch return self.base_lr * (math.cos(theta) + 1.0) * 0.5
def cos(self): self.result=False self.current=math.cos(math.radians(float(txtDisplay.get()))) self.display(self.current)
def a_cosine_learning_rate(current_step, base_lr, warmup_steps, decay_steps): base = float(current_step - warmup_steps) / float(decay_steps) learning_rate = (1 + math.cos(base * math.pi)) / 2 * base_lr return learning_rate
def plot(ax, filename, desired_step=None, robot_overlay=False): with open(filename) as f: data = f.read() data = data.split('\n') del data[0] # Remove first comment line del data[-1] # Remove last empty line if 'robots.dat' in filename: X = [] Y = [] DX = [] DY = [] for row in data: tokens = row.split(',') step = int(tokens[0]) if step == desired_step: for i in range(1, len(tokens), 3): X.append(float(tokens[i])) Y.append(float(tokens[i+1])) theta = float(tokens[i+2]) DX.append(cos(theta)) DY.append(sin(theta)) size = 9 * CM_TO_PIXELS #plt.scatter(X, Y, color='g', s=size_factor*size) if not robot_overlay: scaled_scatter_plot(ax, X, Y, 'g', size) plt.quiver(X, Y, DX, DY, scale=15.0) elif 'pucks.dat' in filename: X = [] Y = [] for row in data: tokens = row.split(',') step = int(tokens[0]) if step == desired_step: for i in range(1, len(tokens), 2): X.append(float(tokens[i])) Y.append(float(tokens[i+1])) size = 2.3 * CM_TO_PIXELS #plt.scatter(X, Y, color='r', s=size_factor*size) scaled_scatter_plot(ax, X, Y, 'r', size) elif 'landmarks.dat' in filename: X = [] Y = [] for row in data: tokens = row.split(',') for i in range(0, len(tokens), 2): X.append(float(tokens[i])) Y.append(float(tokens[i+1])) size = 6 * CM_TO_PIXELS #plt.scatter(X, Y, color='b', s=size_factor*size) scaled_scatter_plot(ax, X, Y, 'b', size, filled=False) plt.axis('square') plt.xlim(0, width) plt.ylim(0, height) plt.tick_params( axis='both', which='both', top='off', bottom='off', left='off', right='off', labelbottom='off', labelleft='off') if robot_overlay: plt.title('') else: plt.title(desired_step)
cam_pose[2, -1] = args.radius cam_poses.append(cam_pose) for i in range(1, num_images): cam_pose = torch.eye(4, device=device) #angle = (math.pi / 6.0) * i # 30 degs angle = (math.pi / 3.0) * i # 30 degs # R_x #cam_pose_2[1, 1] = math.cos(angle) #cam_pose_2[1, 2] = -math.sin(angle) #cam_pose_2[2, 1] = math.sin(angle) #cam_pose_2[2, 2] = math.cos(angle) # R_y cam_pose[0, 0] = math.cos(angle) cam_pose[2, 0] = -math.sin(angle) cam_pose[0, 2] = math.sin(angle) cam_pose[2, 2] = math.cos(angle) # R_z #cam_pose_2[0, 0] = math.cos(angle) #cam_pose_2[0, 1] = -math.sin(angle) #cam_pose_2[1, 0] = math.sin(angle) #cam_pose_2[1, 1] = math.cos(angle) cam_poses.append(cam_pose) ''' #angle = 0 #elevation = 0 #radius = args.radius
print("\nResult(task_3) for a = %d, b = %d, c = %d is equal %d" % (a, b, c, result)) #------------------------------------------------------------ # Task4 a = 101 b = 144 c = 155 result = (a - b * c) / (a + b) % c print("\nResult(task_4) for a = %d, b = %d, c = %d is equal %d" % (a, b, c, result)) #------------------------------------------------------------ # Task5 a = 12 b = 11241 c = 123 result = math.fabs(a - b) / (a + b)**3 - math.cos(c) print("\nResult(task_5) for a = %d, b = %d, c = %d is equal %0.4f" % (a, b, c, result)) #------------------------------------------------------------ # Task6 a = 115 b = 222 c = 666 result = (math.log1p(1 + c) / -b)**4 + math.fabs(a) print("\nResult(task_6) for a = %d, b = %d, c = %d is equal %d" % (a, b, c, result)) #------------------------------------------------------------
def _get_lr(_base_lr, now_epoch, _t_epoch=Config.t_epoch, _eta_min=1e-05): return _eta_min + (_base_lr - _eta_min) * ( 1 + math.cos(math.pi * now_epoch / _t_epoch)) / 2
def goAngle(x0, y0, angle, length): return x0 + cos(angle)*length, y0 + sin(angle)*length
def inverseKinematics(xB, yB): # Actuator length constants. LENGTHONE = 17 # Length of the z-axis actuator. LENGTHTWO = 10.5 # Length of the shortest actuator. # Actuator yam coordinates for desired endpoint calculation. ARMPOSITIONX = 12 ARMPOSITIONY = -6 #print("Actuator lengths are [1,2]:[", LENGTHONE, ",", LENGTHTWO, "]", sep="") #print("Arm positions are registerd as [x, y]:[", ARMPOSITIONX, ", ", ARMPOSITIONY, "]", sep="") x = xB # abs(xB)# - ARMPOSITIONX) y = yB # abs(yB)# - ARMPOSITIONY) temp = (x * x + y * y - LENGTHONE * LENGTHONE - LENGTHTWO * LENGTHTWO) / (2 * LENGTHONE * LENGTHTWO) if temp > 1: print("Rounding to 1") temp = 1 elif temp < -1: print("Rounding to -1") temp = -1 theta2 = math.acos(temp) try: theta1A = math.atan(y / x) except: print("issue... setting theta1A to 0") theta1A = 0 # This should work since it means alpha == 0 (see slide 31). # ((math.atan(y/x)) + (math.atan( (LENGTHTWO*math.sin(theta2)) / (LENGTHONE + LENGTHTWO*math.cos(theta2)) ))) # If the negative case is present... if 1: # XXX how to deal with case divisions and when theta2 == 0 theta2 = theta2 * -1 theta1B = math.atan((LENGTHTWO * math.sin(theta2)) / (LENGTHONE + LENGTHTWO * math.cos(theta2))) theta1 = theta1A + theta1B # Else the case is positive else: print("Positive case identified!") theta1B = math.atan((LENGTHTWO * math.sin(theta2)) / (LENGTHONE + LENGTHTWO * math.cos(theta2))) theta1 = theta1A - theta1B # print(theta1, theta2) """print("x is ", x, ", x*x is ", x*x, sep = "") print("y is ", y, ", y*y is ", y*y, sep = "") print("x*x + y*y is", x*x + y*y) print("l1 is ", LENGTHONE , ", l1 squared is ", LENGTHONE*LENGTHONE, sep = "") print("l2 is ", LENGTHTWO, ", l2 squared is ", LENGTHTWO*LENGTHTWO, sep = "") print("2*l1*l2 is", 2*LENGTHONE*LENGTHTWO) print("Their sum is", (x*x + y*y - LENGTHONE*LENGTHONE - LENGTHTWO*LENGTHTWO)) print("Divided, their value is", (x*x + y*y - LENGTHONE*LENGTHONE - LENGTHTWO*LENGTHTWO) / (2*LENGTHONE*LENGTHTWO)) print("In cos-1, the final value is", math.acos( (x*x + y*y - LENGTHONE*LENGTHONE - LENGTHTWO*LENGTHTWO) / (2*LENGTHONE*LENGTHTWO) )) print() print("(y/x) is ", y/x, ", where tan-1(y/x) is ,", math.atan(y/x), sep = "") print("sin(theta2) is ", math.sin(theta2), ", where it is then scaled with l2 to ", (LENGTHTWO*math.sin(theta2)), sep = "") print("cos(theta2) is ", math.cos(theta2), ", where it is then scaled with l2 and l1 added to ", (LENGTHONE + LENGTHTWO*math.cos(theta2)), sep = "") print("tan-1 of this is then", math.atan( (LENGTHTWO*math.sin(theta2)) / (LENGTHONE + LENGTHTWO*math.cos(theta2)) )) """ # print("Together, they make", ((math.atan(y/x)) + (math.atan( (LENGTHTWO*math.sin(theta2)) / (LENGTHONE + LENGTHTWO*math.cos(theta2)) ))) ) # print("\nJust to be clear, our values are", theta1, "and", theta2) # Covert to roboarm-friendly jargen. theta1 = round(math.degrees(theta1)) # - 90 theta2 = (round(math.degrees(theta2)) + 90) if theta2 < 0: theta2 = theta2 * 0.8 else: theta2 = theta2 * 1.1 # print("Which then converts to", theta1, "and", theta2, "\n") # print("Theta1 is,", theta1, "--- and Theta2 is,", theta2, "---") return theta1, theta2
# for k in range(int(1/grid.scale)): # grid.iteration() # grid.iteration() #self.grids[1].update_border(self.grids[0]) def snap(self, num): # self.grids[0].snap("0-" + str(int(num)).zfill(3) + "a") # self.grids[0].update_all(self.grids[1]) for i, grid in enumerate(self.grids): grid.snap(str(int(i)) + "-" + str(int(num)).zfill(3)) self.grids[0].update_all_all(self.grids[1]) self.grids[0].snap("_sum_"+str(int(i)) + "-" + str(int(num)).zfill(3)) self.grids[0].save_to_file() cylinder = lambda x,y: ((x-cx)**2+(y-cy)**2<r**2) | (y == 0) | (y == ly) prism = lambda x,y: (abs(x-cx) + abs(y-cy) < r) | (y==0) | (y==ly) g = Chimera() g.add_grid(nx,ny,fobstacle=prism, main=True) g.add_grid(320, 320, fobstacle=prism, scale=1/scale, v=np.array([130-40*sqrt(2), ly/2]), R=np.array([[math.cos(math.pi/4), math.sin(math.pi/4)], [-math.sin(math.pi/4), math.cos(math.pi/4)]])) g.grids[1].update_border(g.grids[0]) temp = 250 for time in range(maxIter+1): print("Iteration", time) g.iteration() if (time%temp==0): g.snap(time/temp)