def get_zenith(Latitude, Longitude, d, hour, minute, timezone):
gamma_val = ((2 * math.pi) / 365) * ((d - 1) + (hour - 12) / 24)
decl_angle = 0.006918 - (0.399912 * math.cos(gamma_val)) + 0.070257 * math.sin(gamma_val) - 0.006758 * math.cos(
2 * gamma_val) \
+ 0.000907 * math.sin(2 * gamma_val) - 0.002697 * math.cos(3 * gamma_val) + 0.00148 * math.sin(
3 * gamma_val)
eq_time = 229.18 * (
0.000075 + 0.001868 * math.cos(gamma_val) - 0.032077 * math.sin(gamma_val) - 0.014615 * math.cos(2 * gamma_val)
- 0.040849 * math.sin(2 * gamma_val))
time_offset = eq_time - 4 * Longitude + 60*timezone
true_solar_time = hour * 60 + minute + time_offset
solar_hour_angle = true_solar_time / 4 - 180
Z_deg = (180/math.pi)*math.acos((math.sin(Latitude * (math.pi / 180)) * math.sin(decl_angle)) + (
math.cos(Latitude * (math.pi / 180)) * math.cos(decl_angle) * math.cos(solar_hour_angle * (math.pi / 180))))
return Z_deg
python类acos()的实例源码
def camPosToQuaternion(cx, cy, cz):
camDist = math.sqrt(cx * cx + cy * cy + cz * cz)
cx = cx / camDist
cy = cy / camDist
cz = cz / camDist
axis = (-cz, 0, cx)
angle = math.acos(cy)
a = math.sqrt(2) / 2
b = math.sqrt(2) / 2
w1 = axis[0]
w2 = axis[1]
w3 = axis[2]
c = math.cos(angle / 2)
d = math.sin(angle / 2)
q1 = a * c - b * d * w1
q2 = b * c + a * d * w1
q3 = a * d * w2 + b * d * w3
q4 = -b * d * w2 + a * d * w3
return (q1, q2, q3, q4)
def SAM(s1, s2):
"""
Computes the spectral angle mapper between two vectors (in radians).
Parameters:
s1: `numpy array`
The first vector.
s2: `numpy array`
The second vector.
Returns: `float`
The angle between vectors s1 and s2 in radians.
"""
try:
s1_norm = math.sqrt(np.dot(s1, s1))
s2_norm = math.sqrt(np.dot(s2, s2))
sum_s1_s2 = np.dot(s1, s2)
angle = math.acos(sum_s1_s2 / (s1_norm * s2_norm))
except ValueError:
# python math don't like when acos is called with
# a value very near to 1
return 0.0
return angle
def distGEO(x1,y1,x2,y2):
print("Implementation is wrong")
assert False
PI = 3.141592
deg = int(x1 + .5)
min_ = x1 - deg
lat1 = PI * (deg + 5.*min_/3)/180.
deg = int(y1 + .5)
min_ = y1 - deg
long1 = PI * (deg + 5.*min_/3)/180.
deg = int(x2 + .5)
min_ = x2 - deg
lat2 = PI * (deg + 5.*min_/3)/180.
deg = int(y2 + .5)
min_ = y2 - deg
long2 = PI * (deg + 5.*min_/3)/180.
RRR = 6378.388
q1 = math.cos( long1 - long2 );
q2 = math.cos( lat1 - lat2 );
q3 = math.cos( lat1 + lat2 );
return int(RRR * math.acos(.5*((1.+q1)*q2 - (1.-q1)*q3)) + 1.)
def distGEO(x1,y1,x2,y2):
print("Implementation is wrong")
assert False
PI = 3.141592
deg = int(x1 + .5)
min_ = x1 - deg
lat1 = PI * (deg + 5.*min_/3)/180.
deg = int(y1 + .5)
min_ = y1 - deg
long1 = PI * (deg + 5.*min_/3)/180.
deg = int(x2 + .5)
min_ = x2 - deg
lat2 = PI * (deg + 5.*min_/3)/180.
deg = int(y2 + .5)
min_ = y2 - deg
long2 = PI * (deg + 5.*min_/3)/180.
RRR = 6378.388
q1 = math.cos( long1 - long2 );
q2 = math.cos( lat1 - lat2 );
q3 = math.cos( lat1 + lat2 );
return int(RRR * math.acos(.5*((1.+q1)*q2 - (1.-q1)*q3)) + 1.)
def angleOfVector(p1, v, p2):
"""
Accepts p1, v, and p2 of class Point or coordinate pairs
Returns the angle in radians between the vector v-to-p1 and vector v-to-p2
"""
#L1=distance(p1, p2)
#L2=distance(p1, p3)
#L3=distance(p2, p3)
#return math.acos((L1**2+L2**2-L3**2)/(2*L1*L2))
p1 = dPnt(p1)
p2 = dPnt(p2)
angle1 = angleOfLine(v, p1)
angle2 = angleOfLine(v, p2)
#get the absolute angle
angle = abs(angle1 - angle2)
#get the smallest angle of the vector, should not be greater than a straight line
if angle > pi:
angle = 2*pi - angle
return angle
def angleOfVector(p1, v, p2):
"""
Accepts p1, v, and p2 of class Point or coordinate pairs
Returns the angle in radians between the vector v-to-p1 and vector v-to-p2
"""
#L1=distance(p1, p2)
#L2=distance(p1, p3)
#L3=distance(p2, p3)
#return math.acos((L1**2+L2**2-L3**2)/(2*L1*L2))
p1 = dPnt(p1)
p2 = dPnt(p2)
angle1 = angleOfLine(v, p1)
angle2 = angleOfLine(v, p2)
#get the absolute angle
angle = abs(angle1 - angle2)
#get the smallest angle of the vector, should not be greater than a straight line
if angle > pi:
angle = 2*pi - angle
return angle
def dist_sqr_at_l(self, p):
"""return squared distance and arc length at nearest point to p on biarc"""
p1, t1, cha1 = biarc.point_on_circle(self.p0, self.J, self.t0, p)
p2, t2, cha2 = biarc.point_on_circle(self.J, self.p1, self.Jt, p)
c1, k1, a1, l1, c2, k2, a2, l2 = self.circleparameters()
mind, minl = None, None
if 0 < t1 < 1:
mind = la.norm2(p1 - p)
aa1 = 2 * m.acos(cha1)
minl = aa1 / abs(k1) if k1 != 0 else t1 * l1
if 0 < t2 < 1 and (not mind or la.norm2(p2 - p) < mind):
mind = la.norm2(p2 - p)
aa2 = 2 * m.acos(cha2)
minl = l1 + (aa2 / abs(k2) if k2 != 0 else t2 * l2)
return mind, minl
###########################################################################
def proj_xy(self, t, next=None):
"""
length of projection of sections at crossing line / circle intersections
deformation unit vector for profil in xy axis
so f(x_profile) = position of point in xy plane
"""
if next is None:
return self.normal(t).v.normalized(), 1
v0 = self.normal(1).v.normalized()
v1 = next.normal(0).v.normalized()
direction = v0 + v1
adj = (v0 * self.length) * (v1 * next.length)
hyp = (self.length * next.length)
c = min(1, max(-1, adj / hyp))
size = 1 / cos(0.5 * acos(c))
return direction.normalized(), min(3, size)
def _get_defects_count(array, contour, defects, verbose = False):
ndefects = 0
for i in range(defects.shape[0]):
s,e,f,_ = defects[i,0]
beg = tuple(contour[s][0])
end = tuple(contour[e][0])
far = tuple(contour[f][0])
a = _get_eucledian_distance(beg, end)
b = _get_eucledian_distance(beg, far)
c = _get_eucledian_distance(end, far)
angle = math.acos((b ** 2 + c ** 2 - a ** 2) / (2 * b * c)) * 57
if angle <= 90:
ndefects = ndefects + 1
if verbose:
cv2.circle(array, far, 3, _COLOR_RED, -1)
if verbose:
cv2.line(array, beg, end, _COLOR_RED, 1)
return array, ndefects
def orien(current,initial,others):
temp=[]
#print others
f=0
for i in others:
if i != current:
#print i,current,initial
a=length(current,initial)
b=length(i,initial)
c=length(current,i)
d=math.acos((a*a + b*b - c*c) / (2*a*b))
temp.append((d,f))
else:
temp.append((0,f))
f+=1
return temp
def orien(current,initial,others):
temp=[]
#print others
f=0
for i in others:
if i != current:
#print i,current,initial
a=length(current,initial)
b=length(i,initial)
c=length(current,i)
d=math.acos((a*a + b*b - c*c) / (2*a*b))
temp.append((d,f))
else:
temp.append((0,f))
f+=1
return temp
def interpolate(self, other, this_weight):
q0, q1 = np.roll(self.q, shift=1), np.roll(other.q, shift=1)
u = 1 - this_weight
assert(u >= 0 and u <= 1)
cos_omega = np.dot(q0, q1)
if cos_omega < 0:
result = -q0[:]
cos_omega = -cos_omega
else:
result = q0[:]
cos_omega = min(cos_omega, 1)
omega = math.acos(cos_omega)
sin_omega = math.sin(omega)
a = math.sin((1-u) * omega)/ sin_omega
b = math.sin(u * omega) / sin_omega
if abs(sin_omega) < 1e-6:
# direct linear interpolation for numerically unstable regions
result = result * this_weight + q1 * u
result /= math.sqrt(np.dot(result, result))
else:
result = result*a + q1*b
return Quaternion(np.roll(result, shift=-1))
# To conversions
def to_angle_axis(self):
""" Return axis-angle representation """
q = np.roll(self.q, shift=1)
halftheta = math.acos(q[0])
if abs(halftheta) < 1e-12:
return 0, np.array((0, 0, 1))
else:
theta = halftheta * 2
axis = np.array(q[1:4]) / math.sin(halftheta)
return theta, axis
def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
"""Return spherical linear interpolation between two quaternions.
>>> q0 = random_quaternion()
>>> q1 = random_quaternion()
>>> q = quaternion_slerp(q0, q1, 0.0)
>>> numpy.allclose(q, q0)
True
>>> q = quaternion_slerp(q0, q1, 1.0, 1)
>>> numpy.allclose(q, q1)
True
>>> q = quaternion_slerp(q0, q1, 0.5)
>>> angle = math.acos(numpy.dot(q0, q))
>>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \
numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle)
True
"""
q0 = unit_vector(quat0[:4])
q1 = unit_vector(quat1[:4])
if fraction == 0.0:
return q0
elif fraction == 1.0:
return q1
d = numpy.dot(q0, q1)
if abs(abs(d) - 1.0) < _EPS:
return q0
if shortestpath and d < 0.0:
# invert rotation
d = -d
q1 *= -1.0
angle = math.acos(d) + spin * math.pi
if abs(angle) < _EPS:
return q0
isin = 1.0 / math.sin(angle)
q0 *= math.sin((1.0 - fraction) * angle) * isin
q1 *= math.sin(fraction * angle) * isin
q0 += q1
return q0
transformations.py 文件源码
项目：Neural-Networks-for-Inverse-Kinematics
作者: paramrajpura
项目源码
文件源码
阅读 23
收藏 0
点赞 0
评论 0
def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
"""Return spherical linear interpolation between two quaternions.
>>> q0 = random_quaternion()
>>> q1 = random_quaternion()
>>> q = quaternion_slerp(q0, q1, 0)
>>> numpy.allclose(q, q0)
True
>>> q = quaternion_slerp(q0, q1, 1, 1)
>>> numpy.allclose(q, q1)
True
>>> q = quaternion_slerp(q0, q1, 0.5)
>>> angle = math.acos(numpy.dot(q0, q))
>>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \
numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle)
True
"""
q0 = unit_vector(quat0[:4])
q1 = unit_vector(quat1[:4])
if fraction == 0.0:
return q0
elif fraction == 1.0:
return q1
d = numpy.dot(q0, q1)
if abs(abs(d) - 1.0) < _EPS:
return q0
if shortestpath and d < 0.0:
# invert rotation
d = -d
numpy.negative(q1, q1)
angle = math.acos(d) + spin * math.pi
if abs(angle) < _EPS:
return q0
isin = 1.0 / math.sin(angle)
q0 *= math.sin((1.0 - fraction) * angle) * isin
q1 *= math.sin(fraction * angle) * isin
q0 += q1
return q0
def __init__(self, arg):
if isinstance(arg, numbers.Real):
# We precompute sin and cos for rotations
self.angle = arg
self.cos = math.cos(self.angle)
self.sin = math.sin(self.angle)
elif isinstance(arg, Point):
# Point angle is the trigonometric angle of the vector
# [origin, Point]
pt = arg
try:
self.cos = pt.x / pt.length()
self.sin = pt.y / pt.length()
except ZeroDivisionError:
self.cos = 1
self.sin = 0
self.angle = math.acos(self.cos)
if self.sin < 0:
self.angle = -self.angle
else:
raise TypeError("Angle is defined by a number or a Point")
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
timezone = self.timezone # in hours, east is positive
longitude= self.long # in decimal degrees, east is positive
latitude = self.lat # in decimal degrees, north is positive
time = self.time # percentage past midnight, i.e. noon is 0.5
day = self.day # daynumber 1=1/1/1900
Jday =day+2415018.5+time-timezone/24 # Julian day
Jcent =(Jday-2451545)/36525 # Julian century
Manom = 357.52911+Jcent*(35999.05029-0.0001537*Jcent)
Mlong = 280.46646+Jcent*(36000.76983+Jcent*0.0003032)%360
Eccent = 0.016708634-Jcent*(0.000042037+0.0001537*Jcent)
Mobliq = 23+(26+((21.448-Jcent*(46.815+Jcent*(0.00059-Jcent*0.001813))))/60)/60
obliq = Mobliq+0.00256*cos(rad(125.04-1934.136*Jcent))
vary = tan(rad(obliq/2))*tan(rad(obliq/2))
Seqcent = sin(rad(Manom))*(1.914602-Jcent*(0.004817+0.000014*Jcent))+sin(rad(2*Manom))*(0.019993-0.000101*Jcent)+sin(rad(3*Manom))*0.000289
Struelong= Mlong+Seqcent
Sapplong = Struelong-0.00569-0.00478*sin(rad(125.04-1934.136*Jcent))
declination = deg(asin(sin(rad(obliq))*sin(rad(Sapplong))))
eqtime = 4*deg(vary*sin(2*rad(Mlong))-2*Eccent*sin(rad(Manom))+4*Eccent*vary*sin(rad(Manom))*cos(2*rad(Mlong))-0.5*vary*vary*sin(4*rad(Mlong))-1.25*Eccent*Eccent*sin(2*rad(Manom)))
hourangle= deg(acos(cos(rad(90.833))/(cos(rad(latitude))*cos(rad(declination)))-tan(rad(latitude))*tan(rad(declination))))
self.solarnoon_t=(720-4*longitude-eqtime+timezone*60)/1440
self.sunrise_t =self.solarnoon_t-hourangle*4/1440
self.sunset_t =self.solarnoon_t+hourangle*4/1440
def get_angle(v1,v2): # v1,v2 must be unit vectors
denom = (math.sqrt(((v1[0]*v1[0]) + (v1[1]*v1[1]) + (v1[2]*v1[2]))) *
math.sqrt(((v2[0]*v2[0]) + (v2[1]*v2[1]) + (v2[2]*v2[2]))))
if denom>1e-10:
result = ( (v1[0]*v2[0]) + (v1[1]*v2[1]) + (v1[2]*v2[2]) ) / denom
else:
result = 0.0
result = math.acos(result)
return result
#------------------------------------------------------------------------------
def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
"""Return spherical linear interpolation between two quaternions.
>>> q0 = random_quaternion()
>>> q1 = random_quaternion()
>>> q = quaternion_slerp(q0, q1, 0.0)
>>> numpy.allclose(q, q0)
True
>>> q = quaternion_slerp(q0, q1, 1.0, 1)
>>> numpy.allclose(q, q1)
True
>>> q = quaternion_slerp(q0, q1, 0.5)
>>> angle = math.acos(numpy.dot(q0, q))
>>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \
numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle)
True
"""
q0 = unit_vector(quat0[:4])
q1 = unit_vector(quat1[:4])
if fraction == 0.0:
return q0
elif fraction == 1.0:
return q1
d = numpy.dot(q0, q1)
if abs(abs(d) - 1.0) < _EPS:
return q0
if shortestpath and d < 0.0:
# invert rotation
d = -d
q1 *= -1.0
angle = math.acos(d) + spin * math.pi
if abs(angle) < _EPS:
return q0
isin = 1.0 / math.sin(angle)
q0 *= math.sin((1.0 - fraction) * angle) * isin
q1 *= math.sin(fraction * angle) * isin
q0 += q1
return q0
