def distances_per_degree(lat, a=SEMI_MAJOR_AXIS, inverse_flattening=INVERSE_FLATTENING):
""" Returns a tuple containing arc distances in the units of the semi-major axis.
The first is the distance along an arc of one degree at a constant latitude (lat).
The second is the distance along an arc of one degree at a constant lng centered on the given lat.
Arguments:
lat - the latitude at which the distances apply
a - the semi-major axis of the ellipsoid for the coordinate reference system
inverse_flattening - the inverse of the ellipsoid's flattening parameter
"""
f = 1.0 / inverse_flattening
e_squared = f * (2.0 - f) # e^2 = 2f - f^2
# N - radius of curvature in the prime vertical, (tangent to ellipsoid at latitude)
# N(lat) = a/(1-e^2*sin^2(lat))^0.5
N = a / math.sqrt(1.0 - e_squared * (math.pow(math.sin(lat * math.pi / 180.0), 2.0)))
# M - radius of curvature in the prime meridian, (tangent to ellipsoid at latitude)
# M(lat) = a(1-e^2)/(1-e^2*sin^2(lat))^1.5
M = a * (1.0 - e_squared) / math.pow(1.0 - e_squared * math.pow(math.sin(lat * math.pi / 180.0), 2.0), 1.5)
# longitude is irrelevant for the calculations to follow so simplify by using longitude = 0, so Y = 0
# X = Ncos(lat)cos(long). long = 0, so cos(long) = 1.0
X = N * math.cos(lat * math.pi / 180.0) * 1.0
lngdistanceperdegree = math.pi * X / 180.0
latdistanceperdegree = math.pi * M / 180.0
return (lngdistanceperdegree, latdistanceperdegree)
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 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 AngularDistance(RA1, DEC1, RA2, DEC2):
try:
RA1 = float(RA1)
DEC1 = float(DEC1)
RA2 = float(RA2)
DEC2 = float(DEC2)
# Converting everything to radians
ra1rad = RA1 * math.pi/180.
ra2rad = RA2 * math.pi/180.
dec1rad = DEC1 * math.pi/180.
dec2rad = DEC2 * math.pi/180.
# Calculate scalar product for determination of angular separation
x=math.cos(ra1rad)*math.cos(dec1rad)*math.cos(ra2rad)*math.cos(dec2rad)
y=math.sin(ra1rad)*math.cos(dec1rad)*math.sin(ra2rad)*math.cos(dec2rad)
z=math.sin(dec1rad)*math.sin(dec2rad)
rad=math.acos(x+y+z)
# Use Pythargoras approximation if rad < 1 arcsec
if rad<0.000004848:
rad=math.sqrt((math.cos(dec1rad)*(ra1rad-ra2rad))**2+(dec1rad-dec2rad)**2)
pass
# Angular separation in degrees
Angle = rad*180/math.pi
return Angle
except Exception, message:
print message
return float('nan')
def 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 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 vorbiswindow(j, K):
if j < 0:
return 0
elif j >= K:
return 0
z = sin(pi / K * (j + 0.5))
return sin(pi * 0.5 * z * z)
def 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 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 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 execute(self, context):
A = 6.283185307179586476925286766559 / 3
verts = [(sin(A * 1), 0.0, cos(A * 1)),
(sin(A * 2), 0.0, cos(A * 2)),
(sin(A * 3), 0.0, cos(A * 3)),
]
faces = [(0, 1, 2)]
mesh = bpy.data.meshes.new("Cube")
bm = bmesh.new()
for v_co in verts:
bm.verts.new(v_co)
for f_idx in faces:
bm.faces.new([bm.verts[i] for i in f_idx])
bm.to_mesh(mesh)
mesh.update()
object_utils.object_data_add(context, mesh)
return{'FINISHED'}
def __init__(self,pos,direction,d_range):
self.surface = pygame.surface.Surface((50,50))
self.surface.fill((0,250,200))
self.rect = pygame.rect.Rect(pos[0],pos[1],50,50)
self.dir = direction
self.dist = 0
self.range = d_range
speed = 2
dx = 0
dy = 0
if self.dir>0:
if self.dir>90:
dy = speed*math.sin(180-self.dir)
dx = speed*math.cos(180-self.dir)
else:
dy = speed*math.sin(self.dir)
dx = speed*math.cos(self.dir)
else:
if self.dir<-90:
dy = speed*math.sin(180+self.dir)
dx = speed*math.cos(180+self.dir)
else:
dy = speed*math.sin(self.dir)
dx = speed*math.cos(self.dir)
self.dx = dx
self.dy = dy
def __init__(self, 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 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 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 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 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 flange(typeofFlange):
#variables
points = [] #collection
nP = 100
nSin = 10
#conditional assignment
if (typeofFlange == "floppy"):
t = 3
r = 15
amp = 10
if (typeofFlange == "extrafloppy"):
t = 3
r = 30
amp = 20
#iteration to calculate points
for i in rs.frange(0, nP, 1):
x = r * math.sin(i*360/(nP-1))
y = r * math.cos(i*360/(nP-1))
z = amp*math.sin(i*nSin*360/(nP-1))
#Feature
point = rs.AddPoint([x,y,z])
points.append(point)
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 update(self):
if self.turning_right:
self.rot -= 5
if self.turning_left:
self.rot += 5
a = [0.0,0.0]
if self.boost_endtime > rabbyt.get_time():
f = 3*(self.boost_endtime - rabbyt.get_time())/self.boost_length
a[0] += cos(radians(self.boost_rot))*f
a[1] += sin(radians(self.boost_rot))*f
self.create_boost_particle()
if self.accelerating:
a[0] += cos(radians(self.rot))*.9
a[1] += sin(radians(self.rot))*.9
self.create_dust_particle(self.dust_r)
self.create_dust_particle(self.dust_l)
ff = .9 # Friction Factor
self.velocity[0] *= ff
self.velocity[1] *= ff
self.velocity[0] += a[0]
self.velocity[1] += a[1]
self.x += self.velocity[0]
self.y += self.velocity[1]
def __init__(self, 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 distance(origin, destination):
"""
Calculates both distance and bearing
"""
lat1, lon1 = origin
lat2, lon2 = destination
if lat1>1000:
(lat1,lon1)=dm2dd(lat1,lon1)
(lat2,lon2)=dm2dd(lat2,lon2)
print('converted to from ddmm to dd.ddd')
radius = 6371 # km
dlat = math.radians(lat2-lat1)
dlon = math.radians(lon2-lon1)
a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) \
* math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
d = radius * c
def calcBearing(lat1, lon1, lat2, lon2):
dLon = lon2 - lon1
y = math.sin(dLon) * math.cos(lat2)
x = math.cos(lat1) * math.sin(lat2) \
- math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
return math.atan2(y, x)
bear= math.degrees(calcBearing(lat1, lon1, lat2, lon2))
return d,bear
def sumVectors(self, vectors):
""" sum all vectors (including targetvector)"""
endObstacleVector = (0,0)
##generate endvector of obstacles
#sum obstaclevectors
for vector in vectors:
vectorX = math.sin(math.radians(vector[1])) * vector[0] # x-position
vectorY = math.cos(math.radians(vector[1])) * vector[0] # y-position
endObstacleVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
#mean obstaclevectors
if len(vectors) > 0:
endObstacleVector = (endObstacleVector[0]/len(vectors), endObstacleVector[1]/len(vectors))
#add targetvector
targetVector = self.target
if targetVector != 0 and targetVector != None:
vectorX = math.sin(math.radians(targetVector[1])) * targetVector[0] # x-position
vectorY = math.cos(math.radians(targetVector[1])) * targetVector[0] # y-position
endVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
#endVector = (endVector[0]/2, endVector[1]/2)
else:
endVector = endObstacleVector
return endVector
def 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 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 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 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 __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 PointsFromAngle(angle):
### Returns The Unit Direction Vector With Given Angle ###
return math.cos(angle), math.sin(angle)
def SunShape(x, y, r, e, l, b):
p, a, h=[], 2*pi/e, r*l
c, d=[0, -a/2][b], [a/2, 0][b]
for i in range(e):
p+=[(x+r*cos(i*a+c), y+r*sin(i*a+c)), (x+h*cos(i*a+d), y+h*sin(i*a+d))]
canvas.create_polygon(p, fill="white")
def calculate_distance(lng1, lat1, lng2, lat2): # 经度和纬度 longitude and latitude，计算球上两点的距离
RADIUS = 6378.137 # 半径，单位km
PI = math.pi
return 2 * RADIUS * math.asin(math.sqrt(pow(math.sin(PI * (lat1 - lat2) / 360), 2) + math.cos(PI * lat1 / 180) * \
math.cos(PI * lat2 / 180) * pow(math.sin(PI * (lng1 - lng2) / 360), 2)))
def f(x):
return math.sin(x) - 3 * math.cos(x**2) + 2 * (math.sin(x))**2
def dzjolt(self, dz): dzj = self.Ajolt * math.sin(self.tjolt * 20) f = min(math.exp(-self.tjolt), 0.05 * self.tjolt) return math.mix(dz, dzj, f)
def sway(self): return self.Asway * math.sin(math.tau * (self.t / self.Tsway + self.swayphase))
print("Find the correct paralltic angle")
# https://en.wikipedia.org/wiki/HR_8799
Declination = 21 + 8 * (1 / 60) + 0.302 * (1 / 3600)
print("Declination of HR8799; 21° 08' 03.302''")
print("Declination=", Declination, "°")
Declination = math.radians(Declination)
# https://www.ifa.hawaii.edu/mko/coordinates.shtml
Geographical_latitude = 19 + 49 * (1 / 60) + 35.61788 * (1 / 3600)
print("Geographical latitude of Keck 2: 19° 49' 35.61788''")
print("Geographical latitude=", Geographical_latitude, "°")
Geographical_latitude = math.radians(Geographical_latitude)
para_angle_thm = np.zeros(len(fn_list))
for i in range(len(fn_list)):
x = (math.sin(
HA[i])) / (math.tan(Geographical_latitude) * math.cos(Declination) -
math.sin(Declination) * math.cos(HA[i]))
para_angle_thm[i] = math.atan(x)
start = time.time()
for img, angle in zip(imgcube, para_angle_thm):
img_cADI += ndimage.rotate(
img - PSF_cADI, # subtract the PSF before rotationg
-angle, # in degree. rotate the image counter-clockwisely if the value is positive
reshape=False) # retain the same shape as the input image
plt.ion()
plt.figure(figsize=(6, 6))
plt.imshow(img_cADI, cmap="gray")
plt.xlim(344, 680)
plt.ylim(344, 680)
import random
import math
import pygame
background_colour = (255,255,255)
(width, height) = (1024, 720)
gravity = (math.pi/2, 0.08)
drag = 0.999
elasticity = 0.95
def addVectors((angle1,length1), (angle2,length2)):
x = math.cos(angle1)*length1 + math.cos(angle2)*length2
y = math.sin(angle1)*length1+math.sin(angle2)*length2
length = math.sqrt(x*x+y*y)
angle = math.atan2(y,x)
return (angle, length)
def findParticle(particles, x, y):
for p in particles:
if math.sqrt((p.x-x)**2+(p.y-y)**2) <= p.size:
return p
return None
def collide(p1,p2):
dx = p1.x - p2.x
dy = p1.y - p2.y
lengd = math.sqrt(dx**2+dy**2)
if lengd < (p1.size + p2.size):
tangAngle = math.atan2(dy,dx)
(p1.angle,p2.angle) = (p1.angle + math.pi/2.0 - tangAngle, p2.angle + math.pi/2.0 - tangAngle)
(p1.speed,p2.speed) = (p2.speed, p1.speed)
#########################################################
# FINDING THETAS #
#########################################################
theta0 = 2 * math.pi * np.random.randn(
) # initial theta; consider adding 2 pi times randn
theta_list = np.empty(colnum) # list of all thetas
euler_theta(theta0, t, theta_list, func_theta) # stored in theta_list
#########################################################
# FINDING POSITION #
#########################################################
r_x0 = math.cos(theta0) # initial x-coordinate of position
r_y0 = math.sin(theta0) # initial y-coordinate of position
euler_r(r_x0, t, all_r[i, :, 0], theta_list, funcx_r) # x-coordinates
euler_r(r_y0, t, all_r[i, :, 1], theta_list, funcy_r) # y-coordinates
#########################################################
# PLOT RESULTS #
#########################################################
# Plots Theta values
plt.plot(t, theta_list, 'g')
plt.xlabel('Time (Seconds)')
plt.ylabel('Angle (Radians)')
plt.title('Angle v. Time: Trial ' + str((i + 1)))
def move(self):
self.x += math.cos(self.angle) * self.speed
self.y += math.sin(self.angle) * self.speed
(self.angle, self.speed) = addVectors((self.angle,self.speed),gravity)
self.speed*=drag
# readFile = open('dataDynamic/'+filename+'.txt', 'r')
# filename = str(i)
# readFile = open('dataraw/'+filename+'.txt', 'r')
readFile = open('out.txt', 'r')
txt = readFile.readlines()
readFile.close()
x = []
y = []
xy = []
fig = plt.figure()
for i in range(len(txt)):
if i % 2 == 0:
tmp = txt[i]
r = int(tmp.split(' ')[0])
angle = int(tmp.split(' ')[1])
x.append(r * math.sin(math.radians(angle)))
y.append(r * math.cos(math.radians(angle)))
xy.append((int(r * math.sin(math.radians(angle))),
int(r * math.cos(math.radians(angle)))))
def select_x(tmp):
return tmp[0]
xy.sort(key=select_x)
# print(xy)
def plot():
ax = fig.add_subplot(1, 1, 1)
ax.grid(True)
def funcy_r(theta,
posit_y): # differential equation for y-component of position
return (v0 * math.sin(theta) + k * math.cos(posit_y))
# Draw a solid blue circle in the center
#pygame.draw.circle(screen, (255, 0, 0), (startx, starty), 1)
anglectr = angleStart
for r in list:
coordinates = []
radian = anglectr * (math.pi / 180)
#print(str(r) +" at angle of "+str(radian))
anglectr = anglectr + 0.25
x = float(scaling) * float(r) * math.cos(radian)
y = float(scaling) * float(r) * math.sin(radian)
coordinates.append(x)
coordinates.append(y)
#print(angleStart)
#pygame.draw.circle(screen, (0, 255, 0), (x+startx, y+starty), 1)
pointlist.append(coordinates)
#time.sleep(0.1)
#pygame.display.flip()
#print(pointlist)
#time.sleep(50)
# Flip the display
import matplotlib.pyplot as plt
wbeamX=0.35 wbeamY=0.5 wcolumnZ=0.40 deckTh=0.20 wallTh=0.5 footTh=0.7 #Actions qdeck1=1e3 #N/m2 qdeck2=2e3 #N/m2 Qbeam=3e3 #N/m qunifBeam=5e3 qLinDeck2=30 #N/m Qwheel=5e3 #N firad=math.radians(31) #internal friction angle (radians) KearthPress=(1-math.sin(firad))/(1+math.sin(firad)) #Active coefficient of p densSoil=800 #mass density of the soil (kg/m3) densWater=1000 #mass density of the water (kg/m3) #Materials concrete=EHE_materials.HA30 reinfSteel=EHE_materials.B500S # concrete=SIA262_materials.c30_37 # reinfSteel=SIA262_materials.B500B eSize= 0.2 #length of elements # *** GEOMETRIC model (points, lines, surfaces) - SETS *** FEcase= xc.FEProblem() preprocessor=FEcase.getPreprocessor
def __getCosAndSin(self, angle):
return math.cos(angle), math.sin(angle)
# common functions
def twist_mesh(vertex_list, mesh, Axis, Angle, StartFrom, EndAt):
StartFrom, EndAt = min(StartFrom, EndAt) / 100.0, max(StartFrom,
EndAt) / 100.0
Angle = pi * Angle / 180.0
def rot_x((x, y, z), angle):
c = cos(angle)
s = sin(angle)
return (x, y * c - z * s, y * s + z * c)
def rot_y((x, y, z), angle):
c = cos(angle)
s = sin(angle)
return (x * c - z * s, y, x * s + z * c)
def rot_z((x, y, z), angle):
c = cos(angle)
s = sin(angle)
return (x * c - y * s, x * s + y * s, z)
if Axis == 'x':
d = max_x - min_x
rd = EndAt - StartFrom
for index, v in enumerate(mesh.verts):
t = (vertex_list[index][0] - min_x) / d
if StartFrom <= t <= EndAt:
ang = Angle * (t - StartFrom) / rd
v.co[0], v.co[1], v.co[2] = rot_x(vertex_list[index], ang)
def calcula_distancia_do_projetil(v,yo,t):
a = (v**2)/(2*9.8)
b = (1+(2*9.8*yo)/(v**2*(math.sin(t))**2))**(1/2)
c = math.sin(2*t)
return ((a)*(1+b)*(c))
def ovector(self, angle):
return math.cos(angle),math.sin(angle)
def g0(self, L):
"""
acceleration due to gravity at the elipsoid surface at latitude L
"""
return 9.7803267715 * (1 + 0.001931851353 * sin(L)**2) / \
sqrt(1 - 0.0066943800229 * sin(L)**2)
# issue 499
data = [1, 2, 3]
data = (item for item in data)
assert list(data) == [1, 2, 3]
# issue 557
from math import sin, log
class N:
def __float__(self):
return 42.0
num = N()
assert sin(num) == -0.9165215479156338
assert log(num) == 3.7376696182833684
# issue 560
class Base:
@classmethod
def test(cls):
return cls
class Derived(Base):
def __init__(self):
pass
import math
import numpy as np
from AirSimClient import *
client = MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
#client.takeoff()
#client.hover()
client.moveToZ(-6, 5)
duration = 1
speed_x = 3
speed_y = 3
pitch, roll, yaw = client.getPitchRollYaw()
vel = client.getVelocity()
vx = math.cos(yaw) * speed_x - math.sin(yaw) * speed_y
vy = math.sin(yaw) * speed_x + math.cos(yaw) * speed_y
drivetrain = DrivetrainType.ForwardOnly
yaw_mode = YawMode(is_rate=False, yaw_or_rate=0)
print(vel.z_val)
client.moveByVelocity(vx=0,
vy=0.5,
vz=0,
duration=duration + 5,
drivetrain=drivetrain,
yaw_mode=yaw_mode)
print(vel.z_val)
def load_data():
global canvas_extents, sensor_canvas_extents, world_extents, max_scanner_range
for filename in all_file_names:
logfile.read(filename)
global draw_objects
draw_objects = []
scale.configure(to=logfile.size() - 1)
# Insert: landmarks.
draw_objects.append(
Landmarks(logfile.landmarks, world_canvas, canvas_extents,
world_extents))
# Insert: reference trajectory.
positions = [
to_world_canvas(pos, canvas_extents, world_extents)
for pos in logfile.reference_positions
]
draw_objects.append(
Trajectory(positions,
world_canvas,
world_extents,
canvas_extents,
cursor_color="red",
background_color="#FFB4B4"))
# Insert: filtered trajectory.
if logfile.filtered_positions:
if len(logfile.filtered_positions[0]) > 2:
positions = [
tuple(
list(to_world_canvas(pos, canvas_extents, world_extents)) +
[pos[2]]) for pos in logfile.filtered_positions
]
else:
positions = [
to_world_canvas(pos, canvas_extents, world_extents)
for pos in logfile.filtered_positions
]
# If there is error ellipses, insert them as well.
draw_objects.append(
Trajectory(positions,
world_canvas,
world_extents,
canvas_extents,
standard_deviations=logfile.filtered_stddev,
cursor_color="blue",
background_color="lightblue",
position_stddev_color="#8080ff",
theta_stddev_color="#c0c0ff"))
# Insert: scanner data.
draw_objects.append(
ScannerData(logfile.scan_data, sensor_canvas, sensor_canvas_extents,
max_scanner_range))
# Insert: detected cylinders, in scanner coord system.
if logfile.detected_cylinders:
positions = [[
to_sensor_canvas(pos, sensor_canvas_extents, max_scanner_range)
for pos in cylinders_one_scan
] for cylinders_one_scan in logfile.detected_cylinders]
draw_objects.append(Points(positions, sensor_canvas, "#88FF88"))
# Insert: detected cylinders, transformed into world coord system.
if logfile.detected_cylinders and logfile.filtered_positions and \
len(logfile.filtered_positions[0]) > 2:
positions = []
for i in xrange(
min(len(logfile.detected_cylinders),
len(logfile.filtered_positions))):
this_pose_positions = []
pos = logfile.filtered_positions[i]
dx = cos(pos[2])
dy = sin(pos[2])
for pole in logfile.detected_cylinders[i]:
x = pole[0] * dx - pole[1] * dy + pos[0]
y = pole[0] * dy + pole[1] * dx + pos[1]
p = to_world_canvas((x, y), canvas_extents, world_extents)
this_pose_positions.append(p)
positions.append(this_pose_positions)
draw_objects.append(Points(positions, world_canvas, "#88FF88"))
# Insert: world objects, cylinders.
if logfile.world_cylinders:
positions = [[
to_world_canvas(pos, canvas_extents, world_extents)
for pos in cylinders_one_scan
] for cylinders_one_scan in logfile.world_cylinders]
draw_objects.append(Points(positions, world_canvas, "#DC23C5"))
# Start new canvas and do all background drawing.
world_canvas.delete(ALL)
sensor_canvas.delete(ALL)
for d in draw_objects:
d.background_draw()
import math
import random
import time
import datetime
from datetime import date
print("Sqrt : ",math.sqrt(25))
print("Degrees : ",math.degrees(2))
print("Pi : ",math.pi)
print("Sin : ",math.sin(2))
print("Cos : ",math.cos(0.5))
print("Tan : ",math.tan(0.23))
print("Factorial : ",math.factorial(4))
print("Randint : ",random.randint(0,5))
print("Random : ",random.random())
print("Random * 100 : ",random.random() * 100)
List = [1,4,True,800,"python",27,"hello"]
print("Random Choice : ",random.choice(List))
print("Time : ",time.time())
print("Date : ",date.fromtimestamp(454554))
def lla2utm(self, lla):
"""
Converts lat, lon, alt to Universal Transverse Mercator coordinates
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m)
Output: utm - (easting, northing, upping) in (m, m, m)
info - (zone, scale factor)
Algorithm from:
Snyder, J. P., Map Projections-A Working Manual, U.S. Geol. Surv.
Prof. Pap., 1395, 1987
Code segments from pygps project, Russ Nelson
"""
# Decompose lla
lat = lla[0]
lon = lla[1]
alt = lla[2]
# Determine the zone number
zoneNumber = int((lon+180.)/6) + 1
# Special zone for Norway
if (56. <= lat < 64.) and (3. <= lon < 12.):
zoneNumber = 32
# Special zones for Svalbard
if 72. <= lat < 84.:
if 0. <= lon < 9.:
zoneNumber = 31
elif 9. <= lon < 21.:
zoneNumber = 33
elif 21. <= lon < 33.:
zoneNumber = 35
elif 33. <= lon < 42.:
zoneNumber = 37
# Format the zone
zone = "%d%c" % (zoneNumber, self.utmLetterDesignator(lat))
# Determine longitude origin
lonOrigin = (zoneNumber - 1) * 6 - 180 + 3
# Convert to radians
latRad = geo.deg2rad(lat)
lonRad = geo.deg2rad(lon)
lonOriginRad = geo.deg2rad(lonOrigin)
# Conversion constants
k0 = 0.9996
eSquared = self.e**2
ePrimeSquared = eSquared/(1.-eSquared)
N = self.a/sqrt(1.-eSquared*sin(latRad)**2)
T = tan(latRad)**2
C = ePrimeSquared*cos(latRad)**2
A = (lonRad - lonOriginRad)*cos(latRad)
M = self.a*(
(1. -
eSquared/4. -
3.*eSquared**2/64. -
5.*eSquared**3/256)*latRad -
(3.*eSquared/8. +
3.*eSquared**2/32. +
45.*eSquared**3/1024.)*sin(2.*latRad) +
(15.*eSquared**2/256. +
45.*eSquared**3/1024.)*sin(4.*latRad) -
(35.*eSquared**3/3072.)*sin(6.*latRad))
M0 = 0.
# Calculate coordinates
x = k0*N*(
A+(1-T+C)*A**3/6. +
(5.-18.*T+T**2+72.*C-58.*ePrimeSquared)*A**5/120.) + 500000.
y = k0*(
M-M0+N*tan(latRad)*(
A**2/2. +
(5.-T+9.*C+4.*C**2)*A**4/24. +
(61.-58.*T+T**2+600.*C-330.*ePrimeSquared)*A**6/720.))
# Calculate scale factor
k = k0*(1 +
(1+C)*A**2/2. +
(5.-4.*T+42.*C+13.*C**2-28.*ePrimeSquared)*A**4/24. +
(61.-148.*T+16.*T**2)*A**6/720.)
utm = [x, y, alt]
info = [zone, k]
return utm, info
# print
# math
# 'math'
#
# After you have written and run the code to learn the TYPE
# of each of the above, change the above _TODO_ to DONE.
###############################################################################
print(type(3.14))
print(type("hello"))
print(type('hello'))
print(type('a b c'))
print(type(3+3))
print(type("3"+"3"))
print(type(2**100))
print(type(2.0**100))
print(type(math.sin(8)))
print(type(math.sin))
print(type(print))
print(type(math))
print(type('math'))
###############################################################################
#
# DONE: 6.
# Ensure that no blue bars on the scrollbar-thing to the right remain.
# Run one more time to be sure that all is still OK.
#
# Then COMMIT-and-PUSH your work as before:
# 1. Select VCS from the menu bar (above).
# 2. Choose Commit from the pull-down menu that appears.
# 3. In the Commit Changes window that pops up,
# press the Commit and Push button.
"""
def __timeout_callback(self, widget):
self.anicount += 1
# Pre start animation
if self.anicount < 42:
self.glarea.window.invalidate_rect(self.glarea.allocation, False)
#print self.glarea.allocation
self.cnt += 1
if self.cnt >= 1000:
self.cnt = 0
if not self.enable:
return True
self.angle += 1.0
if (self.angle >= 360.0):
self.angle2 -= 360.0
self.angle2 += 2.0
if (self.angle2 >= 360.0):
self.angle2 -= 360.0
self.angle3 += 3.0
if (self.angle >= 360.0):
self.angle -= 360.0
self.angle5 += 5.0
if (self.angle5 >= 360.0):
self.angle5 -= 360.0
self.angle7 += 7.0
if (self.angle7 >= 360.0):
self.angle7 -= 360.0
if self.cnt % 50 == 0:
self.twinkle += 1
if self.twinkle % 2 == 0:
self.starcol += 0.02
else:
self.starcol -= 0.02
t = self.angle * math.pi / 180.0
if t > math.pi:
t = 2.0 * math.pi - t
t2 = self.angle * math.pi / 180.0
self.pos_y = 2.0 * (math.sin (t) + 0.4 * math.sin (3.0*t)) - 1.0
self.pos_y2 = 2.0 * (math.sin (t))
self.pos_x = 2.0 * (math.sin (t/2)) - 1
self.pos_x2 = 2.0 * (math.sin (t2/2)) - 1
# Invalidate whole window.
self.glarea.window.invalidate_rect(self.glarea.allocation, False)
# Update window synchronously (fast).
#self.glarea.window.process_updates(False)
return True
def nodeRotate(self, ox, oy, angle): angle = radians(angle) newX = ox + (self.position.x-ox)*cos(angle) - (self.position.y-oy)*sin(angle) newY = oy + (self.position.x-ox)*sin(angle) + (self.position.y-oy)*cos(angle) self.position = (round(newX,2), round(newY,2))
def drawStar(x=dimention / 2, y=dimention / 3, a=dimention):
glBegin(GL_POLYGON)
R = a / 5
r = R * math.sqrt(2) * math.cos(math.pi * 72 / 180)
for n in range(5):
# glVertex2f(x + R * math.cos(math.pi * (90 + 72 * n) / 180),
# y + R * math.sin(math.pi * (90 + 72 * n) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * n) / 180),
y + r * math.sin(math.pi * (126 + 72 * n) / 180))
glEnd()
'''The for loop result was not turn out as expectation
Thus, I have to draw separate 1 pentagon and 5 triangles
'''
glBegin(GL_TRIANGLES)
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 4) / 180),
y + r * math.sin(math.pi * (126 + 72 * 4) / 180))
glVertex2f(x + R * math.cos(math.pi * (90 + 72 * 0) / 180),
y + R * math.sin(math.pi * (90 + 72 * 0) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 0) / 180),
y + r * math.sin(math.pi * (126 + 72 * 0) / 180))
glEnd()
glBegin(GL_TRIANGLES)
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 0) / 180),
y + r * math.sin(math.pi * (126 + 72 * 0) / 180))
glVertex2f(x + R * math.cos(math.pi * (90 + 72 * 1) / 180),
y + R * math.sin(math.pi * (90 + 72 * 1) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 1) / 180),
y + r * math.sin(math.pi * (126 + 72 * 1) / 180))
glEnd()
glBegin(GL_TRIANGLES)
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 1) / 180),
y + r * math.sin(math.pi * (126 + 72 * 1) / 180))
glVertex2f(x + R * math.cos(math.pi * (90 + 72 * 2) / 180),
y + R * math.sin(math.pi * (90 + 72 * 2) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 2) / 180),
y + r * math.sin(math.pi * (126 + 72 * 2) / 180))
glEnd()
glBegin(GL_TRIANGLES)
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 2) / 180),
y + r * math.sin(math.pi * (126 + 72 * 2) / 180))
glVertex2f(x + R * math.cos(math.pi * (90 + 72 * 3) / 180),
y + R * math.sin(math.pi * (90 + 72 * 3) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 3) / 180),
y + r * math.sin(math.pi * (126 + 72 * 3) / 180))
glEnd()
glBegin(GL_TRIANGLES)
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 3) / 180),
y + r * math.sin(math.pi * (126 + 72 * 3) / 180))
glVertex2f(x + R * math.cos(math.pi * (90 + 72 * 4) / 180),
y + R * math.sin(math.pi * (90 + 72 * 4) / 180))
glVertex2f(x + r * math.cos(math.pi * (126 + 72 * 4) / 180),
y + r * math.sin(math.pi * (126 + 72 * 4) / 180))
glEnd()
def rotatebot(rot_angle):
global yaw
# create Twist object
twist = Twist()
# set up Publisher to cmd_vel topic
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# set the update rate to 1 Hz
rate = rospy.Rate(1)
# get current yaw angle
current_yaw = np.copy(yaw)
# log the info
rospy.loginfo(['Current: ' + str(math.degrees(current_yaw))])
# we are going to use complex numbers to avoid problems when the angles go from
# 360 to 0, or from -180 to 180
c_yaw = complex(math.cos(current_yaw), math.sin(current_yaw))
# calculate desired yaw
target_yaw = current_yaw + math.radians(rot_angle)
# convert to complex notation
c_target_yaw = complex(math.cos(target_yaw), math.sin(target_yaw))
rospy.loginfo(['Desired: ' + str(math.degrees(cmath.phase(c_target_yaw)))])
# divide the two complex numbers to get the change in direction
c_change = c_target_yaw / c_yaw
# get the sign of the imaginary component to figure out which way we have to turn
c_change_dir = np.sign(c_change.imag)
# set linear speed to zero so the TurtleBot rotates on the spot
twist.linear.x = 0.0
# set the direction to rotate
twist.angular.z = c_change_dir * rotate_speed
# start rotation
pub.publish(twist)
# we will use the c_dir_diff variable to see if we can stop rotating
c_dir_diff = c_change_dir
# rospy.loginfo(['c_change_dir: ' + str(c_change_dir) + ' c_dir_diff: ' + str(c_dir_diff)])
# if the rotation direction was 1.0, then we will want to stop when the c_dir_diff
# becomes -1.0, and vice versa
while (c_change_dir * c_dir_diff > 0):
# get current yaw angle
current_yaw = np.copy(yaw)
# get the current yaw in complex form
c_yaw = complex(math.cos(current_yaw), math.sin(current_yaw))
rospy.loginfo('While Yaw: %f Target Yaw: %f',
math.degrees(current_yaw), math.degrees(target_yaw))
# get difference in angle between current and target
c_change = c_target_yaw / c_yaw
# get the sign to see if we can stop
c_dir_diff = np.sign(c_change.imag)
# rospy.loginfo(['c_change_dir: ' + str(c_change_dir) + ' c_dir_diff: ' + str(c_dir_diff)])
rospy.loginfo(['test'])
angle_to_go = abs(target_yaw - current_yaw)
twist.linear.x = 0.0
twist.angular.z = c_change_dir * rotate_speed * (angle_to_go / pi)
pub.publish(twist)
rate.sleep()
if math.degrees(target_yaw) - 10 <= math.degrees(
current_yaw) <= math.degrees(target_yaw) + 10:
break
rospy.loginfo(['End Yaw: ' + str(math.degrees(current_yaw))])
rospy.loginfo(['exits loop'])
# set the rotation speed to 0
twist.angular.z = 0.0
# stop the rotation
time.sleep(1)
pub.publish(twist)