def CalcPerspectiveCorrection(alpha, X, FOV=FOV):
alpha = WrapAngle(alpha)
if FOV is None:
return alpha
if 0 <= alpha <= 180:
costheta = (FOV*math.cos(alpha*math.pi/180.0)-X*math.sin(alpha*math.pi/180.0))/(FOV+max(2, abs(X)+1)*math.sin(alpha*math.pi/180.0))
try:
if costheta > 1:
costheta = 1
raise ValueError
elif costheta < -1:
costheta = -1
raise ValueError
except ValueError:
logging.error('Clipped rotation angle: (alpha=%s, X=%s), it is a bug!' % (alpha, X))
theta = math.acos(costheta)*180/math.pi
else:
costheta = (FOV*math.cos(alpha*math.pi/180.0)-X*math.sin(alpha*math.pi/180.0))/(FOV-max(2, abs(X)+1)*math.sin(alpha*math.pi/180.0))
try:
if costheta > 1:
costheta = 1
raise ValueError
elif costheta < -1:
costheta = -1
raise ValueError
except ValueError:
logging.error('Clipped rotation angle: (alpha=%s, X=%s), it is a bug!' % (alpha, X))
theta = -math.acos(costheta)*180/math.pi
return WrapAngle(theta)
def legIK(self, X, Y, Z, resolution):
""" Compute leg servo positions. """
ans = [0,0,0,0] # (coxa, femur, tibia)
try:
# first, make this a 2DOF problem... by solving coxa
ans[0] = radToServo(atan2(X,Y), resolution)
trueX = int(sqrt(sq(X)+sq(Y))) - self.L_COXA
im = int(sqrt(sq(trueX)+sq(Z))) # length of imaginary leg
# get femur angle above horizon...
q1 = -atan2(Z,trueX)
d1 = sq(self.L_FEMUR)-sq(self.L_TIBIA)+sq(im)
d2 = 2*self.L_FEMUR*im
q2 = acos(d1/float(d2))
ans[1] = radToServo(q1+q2, resolution)
# and tibia angle from femur...
d1 = sq(self.L_FEMUR)-sq(im)+sq(self.L_TIBIA)
d2 = 2*self.L_TIBIA*self.L_FEMUR;
ans[2] = radToServo(acos(d1/float(d2))-1.57, resolution)
except:
if self.debug:
"LegIK FAILED"
return [1024,1024,1024,0]
if self.debug:
print "LegIK:",ans
return ans
def ikLegPlane(x,y, servoFemur,servoTibia, lFemur=76.2,lTibia=107.95):
'''
IK for the leg, considering the plane in which the femur and tibia are.
Returns a list of possible angles of the femur and tibia (0 or 1, maybe 2).
Dimensions are in mm, angles in degrees.
See the following for direction of x,y. Origin is at the base of the femur.
/\
Femur / \
^ / \ Tibia
|y / \
| \
--> \
x
'''
lsqu = x**2+y**2
# Gamma is given in [-pi/2;3pi/2]
gamma = DEG( (x == 0 and (y>0 and pi/2 or -pi/2)) or atan(y/x) + (x<0 and pi) )
try:
epsilon = DEG( acos( (lsqu+lFemur*lFemur-lTibia*lTibia)/(2*lFemur*sqrt(lsqu)) ) )
beta = DEG( acos( (lsqu-lFemur*lFemur-lTibia*lTibia)/(2*lFemur*lTibia) ) )
except ValueError:
return []
lIK = []
#print(epsilon, beta, gamma)
a = servoFemur.findAngleInDomain(epsilon+gamma)
b = servoTibia.findAngleInDomain(beta)
if a != None and b != None:
lIK.append( (a, b) )
a = servoFemur.findAngleInDomain(-epsilon+gamma)
b = servoTibia.findAngleInDomain(-beta)
if a != None and b != None:
lIK.append( (a, b) )
return lIK
def VectorMath(ExtPts):
aDx = ExtPts[1].X-ExtPts[0].X
aDy = ExtPts[1].Y-ExtPts[0].Y
bDx = ExtPts[-2].X-ExtPts[0].X
bDy = ExtPts[-2].Y-ExtPts[0].Y
VectorA=[aDx,aDy]
VectorB=[bDx,bDy]
VectorX=[1,0]
VectorY=[0,1]
A_Len = np.linalg.norm(VectorA)
B_Len = np.linalg.norm(VectorB)
dpAB=dotProduct(VectorA, VectorB)
AngAB=math.acos(dpAB/A_Len/B_Len)
cpAB=(A_Len*B_Len)*math.sin(AngAB)
dpAX=dotProduct(VectorA, VectorX)
AngAx=math.acos(dpAX/A_Len/1.0)*math.copysign(1,aDy)
dpBX=dotProduct(VectorB, VectorX)
AngBx=math.acos(dpBX/B_Len/1.0)*math.copysign(1,bDy)
eParts=[A_Len,B_Len, AngAB, AngAx, AngBx,cpAB]
return(eParts)
def ikCalc(bodyHeight, footDistance): # ik Calc
# bodyHeight = 35
# footDistance = 80
femurLength = 70
tibiaLength = 111
hypo = math.sqrt(((math.pow(footDistance, 2)) + (math.pow(bodyHeight, 2))))
alpha = (
(
math.acos(
(math.pow(femurLength, 2) + math.pow(hypo, 2) - math.pow(tibiaLength, 2)) / (2 * femurLength * hypo)
)
)
* 180
/ math.pi
)
gamma = (math.atan2(bodyHeight, footDistance)) * 180 / math.pi
beta = 90 - alpha + gamma
beta = int(beta * 100) / 100.0
delta = (
(
math.acos(
(math.pow(femurLength, 2) + math.pow(tibiaLength, 2) - math.pow(hypo, 2))
/ (2 * femurLength * tibiaLength)
)
)
* 180
/ math.pi
)
epsilon = 180 - delta
epsilon = int(epsilon * 100) / 100.0
print "Body Height: " + str(bodyHeight) + " Foot Distance: " + str(footDistance) + " Beta: " + str(
beta
) + " Epsilon: " + str(epsilon)
return (beta, epsilon)
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 my_solid_angle(center, coords):
"""
Helper method to calculate the solid angle of a set of coords from the
center.
Args:
center:
Center to measure solid angle from.
coords:
List of coords to determine solid angle.
Returns:
The solid angle.
"""
o = np.array(center)
r = [np.array(c) - o for c in coords]
r.append(r[0])
n = [np.cross(r[i + 1], r[i]) for i in range(len(r) - 1)]
n.append(np.cross(r[1], r[0]))
phi = 0.0
for i in range(len(n) - 1):
try:
value = math.acos(-np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1])))
except ValueError:
mycos = -np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1]))
if 0.999999999999 < mycos < 1.000000000001:
value = math.acos(1.0)
elif -0.999999999999 > mycos > -1.000000000001:
value = math.acos(-1.0)
else:
raise SolidAngleError(mycos)
phi += value
return phi + (3 - len(r)) * math.pi
def angle_to(self, obj):
if isinstance(obj,Line):
return angular_unit*math.acos(min(1,abs(dot(self.t,obj.t))))
elif isinstance(obj,Plane):
return angular_unit*(math.pi/2 - math.acos(min(1,abs(dot(self.t,obj.n)))))
else:
raise RuntimeError("Cannot calculate angle to object of this type")
def __init__(self, from_obj, to_obj):
"""Create an affine transformation (rotation+translation) that
takes from_obj to to_obj in some sense.
"""
# self.dr is always defined
self.q = None # rotation quaternion, if applicable
if isinstance(from_obj,Point) and isinstance(to_obj,Point):
self.dr = to_obj.r - from_obj.r
elif isinstance(from_obj,Point) and isinstance(to_obj,Line):
# move point to closest point on line
p = from_obj.projected_on(to_obj)
self.dr = p.r - from_obj.r
elif isinstance(from_obj,Point) and isinstance(to_obj,Plane):
# move point to nearest point in plane
p = from_obj.projected_on(to_obj)
self.dr = p.r - from_obj.r
elif isinstance(from_obj,Line) and isinstance(to_obj,Line):
if dot(from_obj.t,to_obj.t) < 1 - 1e-14:
self.q = qrotor(cross(from_obj.t,to_obj.t),
math.acos(dot(from_obj.t,to_obj.t)))
self.dr = orthogonalized_to(to_obj.r - qrotate(self.q,from_obj.r),to_obj.t)
else:
self.dr = orthogonalized_to(to_obj.r - from_obj.r,to_obj.t)
elif isinstance(from_obj,Line) and isinstance(to_obj,Plane):
lp = from_obj.projected_on(to_obj)
return Movement.__init__(self,from_obj,lp)
elif isinstance(from_obj,Plane) and isinstance(to_obj,Plane):
if dot(from_obj.n,to_obj.n) < 1 - 1e-14:
self.q = qrotor(cross(from_obj.n,to_obj.n),
math.acos(dot(from_obj.n,to_obj.n)))
self.dr = to_obj.r - qrotate(self.q,from_obj.r)
else:
self.dr = orthogonalized_to(to_obj.r - from_obj.r,to_obj.n)
def tf_person_2_contam_grid(self, x, y, theta, orientation, frame_id):
# transform from ellipse frame to map frame and get ellipse data
# FORMULA ASSUMES LASER AND MAP ARE ROTATED ONLY AROUND Z AXIS
try:
self.tf_listener.waitForTransform(self.ogrid_frame_id, frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans, rot) = self.tf_listener.lookupTransform(self.ogrid_frame_id, "/" + frame_id, rospy.Time(0))
angle = acos(rot[3]) * 2
center = (x * cos(angle) - y * sin(angle) + trans[0], x * sin(angle) + y * cos(angle) + trans[1])
q = orientation
w = q.w * rot[3] - q.x * rot[0] - q.y * rot[1] - q.z * rot[2]
theta = acos(w) * 2
"""
x = ellipse.pose.position.x
y = ellipse.pose.position.y
angle = acos(rot[3]) * 2
center = (x*cos(angle) - y*sin(angle) + trans[0], x*sin(angle) + y*cos(angle) + trans[1])
(a, b) = (ellipse.scale.x/2, ellipse.scale.y/2)
q = ellipse.pose.orientation
w = (q.w*rot[3] - q.x*rot[0] - q.y*rot[1] - q.z*rot[2])
theta = acos(w)*2
"""
return center, theta
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "no tf found"
return None
def _compute_disk_overlap(d, r1, r2):
"""
Compute surface overlap between two disks of radii ``r1`` and ``r2``,
with centers separated by a distance ``d``.
Parameters
----------
d : float
Distance between centers.
r1 : float
Radius of the first disk.
r2 : float
Radius of the second disk.
Returns
-------
vol: float
Volume of the overlap between the two disks.
"""
ratio1 = (d ** 2 + r1 ** 2 - r2 ** 2) / (2 * d * r1)
ratio1 = np.clip(ratio1, -1, 1)
acos1 = math.acos(ratio1)
ratio2 = (d ** 2 + r2 ** 2 - r1 ** 2) / (2 * d * r2)
ratio2 = np.clip(ratio2, -1, 1)
acos2 = math.acos(ratio2)
a = -d + r2 + r1
b = d - r2 + r1
c = d + r2 - r1
d = d + r2 + r1
area = (r1 ** 2 * acos1 + r2 ** 2 * acos2 -
0.5 * sqrt(abs(a * b * c * d)))
return area / (math.pi * (min(r1, r2) ** 2))
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1):
"""For a 2R arm centered at the origin, solves for the joint angles
(q1,q2) that places the end effector at (x,y).
The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')].
"""
D = vectorops.norm((x,y))
thetades = math.atan2(y,x)
if D == 0:
raise ValueError("(x,y) at origin, infinite # of solutions")
c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2)
q2s = []
if c2 < -1:
print "solve_2R_inverse_kinematics: (x,y) inside inner circle"
return []
elif c2 > 1:
print "solve_2R_inverse_kinematics: (x,y) out of reach"
return []
else:
if c2 == 1:
q2s = [math.acos(c2)]
else:
q2s = [math.acos(c2),-math.acos(c2)]
res = []
for q2 in q2s:
thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2))
q1 = thetades - thetaactual
res.append((q1,q2))
return res
def matrix2abcabg(lattice):
la,lb,lc = lattice
a,b,c = length(la),length(lb),length(lc)
gamma = math.acos(dot(la,lb)/a/b)*180/math.pi
beta = math.acos(dot(la,lc)/a/c)*180/math.pi
alpha = math.acos(dot(lb,lc)/b/c)*180/math.pi
return a,b,c,alpha,beta,gamma
def getAngle(self,cc):
r = ( (cc[2] - cc[0]) ** 2 + (cc[3] - cc[1] ) ** 2 ) ** 0.5
print('current R is:%8.4f' % r)
print (cc)
x = cc[2] - cc[0]
y = cc[3] - cc[1]
print(x,y)
sina = y / r
cosa = x / r
starta = float()
if y > 0 and x > 0:
starta=math.asin ( sina ) ## 0 ~ PI/2
print ("q1: %f" % starta)
else:
if y > 0 and x <=0:
starta = math.acos(cosa) ## PI/2 ~ PI
print ("q2: %f" % starta)
else:
if y <=0 and x <=0:
starta = math.pi*2 - math.acos(cosa)
print("q3: %f" % starta)
else:
starta = math.pi * 2 + math.asin(sina)
print("q4: %f" % starta)
print("The angle of the hand is: %f" % starta)
print('The coords of the hand are (after calculated from the Angle: is %s' % cc)
return (starta)
def transform(x,y,z):
initial_angle=math.acos(L/(4*l))#
bed_angle=math.asin((z-square_z)/l)#
leg_offset=l*math.cos(bed_angle)
yprime=y+y_offset-leg_offset
xprime=x+L/2
topz=((d/2-y)/d)*front_z+(1-(d/2-y)/d)*back_z
bed_tilt=math.atan2(-back_z+front_z,d)
yprime-=z*math.sin(bed_tilt)
zprime=topz-z*math.cos(bed_tilt)
left_leg=math.sqrt(xprime*xprime+yprime*yprime)
right_leg=math.sqrt((L-xprime)*(L-xprime)+yprime*yprime)
left_elbow=math.acos((left_leg*left_leg-2*l*l)/(-2*l*l))
right_elbow=math.acos((right_leg*right_leg-2*l*l)/(-2*l*l))
left_small_angle=(math.pi-left_elbow)/2
right_small_angle=(math.pi-right_elbow)/2
left_virtual=math.atan(-yprime/xprime)
right_virtual=math.atan(-yprime/(L-xprime))
left_drive=left_small_angle+left_virtual-initial_angle
right_drive=right_small_angle+right_virtual-initial_angle
left_stepper=-left_drive+(math.pi-left_elbow)*mechanical_advantage
right_stepper=-right_drive+(math.pi-right_elbow)*mechanical_advantage
#print "left_angle: "+str(left_drive)+" left_elbow: "+str(math.pi-left_elbow)
#print "right_angle: "+str(left_drive)+" right_elbow: "+str(math.pi-right_elbow)
return left_stepper*200/math.pi,right_stepper*200/math.pi,zprime
def process(self, iEvent, event):
self.readCollections( iEvent )
eventNumber = iEvent.eventAuxiliary().id().event()
#print 'Event ',eventNumber
# creating a "sub-event" for this analyzer
myEvent = Event(event.iEv)
setattr(event, self.name, myEvent)
event = myEvent
self.buildGen(event)
self.counters.counter('LLX').inc('All events')
if event.llX : self.counters.counter('LLXGen').inc('All events')
event.step = 0
self.buildLeptonList ( event )
if len(self.leptonJets) < 2:
return 0
else:
event.step +=1
self.counters.counter('LLX').inc('Two Leptons')
if event.llX : self.counters.counter('LLXGen').inc('Two Leptons')
self.buildJetList( event )
event.jets = self.jets
#self.fitmZ()
self.buildMiss( event )
acol = self.leptons[0].px() * self.leptons[1].px() + \
self.leptons[0].py() * self.leptons[1].py() + \
self.leptons[0].pz() * self.leptons[1].pz()
acol /= self.leptons[0].p() * self.leptons[1].p()
if acol >= 1.0 : acol = 1. - 1E-12
if acol <= -1.0 : acol = -1. + 1E-12
acol = acos(acol)*180./pi
acop = self.leptons[0].px() * self.leptons[1].px() + \
self.leptons[0].py() * self.leptons[1].py()
acop /= self.leptons[0].pt() * self.leptons[1].pt()
if acop >= 1.0 : acop = 1. - 1E-12
if acop <= -1.0 : acop = -1. + 1E-12
acop = acos(acop)*180./pi
vect1 = TVector3(self.leptons[0].px(), self.leptons[0].py(), self.leptons[0].pz())
vect2 = TVector3(self.leptons[1].px(), self.leptons[1].py(), self.leptons[1].pz())
cross = vect1.Unit().Cross(vect2.Unit())
cross = abs(cross.z())
cross = asin(cross) * 180./pi
event.acol = acol
event.acop = acop
event.cross = cross
self.counters.counter('LLX').inc('passing')
if event.llX : self.counters.counter('LLXGen').inc('passing')
event.step += 1
return True
def geo_centroid(lat_array, lon_array, load_array):
geo_data = pd.DataFrame(columns=['lat', 'lon', 'load'])
geo_data.lat = lat_array
geo_data.lon = lon_array
geo_data.load = load_array
R = 3963.1676 # miles
geo_data['xi'] = [math.radians(90-x) for x in geo_data.lat]
geo_data['theta'] = [math.radians(x) for x in geo_data.lon]
geo_data['X'] = [R*math.sin(x)*math.cos(t) for (x,t) in zip(geo_data.xi, geo_data.theta)]
geo_data['Y'] = [R*math.sin(x)*math.sin(t) for (x,t) in zip(geo_data.xi, geo_data.theta)]
geo_data['Z'] = [R*math.cos(x) for x in geo_data.xi]
X_centroid = np.average(geo_data.X, weights=geo_data.load)
Y_centroid = np.average(geo_data.Y, weights=geo_data.load)
Z_centroid = np.average(geo_data.Z, weights=geo_data.load)
x_var = np.average((geo_data.X-X_centroid)**2, weights=geo_data.load)
y_var = np.average((geo_data.Y-Y_centroid)**2, weights=geo_data.load)
z_var = np.average((geo_data.Z-Z_centroid)**2, weights=geo_data.load)
porsigma = math.sqrt(x_var + y_var + z_var)
L = math.sqrt(X_centroid*X_centroid + Y_centroid*Y_centroid + Z_centroid*Z_centroid)
por = L/R
xi = math.acos(Z_centroid/L)
theta = math.acos(X_centroid/L/math.sin(xi)) if Y_centroid/math.sin(xi) > 0 else math.acos(X_centroid/L/math.sin(xi))*(-1)
lat_centroid = 90 - xi/math.radians(1)
lon_centroid = theta/math.radians(1)
return [round(lat_centroid,3), round(lon_centroid,3), round(por,4), round(porsigma,2)]
def rpy(R):
"""Converts a rotation matrix to a roll,pitch,yaw angle triple.
The result is given in radians."""
sign = lambda x: 1 if x > 0 else (-1 if x < 0 else 0)
m = matrix(R)
b = -math.asin(m[2][0]) # m(2,0)=-sb
cb = math.cos(b)
if abs(cb) > 1e-7:
ca = m[0][0]/cb #m(0,0)=ca*cb
ca = min(1.0,max(ca,-1.0))
if sign(m[1][0]) == sign(cb): #m(1,0)=sa*cb
a = math.acos(ca);
else:
a = 2*math.pi - math.acos(ca)
cc = m[2][2] / cb #m(2,2)=cb*cc
cc = min(1.0,max(cc,-1.0))
if sign(m[2][1]) == sign(cb): #m(2,1)=cb*sc
c = math.acos(cc)
else:
c = math.pi*2 - math.acos(cc)
else:
#b is close to 90 degrees, i.e. cb=0
#this reduces the degrees of freedom, so we can set c=0
c = 0
#m(0,1)=-sa
a = -math.asin(m[0][1]);
if sign(math.cos(a)) != sign(m[1][1]): #m(1,1)=ca
a = math.pi - a;
return c,b,a
def leg_ik(x,y,z,l1=L1, l2=L2, l3=L3, alpha=ALPHA, beta=BETA):
theta1 = math.atan2(y, x)
p1 = {
"x": l1 * math.cos(theta1),
"y": l1 * math.sin(theta1),
"z": 0
}
d13 = math.sqrt( (x - p1["x"])**2 + (y - p1["y"])**2 )
a = math.atan2(z, d13)
d = math.sqrt( ((x - p1["x"])**2) + ((y - p1["y"])**2) + ((z - p1["z"])**2) )
# in the case of a planar angle, theta3 is equal to 0 because the d length is equal to l2 + l3
if x==0:
theta3=0
theta3C=0
b=0
theta2=0
theta2C=0
else:
theta3 = math.radians(180) - math.acos( (l2**2 + l3**2 - d**2) / (2 * l2 * l3))
theta3C = theta3 + alpha + beta - math.radians(90)
b = math.acos((l2**2 + d**2 - l3**2) / (2 * l2 * d) )
theta2 = b+a
theta2C = -theta2 - alpha
print math.degrees(theta1)
print math.degrees(theta2C)
print math.degrees(theta3C)
return [math.degrees(theta1), math.degrees(theta2C), math.degrees(theta3C)]
def setSSS(self, lenp3, lenp1, lenp2):
self.lenp3 = lenp3
self.lenp1 = lenp1
self.lenp2 = lenp2
self.ap1 = math.acos(((self.lenp2 * self.lenp2 + self.lenp3 * self.lenp3) - self.lenp1 * self.lenp1) / (2* self.lenp2 * self.lenp3))
self.ap2 = math.acos(((self.lenp1 * self.lenp1 + self.lenp3 * self.lenp3) - self.lenp2 * self.lenp2) / (2* self.lenp1 * self.lenp3))
self.ap3 = math.acos(((self.lenp1 * self.lenp1 + self.lenp2 * self.lenp2) - self.lenp3 * self.lenp3) / (2* self.lenp1 * self.lenp2))
def test_funcs_multi(self):
pi = math.pi
# sin family
self.assertQuantity(data.sin(Quantity( (0,pi/2), (2,2))), (0,1), (2,0))
self.assertQuantity(data.sinh(Quantity((0,1), (2,2))), (0, math.sinh(1)), (2, math.cosh(1)*2))
self.assertQuantity(data.asin(Quantity((0,0.5), (2,2))), (0,math.asin(0.5)), (2,2/math.sqrt(1-0.5**2)))
self.assertQuantity(data.asinh(Quantity((0,1), (2,2))), (0,math.asinh(1)), (2,2/math.sqrt(1+1**2)))
# cos family
self.assertQuantity(data.cos(Quantity((0,pi/2), (2,2))), (1,0), (0,-2))
self.assertQuantity(data.cosh(Quantity((0,1), (2,2))), (1,math.cosh(1)), (0,math.sinh(1)*2))
self.assertQuantity(data.acos(Quantity((0,0.5), (2,2))), (math.acos(0),math.acos(0.5)), (-2,-2/math.sqrt(1-0.5**2)))
self.assertQuantity(data.acosh(Quantity((2,3), (2,2))), (math.acosh(2), math.acosh(3)), (2/math.sqrt(2**2-1),2/math.sqrt(3**2-1)))
# tan family
self.assertQuantity(data.tan(Quantity((0,1), (2,2))), (0,math.tan(1)), (2,2/math.cos(1)**2))
self.assertQuantity(data.tanh(Quantity((0,1), (2,2))), (0,math.tanh(1)), (2,2/math.cosh(1)**2))
self.assertQuantity(data.atan(Quantity((0,1), (2,2))), (0, math.atan(1)), (2,2/(1+1**2)))
self.assertQuantity(data.atan2(Quantity((0,1), (2,2)), Quantity((1,1), (0,0))), (0,math.atan(1)), (2,2/(1+1**2)))
self.assertQuantity(data.atanh(Quantity((0,0.5), (2,2))), (0,math.atanh(0.5)), (2,2/(1-0.5**2)))
#misc
self.assertQuantity(data.sqrt(Quantity((1,4), (2,2))), (1,2), (1,1/2))
self.assertQuantity(data.exp(Quantity((1,4), (2,2))), (math.e, math.e**4), (2 * math.e,2*math.e**4))
self.assertQuantity(data.log(Quantity((1,4), (2,2))), (0, math.log(4)), (2,1/2))
def get_sector(prev_v, spike_v, next_v, threshold=0.0):
'''
Returns 2 vectors that define the sector rays.
The vectors have unit lengths
The vector pair is right-hand
'''
vec1 = (spike_v - prev_v).normalized()
vec2 = (spike_v - next_v).normalized()
cosine = Geom2.cos_angle(vec1, vec2)
sine = Geom2.sin_angle(vec1, vec2)
sector_angle = 2 * math.pi - math.acos(cosine) if sine < 0 else math.acos(cosine)
# degrees to radian
clearance = math.pi * threshold / 180.0
sector_angle_plus = sector_angle + clearance
# limit sector opening to 180 degrees:
sector_angle_plus = min(sector_angle_plus, math.pi)
clearance = .5 * (sector_angle_plus - sector_angle)
cosine = math.cos(clearance)
sine = math.sin(clearance)
# rotate sector vectors to increase angle for clearance
return (
Geom2.mul_mtx_2x2((cosine, sine, -sine, cosine), vec1),
Geom2.mul_mtx_2x2((cosine, -sine, sine, cosine), vec2)
)
def rotation_by2vec(orient,orient2):
initial_orient=[0,0,1]
initial_orient2=[1,0,0]
initial_orient=vec_normalize(initial_orient)
initial_orient2=vec_normalize(initial_orient2)
orient= vec_normalize(orient)
rotation_angle= math.acos(dot_product(initial_orient,orient))
rotation_axis=cross_product(initial_orient,orient)
if vec_distance(rotation_axis,[0,0,0]) == 0:
# if initial_orient and orient are parellel then we can use any vector perpendicular to this line as the rotation axis
rotation_axis=perp_vec(orient)
rotation_axis= vec_normalize(rotation_axis)
rotation_matr1= rotation_matrix(rotation_axis,rotation_angle)
initial_orient2= matrix_vec_multiply(rotation_matr1,initial_orient2)
orient2= vec_scale(orient2,1 - dot_product(orient2,orient))
orient2= vec_normalize(orient2)
rotation_angle= math.acos(dot_product(initial_orient2,orient2))
rotation_axis= cross_product(initial_orient2,orient2)
if vec_distance(rotation_axis,[0,0,0]) == 0:
# if initial_orient and orient are parellel then we can use any vector perpendicular to this line as the rotation axis
rotation_axis=perp_vec(orient2)
rotation_axis= vec_normalize(rotation_axis)
rotation_matr2= rotation_matrix(rotation_axis,rotation_angle)
rotation_matr= matrix_multiply(rotation_matr2,rotation_matr1)
return rotation_matr
def aggregateAngularForce(points1, points2): unitVecs = [] com1 = PointAligner.centerOfMass(points1) com2 = PointAligner.centerOfMass(points2) #calculate unit vectors in each theta angle for (x1,y1) in points1: for (x2,y2) in points2: if (x1==x2 and y1==y2): continue r = PointAligner.distance(x1,y1,x2,y2) x3 = x1-x2 y3 = y1-y2 tanVec = [com2[1]-y2,-(com2[0]-x2)] if(tanVec[0]==0 and tanVec[1]==0): continue theta = math.acos( (x3*tanVec[0]-y3*tanVec[1]) / (PointAligner.length(x3,y3) * PointAligner.length(tanVec[0],tanVec[1]))) unitVecs.append( [[math.cos(theta), math.sin(theta)],r] ) #average the unit vectors aggregate = [0,0] totalWeight = 0 for [[x,y],r] in unitVecs: aggregate[0] += x*r aggregate[1] += y*r totalWeight += r print "agg = " + `aggregate` if aggregate==[0,0]: return 0; return math.acos( (1*aggregate[0]) / ( PointAligner.length(aggregate[0],aggregate[1])))
def get_sight_direction(self):
dx, dy, dz = self.get_sight_vector()
dz = -dz
angle = 0
if dz == 0:
if dx > 0:
angle = 0
elif dx < 0:
angle = pi
elif dz > 0:
angle = acos(dx / sqrt(dx * dx + dz * dz))
else:
angle = -acos(dx / sqrt(dx * dx + dz * dz))
if angle < 0: angle += 2 * pi
angle *= 180 / pi
if 0 < angle <= 45 or 315 < angle <= 360:
direction = G.EAST
elif 45 < angle <= 135:
direction = G.NORTH
elif 135 < angle <= 225:
direction = G.WEST
elif 225 < angle <= 315:
direction = G.SOUTH
return (dx, dy, dz), direction, angle
def rgb2hsi(_red: float, _green: float, _blue: float) -> tuple:
"""RGB -> HSI"""
_red = round(_red * 255, 6)
_green = round(_green * 255, 6)
_blue = round(_blue * 255, 6)
if isinstance(_red, str) or _red < 0:
_red = 0
if isinstance(_green, str) or _green < 0:
_green = 0
if isinstance(_blue, str) or _green < 0:
_blue = 0
_rgbmin = min(_red, _green, _blue)
_rgbplus = _red + _green + _blue
_red2 = _red * _red
_green2 = _green * _green
_blue2 = _blue * _blue
_var0 = (_red * _green) - (_red * _blue) - (_green * _blue)
_squareroot = round(sqrt(_red2 + _green2 + _blue2 - _var0), 6)
_var1 = _red - (_green * 0.5) - (_blue * 0.5)
_var2 = _var1 / _squareroot
_radconv = 57.2958279088
_intensity = (_red + _green + _blue) * 0.33333333
if _rgbplus == 765:
_sat = 0
_hue = 0
return _hue, _sat, round(_intensity, 6)
if _intensity > 0:
_sat = 1 - (_rgbmin / _intensity)
elif _intensity == 0:
_sat = 0
if _green >= _blue:
_hue = _radconv * acos(_var2)
elif _blue > _green:
_hue = 360 - (_radconv * acos(_var2))
return round(_hue, 6), round(_sat, 6), round(_intensity, 6)
def calculate_reciprocal_lattice(lattice):
"""calculate reciprocal lattice
:param lattice:
:return:
"""
# check
assert isinstance(lattice, Lattice)
# calculate reciprocal lattice
lattice_star = Lattice(1, 1, 1, 0, 0, 0)
# calculate volume
volume = 2 * lattice.get_a() * lattice.get_b() * lattice.get_c() * numpy.sqrt(
m_sin((lattice.get_alpha() + lattice.get_beta() + lattice.get_gamma()) / 2) *
m_sin((-lattice.get_alpha() + lattice.get_beta() + lattice.get_gamma()) / 2) *
m_sin((lattice.get_alpha() - lattice.get_beta() + lattice.get_gamma()) / 2) *
m_sin((lattice.get_alpha() + lattice.get_beta() - lattice.get_gamma()) / 2))
# v_start = (2 * numpy.pi) ** 3. / volume
# calculate a*, b*, c*
lattice_star.set_a(2 * numpy.pi * lattice.get_b() * lattice.get_c() * m_sin(lattice.get_alpha()) / volume)
lattice_star.set_b(2 * numpy.pi * lattice.get_a() * lattice.get_c() * m_sin(lattice.get_beta()) / volume)
lattice_star.set_c(2 * numpy.pi * lattice.get_b() * lattice.get_a() * m_sin(lattice.get_gamma()) / volume)
lattice_star.set_alpha(math.acos((m_cos(lattice.get_beta()) * m_cos(lattice.get_gamma()) - m_cos(lattice.get_alpha())) /
(m_sin(lattice.get_beta()) * m_sin(lattice.get_gamma()))) * 180. / numpy.pi)
lattice_star.set_beta(math.acos((m_cos(lattice.get_alpha()) * m_cos(lattice.get_gamma()) - m_cos(lattice.get_beta())) /
(m_sin(lattice.get_alpha()) * m_sin(lattice.get_gamma()))) * 180. / numpy.pi)
lattice_star.set_gamma(math.acos((m_cos(lattice.get_alpha()) * m_cos(lattice.get_beta()) - m_cos(lattice.get_gamma())) /
(m_sin(lattice.get_alpha()) * m_sin(lattice.get_beta()))) * 180. / numpy.pi)
return lattice_star
def billboard(self, camMatrix, curPos):
'''Sets up the gl matrix such that an object created at curPos will
face camMatrix. curPos should be a list.'''
# Get initial MVectors
camPos = self.matrixRowVector(camMatrix, 3)
curPos = om.MVector(*curPos)
aimProj = om.MVector(camPos.x - curPos.x, 0, camPos.z - curPos.z)
aimProj.normalize()
look = om.MVector(0, 0, 1)
look.normalize()
up = look ^ aimProj
angle = look * aimProj
if angle < 1 and angle > -1:
GLFT.glRotatef(acos(angle) * 180 / pi, up.x, up.y, up.z)
aim = camPos - curPos
aim.normalize()
up = om.MVector(0, 0, 0)
angle = aimProj * aim
if angle < 1 and angle > -1:
if aim[1] < 0:
GLFT.glRotatef(acos(angle) * 180 / pi, 1, 0, 0)
else:
GLFT.glRotatef(acos(angle) * 180 / pi, -1, 0, 0)
def GetVecAngle(x, y):
r_xy = math.sqrt(x * x + y * y)
if (y >= 0):
VecAngle = math.acos(x / r_xy)
else:
VecAngle = 2 * math.pi - math.acos(x / r_xy)
return VecAngle
def get_desired_drot(goal,rot,drot): slow_down_dist = 90*DEGTORAD #radians max_speed = 0.0146221 #radians per timestep dot = Vector(0,1,0).dot(goal.normalise()); axis = goal.cross(Vector(0,1,0)).normalise() alpha = acos(dot) if axis == None and dot == 1: #parallel desired_rot = Quaternion(1,0,0,0) elif axis == None and dot == -1: #opposite desired_rot = Quaternion(0,1,0,0) else: desired_rot = Quaternion(cos(alpha/2),axis.x*sin(alpha/2),axis.y*sin(alpha/2),axis.z*sin(alpha/2)) to_desired = (desired_rot * rot.inverse()).normalise() dist = acos(to_desired.w)*2; if dist > 0: axis = Vector(to_desired.x,to_desired.y,to_desired.z).normalise() if dist < slow_down_dist: theta = max_speed*dist/slow_down_dist else: theta = max_speed return Quaternion(cos(theta/2),axis.x*sin(theta/2),axis.y*sin(theta/2),axis.z*sin(theta/2)) else: return Quaternion(1,0,0,0)
self.x * no.y - self.y * no.x)
def absolute(self):
return pow((self.x**2 + self.y**2 + self.z**2), 0.5)
if __name__ == '__main__':
points = list()
for i in range(4):
a = list(map(float, input().split()))
points.append(a)
a, b, c, d = Points(*points[0]), Points(*points[1]), Points(
*points[2]), Points(*points[3])
x = (b - a).cross(c - b)
y = (c - b).cross(d - c)
angle = math.acos(x.dot(y) / (x.absolute() * y.absolute()))
print("%.2f" % math.degrees(angle))
'''
###Default Arguments
class EvenStream(object):
def __init__(self):
self.current = 0
def get_next(self):
to_return = self.current
self.current += 2
return to_return
class Oddstream(object):
def __init__(self):
self.current = 1
def get_next(self):
def getPath_halfTrot(self):
alpha1 = np.array(
[np.zeros(nh),
np.zeros(nh),
np.zeros(nh),
np.zeros(nh)])
alpha2 = np.array(
[np.zeros(nh),
np.zeros(nh),
np.zeros(nh),
np.zeros(nh)])
path1 = np.array(
[np.zeros(nh),
np.zeros(nh),
np.zeros(nh),
np.zeros(nh)])
path2 = np.array(
[np.zeros(nh),
np.zeros(nh),
np.zeros(nh),
np.zeros(nh)])
alpha_1, alpha_2, alpha_3, alpha_4 = np.zeros(
nh, dtype=float), np.zeros(nh, dtype=float), np.zeros(
nh, dtype=float), np.zeros(nh, dtype=float)
l_1, l_2, l_3, l_4 = np.zeros(nh, dtype=float), np.zeros(
nh, dtype=float), np.zeros(nh, dtype=float), np.zeros(nh,
dtype=float)
print('Calculating equations of l and alpha')
l1_t = ls1_t[0] + ls1_t[1] * t1_th + ls1_t[2] * np.power(
t1_th, 2) + ls1_t[3] * np.power(t1_th, 3) + ls1_t[4] * np.power(
t1_th, 4) + ls1_t[5] * np.power(
t1_th, 5) + ls1_t[6] * np.power(t1_th, 6)
l2_t = ls2_t[0] + ls2_t[1] * t2_th + ls2_t[2] * np.power(
t2_th, 2) + ls2_t[3] * np.power(t2_th, 3) + ls2_t[4] * np.power(
t2_th, 4) + ls2_t[5] * np.power(
t2_th, 5) + ls2_t[6] * np.power(t2_th, 6)
a1_t = as1_t[0] + as1_t[1] * t1_th + as1_t[2] * np.power(
t1_th, 2) + as1_t[3] * np.power(t1_th, 3) + as1_t[4] * np.power(
t1_th, 4) + as1_t[5] * np.power(
t1_th, 5) + as1_t[6] * np.power(t1_th, 6)
a2_t = as2_t[0] + as2_t[1] * t2_th + as2_t[2] * np.power(
t2_th, 2) + as2_t[3] * np.power(t2_th, 3) + as2_t[4] * np.power(
t2_th, 4) + as2_t[5] * np.power(
t2_th, 5) + as2_t[6] * np.power(t2_th, 6)
print('Calculating path of each leg!')
print('Length is ', nh)
for i in range(nh):
if (i < nh / 4):
alpha_1[i] = a1_t[i]
l_1[i] = l1_t[i]
alpha_2[i] = a2_t[int(i + nh / 2)]
l_2[i] = l2_t[int(i + nh / 2)]
alpha_3[i] = a1_t[i]
l_3[i] = l1_t[i]
alpha_4[i] = a2_t[int(i + nh / 2)]
l_4[i] = l2_t[int(i + nh / 2)]
elif (i < nh / 2):
alpha_1[i] = 0
l_1[i] = l_down[int(i - nh / 4)]
alpha_2[i] = a2_t[int(i + nh / 2)]
l_2[i] = l2_t[int(i + nh / 2)]
alpha_3[i] = 0
l_3[i] = l_down[int(i - nh / 4)]
alpha_4[i] = a2_t[int(i + nh / 2)]
l_4[i] = l2_t[int(i + nh / 2)]
elif (i < 3 * nh / 4):
alpha_1[i] = a2_t[i]
l_1[i] = l2_t[i]
alpha_2[i] = a1_t[int(i - nh / 2)]
l_2[i] = l1_t[int(i - nh / 2)]
alpha_3[i] = a2_t[i]
l_3[i] = l2_t[i]
alpha_4[i] = a1_t[int(i - nh / 2)]
l_4[i] = l1_t[int(i - nh / 2)]
else:
alpha_1[i] = a2_t[i]
l_1[i] = l2_t[i]
alpha_2[i] = 0
l_2[i] = l_down[int(i - 3 * nh / 4)]
alpha_3[i] = a2_t[i]
l_3[i] = l2_t[i]
alpha_4[i] = 0
l_4[i] = l_down[int(i - 3 * nh / 4)]
alpha_s = np.array([alpha_1, alpha_2, alpha_3, alpha_4])
l_s = np.array([l_1, l_2, l_3, l_4])
print('Alphas ', alpha_s[0][i], ' ', alpha_s[2][i])
#print('L ',l_s)
for j in range(4):
alpha1[j][i] = (alpha_s[j][i] - math.pi / 2)
alpha2[j][i] = (math.acos(
(((globals.l1**2) + ((l_s[j][i])**2) - (globals.l2**2)) /
(2 * globals.l1 * l_s[j][i]))))
self.leftShoulderxh[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[1][0])
self.rightShoulderxh[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[2][0])
self.leftShoulderyh[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[1][1])
self.rightShoulderyh[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[2][1])
theta1_ht[j][i] = (math.acos(self.rightShoulderyh[j][i] /
globals.l1))
theta2_ht[j][i] = (math.acos(self.leftShoulderyh[j][i] /
globals.l1))
#if(i>nh-2):
# path1[j] = (self.setAngles(theta1[j]))
# path2[j] = (self.setAngles(theta2[j]))
print('Iteration ', i)
print(theta1_ht)
print(theta2_ht)
print('Trot Half Path calculated!')
return [theta1_ht, theta2_ht]
def getPath_Trot(self):
alpha1 = np.array([np.zeros(n), np.zeros(n), np.zeros(n), np.zeros(n)])
alpha2 = np.array([np.zeros(n), np.zeros(n), np.zeros(n), np.zeros(n)])
path1 = np.array([np.zeros(n), np.zeros(n), np.zeros(n), np.zeros(n)])
path2 = np.array([np.zeros(n), np.zeros(n), np.zeros(n), np.zeros(n)])
alpha_1, alpha_2, alpha_3, alpha_4 = np.zeros(
n, dtype=float), np.zeros(n, dtype=float), np.zeros(
n, dtype=float), np.zeros(n, dtype=float)
l_1, l_2, l_3, l_4 = np.zeros(n, dtype=float), np.zeros(
n, dtype=float), np.zeros(n, dtype=float), np.zeros(n, dtype=float)
print('Calculating equations of l and alpha')
# For the front legs
l1_t = ls1_t[0] + ls1_t[1] * t1_trot + ls1_t[2] * np.power(
t1_trot,
2) + ls1_t[3] * np.power(t1_trot, 3) + ls1_t[4] * np.power(
t1_trot, 4) + ls1_t[5] * np.power(
t1_trot, 5) + ls1_t[6] * np.power(t1_trot, 6)
l2_t = ls2_t[0] + ls2_t[1] * t2_trot + ls2_t[2] * np.power(
t2_trot,
2) + ls2_t[3] * np.power(t2_trot, 3) + ls2_t[4] * np.power(
t2_trot, 4) + ls2_t[5] * np.power(
t2_trot, 5) + ls2_t[6] * np.power(t2_trot, 6)
a1_t = as1_t[0] + as1_t[1] * t1_trot + as1_t[2] * np.power(
t1_trot,
2) + as1_t[3] * np.power(t1_trot, 3) + as1_t[4] * np.power(
t1_trot, 4) + as1_t[5] * np.power(
t1_trot, 5) + as1_t[6] * np.power(t1_trot, 6)
a2_t = as2_t[0] + as2_t[1] * t2_trot + as2_t[2] * np.power(
t2_trot,
2) + as2_t[3] * np.power(t2_trot, 3) + as2_t[4] * np.power(
t2_trot, 4) + as2_t[5] * np.power(
t2_trot, 5) + as2_t[6] * np.power(t2_trot, 6)
# For the back legs
l1_tBack = ls1_tBack[0] + ls1_tBack[1] * t1_trot + ls1_tBack[
2] * np.power(t1_trot, 2) + ls1_tBack[3] * np.power(
t1_trot, 3) + ls1_tBack[4] * np.power(
t1_trot, 4) + ls1_tBack[5] * np.power(
t1_trot, 5) + ls1_tBack[6] * np.power(t1_trot, 6)
l2_tBack = ls2_tBack[0] + ls2_tBack[1] * t2_trot + ls2_tBack[
2] * np.power(t2_trot, 2) + ls2_tBack[3] * np.power(
t2_trot, 3) + ls2_tBack[4] * np.power(
t2_trot, 4) + ls2_tBack[5] * np.power(
t2_trot, 5) + ls2_tBack[6] * np.power(t2_trot, 6)
a1_tBack = as1_tBack[0] + as1_tBack[1] * t1_trot + as1_tBack[
2] * np.power(t1_trot, 2) + as1_tBack[3] * np.power(
t1_trot, 3) + as1_tBack[4] * np.power(
t1_trot, 4) + as1_tBack[5] * np.power(
t1_trot, 5) + as1_tBack[6] * np.power(t1_trot, 6)
a2_tBack = as2_tBack[0] + as2_tBack[1] * t2_trot + as2_tBack[
2] * np.power(t2_trot, 2) + as2_tBack[3] * np.power(
t2_trot, 3) + as2_tBack[4] * np.power(
t2_trot, 4) + as2_tBack[5] * np.power(
t2_trot, 5) + as2_tBack[6] * np.power(t2_trot, 6)
print('Calculating path of each leg!')
print('Length is ', len(t1_trot) + len(t2_trot) - 1)
for i in range(n):
if (i < n / 2):
alpha_1[i] = a1_t[i]
l_1[i] = l1_t[i]
alpha_2[i] = a2_tBack[i]
l_2[i] = l2_tBack[i]
alpha_3[i] = a1_tBack[i]
l_3[i] = l1_tBack[i]
alpha_4[i] = a2_t[i]
l_4[i] = l2_t[i]
else:
alpha_1[i] = a2_t[int(i - n / 2)]
l_1[i] = l2_t[int(i - n / 2)]
alpha_2[i] = a1_tBack[int(i - n / 2)]
l_2[i] = l1_tBack[int(i - n / 2)]
alpha_3[i] = a2_tBack[int(i - n / 2)]
l_3[i] = l2_tBack[int(i - n / 2)]
alpha_4[i] = a1_t[int(i - n / 2)]
l_4[i] = l1_t[int(i - n / 2)]
alpha_s = np.array([alpha_1, alpha_2, alpha_3, alpha_4])
l_s = np.array([l_1, l_2, l_3, l_4])
print('Alphas ', alpha_s[0][i], ' ', alpha_s[2][i])
#print('L ',l_s)
for j in range(4):
alpha1[j][i] = (alpha_s[j][i] - math.pi / 2)
alpha2[j][i] = (math.acos(
(((globals.l1**2) + ((l_s[j][i])**2) - (globals.l2**2)) /
(2 * globals.l1 * l_s[j][i]))))
self.leftShoulderx[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[1][0])
self.rightShoulderx[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[2][0])
self.leftShouldery[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[1][1])
self.rightShouldery[j][i] = (self.symmetric(
alpha1[j][i], alpha2[j][i], globals.l1, globals.l2)[2][1])
theta1_t[j][i] = (math.acos(self.rightShouldery[j][i] /
globals.l1))
theta2_t[j][i] = (math.acos(self.leftShouldery[j][i] /
globals.l1))
print('Iteration ', i)
print('Trot Path calculated!')
print(theta1_t)
print(theta2_t)
return [theta1_t, theta2_t]
def distance(slat, slon, elat, elon):
dist = 6371.01 * acos(
sin(slat) * sin(elat) + cos(slat) * cos(elat) * cos(slon - elon))
return dist
import math
lat1 = float(input('ingresa la latitud 1:'))
lon1 = float(input('ingresa la longitud 1:'))
lat2 = float(input('ingresa la latitud 2:'))
lon2 = float(input('ingresa la longitud 2:'))
t1 = math.radians(lat1)
t2 = math.radians(lat2)
g1 = math.radians(lon1)
g2 = math.radians(lon2)
distancia = 6371.01 * math.acos(
math.sin(t1) * math.sin(t2) +
math.cos(t1) * math.cos(t2) * math.cos(g1 - g2))
print('la distancia entre los puntos es:', distancia, 'km')
def transitcalcloop(): #Just need to add the save functions
plandataarray = np.load(
'plandataarray.npy') #load the planetary data array
stellardataarray = np.load(
'Stellardataarray.npy'
) #load the stellar data array (No longer need to run LDCcalc beforehand
flats = np.loadtxt('flats.txt')
for line in plandataarray: #now loop through the planetary data array, in which line represents a single planet
print 'For Kepler {}, planet {}:'.format(line['KeplerID'],
line['planetnumber'])
if line['KeplerID'] in flats and os.path.exists(
'kepler-{0}-planet-{1}-analyticaltransitlc.npy'.format(
line['KeplerID'], line['planetnumber'])) == False:
Teff, Logg, Metalicity, Rheader = stellarextractor(
line['KeplerID'], stellardataarray) #extract the gamma values
Inclination = math.acos(
line['Impactpar'] / line['a/R_star']
) #calculate the inclination from the normalised impactpar
#print Inclination
Planetaryradius = line['R_planet'] #extract the planetary radius
Planetaryradius_SI = Planetaryradius * 6.3675E6 #and convert it from earth radii to meters
Orbitalperiod = line['Orbitalperiod'] #Extract the orbital period
Orbitalperiod_SI = Orbitalperiod * 86400 #and convert it from days to seconds
Keplerlc = np.load('kepler-{0}-planet-{1}-trandata.npy'.format(
line['KeplerID'],
line['planetnumber'])) #extract the kepler transit data
tvalues = np.sort(
Keplerlc['Foldtime']
) #create an array of times associated with each data point and sort them
Keplertransit = np.sort(
Keplerlc,
order='Foldtime') #sort the kepler data in the same way
sigma = sigmacalc(
Keplertransit
) #Calculate the standard deviation of the surrounding data points
Inclination_fit, Planetaryradius_SI_fit, Teff_fit, Logg_fit, Metalicity_fit, Mstar_SI, Rstar_SI, Stellardensity_SI, semimajoraxis_SI, T_dur_SI, errors, covar = \
zandtfit(Inclination, Planetaryradius_SI, Keplerlc, Teff, Logg, Metalicity, sigma, Orbitalperiod_SI, line['KeplerID'], line['planetnumber']) #calculate the t and z values
gamma1, gamma2, = gammainterpolator(
Teff_fit, Logg_fit,
np.log10(Metalicity_fit)) #calculate the LDC
zvalues = zandtcalculator(tvalues, Stellardensity_SI,
Inclination_fit,
Orbitalperiod_SI) #calculate the zvalues
#print zvalues #testing
#print tvalues
#print 'The minimum zvalue is:'
#print np.min(zvalues)
norm_impactparameter = (semimajoraxis_SI *
math.cos(Inclination_fit) / Rstar_SI)
print 'The transit duration is:'
print T_dur_SI / 86400
Rplanratio = Planetaryradius_SI_fit / Rstar_SI #Calculate the ratio of planetary radius to stellar radius
#print 'The Planetary Ratio is'
print Logg, Logg_fit
f = transit.occultquad(
zvalues, Rplanratio,
[gamma1, gamma2
]) #calculate/find the analytical transit lightcurve
##transitplot(tvalues, f, line['KeplerID'], line['planetnumber']) #plot the analytical transit lightcurve
comtransitplot(
tvalues, f, Keplertransit['Foldtime'], Keplertransit['Flux'],
line['KeplerID'], line['planetnumber'], line['KOI']
) #plot both the kepler and analytical transit data on the same graph
analyticaltransitlc = np.vstack(
(tvalues, f)
) #make a single array to contain the analytical transit lightcurve
chi, chireduced, DOF, chifull, chireducedfull, DOFfull = chicalc(
Keplertransit, analyticaltransitlc, sigma,
T_dur_SI) #calculate the chisquared values
#print analyticaltransitlc
#print 'The new impactparameter is {}+-{}, the new duration is {}+-{} and the new radius is {}+-{}'. format(impactpar, errors[0], T_dur, errors[1], R_plan, errors[2])
print 'Saving'
np.save('kepler-{0}-planet-{1}-analyticaltransitlc'.format(
line['KeplerID'], line['planetnumber']),
analyticaltransitlc) #save the analytical transit
np.save('kepler-{0}-planet-{1}-covariance'.format(
line['KeplerID'], line['planetnumber']),
covar) #save the covariance array
saveplandata(line, Inclination_fit, T_dur_SI,
Planetaryradius_SI_fit, semimajoraxis_SI,
line['planetnumber'], line['KeplerID'], errors,
chireduced, norm_impactparameter, DOF, chireducedfull,
DOFfull)
savefittedstellardata(line['KeplerID'], line['planetnumber'],
Mstar_SI, Rstar_SI, Stellardensity_SI, Teff,
Teff_fit, errors[2], Logg, Logg_fit,
errors[3], Metalicity, Metalicity_fit,
errors[4], Rheader, gamma1, gamma2,
chireduced, DOF, chireducedfull, DOFfull)
else:
print 'Already Done'
return
def haversine(lat1, long1, lat2, long2):
# p = pi/180
# p = 0.017453292519943295
# a = 0.5 - cos((lat2 - lat1) * p) / 2 + cos(lat1 * p) * cos(lat2 * p) * (1 - cos((long2 - long1) * p)) / 2
return 6371.01 * acos(
sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(long1 - long2))
def arccos(self, v):
if not -1.0 <= v <= 1.0:
return rfloat.NAN
return math.acos(v)
def start(args):
cap = cv2.VideoCapture(0)
os.system(args)
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
Pauseflag = 0
try:
while True:
ret, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
cv2.rectangle(img, (300, 300), (100, 100), (0, 255, 0), 0)
crop_img = img[100:300, 100:300]
grey = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
value = (35, 35)
blurred = cv2.GaussianBlur(grey, value, 0)
_, thresh1 = cv2.threshold(blurred, 127, 255,
cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
cv2.imshow('Thresholded', thresh1)
(version, _, _) = cv2.__version__.split('.')
if version == '3':
image, contours, hierarchy = cv2.findContours(
thresh1.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
elif version == '2':
contours, hierarchy = cv2.findContours(thresh1.copy(),
cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
cnt = max(contours, key=lambda x: cv2.contourArea(x))
x, y, w, h = cv2.boundingRect(cnt)
cv2.rectangle(crop_img, (x, y), (x + w, y + h), (0, 0, 255), 0)
hull = cv2.convexHull(cnt)
drawing = np.zeros(crop_img.shape, np.uint8)
cv2.drawContours(drawing, [cnt], 0, (0, 255, 0), 0)
cv2.drawContours(drawing, [hull], 0, (0, 0, 255), 0)
hull = cv2.convexHull(cnt, returnPoints=False)
defects = cv2.convexityDefects(cnt, hull)
count_defects = 0
cv2.drawContours(thresh1, contours, -1, (0, 255, 0), 3)
for i in range(defects.shape[0]):
s, e, f, d = defects[i, 0]
start = tuple(cnt[s][0])
end = tuple(cnt[e][0])
far = tuple(cnt[f][0])
a = math.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2)
b = math.sqrt((far[0] - start[0])**2 + (far[1] - start[1])**2)
c = math.sqrt((end[0] - far[0])**2 + (end[1] - far[1])**2)
angle = math.acos((b**2 + c**2 - a**2) / (2 * b * c)) * 57
if angle <= 90:
count_defects += 1
cv2.circle(crop_img, far, 1, [255, 0, 0], 3)
cv2.line(crop_img, start, end, [0, 255, 0], 2)
if count_defects == 1:
cv2.putText(img, "Volume 40%", (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, 2)
call(["amixer", "-D", "pulse", "sset", "Master", "40%"])
elif count_defects == 2:
cv2.putText(img, "Volume 60%", (5, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, 2)
call(["amixer", "-D", "pulse", "sset", "Master", "60%"])
elif count_defects == 3:
cv2.putText(img, "Volume 80%", (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, 2)
call(["amixer", "-D", "pulse", "sset", "Master", "80%"])
elif count_defects == 4:
cv2.putText(img, "Volume 100%", (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, 2)
call(["amixer", "-D", "pulse", "sset", "Master", "100%"])
else:
call(["amixer", "-D", "pulse", "sset", "Master", "0%"])
cv2.putText(img,"Volume 0%", (50,50),\
cv2.FONT_HERSHEY_SIMPLEX, 2, 2)
cv2.imshow('Gesture', img)
all_img = np.hstack((drawing, crop_img))
cv2.imshow('Contours', all_img)
k = cv2.waitKey(10)
if k == 27:
break
for (x, y, w, h) in faces:
print("Face is facing front")
os.system("vlc-ctrl play")
Pauseflag = 1
if Pauseflag == 0:
print("Face is not facing front")
os.system("vlc-ctrl pause")
Pauseflag = 0
except KeyboardInterrupt:
print("Closing the application!!! [Interrupted]")
cap.release()
def analyze_rot_sweeps(path, n_sweeps=None, correlation_length=300, directory_prefix='Sweep'):
"""Analyze the rotational correlation at each window for each sweep
Data currently only available for DSPC
Params
------
path : str
The path to the directory with the data for each sweep
n_sweeps : int
The number of sweeps to analyze
correlation_length : float
Desired force autocorrelation length in ps
directory_prefix : str, default = 'Sweep'
Prefix of directories in path that contain the force ACF data. E.g., if
the data is in sweep<N>, use directory_prefix='sweep'
Returns
-------
This function prints the rotational ACF at each window from each sweep.
"""
import glob
sweep_dirs = natsort.natsorted(glob.glob(os.path.join(
path, '{0}*/'.format(directory_prefix))))
if n_sweeps is None:
n_sweeps = len(sweep_dirs)
n_windows = np.loadtxt(os.path.join(sweep_dirs[0], 'y0list.txt')).shape[0]
info = [(7, 3, 'water')] # 7 water molecules per simulation
# loop over sweeps
for sweep_dir in sweep_dirs[:n_sweeps]:
window_order = np.arange(0, 31, 5)
theta_by_window = list()
for i in range(n_windows):
theta_by_window.append(list())
for sim in range(5):
with open(sweep_dir+'tracerpos'+str(sim+1)+'.xyz', 'r') as trj:
IDs = read_frame_lammpstrj(trj, getIDs=True)
IDs = np.sort(IDs)
IDdic = []
for iID, ID in enumerate(IDs):
IDdic.append((ID, iID))
d_ID = {int(x[0]): x[1] for x in IDdic}
while True:
try:
xyz, types, step, box = read_frame_lammpstrj(trj, IDdic=d_ID)
except:
print('Reached end of file')
break
system = System(system_info=info, box=box)
system.convert_from_traj(xyz, types)
for i, gbb in enumerate(system.gbbs):
gbb.masses = np.asarray([15.994, 1.008, 1.008]).reshape((3, 1))
gbb.calc_com()
sorted_gbbs = sorted(system.gbbs, key=lambda x: x.com[2])
for window, gbb in zip(window_order, sorted_gbbs):
gbb.load_xml_prototype('water.xml', skip_coords=True, skip_types=False, skip_masses=True)
director = calc_director(Gbb.calc_inertia_tensor(gbb))
theta_by_window[window].append(math.degrees(math.acos(director[2]))-90)
#theta_by_window[window].append(director[2])
window_order += 1
# all data for this sweep has been collected and can be processed
print('Window')
for window in range(n_windows):
dstep = 1.0 # ps
#print('{0}'.format(window))
funlen = int(correlation_length/dstep)
RACF = rotacf(np.asarray(theta_by_window[window]), funlen)
time = np.arange(0, funlen*dstep, dstep)
np.savetxt(os.path.join(sweep_dir, 'rcorr{0}.dat'.format(window)),
np.vstack((time, RACF)).T, fmt='%.3f')
np.savetxt(os.path.join(sweep_dir, 'orient{0}.dat'.format(window)),
np.vstack(theta_by_window[window]).T, fmt='%.3f')
print(window, np.mean(theta_by_window[window]),np.std(theta_by_window[window]))
print('Finished analyzing data in {0}'.format(sweep_dir))
def angle3D(self, value):
# Returns the angle between this Vector and another Vector
# return math.acos(round(self.normalize().dot(value.normalize()), 4))
return math.acos(max(min(self.normalize().dot(value.normalize()), 1), -1))
uvspec.inp["data_files_path"] = libradtranpath + 'data'
uvspec.inp[
"atmosphere_file"] = libradtranpath + 'data/atmmod/' + atmosphere + '.dat'
uvspec.inp["albedo"] = '0.2'
uvspec.inp["rte_solver"] = rte_eq
if Mod == 'rt':
uvspec.inp["mol_abs_param"] = molmodel + ' ' + molresol
else:
uvspec.inp["mol_abs_param"] = molmodel
# Convert airmass into zenith angle
am = airmass
sza = math.acos(1. / am) * 180. / math.pi
# Should be no_absorption
if runtype == 'aerosol_default':
uvspec.inp["aerosol_default"] = ''
elif runtype == 'aerosol_special':
uvspec.inp["aerosol_set_tau_at_wvl"] = '500 0.02'
if runtype == 'no_scattering':
uvspec.inp["no_scattering"] = ''
if runtype == 'no_absorption':
uvspec.inp["no_absorption"] = ''
# set up the ozone value
uvspec.inp["mol_modify"] = ozon_str
def bell_base_main_hole_outline(ai_c):
""" generate the A-outline of the main hole of the bell_base part
"""
### constant
radian_epsilon = math.pi / 1000
### intermediate parameters
bell_face_width_10 = ai_c['bell_face_width'] / 10.0
wall_thickness = max(ai_c['face_thickness'], ai_c['side_thickness'])
ect = ai_c['bell_extra_cut_thickness']
### sub-outline construction
sol = []
sol.append((-3 * bell_face_width_10,
-5 * bell_face_width_10 + 1.5 * wall_thickness,
ai_c['bell_cnc_router_bit_radius']))
sol.append((-3 * bell_face_width_10, -5 * bell_face_width_10 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
sol.append((-1 * bell_face_width_10, -5 * bell_face_width_10 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
sol.append(
(-1 * bell_face_width_10, -5 * bell_face_width_10 + wall_thickness, 0))
sol.append(
(1 * bell_face_width_10, -5 * bell_face_width_10 + wall_thickness, 0))
sol.append((1 * bell_face_width_10, -5 * bell_face_width_10 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
sol.append((3 * bell_face_width_10, -5 * bell_face_width_10 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
sol.append((3 * bell_face_width_10,
-5 * bell_face_width_10 + 1.5 * wall_thickness,
ai_c['bell_cnc_router_bit_radius']))
if (ai_c['z_hole_radius'] > 0):
lAB = 2 * bell_face_width_10 - wall_thickness
lAC = ai_c['z_hole_position_length']
lCD = ai_c['z_hole_external_radius']
aBAC = math.pi / 4
lCB = math.sqrt(lAB**2 + lAC**2 - 2 * lAB * lAC *
math.cos(aBAC)) # law of cosines in the triangle ABC
cos_aACB = (lCB**2 + lAC**2 - lAB**2) / (2 * lCB * lAC)
if (abs(cos_aACB) > 1):
six.print_(
("ERR359: Error, cos_aACB {:0.3f} is out of the range -1..1".
format(cos_aACB)))
sys.exit(2)
aACB = math.acos(cos_aACB)
cos_aBCD = lCD / lCB
if (abs(cos_aBCD) > 1):
six.print_(
("ERR364: Error, cos_aBCD {:0.3f} is out of the range -1..1".
format(cos_aBCD)))
sys.exit(2)
aBCD = math.acos(cos_aBCD)
aACD = aACB + aBCD
#print("dbg370: aACB {:0.3f} aBCD {:0.3f} aACD {:0.3f}".format(aACB, aBCD, aACD))
lEC = math.sqrt(2) * wall_thickness + lAC
Cx = 5 * bell_face_width_10 - lEC * math.cos(math.pi / 4) # sqrt(2)/2
Cy = -5 * bell_face_width_10 + lEC * math.sin(math.pi / 4)
Fx = Cx - ai_c['z_hole_external_radius'] * math.cos(math.pi / 4)
Fy = Cy + ai_c['z_hole_external_radius'] * math.sin(math.pi / 4)
Dx = Cx + ai_c['z_hole_external_radius'] * math.cos(-math.pi / 4 -
aACD)
Dy = Cy + ai_c['z_hole_external_radius'] * math.sin(-math.pi / 4 -
aACD)
Gx = Cx + ai_c['z_hole_external_radius'] * math.cos(-math.pi / 4 +
aACD)
Gy = Cy + ai_c['z_hole_external_radius'] * math.sin(-math.pi / 4 +
aACD)
if (aACD > math.pi - radian_epsilon):
sol.append((Fx, Fy, ai_c['bell_cnc_router_bit_radius']))
else:
sol.append((Dx, Dy, 0))
sol.append((Fx, Fy, Gx, Gy, 0))
#print("dbg351: sol:", sol)
### outline construction
r_ol = []
for i in range(4):
r_ol.extend(cnc25d_api.outline_rotate(sol, 0.0, 0.0, i * math.pi / 2))
r_ol = cnc25d_api.outline_close(r_ol)
### return
return (r_ol)
def angle(v1, v2):
return acos(dotproduct(v1, v2) / (length(v1) * length(v2)))
vectors = [[60, 54], [54, 48], [63, 57], [57, 51], [21, 12], [12, 3],
[24, 15], [15, 6]]
result = []
for vector in vectors:
v1 = []
v2 = []
for i in range(3):
t1 = res['pose'][vector[0] + i] - res['pose'][vector[1] + i]
t2 = res2['pose'][vector[0] + i] - res2['pose'][vector[1] + i]
v1.append(t1)
v2.append(t2)
np_v1 = np.array(v1)
np_v2 = np.array(v2)
cos_theta = sum(np_v1 * np_v2) / math.sqrt(
sum(np_v1**2) * sum(np_v2**2))
rad = abs(math.acos(cos_theta))
if rad > math.pi:
rad -= math.pi
result.append(1 - 5 * rad / math.pi)
#result.append(1 / (1 + rad / math.pi))
similarity = sum(result) / len(result) * 100
# print(str(similarity)+"%")
ls.append(similarity)
x.append(ind)
"""
if ind == 1020:
source = cv2.imread("./result/"+path1+"/"+str(ind)+".png")
compare = cv2.imread("./result/"+path2+"/"+str(ind-17)+".png")
cv2.imshow("source", source)
cv2.imshow("compare", compare)
cv2.waitKey(0)
def angle(self, value):
# Returns the 2D angle between this Vector and another Vector
return math.acos(round(self.flatten().normalize().dot(value.flatten().normalize()), 4))
def acos(tdw, number):
'''Python build-in function acos'''
return math.acos(number)
def bell_face_outline(ai_c):
""" generate the external-outline (type-a) of the bell_face part
"""
### precision
radian_epsilon = math.pi / 1000
### leg slope inclination angle
lAB = ai_c['bell_face_width'] / 2.0 - ai_c['leg_spare_width']
lBC = ai_c['leg_length']
lCD = ai_c['axle_external_radius']
lAC = math.sqrt(lAB**2 + lBC**2)
cos_aBAC = float(lAB) / lAC
if (abs(cos_aBAC) > 1):
six.print_(
("ERR063: Error, cos_aBAC {:0.3f} is out of the range -1..1".
format(cos_aBAC)))
six.print_(("dbg064: lAC {:0.3f} lAB {:0.3f}".format(lAC, lAB)))
sys.exit(2)
aBAC = math.acos(cos_aBAC)
#lAD = math.sqrt(lAC**2-lCD**2)
aCAD = math.asin(float(lCD) / lAC)
leg_inclination_angle = aBAC + aCAD # aBAD
leg_ext_axle_angle = math.pi / 2 - leg_inclination_angle
#print("dbg072: leg_inclination_angle {:0.3f} leg_ext_axle_angle {:0.3f}".format(leg_inclination_angle, leg_ext_axle_angle))
#print("dbg073: aBAC {:0.3f} aCAD {:0.3f} lAC {:0.3f}".format(aBAC, aCAD, lAC))
### smoothing leg
if (ai_c['leg_spare_width'] > 0):
# coordiante of A
Ax = -1 * ai_c['bell_face_width'] / 2.0 + ai_c['leg_spare_width']
Ay = ai_c['base_thickness'] + ai_c['bell_face_height']
# coordiante of C
Dx = -1 * ai_c['axle_external_radius'] * math.cos(leg_ext_axle_angle)
Dy = ai_c['base_thickness'] + ai_c['bell_face_height'] + ai_c[
'leg_length'] + ai_c['axle_external_radius'] * math.sin(
leg_ext_axle_angle)
# coordiante of E
Ex = -1 * ai_c['bell_face_width'] / 2.0
Ey = ai_c['base_thickness'] + ai_c['bell_face_height']
# equation of the line AC
(AClx, ACly, ACk, lAC, xAC) = small_geometry.line_equation(
(Ax, Ay), (Dx, Dy), "line_AC")
# coordinate of F: the point at the distance leg_smoothing_radius of AC an A
(FX, FY, ACkF) = small_geometry.line_distance_point(
(Ax, Ay), (Dx, Dy), ai_c['leg_smoothing_radius'], "point_F")
# coordinate of G: the intersection of the circle of center E and radius leg_smoothing_radius and the line parallet to AC passing through F
(Gx, Gy, line_circle_intersection_status
) = small_geometry.line_circle_intersection(
(AClx, ACly, ACkF), (Ex, Ey), ai_c['leg_smoothing_radius'],
(Dx, Dy), math.pi / 2, "point_G")
# coordinate of H: projection of G on the line (AC)
(Hx, Hy) = small_geometry.line_point_projection((AClx, ACly, ACk),
(Gx, Gy), "point_H")
# angle (Gx, GH)
axGH = math.atan2(Hy - Gy, Hx - Gx)
if (abs(axGH + leg_ext_axle_angle) > radian_epsilon): # check axGH
six.print_((
"ERR097: Error, axGH {:0.3f} is not equal to -1*leg_ext_axle_angle {:0.3f}"
.format(axGH, leg_ext_axle_angle)))
sys.exit(2)
# angle (Gx, GE)
axGE = math.atan2(Ey - Gy, Ex - Gx)
# angle (Gx, GI)
axGI = (axGH + axGE) / 2.0
# coordinate of I
Ix = Gx + math.cos(axGI) * ai_c['leg_smoothing_radius']
Iy = Gy + math.sin(axGI) * ai_c['leg_smoothing_radius']
### intermediate parameters
axle_center_y = ai_c['base_thickness'] + ai_c['bell_face_height'] + ai_c[
'leg_length']
bell_face_height_5 = ai_c['bell_face_height'] / 5.0
bell_face_width_10 = ai_c['bell_face_width'] / 10.0
bell_face_width_2 = ai_c['bell_face_width'] / 2.0
ect = ai_c['bell_extra_cut_thickness']
st = ai_c['side_thickness']
bt = ai_c['base_thickness']
### bell_face_outline
r_ol = []
r_ol.append(
(bell_face_width_2, ai_c['base_thickness'] + ai_c['bell_face_height'],
0)) # top-right, CCW
#r_ol.append((bell_face_width_2-ai_c['leg_spare_width'], ai_c['base_thickness']+ai_c['bell_face_height'], 0)) # leg
if (ai_c['leg_spare_width'] > 0):
r_ol.append((-1 * Ix, Iy, -1 * Hx, Hy, 0))
r_ol.append((ai_c['axle_external_radius'] * math.cos(leg_ext_axle_angle),
axle_center_y +
ai_c['axle_external_radius'] * math.sin(leg_ext_axle_angle),
0)) # axle
r_ol.append(
(0, axle_center_y + ai_c['axle_external_radius'],
-1 * ai_c['axle_external_radius'] * math.cos(leg_ext_axle_angle),
axle_center_y +
ai_c['axle_external_radius'] * math.sin(leg_ext_axle_angle), 0))
#r_ol.append((-1*bell_face_width_2+ai_c['leg_spare_width'], ai_c['base_thickness']+ai_c['bell_face_height'], 0)) # leg
if (ai_c['leg_spare_width'] > 0):
r_ol.append((Hx, Hy, 0))
r_ol.append((Ix, Iy, -1 * bell_face_width_2,
ai_c['base_thickness'] + ai_c['bell_face_height'], 0))
else:
r_ol.append((-1 * bell_face_width_2,
ai_c['base_thickness'] + ai_c['bell_face_height'],
0)) # left-side
r_ol.append((-1 * bell_face_width_2,
ai_c['base_thickness'] + 4 * bell_face_height_5 + ect, 0))
r_ol.append((-1 * bell_face_width_2 + st + ect,
ai_c['base_thickness'] + 4 * bell_face_height_5 + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((-1 * bell_face_width_2 + st + ect,
ai_c['base_thickness'] + 3 * bell_face_height_5 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((-1 * bell_face_width_2,
ai_c['base_thickness'] + 3 * bell_face_height_5 - ect, 0))
r_ol.append((-1 * bell_face_width_2,
ai_c['base_thickness'] + 2 * bell_face_height_5 + ect, 0))
r_ol.append((-1 * bell_face_width_2 + st + ect,
ai_c['base_thickness'] + 2 * bell_face_height_5 + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((-1 * bell_face_width_2 + st + ect,
ai_c['base_thickness'] + 1 * bell_face_height_5 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((-1 * bell_face_width_2,
ai_c['base_thickness'] + 1 * bell_face_height_5 - ect, 0))
r_ol.append(
(-1 * bell_face_width_2, ai_c['base_thickness'] + ect, 0)) # bottom
r_ol.append((-3 * bell_face_width_10 + ect, ai_c['base_thickness'] + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((-3 * bell_face_width_10 + ect, 0, 0))
r_ol.append((-1 * bell_face_width_10 - ect, 0, 0))
r_ol.append((-1 * bell_face_width_10 - ect, ai_c['base_thickness'] + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((1 * bell_face_width_10 + ect, ai_c['base_thickness'] + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((1 * bell_face_width_10 + ect, 0, 0))
r_ol.append((3 * bell_face_width_10 - ect, 0, 0))
r_ol.append((3 * bell_face_width_10 - ect, ai_c['base_thickness'] + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append(
(bell_face_width_2, ai_c['base_thickness'] + ect, 0)) # right-side
r_ol.append((bell_face_width_2,
ai_c['base_thickness'] + 1 * bell_face_height_5 - ect, 0))
r_ol.append((bell_face_width_2 - (st + ect),
ai_c['base_thickness'] + 1 * bell_face_height_5 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((bell_face_width_2 - (st + ect),
ai_c['base_thickness'] + 2 * bell_face_height_5 + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((bell_face_width_2,
ai_c['base_thickness'] + 2 * bell_face_height_5 + ect, 0))
r_ol.append((bell_face_width_2,
ai_c['base_thickness'] + 3 * bell_face_height_5 - ect, 0))
r_ol.append((bell_face_width_2 - (st + ect),
ai_c['base_thickness'] + 3 * bell_face_height_5 - ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((bell_face_width_2 - (st + ect),
ai_c['base_thickness'] + 4 * bell_face_height_5 + ect,
-1 * ai_c['bell_cnc_router_bit_radius']))
r_ol.append((bell_face_width_2,
ai_c['base_thickness'] + 4 * bell_face_height_5 + ect, 0))
r_ol.append((r_ol[0][0], r_ol[0][1], 0)) # close
return (r_ol)
def project():
cap = cv2.VideoCapture(0)
#while(cap.isOpened()):
# read image
ret, img = cap.read()
# get hand data from the rectangle sub window on the screen
cv2.rectangle(img, (400, 400), (100, 100), (0, 255, 0), 0)
crop_img = img[100:400, 100:400]
# convert to grayscale
grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# applying gaussian blur
value = (35, 35)
blurred = cv2.GaussianBlur(grey, value, 0)
# thresholdin: Otsu's Binarization method
_, thresh1 = cv2.threshold(blurred, 2, 255,
cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
cv2.imshow('Thresholded', thresh1)
contours, hierarchy = cv2.findContours(thresh1.copy(), cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
# find contour with max area
cnt = max(contours, key=lambda x: cv2.contourArea(x))
# create bounding rectangle around the contour
x, y, w, h = cv2.boundingRect(cnt)
cv2.rectangle(crop_img, (x, y), (x + w, y + h), (0, 0, 255), 0)
# finding convex hull
hull = cv2.convexHull(cnt)
# drawing contours
drawing = np.zeros(crop_img.shape, np.uint8)
cv2.drawContours(drawing, [cnt], 0, (0, 255, 0), 0)
cv2.drawContours(drawing, [hull], 0, (0, 0, 255), 0)
# finding convex hull
hull = cv2.convexHull(cnt, returnPoints=False)
# finding convexity defects
defects = cv2.convexityDefects(cnt, hull)
count_defects = 0
cv2.drawContours(thresh1, contours, -1, (0, 255, 0), 3)
# applying Cosine Rule to find angle for all defects (between fingers)
# with angle > 90 degrees and ignore defects
for i in range(defects.shape[0]):
s, e, f, d = defects[i, 0]
start = tuple(cnt[s][0])
end = tuple(cnt[e][0])
far = tuple(cnt[f][0])
# find length of all sides of triangle
a = math.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2)
b = math.sqrt((far[0] - start[0])**2 + (far[1] - start[1])**2)
c = math.sqrt((end[0] - far[0])**2 + (end[1] - far[1])**2)
# apply cosine rule here
angle = math.acos((b**2 + c**2 - a**2) / (2 * b * c)) * 57
# ignore angles > 90 and highlight rest with red dots
if angle <= 90:
count_defects += 1
cv2.circle(crop_img, far, 1, [0, 0, 255], -1)
#dist = cv2.pointPolygonTest(cnt,far,True)
# draw a line from start to end i.e. the convex points (finger tips)
# (can skip this part)
cv2.line(crop_img, start, end, [0, 255, 0], 2)
#cv2.circle(crop_img,far,5,[0,0,255],-1)
# define actions required
if count_defects == 1:
result = 2
elif count_defects == 2:
result = 3
elif count_defects == 3:
result = 4
elif count_defects == 4:
result = 5
else:
result = 1
# show appropriate images in windows
cv2.imshow('Gesture', img)
all_img = np.hstack((drawing, crop_img))
cv2.imshow('Contours', all_img)
k = cv2.waitKey(10)
#if k==27:
#break
return result
def angle(u, v=Vector(1, 0, 0), normal=Vector(0, 0, 1)):
"""Return the angle in radians between the two vectors.
It uses the definition of the dot product
::
A * B = |A||B| cos(angle)
If only one vector is given, the angle is between that one and the
horizontal (+X).
If a third vector is given, it is the normal used to determine
the sign of the angle.
This normal is used to calculate a `factor` as the dot product
with the cross product of the first two vectors.
::
C = A x B
factor = normal * C
If the `factor` is positive the angle is positive, otherwise
it is the opposite sign.
Parameters
----------
u : Base::Vector3
The first vector.
v : Base::Vector3, optional
The second vector to test against the first one.
It defaults to `(1, 0, 0)`, or +X.
normal : Base::Vector3, optional
The vector indicating the normal.
It defaults to `(0, 0, 1)`, or +Z.
Returns
-------
float
The angle in radians between the vectors.
It is zero if the magnitude of one of the vectors is zero,
or if they are colinear.
"""
typecheck([(u, Vector), (v, Vector)], "angle")
ll = u.Length * v.Length
if ll == 0:
return 0
# The dot product indicates the projection of one vector over the other
dp = u.dot(v)/ll
# Due to rounding errors, the dot product could be outside
# the range [-1, 1], so let's force it to be within this range.
if dp < -1:
dp = -1
elif dp > 1:
dp = 1
ang = math.acos(dp)
# The cross product compared with the provided normal
normal1 = u.cross(v)
coeff = normal.dot(normal1)
if coeff >= 0:
return ang
else:
return -ang
def calculate_with_zenith(self, date, zenith):
"""Computes the sunset and sunrise for the current day, in local time"""
# Calculate the day of the year
N = date.timetuple().tm_yday
# Convert the longitude to hour value and calculate an approximate time
lngHour = self.longitude / 15
t_rise = N + ((6 - lngHour) / 24)
t_set = N + ((18 - lngHour) / 24)
# Calculate the Sun's mean anomaly
M_rise = (0.9856 * t_rise) - 3.289
M_set = (0.9856 * t_set) - 3.289
# Calculate the Sun's true longitude, and adjust angle to be between 0
# and 360
L_rise = (M_rise + (1.916 * math.sin(math.radians(M_rise))) + (0.020 * math.sin(math.radians(2 * M_rise))) + 282.634) % 360
L_set = (M_set + (1.916 * math.sin(math.radians(M_set))) + (0.020 * math.sin(math.radians(2 * M_set))) + 282.634) % 360
# Calculate the Sun's right ascension, and adjust angle to be between 0 and
# 360
RA_rise = (math.degrees(math.atan(0.91764 * math.tan(math.radians(L_rise))))) % 360
RA_set = (math.degrees(math.atan(0.91764 * math.tan(math.radians(L_set))))) % 360
# Right ascension value needs to be in the same quadrant as L
Lquadrant_rise = (math.floor(L_rise / 90)) * 90
RAquadrant_rise = (math.floor(RA_rise / 90)) * 90
RA_rise = RA_rise + (Lquadrant_rise - RAquadrant_rise)
Lquadrant_set = (math.floor(L_set / 90)) * 90
RAquadrant_set = (math.floor(RA_set / 90)) * 90
RA_set = RA_set + (Lquadrant_set - RAquadrant_set)
# Right ascension value needs to be converted into hours
RA_rise = RA_rise / 15
RA_set = RA_set / 15
# Calculate the Sun's declination
sinDec_rise = 0.39782 * math.sin(math.radians(L_rise))
cosDec_rise = math.cos(math.asin(sinDec_rise))
sinDec_set = 0.39782 * math.sin(math.radians(L_set))
cosDec_set = math.cos(math.asin(sinDec_set))
# Calculate the Sun's local hour angle
cos_zenith = math.cos(math.radians(zenith))
radian_lat = math.radians(self.latitude)
sin_latitude = math.sin(radian_lat)
cos_latitude = math.cos(radian_lat)
cosH_rise = (cos_zenith - (sinDec_rise * sin_latitude)) / (cosDec_rise * cos_latitude)
cosH_set = (cos_zenith - (sinDec_set * sin_latitude)) / (cosDec_set * cos_latitude)
# Finish calculating H and convert into hours
H_rise = (360 - math.degrees(math.acos(cosH_rise))) / 15
H_set = math.degrees(math.acos(cosH_set)) / 15
# Calculate local mean time of rising/setting
T_rise = H_rise + RA_rise - (0.06571 * t_rise) - 6.622
T_set = H_set + RA_set - (0.06571 * t_set) - 6.622
# Adjust back to UTC, and keep the time between 0 and 24
UT_rise = (T_rise - lngHour) % 24
UT_set = (T_set - lngHour) % 24
# Convert UT value to local time zone of latitude/longitude
localT_rise = (UT_rise + self.localOffset) % 24
localT_set = (UT_set + self.localOffset) % 24
# Conversion
h_rise = int(localT_rise)
m_rise = int(localT_rise % 1 * 60)
h_set = int(localT_set)
m_set = int(localT_set % 1 * 60)
# Create datetime objects with same date, but with hour and minute
# specified
rise_dt = date.replace(hour=h_rise, minute=m_rise)
set_dt = date.replace(hour=h_set, minute=m_set)
return rise_dt, set_dt
def make_profile(self, profile, idmat,
x_offset, z_offset, extend, verts, faces, matids, uvs):
# self.set_offset(x_offset)
n_roofs = len(self.segs) - 1
if n_roofs < 0:
return
sections = []
f = self.segs[0]
# first step
if extend != 0:
t = -extend / self.segs[0].line.length
n = f.line.sized_normal(t, 1)
# n.p = f.lerp(x_offset)
sections.append((n, f.dz / f.length, f.z0 + f.dz * t))
# add first section
n = f.line.sized_normal(0, 1)
# n.p = f.lerp(x_offset)
sections.append((n, f.dz / f.length, f.z0))
for s, f in enumerate(self.segs):
n = f.line.sized_normal(1, 1)
# n.p = f.lerp(x_offset)
sections.append((n, f.dz / f.length, f.z0 + f.dz))
if extend != 0:
t = 1 + extend / self.segs[-1].line.length
n = f.line.sized_normal(t, 1)
# n.p = f.lerp(x_offset)
sections.append((n, f.dz / f.length, f.z0 + f.dz * t))
user_path_verts = len(sections)
f = len(verts)
if user_path_verts > 0:
user_path_uv_v = []
n, dz, z0 = sections[-1]
sections[-1] = (n, dz, z0)
n_sections = user_path_verts - 1
n, dz, zl = sections[0]
p0 = n.p
v0 = n.v.normalized()
for s, section in enumerate(sections):
n, dz, zl = section
p1 = n.p
if s < n_sections:
v1 = sections[s + 1][0].v.normalized()
dir = (v0 + v1).normalized()
scale = 1 / cos(0.5 * acos(min(1, max(-1, v0 * v1))))
for p in profile:
x, y = n.p + scale * (x_offset + p.x) * dir
z = zl + p.y + z_offset
verts.append((x, y, z))
if s > 0:
user_path_uv_v.append((p1 - p0).length)
p0 = p1
v0 = v1
# build faces using Panel
lofter = Lofter(
# closed_shape, index, x, y, idmat
True,
[i for i in range(len(profile))],
[p.x for p in profile],
[p.y for p in profile],
[idmat for i in range(len(profile))],
closed_path=False,
user_path_uv_v=user_path_uv_v,
user_path_verts=user_path_verts
)
faces += lofter.faces(16, offset=f, path_type='USER_DEFINED')
matids += lofter.mat(16, idmat, idmat, path_type='USER_DEFINED')
v = Vector((0, 0))
uvs += lofter.uv(16, v, v, v, v, 0, v, 0, 0, path_type='USER_DEFINED')
# Therefore ABC = 90
# M is the midpoint of hypotenuse AC
#
# You are given the lengths AB and BC
# Your task is to find MBC (angle Theta , as shown in the figure) in degrees.
from math import sqrt, acos, degrees
if __name__ == '__main__':
ab = int(input())
bc = int(input())
ac = sqrt(ab**2 + bc**2)
m = ac / 2
an = acos(bc / ac)
de = degrees(an)
print(round(de), chr(176), sep='')
def makeRingWing():
YC = [0]
YCP = [0]
YT = [0]
XU = [0, 0]
YU = [0, 0]
XL = [0, 0]
YL = [0, 0]
XDLE = [13.46990, 13.46990]
YDLE = [0.43004, 0.47681]
XDTE = [14.21661, 14.2074]
YDTE = [0.35659, 0.33856]
XC=[0.,0.0,0.005,0.0075,0.0125,0.025\
,0.05,0.075,0.1,0.15,0.2,0.25,0.3\
,0.35,0.4,0.45,0.5,0.55,0.6,0.65\
,0.7,0.75,0.8,0.85,0.9,0.95,1.]
B=[0,0.43756,-0.08136,-0.06496,-0.01926,-0.00185\
,0.00348,0.00156,-0.00113,-0.00058,0.00027\
,0.00080,0.00006,-0.00027,-0.00033,0.00005\
,0.00014,0.00008]
for i in xrange(1, 27):
X = XC[i]
D = 0.4 - X
E = 1.0 - X
if abs(X - 0.0) < 1.0e-20:
X = 1.0e-30
if abs(D) < 1.0e-20:
D = 1.0e-30
if abs(E) < 1.0e-20:
E = 1.0e-30
ycVal1=-0.049921*(0.5*D*D*math.log(abs(D))-0.5*E*E*math.log(E)\
+0.25*E*E-0.25*D*D)
ycVal2 = ycVal1 + 0.029953 * (X * math.log(X) + 0.227828 -
0.531076 * X)
YC.append(ycVal2)
ycpVal1=-0.049921*(E*math.log(E)-D*math.log(abs(D)))\
+0.02995253*(math.log(X)+0.4689244)
YCP.append(ycpVal1)
for i in xrange(1, 27):
X = XC[i]
if i >= 16:
XC1 = 1.0 - X
yVal=0.03333+1.696969*XC1-1.441945*XC1**2\
-0.366363*XC1**3+0.333049*XC1**4
# print yVal
# print yVal*0.1
YT.append(yVal * 0.1)
else:
OM = math.acos(2.0 * X - 1.)
YY = 0.
for j in xrange(1, 18):
# print j
YY = YY + (B[j] * math.sin(j * OM))
# print YY
# print YY*0.1
YT.append(YY * 0.1)
for i in xrange(2, 27):
TH = math.atan(YCP[i])
# print "TH =",math.degrees(TH)
SINTH = math.sin(TH)
COSTH = math.cos(TH)
XU.append(XC[i] - YT[i] * SINTH)
YU.append(YC[i] + YT[i] * COSTH)
XL.append(XC[i] + YT[i] * SINTH)
YL.append(YC[i] - YT[i] * COSTH)
for k in xrange(0, 2):
PHI = math.atan2((YDTE[k] - YDLE[k]), (XDTE[k] - XDLE[k]))
CS = math.cos(PHI)
SN = math.sin(PHI)
CHORD = math.sqrt((YDTE[k] - YDLE[k])**2 + (XDTE[k] - XDLE[k])**2)
listStart = vertCounter
for i in xrange(1, 27):
x = XDLE[k] + (CHORD * (XU[i] * CS - YU[i] * SN))
y = YDLE[k] + (CHORD * (XU[i] * SN + YU[i] * CS))
createPoint(x, y, 0)
lp1 = vertCounter - 1
listEnd = vertCounter
createCurve(listStart, listEnd)
createRevolution(c["Curve_{0}".format(curveCounter - 1)])
listStart = vertCounter
for i in xrange(1, 27):
# XLL=XL[i]
x = XDLE[k] + (CHORD * (XL[i] * CS - YL[i] * SN))
y = YDLE[k] + (CHORD * (XL[i] * SN + YL[i] * CS))
createPoint(x, y, 0)
listEnd = vertCounter
lp2 = vertCounter - 1
createCurve(listStart, listEnd)
createRevolution(c["Curve_{0}".format(curveCounter - 1)])
createLine(lp1, lp2)
createRevolution(li["Line_{0}".format(lineCounter - 1)])
createShell([r["Revolution_{0}".format(revolutionCounter-1)],\
r["Revolution_{0}".format(revolutionCounter-2)],\
r["Revolution_{0}".format(revolutionCounter-3)]]\
,showInStudy=True,name="Ringwing_"+str(k+1))
d_countersink = 0.20 * 0.0254 # [m] #4 nylon screw countersnink hole diameter
d_screw_hole = 0.089 * 0.0254 # [m] #4-32 FIXME or #4-40?? tap hole
# ---------------------------------------------------------------------------------------------------------------------
# Cylindrical Patch
# This patch length calculation is from Chapter 7 of The Antenna Engineering Handbook by Richard C. Johnson and Henry Jasik
# Also see our notes on http://psas.pdx.edu/AntennaDesignLV2/
H = t_gnd + h + t_pcb
z_0 = sqrt(mu/epsilon) * log(1 + 2 * h / airframe_od) / (2 * pi)
L = pi *(airframe_od + H) # [m] Note: includes t_pcb, but the copper is on the *inside*. We claim this doesn't make a difference, since we really want to know the length of the FR4.
BL = L / (120 * pi * lambda_0) * (-0.540754132818691 - 2 * log(f * h / c))
GL = L / (120 * lambda_0)
theta = acos((BL * BL + GL * GL-1/(z_0*z_0))/sqrt( ((BL*BL+GL*GL) ** 2)+1/(z_0 ** 4)+2*(BL-GL)*(BL+GL)/(z_0*z_0)))
patch_length = (theta * lambda_0) / (2 * pi * sqrt(epsilon_r)) # [m] electrical patch length
dprint(1,"Patch electrical length = " + str(patch_length) + " m (" + str(patch_length/0.0254) + "in)")
patch_width = pi * (airframe_od + 2 * t_gnd + 2 * h) # [m] Width of radiator
dprint(1,"Patch width = " + str(patch_width) + " m (" + str(patch_width/0.0254) + "in)")
if patch_width > (18 * 0.0254): # 11 x 17 in PCB
print('Patch_width of ' + str(patch_width / 0.0254) + ' inches is wider than 18 in maximum')
sys.exit(1)
# ---------------------------------------------------------------------------------------------------------------------
# Calculating feeds and levels. Need at least two feed points.
# Calculate the "fractional" number of feeds given the width of the patch (which is only dependent on the airframe)
feeds = patch_width / (lambda_0 / sqrt(epsilon_r))
if Opposite == "x" or V4 == 0 or Opposite == "":
R = 1;
if Opposite != "x" and V4 == 1 and Opposite != "":
print("Please Type A Real Opposite, Type x Or Hit Enter");
if Theta == "x":
if Hypotenuse == "":
ans = math.atan(O / A);
D = 0;
elif Adjacent == "":
ans = math.asin(O / H);
D = 0;
else:
ans = math.acos(A / H);
D = 0;
if Hypotenuse == "x":
if Theta == "": # H, O, A
ans = math.sqrt(math.pow(O, 2) + math.pow(O, 2));
elif Adjacent == "": # H, O, T Sin
ans = O * math.sin(math.radians(T));
else: # H, A, T Cos
ans = A * math.cos(math.radians(T));
if Adjacent == "x":
if Theta == "": # A, H, O
def get_niggli_reduced_lattice(self, tol=1e-5):
"""
Get the Niggli reduced lattice using the numerically stable algo
proposed by R. W. Grosse-Kunstleve, N. K. Sauter, & P. D. Adams,
Acta Crystallographica Section A Foundations of Crystallography, 2003,
60(1), 1-6. doi:10.1107/S010876730302186X
Args:
tol (float): The numerical tolerance. The default of 1e-5 should
result in stable behavior for most cases.
Returns:
Niggli-reduced lattice.
"""
# lll reduction is more stable for skewed cells
matrix = self.lll_matrix
a = matrix[0]
b = matrix[1]
c = matrix[2]
e = tol * self.volume**(1 / 3)
# Define metric tensor
G = [[dot(a, a), dot(a, b), dot(a, c)],
[dot(a, b), dot(b, b), dot(b, c)],
[dot(a, c), dot(b, c), dot(c, c)]]
G = np.array(G)
# This sets an upper limit on the number of iterations.
for count in range(100):
# The steps are labelled as Ax as per the labelling scheme in the
# paper.
(A, B, C, E, N, Y) = (G[0, 0], G[1, 1], G[2, 2], 2 * G[1, 2],
2 * G[0, 2], 2 * G[0, 1])
if A > B + e or (abs(A - B) < e and abs(E) > abs(N) + e):
# A1
M = [[0, -1, 0], [-1, 0, 0], [0, 0, -1]]
G = dot(transpose(M), dot(G, M))
if (B > C + e) or (abs(B - C) < e and abs(N) > abs(Y) + e):
# A2
M = [[-1, 0, 0], [0, 0, -1], [0, -1, 0]]
G = dot(transpose(M), dot(G, M))
continue
l = 0 if abs(E) < e else E / abs(E)
m = 0 if abs(N) < e else N / abs(N)
n = 0 if abs(Y) < e else Y / abs(Y)
if l * m * n == 1:
# A3
i = -1 if l == -1 else 1
j = -1 if m == -1 else 1
k = -1 if n == -1 else 1
M = [[i, 0, 0], [0, j, 0], [0, 0, k]]
G = dot(transpose(M), dot(G, M))
elif l * m * n == 0 or l * m * n == -1:
# A4
i = -1 if l == 1 else 1
j = -1 if m == 1 else 1
k = -1 if n == 1 else 1
if i * j * k == -1:
if n == 0:
k = -1
elif m == 0:
j = -1
elif l == 0:
i = -1
M = [[i, 0, 0], [0, j, 0], [0, 0, k]]
G = dot(transpose(M), dot(G, M))
(A, B, C, E, N, Y) = (G[0, 0], G[1, 1], G[2, 2], 2 * G[1, 2],
2 * G[0, 2], 2 * G[0, 1])
# A5
if abs(E) > B + e or (abs(E - B) < e and 2 * N < Y - e) or\
(abs(E + B) < e and Y < -e):
M = [[1, 0, 0], [0, 1, -E / abs(E)], [0, 0, 1]]
G = dot(transpose(M), dot(G, M))
continue
# A6
if abs(N) > A + e or (abs(A - N) < e and 2 * E < Y - e) or\
(abs(A + N) < e and Y < -e):
M = [[1, 0, -N / abs(N)], [0, 1, 0], [0, 0, 1]]
G = dot(transpose(M), dot(G, M))
continue
# A7
if abs(Y) > A + e or (abs(A - Y) < e and 2 * E < N - e) or\
(abs(A + Y) < e and N < -e):
M = [[1, -Y / abs(Y), 0], [0, 1, 0], [0, 0, 1]]
G = dot(transpose(M), dot(G, M))
continue
# A8
if E + N + Y + A + B < -e or\
(abs(E + N + Y + A + B) < e < Y + (A + N) * 2):
M = [[1, 0, 1], [0, 1, 1], [0, 0, 1]]
G = dot(transpose(M), dot(G, M))
continue
break
A = G[0, 0]
B = G[1, 1]
C = G[2, 2]
E = 2 * G[1, 2]
N = 2 * G[0, 2]
Y = 2 * G[0, 1]
a = math.sqrt(A)
b = math.sqrt(B)
c = math.sqrt(C)
alpha = math.acos(E / 2 / b / c) / math.pi * 180
beta = math.acos(N / 2 / a / c) / math.pi * 180
gamma = math.acos(Y / 2 / a / b) / math.pi * 180
latt = Lattice.from_parameters(a, b, c, alpha, beta, gamma)
mapped = self.find_mapping(latt, e, skip_rotation_matrix=True)
if mapped is not None:
if np.linalg.det(mapped[0].matrix) > 0:
return mapped[0]
else:
return Lattice(-mapped[0].matrix)
raise ValueError("can't find niggli")
def deltaR(a, b):
return math.sqrt(
math.pow(a.eta() - b.eta(), 2) +
math.pow(math.acos(math.cos(a.phi() - b.phi())), 2))
return (int(point[0] * scalex + translatex),
int(point[1] * scaley + translatey))
# Directions of each axis:
# -y
# |
# +-- +x
# /
# +z
# Set up the points of the sphere:
points = [(0, 0, 1.0), (0, 0, -1.0)]
for latitude in range(NUM_LAT_POINTS):
for longitude in range(NUM_LON_POINTS):
lat = math.acos(2 * (latitude / float(NUM_LAT_POINTS)) - 1) - (math.pi / 2)
lon = 2 * math.pi * (longitude / float(NUM_LON_POINTS))
x = math.cos(lat) * math.cos(lon)
y = math.cos(lat) * math.sin(lon)
z = math.sin(lat)
points.append((x, y, z))
rotatedPoints = [None] * len(points)
rx = ry = rz = 0.0 # Rotation amounts for each axis.
try:
while True: # Main program loop.
# Rotate the cube:
rx += 0.01# + random.randint(1, 20) / 100
rospy.loginfo("Now attempting to transform rotational error.")
#phase 2 angular adjustment
rospy.wait_for_service('rotate_base')
now = rospy.Time()
location.header.stamp = now
listener.waitForTransform("/map", "/base_footprint", now, rospy.Duration(20.0))
try:
point = listener.transformPose("/base_footprint", location)
except Exception, e:
print e
rospy.loginfo("Rotational transform complete.")
try:
rotate = rospy.ServiceProxy('rotate_base', RotateBase)
angle = sign(point.pose.orientation.z)*2*math.acos(point.pose.orientation.w)
rospy.loginfo("Rotating by: %s.", angle)
resp1 = rotate(angle)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
rospy.loginfo("Time for the linear correction!")
#phase 3 linear correction
rospy.wait_for_service('move_base')
now = rospy.Time()
location.header.stamp = now
listener.waitForTransform("/map", "/base_footprint", now, rospy.Duration(4.0))
try: