def cart2sphere(pt3d_cart):
x, y, z = pt3d_cart
rho = np.sqrt(x**2 + y**2 + z**2)
theta = np.atan(y / x)
phi = np.atan((np.sqrt(x**2 + y**2)) / z)
pt3d_sphere = (rho, theta, phi)
return pt3d_sphere
def NACA_four_digit(chord, t, p, m):
# Mean line coordinates
x_fwd = np.linspace(0, p*chord)
x_aft = np.linspace(p*chord, chord)
y_fwd = (m/p**2)*(2*p*(x/chord)-(x/chord)**2)
y_aft = (m/(1-p)**2)*((1-2*p)+2*p*(x/chord)-(x/chord)**2)
# Thickness coordinates
yt_fwd = Naca00XX(chord, t, x_fwd)
yt_aft = Naca00XX(chord, t, x_fwd)
# Angle calculation so that thickness is perpendicular
dydx_fwd = (2*m/p**2)*(p-x/chord)
dydx_aft = (2*m/(1-p)**2)*(p-x/chord)
theta_fwd = np.atan(dydc_fwd)
theta_aft = np.atan(dydc_aft)
# Calculate airfoil coordinates
y_upper = {'x': np.append(x_fwd - yt_fwd*np.sin(theta),
x_aft - yt_aft*np.sin(theta)),
'y': np.append(y_fwd + yt_fwd*np.cos(theta),
y_aft + yt_aft*np.cos(theta))}
y_lower = {'x': np.append(x_fwd + yt_fwd*np.sin(theta),
x_aft + yt_aft*np.sin(theta)),
'y': np.append(y_fwd - yt_fwd*np.cos(theta),
y_aft - yt_aft*np.cos(theta))}
return y_upper, y_lower
def PuissantINV(lat1, lon1, lat2, lon2, ell="GRS80"):
ell_ = ellipsoidInstance(ell)
M1, N1 = ell_.radiusOfCurvature(float(lat1))[:2]
N2 = ell_.radiusOfCurvature(float(lat2))[1]
dlat, dlon = lat2 - lat1, lon2 - lon1
mlat = (lat1 + lat2) / 2
_1_ = N2 * cos(lat2) * dlon
_2_ = M1 * dlat / (1 - 3 * ell_.e1**2 * sin(lat1) * cos(lat1) * dlat /
(2 * (1 - ell_.e1**2 * sin(lat1)**2)))
a12 = atan(_1_, _2_)
X = sin(mlat) / cos(dlat / 2)
da = X * dlon**3 * (X - X**3) / 12
a21 = a12 + da + pi / 2
S12 = N2 * cos(lat2) * dlat / sin(a12)
for i in range(100):
print("a12 :\t{}\na21 :\t{}\nS12 :{} m\n".format(
rad2dms(a12), rad2dms(a21), S12))
_117_ = N2 * cos(lat2) * dlon + S12**3 * sin(a12) / (
6 * N2**2) - S12**3 * sin(a12)**3 * cos(lat2)**2 / (6 * N2**2)
_116_1 = M1 * dlat / (1 -
3 * ell_.e1**2 * sin(lat1) * cos(lat1) * dlat /
(2 * (1 - ell_.e1**2 * sin(lat1)**2)))
_116_2 = S12**2 * tan(lat1) * sin(a12)**2 / (2 * N1)
_116_3 = S12**3 * cos(a12) * sin(a12)**2 * (1 + 3 * tan(lat1)**2) / (
6 * N1**2)
_116_ = _116_1 + _116_2 + _116_3
a12_, a12 = atan(_117_, _116_) - a12, atan(_117_, _116_)
da12 = a12 - a12_
X = sin(mlat) / cos(dlat / 2)
da = X * dlon**3 * (X - X**3) / 12
a21 = a12 + da + pi / 2
S12_, S12 = _117_ / sin(a12) - S12, _117_ / sin(a12)
print("a12 :\t{}\na21 :\t{}\nS12 :{} m\n".format(rad2dms(a12),
rad2dms(a21), S12))
return rad2dms(a12), rad2dms(a21), S12
def Angle(sub_point,pre_point): if sub_point.y-pre_point.y>0: if sub_point.x<pre_point.x: #print 'sub_point.y-pre_point.y>0 && sub_point.x<pre_point.x' angle=numpy.pi-abs(numpy.atan((sub_point.y - pre_point.y)/(sub_point.x - pre_point.x))) if sub_point.x==pre_point.x: #print 'sub_point.y-pre_point.y>0 && sub_point.x==pre_point.x' angle=numpy.pi/2 if sub_point.x>pre_point.x: #print 'sub_point.y-pre_point.y>0 && sub_point.x==sub_point.x>pre_point.x' angle=numpy.atan((sub_point.y - pre_point.y)/(sub_point.x - pre_point.x)) if sub_point.y-pre_point.y<0: if sub_point.x<pre_point.x: #print 'sub_point.y-pre_point.y<0 && sub_point.x<pre_point.x' angle=-(numpy.pi-abs(numpy.atan((sub_point.y - pre_point.y)/(sub_point.x - pre_point.x)))) if sub_point.x==pre_point.x: #print 'sub_point.y-pre_point.y<0 && sub_point.x==pre_point.x' angle=-numpy.pi/2 if sub_point.x>pre_point.x: #print 'sub_point.y-pre_point.y<0 && sub_point.x>pre_point.x' angle=-abs(numpy.atan((sub_point.y - pre_point.y)/(sub_point.x - pre_point.x))) if sub_point.y-pre_point.y==0: if sub_point.x>pre_point.x: #print 'sub_point.y-pre_point.y==0 && sub_point.x>pre_point.x' angle=0.0 if sub_point.x<pre_point.x: #print 'sub_point.y-pre_point.y==0 && sub_point.x<pre_point.x' angle=numpy.pi return angle
def mixing_plot(ax, title):
ax = plot(ax, title)
assert len(self.fields) == 2
f1, f2 = self.fields
ax.plot(self.t, [np.atan(f2(t) / f1(t))
for t in self.t]) #TODO: check this
return ax
def psit_26(zet=None):
# computes temperature structure function
dzet = np.clip(0.35 * zet, None, 50) # stable
psi = -((1 + 0.6667 * zet) ** 1.5 + 0.6667 * (zet - 14.28) * exp(-dzet) + 8.525)
x = (1 - 15 * zet) ** 0.5
psik = 2 * log((1 + x) / 2)
x = (1 - 34.15 * zet) ** 0.3333
psic = (
1.5 * log((1 + x + x ** 2) / 3)
- sqrt(3) * atan((1 + 2 * x) / sqrt(3))
+ 4 * atan(1) / sqrt(3)
)
f = zet ** 2.0 / (1 + zet ** 2)
psi = np.where(zet < 0, (1 - f) * psik + f * psic, psi)
return psi
def parametric2geodetic(parametric_lat: "ndarray",
ell: Ellipsoid = None,
deg: bool = True) -> "ndarray":
"""
converts from parametric latitude to geodetic latitude
like Matlab geodeticLatitudeFromParametric()
Parameters
----------
parametric_lat : "ndarray"
latitude
ell : Ellipsoid, optional
reference ellipsoid (default WGS84)
deg : bool, optional
degrees input/output (False: radians in/out)
Returns
-------
geodetic_lat : "ndarray"
geodetic latiude
Notes
-----
Equations from J. P. Snyder, "Map Projections - A Working Manual",
US Geological Survey Professional Paper 1395, US Government Printing
Office, Washington, DC, 1987, pp. 13-18.
"""
parametric_lat, ell = sanitize(parametric_lat, ell, deg)
geodetic_lat = atan(tan(parametric_lat) / sqrt(1 - (ell.eccentricity)**2))
return degrees(geodetic_lat) if deg else geodetic_lat
def new_theta(theta):
n1 = (m * v_cg**2) / (R_w - h_cg * np.sin(theta - kappa)) * np.cos(kappa)
n2 = F_d * np.sin(eta) * np.cos(beta)
d1 = m * g * np.cos(local_slope())
d2 = (m * v_cg**2) / (R_w - h_cg * np.sin(theta - kappa)) * np.sin(kappa)
d3 = F_d * np.sin(eta) * np.sin(beta)
return np.atan((n1 + n2) / (d1 - d2 - d3))
def action_atan():
Showtemplabel.delete(0, END);
Showlabel.delete(0, END)
Showtemplabel.config(fg='yellow', bg='#8dad96')
Showtemplabel.insert(0, 'atan');
Showtemplabel.place(relx=0.5, rely=0.5, anchor='center')
ans = "0"
Showlabel.insert(0, ans);
Showlabel.place(relx=0.5, rely=0.6, anchor='atan')
num1 = Numberentry1.get();
if(is_number(num1)==True):
num1 = casting(num1)
ans = str(np.atan(num1))
Showtemplabel.delete(0, END);
Showlabel.delete(0, END)
Showtemplabel.config(fg='yellow', bg='#8dad96')
Showtemplabel.insert(0, 'tan_-1');
Showtemplabel.place(relx=0.5, rely=0.5, anchor='center')
Showlabel.insert(0, ans);
Showlabel.place(relx=0.5, rely=0.6, anchor='center')
else:
messagebox.showerror("Error", "Enter a Valid number\ne.g. 123, 0.123, .123, -0.123, 123.456")
def isometric2geodetic(isometric_lat: float, ell: Ellipsoid = None, deg: bool = True) -> float:
"""
converts from isometric latitude to geodetic latitude
like Matlab map.geodesy.IsometricLatitudeConverter.inverse()
Parameters
----------
isometric_lat : float
isometric latitude
ell : Ellipsoid, optional
reference ellipsoid (default WGS84)
deg : bool, optional
degrees input/output (False: radians in/out)
Returns
-------
geodetic_lat : float
geodetic latiude
Notes
-----
Equations from J. P. Snyder, "Map Projections - A Working Manual",
US Geological Survey Professional Paper 1395, US Government Printing
Office, Washington, DC, 1987, pp. 13-18.
"""
# NOT sanitize for isometric2geo
if deg:
isometric_lat = radians(isometric_lat)
conformal_lat = 2 * atan(exp(isometric_lat)) - (pi / 2)
geodetic_lat = conformal2geodetic(conformal_lat, ell, deg=False)
return degrees(geodetic_lat) if deg else geodetic_lat
def _value_from_points(self):
"""Calculate the value for this particular gauge given the point and the circle's center"""
self._determine_quadrant()
raw_angle = -atan(
(self.circle_center[1] - self.needle_point[1])
/
(self.circle_center[0] - self.needle_point[0])
)
if self.verbose:
print(f"The raw angle: {raw_angle * RADS_TO_DEGS}")
print(f"The quadrant : {self.quadrant}")
angle_adjustment = {
1: 0,
2: -180,
3: -180,
4: 0
}
adjusted_angle = raw_angle * RADS_TO_DEGS + angle_adjustment[self.quadrant]
if self.verbose:
print(f"Adjusted angle: {adjusted_angle}")
m, i = trig.get_line_equation(point_a=(self.min_angle, self.min_value), point_b=(self.max_angle, self.max_value))
value_calc = m * adjusted_angle + i
if self.verbose:
print(f"Maybe this is the value: {value_calc:.2f}?")
self.gauge_reading = int(value_calc)
def getCenterandOrientation(self, M):
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
mu11 = M['m11'] / M['m00'] - cx * cy
mu02 = M['m02'] / M['m00'] - cy ^ 2
mu20 = M['m20'] / M['m00'] - cx ^ 2
orientation = 0.5 * np.atan((2 * mu11) / (mu20 - mu02))
return orientation
def boresight2pixel(theta_b, phi_b, dk_b, r_fp, theta_fp):
phi = np.atan(
-np.sin(theta_fp + dk_b) * np.sin(r_fp),
np.cos(theta_b) * np.cos(theta_fp + dk_b) * np.sin(r_fp) +
np.sin(theta_b) * np.cos(r_fp))
theta = -np.sin(theta_b) * np.cos(theta_fp + dk_b) * np.sin(r_fp) + np.cos(
theta_b) * np.cos(r_fp)
return theta, phi
def PSIma(f, g): a = 0.33 b = 0.41 pi = 3.141592654 tangens = scalar(atan((2.0 * g - 1.0) / sqrt(3.0))) * pi /180 tangens = ifthenelse(tangens > pi/2.0, tangens - 2.0 * pi, tangens) PSIma = ln(a + f) - 3.0 * b * f ** (1.0 / 3.0) + b * a ** (1.0 / 3.0) / 2.0 * ln((1 + g) ** 2.0 / (1.0 - g + sqrt(g))) + sqrt(3.0) * b * a ** (1.0 / 3.0) * tangens return PSIma
def genstepfm(nsamp=1024, x=1.0):
x = np.linspace(0.0, x, nsamp)
iw = nsamp / 4 * np.atan(250 * (x - 0.5)) + np.pi * nsamp / 4
phase = cumsum(iw) / nsamp
y = np.cos(phase)
iy = np.sin(phase) * im
ynorm = np.pi / x[-1]
return x, y + iy, iw, ynorm
def ecef2lla(ecef, a=a, b=b, f=f, e=e):
"""
convert ECEF(meters) Cartesian coordinates to geodetic coordinates based on the ellipsoidal coordinates
:param ecef:
[x,y,z] : array of floats in ECEF coordinate (meters)
:return:
[lat, lon, alt] : array of floats; geodetic latitude (radians), geodetic longitude (radians), altitude (meters)
based on:
You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
"""
x, y, z = ecef
r = sqrt(x**2 + y**2 + z**2)
E = sqrt(a**2 - b**2)
# eqn. 4a
u = sqrt(0.5 * (r**2 - E**2) +
0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))
Q = hypot(x, y)
huE = hypot(u, E)
# eqn. 4b
if not (Q == 0 or u == 0):
Beta = atan(huE / u * z / Q)
else:
if z >= 0:
Beta = pi / 2
else:
Beta = -pi / 2
# eqn. 13
eps = ((b * u - a * huE + E**2) * sin(Beta)) / (a * huE * 1 / cos(Beta) -
E**2 * cos(Beta))
Beta += eps
# %% final output
lat = atan(a / b * tan(Beta))
lon = atan2(y, x)
# eqn. 7
alt = hypot(z - b * sin(Beta), Q - a * cos(Beta))
# inside ellipsoid?
inside = x**2 / a**2 + y**2 / a**2 + z**2 / b**2 < 1
if inside:
alt = -alt
return np.array([lat, lon, alt])
def atan(*args, **kw):
arg0 = args[0]
if isinstance(arg0, (int, float, long)):
return _math.atan(*args,**kw)
elif isinstance(arg0, complex):
return _cmath.atan(*args,**kw)
elif isinstance(arg0, _sympy.Basic):
return _sympy.atan(*args,**kw)
else:
return _numpy.atan(*args,**kw)
def tile_to_latlon(x__y, z, add_one=False):
x, y = x__y
if add_one:
x += 1
y += 1
from numpy import sinh, pi, degrees
from numpy import arctan as atan
lat = degrees(atan(sinh(pi * (1.0 - 2.0 * y / float(1 << z)))))
lon = x / float(1 << z) * 360.0 - 180.0
return lat, lon
def convertWorldToLongLat(worldCoordArray):
longitude = np.subtract(
np.multiply((np.divide(worldCoordArray[0], 256)), 360), 180)
insideFunc = np.subtract(np.pi, (np.divide(
np.multiply(np.multiply(2, np.pi), worldCoordArray[1]), 256)))
latitude = np.multiply(np.divide(180, np.pi)), np.atan(
np.multiply(0.5, np.subtract(np.exp(insideFunc), np.exp(-insideFunc))))
return [latitude, longitude]
def imap(self, coords):
# https://en.wikipedia.org/wiki/Mercator_projection#Derivation_of_the_Mercator_projection
m = np.empty(coords.shape)
x = coords[:, 0]
y = coords[:, 1]
lambda_ = x / R
phi = 2 * np.atan(np.exp(y / R) - np.pi / 2)
m[:, 0] = np.degrees(lambda_)
m[:, 1] = np.degrees(phi)
m[:, 2:] = coords[:, 2:]
return m
def calc_angle(x_point_s, y_point_s, x_point_e, y_point_e):
angle = 0
y_se = y_point_e - y_point_s
x_se = x_point_e - x_point_s
if x_se == 0 and y_se > 0:
angle = 360
if x_se == 0 and y_se < 0:
angle = 180
if y_se == 0 and x_se > 0:
angle = 90
if y_se == 0 and x_se < 0:
angle = 270
if x_se > 0 and y_se > 0:
angle = np.atan(x_se / y_se) * 180 / np.pi
elif x_se < 0 and y_se > 0:
angle = 360 + np.atan(x_se / y_se) * 180 / np.pi
elif x_se < 0 and y_se < 0:
angle = 180 + np.atan(x_se / y_se) * 180 / np.pi
elif x_se > 0 and y_se < 0:
angle = 180 + np.atan(x_se / y_se) * 180 / np.pi
return angle
def psiu_40(zet=None):
# computes velocity structure function
dzet = np.clip(0.35 * zet, None, 50) # stable
a = 1
b = 3 / 4
c = 5
d = 0.35
psi = -(a * zet + b * (zet - c / d) * exp(-dzet) + b * c / d)
x = (1 - 18 * zet) ** 0.25
psik = 2 * log((1 + x) / 2) + log((1 + x * x) / 2) - 2 * atan(x) + 2 * atan(1)
x = (1 - 10 * zet) ** 0.3333
psic = (
1.5 * log((1 + x + x ** 2) / 3)
- sqrt(3) * atan((1 + 2 * x) / sqrt(3))
+ 4 * atan(1) / sqrt(3)
)
f = zet ** 2.0 / (1 + zet ** 2)
psi = np.where(zet < 0, (1 - f) * psik + f * psic, psi)
return psi
def inverse_k(H60, d1, a1, a2, a3, d4, d6):
p60 = H60[:3, 3]
p50 = p60 - d6 * H60[:3, 2]
x, y, z = p50
theta1 = atan2(y, x)
eta_x = sqrt(x**2 + y**2) - a1
eta_z = z - d1
d4_tilde = sqrt(a3**2 + d4**2)
s3_tilde = (eta_x**2 + eta_z**2 - d4**2 - a2**2) / (2 * a2 * d4_tilde)
c3_tilde = sqrt(1 - s3_tilde**2)
theta3_tilde = atan2(s3_tilde, c3_tilde)
theta3 = theta3_tilde - atan(a3 / d4)
theta2 = atan2(eta_x * d4_tilde * c3_tilde + eta_z * (d4_tilde * s3_tilde + a2),
eta_x * (d4_tilde * s3_tilde + a2) - eta_z * d4_tilde * c3_tilde)
R03 = matrix([[cos(theta1) * cos(theta2 + theta3), sin(theta1) * cos(theta2 + theta3), sin(theta2 + theta3)],
[sin(theta1), -cos(theta1), 0],
[cos(theta1) * sin(theta2 + theta3), sin(theta1) * sin(theta2 + theta3), -cos(theta2 + theta3)]])
rho = R03 * H60[:3, :3]
theta4 = atan(rho[1, 2] / rho[0, 2])
theta5 = atan2(cos(theta4) * rho[0, 2] + sin(theta4) * rho[1, 2],
-rho[2, 2])
theta6 = atan2(sin(theta4) * rho[0, 0] - cos(theta4) * rho[1, 0],
sin(theta4) * rho[0, 1] - cos(theta4) * rho[1, 1])
return [theta1,
theta2,
theta3,
theta4,
theta5,
theta6]
def angle_generater(sub_point, pre_point):
if sub_point.y - pre_point.y > 0:
if sub_point.x < pre_point.x:
# print 'sub_point.y-pre_point.y>0 && sub_point.x<pre_point.x'
angle = numpy.pi - abs(
numpy.atan(
(sub_point.y - pre_point.y) / (sub_point.x - pre_point.x)))
if sub_point.x == pre_point.x:
# print 'sub_point.y-pre_point.y>0 && sub_point.x==pre_point.x'
angle = numpy.pi / 2
if sub_point.x > pre_point.x:
# print 'sub_point.y-pre_point.y>0 && sub_point.x==sub_point.x>pre_point.x'
angle = numpy.atan(
(sub_point.y - pre_point.y) / (sub_point.x - pre_point.x))
if sub_point.y - pre_point.y < 0:
if sub_point.x < pre_point.x:
# print 'sub_point.y-pre_point.y<0 && sub_point.x<pre_point.x'
angle = -(numpy.pi - abs(
numpy.atan((sub_point.y - pre_point.y) /
(sub_point.x - pre_point.x))))
if sub_point.x == pre_point.x:
# print 'sub_point.y-pre_point.y<0 && sub_point.x==pre_point.x'
angle = -numpy.pi / 2
if sub_point.x > pre_point.x:
# print 'sub_point.y-pre_point.y<0 && sub_point.x>pre_point.x'
angle = -abs(
numpy.atan(
(sub_point.y - pre_point.y) / (sub_point.x - pre_point.x)))
if sub_point.y - pre_point.y == 0:
if sub_point.x > pre_point.x:
# print 'sub_point.y-pre_point.y==0 && sub_point.x>pre_point.x'
angle = 0.0
if sub_point.x < pre_point.x:
# print 'sub_point.y-pre_point.y==0 && sub_point.x<pre_point.x'
angle = numpy.pi
return angle
def find_angle(loc1, loc2):
"""
Calculated the angle between two locations on a grid.
Inputs:
:loc1: (tuple) first location.
:relative: (tuple) second location.
Outputs:
:angle : (float) real-valued angle between loc1 and loc2.
"""
angle = np.atan(loc1[1] / loc2[1])
return angle
def lee_method(img):
# print 'Lee\'s method'
iy = ndimage.filters.sobel(img, axis=0)
ix = ndimage.filters.sobel(img, axis=1)
tilt = np.atan(iy.mean() / ix.mean())
tilt = np.degrees(tilt)
if tilt < 0:
tilt += 360
print('tilt', tilt)
def stdtr(k, t):
"""Student's t distribution, -infinity to t.
See Cephes docs for details.
"""
if k <= 0:
raise ValueError("stdtr: df must be > 0.")
if t == 0:
return 0.5
if t < -2:
rk = k
z = rk / (rk + t * t)
return 0.5 * betai(0.5 * rk, 0.5, z)
# compute integral from -t to + t
if t < 0:
x = -t
else:
x = t
rk = k # degrees of freedom
z = 1 + (x * x) / rk
# test if k is odd or even
if (k & 1) != 0:
# odd k
xsqk = x / sqrt(rk)
p = atan(xsqk)
if k > 1:
f = 1
tz = 1
j = 3
while (j <= (k - 2)) and ((tz / f) > MACHEP):
tz *= (j - 1) / (z * j)
f += tz
j += 2
p += f * xsqk / z
p *= 2 / PI
else:
# even k
f = 1
tz = 1
j = 2
while (j <= (k - 2)) and ((tz / f) > MACHEP):
tz *= (j - 1) / (z * j)
f += tz
j += 2
p = f * x / sqrt(z * rk)
# common exit
if t < 0:
p = -p # note destruction of relative accuracy
p = 0.5 + 0.5 * p
return p
def stdtr(k, t):
"""Student's t distribution, -infinity to t.
See Cephes docs for details.
"""
if k <= 0:
raise ValueError, 'stdtr: df must be > 0.'
if t == 0:
return 0.5
if t < -2:
rk = k
z = rk / (rk + t * t)
return 0.5 * betai(0.5 * rk, 0.5, z)
#compute integral from -t to + t
if t < 0:
x = -t
else:
x = t
rk = k #degrees of freedom
z = 1 + (x * x)/rk
#test if k is odd or even
if (k & 1) != 0:
#odd k
xsqk = x/sqrt(rk)
p = atan(xsqk)
if k > 1:
f = 1
tz = 1
j = 3
while (j <= (k-2)) and ((tz/f) > MACHEP):
tz *= (j-1)/(z*j)
f += tz
j += 2
p += f * xsqk/z
p *= 2/PI
else:
#even k
f = 1
tz = 1
j = 2
while (j <= (k-2)) and ((tz/f) > MACHEP):
tz *= (j-1)/(z*j)
f += tz
j += 2
p = f * x/sqrt(z*rk)
#common exit
if t < 0:
p = -p #note destruction of relative accuracy
p = 0.5 + 0.5 * p
return p
def getAnglesR_roll_pitch_yaw(self, R):
if np.abs(R[0, 2] < 0.1) and np.abs(R[2, 2]) < 0.1:
ang1 = np.sign(-R[2, 1]) * np.pi / 2
ang2 = 0
ang3 = np.arctan(R[2, 0] / R[2, 1])
elif np.abs(R[2, 2] < 0.1) and np.abs(R[0, 2]) > 0.1:
ang2 = np.sign(R[0, 2]) * np.pi / 2
cosbeta = np.sqrt(R[1, 0]**2 + R[1, 1]**2)
ang1 = np.arctan(-R[1, 2] / cosbeta)
ang3 = np.arctan(R[1, 0] / R[1, 1])
elif np.abs(R[2, 2]) < 0.1:
ang3 = np.sign(R[1, 0])*np.pi/2
cosbeta = np.sqrt(R[1, 0]**2+R[1, 1]**2)
ang1 = np.atan(-R[1, 2] / cosbeta)
ang2 = np.atan(R[0, 2] /R[2, 2])
else:
ang3 = np.arctan(R[1, 0] / R[1, 1]);
cosbeta = np.sqrt(R[1, 0]**2 + R[1, 1]**2);
ang1 = np.arctan(-R[1, 2] / cosbeta);
ang2 = np.arctan(R[0, 2]/ R[2, 2]);
return np.array((ang1, ang2, ang3)).reshape((-1, 1))
def viewgeo(x_ul,y_ul,x_ur,y_ur,x_ll,y_ll,x_lr,y_lr): # imput "x",j # imput "y",i # imput cloud height "h" x_u = (x_ul + x_ur) / 2 x_l = (x_ll + x_lr) / 2 y_u = (y_ul + y_ur) / 2 y_l = (y_ll + y_lr) / 2 K_ulr = (y_ul - y_ur)/(x_ul - x_ur) # get k of the upper left and right points K_llr = (y_ll - y_lr) / (x_ll - x_lr) # get k of the lower left and right points K_aver = (K_ulr + K_llr) / 2 omiga_par = np.atan(K_aver) # get the angle of parallel lines k (in pi) # AX(j)+BY(i)+C=0 A = y_u - y_l B = x_l - x_u C = y_l * x_u - x_l * y_u omiga_per = np.atan(B/A) # get the angle which is perpendicular to the trace line return(A,B,C,omiga_par,omiga_per)
def sliding_torque(self, ddq_r, s, dq, q, t):
"""
Given actual state, compute torque necessarly to guarantee convergence
"""
u_computed = self.model.actuator_forces(q, dq, ddq_r)
u_discontinuous = np.dot(self.K(q, t), np.atan(s))
u_tot = u_computed - u_discontinuous
return u_tot
def ellipticFi(phi, psi, m):
if np.any(phi == 0):
scalar = np.isscalar(phi) and np.isscalar(psi) and np.isscalar(m)
phi, psi, m = np.broadcast_arrays(phi, psi, m)
result = np.empty_like(phi, 'D')
index = (phi == 0)
result[index] = ellipticF(atan(sinh(abs(phi[index]))),
1 - m[index]) * sign(psi[index])
result[~index] = ellipticFi(phi[~index], psi[~index], m[~index])
return result.reshape(1)[0] if scalar else result
r = abs(phi)
i = abs(psi)
sinhpsi2 = sinh(i)**2
cscphi2 = 1 / sin(r)**2
cotphi2 = 1 / tan(r)**2
b = -(cotphi2 + m * (sinhpsi2 * cscphi2) - 1 + m)
c = (m - 1) * cotphi2
cotlambda2 = (-b + sqrt(b * b - 4 * c)) / 2
re = ellipticF(atan(1 / sqrt(cotlambda2)), m) * sign(phi)
im = ellipticF(atan(sqrt(np.maximum(0, (cotlambda2 / cotphi2 - 1) / m))),
1 - m) * sign(psi)
return re + 1j * im
def ASBII(ATcur_P, ATxyz_P, ATxyz_K):
"""
| X | Astr. Azimuth , Zenith , Slope
| Y | ---------------+-------> from P to K distance distance
| Z |AT_P | (Apk) (Bpk) (Spk)
|
|
|
| | LAT |
+--------------- | LON |
| | H |AT_P
|
| X | |
| Y | ---------------+
| Z |AT_K
"""
# given====================================================================
# ATcur_P ----> Cur.Coor. (LAT, LON, H)[3x1] of
# ||| Station Point(P) on Avg. Terr. Coor. Sys.
# |||
# ||----LAT ----> Astronomic latitude on Avg. Terr.
# || Coor. Sys.
# ||
# |-----LON ----> Astronomic longtitude on Avg. Terr.
# | Coor. Sys.
# |
# ----- H ----> Orthometric height on Avg. Terr.
# Coor. Sys.
# ATxyz_P ----> Car.Coor. (X, Y, Z)[3x1] of
# Station Point(P) on Avg. Terr. Coor. Sys.
# ATxyz_K ----> Car.Coor. (X, Y, Z)[3x1] of
# Obs. Point(K) on Avg. Terr. Coor. Sys.
#
# asked====================================================================
# Apk ----> Astronomic azimuth
# Spk ----> Distance between sta. and obs. point
# Bpk ----> Zenith distance
#
d = ATxyz_K - ATxyz_P
Apk = atan(
-d[0, 0] * sin(ATcur_P[1, 0].rad) + d[1, 0] * cos(ATcur_P[1, 0].rad),
-d[0, 0] * sin(ATcur_P[0, 0].rad) * cos(ATcur_P[1, 0].rad) -
d[1, 0] * sin(ATcur_P[0, 0].rad) * sin(ATcur_P[1, 0].rad) +
d[2, 0] * cos(ATcur_P[0, 0].rad))
Spk = (d[0, 0]**2 + d[1, 0]**2 + d[2, 0]**2)**.5
Bpk = acos((d[0, 0] * cos(ATcur_P[0, 0].rad) * cos(ATcur_P[1, 0].rad) +
d[1, 0] * cos(ATcur_P[0, 0].rad) * sin(ATcur_P[1, 0].rad) +
d[2, 0] * sin(ATcur_P[0, 0].rad)) / Spk)
return angle(Apk, "rad"), Spk, angle(Bpk, "rad")
def Gxyz2GcurII(Gxyz, ell='GRS80'):
"""
@author: Mustafa Serkan Isik
| x | (direct) | lat |
| y | ----------+---------> | lon |
| z |G | | h |G
|
|
<DATUM PARAMETERS>
"""
# given====================================================================
# Gxyz ----> Car. Coor. (x, y, z)[3x1] of
# Station Point on Geodetic Coor. Sys.
# ell ----> Reference ellipsoid
#
# asked====================================================================
# Gcur ----> Cur.Coor. (lat, lon, h)[3x1] of
# ||| Station Point on Geodetic Coor. Sys.
# |||
# ||----lat ----> Ellipsoidal latitude on Geodetic
# || Coor. Sys.
# ||
# |-----lon ----> Ellipsoidal longtitude on Geodetic
# | Coor. Sys.
# |
# ----- h ----> Ellipsoidal height on Geodetic
# Coor. Sys.
#
ell_ = ellipsoidInstance(ell) # create an ellipsoid instance
lon = atan(Gxyz[1, 0], Gxyz[0, 0])
p = (Gxyz[0, 0]**2 + Gxyz[1, 0]**2)**.5
B = atan(ell_.a * Gxyz[2, 0], ell_.b * p)
lat = atan(Gxyz[2, 0] + ell_.e2**2 * ell_.b * sin(B)**3,
p - ell_.e1**2 * ell_.a * cos(B)**3)
h = p / cos(lat) - ell_.radiusOfCurvature(lat)[1]
return matrix([[angle(lat, "rad")], [angle(lon, "rad")], [h]])
def escalar (x1,y1,x2,y2,img,c,d):
a=((x1+x2)/2)
b=((y1+y2)/2)
dista=(((a-500)**2+(b-1000)**2)**0.5)*3
pend=np.atan((y2-y1)/(x2-x1))
if a<500:
es=(a-500)
else:
es=(500-a)
er=(1000-b)
c.append(es)
d.append(er)
#print 'dist',dista
cv2.line(img,(a,b),(500,1000),(75,50,100),8)
cv2.line(img,(x1,y1),(x2,y2),(50,70,50),5)
return pend
def deltaPhi(v1,v2):
"""
Returns the value of equation 372 in the paper by Salgado and Weygand.
"""
return numpy.atan(numpy.imag(v1*numpy.conj(v2))/numpy.real(v1*numpy.conj(v2)))
def Rswd(DEM, Lat, Trans, DOY, Time):
""" Potential Radiation Equator model
(c) O. van Dam, UU, Tropenbos-Guyana
Version 5, June 2000
NOTE: Copyright: This program is free to use provided·
you refer to the manualfor citation.
Do not distribute without prior approval of the author.
Manual and additional info: [email protected]
-----------------------------------------------------
Model for calculation
incoming potential light energy
-----------------------------------------------------
DEM Input Digital Elevation Model (spatial)
Lat Latitude in decimal degrees (non-spatia)
Trans Transmissivity tau (Gates, 1980) (non-spatial)
DOY Day of Year (non-spatial)
Time Time in hours (non-spatial)"""
# constants
pi = 3.1415 # pi
Sc = 1367.0 # Solar constant (Gates, 1980) [W/m2]
SlopMap = scalar(atan(slope(DEM)))
AspMap = scalar(aspect(DEM)) # aspect [deg]
AtmPcor = ((288.0-0.0065*DEM)/288.0)**5.256 # atmospheric pressure correction [-]
# Solar geometry
# ----------------------------
# SolDec :declination sun per day between +23 & -23 [deg]
# HourAng :hour angle [-] of sun during day
# SolAlt :solar altitude [deg], height of sun above horizon
SolDec = -23.4*cos(360.0*(DOY+10.0)/365.0)
HourAng = 15.0*(Time-12.01)
SolAlt = scalar(asin(scalar(sin(Lat)*sin(SolDec)+cos(Lat)*cos(SolDec)*cos(HourAng))))
# Solar azimuth
# ----------------------------
# SolAzi :angle solar beams to N-S axes earth [deg]
SolAzi = scalar(acos((sin(SolDec)*cos(Lat)-cos(SolDec)*sin(Lat)*cos(HourAng))/cos(SolAlt)))
SolAzi = ifthenelse(Time <= 12.0, SolAzi, 360.0 - SolAzi)
# Additonal extra correction by R.Sluiter, Aug '99
#SolAzi = ifthenelse(SolAzi > 89.994 and SolAzi < 90.0, 90.0, SolAzi)
#SolAzi = ifthenelse(SolAzi > 269.994 and SolAzi < 270.0, 270.0, SolAzi)
SolAzi = ifthenelse(np.logical_and(SolAzi > 89.994, SolAzi < 90.0), 90.0, SolAzi)
SolAzi = ifthenelse(np.logical_and(SolAzi > 269.994, SolAzi < 270.0), 270.0, SolAzi)
# Surface azimuth
# ----------------------------
# cosIncident :cosine of angle of incident; angle solar beams to angle surface
cosIncident = sin(SolAlt)*cos(SlopMap)+cos(SolAlt)*sin(SlopMap)*cos(SolAzi-AspMap)
# Critical angle sun
# ----------------------------
# HoriAng :tan maximum angle over DEM in direction sun, 0 if neg·
# CritSun :tan of maximum angle in direction solar beams
# Shade :cell in sun 1, in shade 0
#HoriAng = horizontan(DEM,directional(SolAzi))
HoriAng = scalar(0.3)
HoriAng = ifthenelse(HoriAng < 0.0, scalar(0.0), HoriAng)
CritSun = ifthenelse(SolAlt > 90.0, scalar(0.0), scalar(atan(HoriAng)))
Shade = ifthenelse(SolAlt > CritSun, scalar(1), scalar(0))
# Radiation outer atmosphere
# ----------------------------
OpCorr = Trans**((sqrt(1229.0+(614.0*sin(SolAlt))**2.0)-614.0*sin(SolAlt))*AtmPcor) # correction for air masses [-]·
Sout = Sc*(1.0+0.034*cos(360.0*DOY/365.0)) # radiation outer atmosphere [W/m2]
Snor = Sout*OpCorr # rad on surface normal to the beam [W/m2]
# Radiation at DEM
# ----------------------------
# Sdir :direct sunlight on a horizontal surface [W/m2] if no shade
# Sdiff :diffuse light [W/m2] for shade and no shade
# Stot :total incomming light Sdir+Sdiff [W/m2] at Hour
# PotRad :avg of Stot(Hour) and Stot(Hour-HourStep)
Sdir = ifthenelse(Snor*cosIncident*Shade < 0.0, 0.0, Snor*cosIncident*Shade)
Sdiff = ifthenelse(Sout*(0.271-0.294*OpCorr)*sin(SolAlt) < 0.0, 0.0, Sout*(0.271-0.294*OpCorr)*sin(SolAlt))
#Rswd = Sdir + Sdiff # Rad [W/m2]
Rswd = Snor
return Rswd
def define_plane_by_planar_stereonet_coordinates(x, y): dir = np.degrees(np.atan2(x,y)) l = sqrt(x**2+y**2) dip = 90 - np.degrees (2 * np.atan(l)) return [dir, dip]
def alignStageMotion_new(masked_file, skeletons_file):
fps = read_fps(skeletons_file)
with tables.File(skeletons_file, 'r+') as fid:
# delete data from previous analysis if any
if not '/stage_movement':
g_stage_movement = fid.create_group('/', 'stage_movement')
else:
g_stage_movement = fid.get_node('/stage_movement')
for field in ['stage_vec', 'is_stage_move', 'frame_diffs']:
if field in g_stage_movement:
fid.remove_node(g_stage_movement, field)
g_stage_movement._v_attrs['has_finished'] = 0
video_timestamp_ind = fid.get_node('/timestamp/raw')[:]
#I can tolerate a nan in the last position
if np.isnan(video_timestamp_ind[-1]):
video_timestamp_ind[-1] = video_timestamp_ind[-2]
if np.any(np.isnan(video_timestamp_ind)):
exit_flag = 80;
warnings.warns('The timestamp is corrupt or do not exist.\n No stage correction processed. Exiting with has_finished flag %i.' , exit_flag)
#turn on the has_finished flag and exit
g_stage_movement._v_attrs['has_finished'] = exit_flag
return
video_timestamp_ind = video_timestamp_ind.astype(np.int)
# Open the information file and read the tracking delay time.
# (help from segworm findStageMovement)
# 2. The info file contains the tracking delay. This delay represents the
# minimum time between stage movements and, conversely, the maximum time it
# takes for a stage movement to complete. If the delay is too small, the
# stage movements become chaotic. We load the value for the delay.
with tables.File(masked_file, 'r') as fid:
xml_info = fid.get_node('/xml_info').read().decode()
g_mask = fid.get_node('/mask')
#%% Read the scale conversions, we would need this when we want to convert the pixels into microns
pixelPerMicronX = 1/g_mask._v_attrs['pixels2microns_x']
pixelPerMicronY = 1/g_mask._v_attrs['pixels2microns_y']
with pd.HDFStore(masked_file, 'r') as fid:
stage_log = fid['/stage_log']
#%this is not the cleaneast but matlab does not have a xml parser from
#%text string
delay_str = xml_info.partition('<delay>')[-1].partition('</delay>')[0]
delay_time = float(delay_str) / 1000;
delay_frames = np.ceil(delay_time * fps);
normScale = np.sqrt((pixelPerMicronX ^ 2 + pixelPerMicronX ^ 2) / 2);
pixelPerMicronScale = normScale * np.array((np.sign(pixelPerMicronX), np.sign(pixelPerMicronY)));
#% Compute the rotation matrix.
#%rotation = 1;
angle = np.atan(pixelPerMicronY / pixelPerMicronX);
if angle > 0:
angle = np.pi / 4 - angle;
else:
angle = np.pi / 4 + angle;
cosAngle = np.cos(angle);
sinAngle = np.sin(angle);
rotation_matrix = np.array(((cosAngle, -sinAngle), (sinAngle, cosAngle)));
#%%
#% Ev's code uses the full vectors without dropping frames
#% 1. video2Diff differentiates a video frame by frame and outputs the
#% differential variance. We load these frame differences.
frame_diffs_d = getFrameDiffVar(masked_file);
#%% Read the media times and locations from the log file.
#% (help from segworm findStageMovement)
#% 3. The log file contains the initial stage location at media time 0 as
#% well as the subsequent media times and locations per stage movement. Our
#% algorithm attempts to match the frame differences in the video (see step
#% 1) to the media times in this log file. Therefore, we load these media
#% times and stage locations.
#%from the .log.csv file
mediaTimes = stage_log['stage_time'].values;
locations = stage_log[['stage_x', 'stage_y']].values;
#%% The shift makes everything a bit more complicated. I have to remove the first frame, before resizing the array considering the dropping frames.
if video_timestamp_ind.size > frame_diffs_d.size + 1:
#%i can tolerate one frame (two with respect to the frame_diff)
#%extra at the end of the timestamp
video_timestamp_ind = video_timestamp_ind[:frame_diffs_d.size + 1];
frame_diffs = np.full(int(np.max(video_timestamp_ind)), np.nan);
dd = video_timestamp_ind - np.min(video_timestamp_ind); #shift data
dd = dd[dd>=0];
if frame_diffs_d.size != dd.size:
exit_flag = 81;
warnings.warn('Number of timestamps do not match the number read movie frames.\n No stage correction processed. Exiting with has_finished flag %i.', exit_flag)
#%turn on the has_finished flag and exit
with tables.File(skeletons_file, 'r+') as fid:
fid.get_node('/stage_movement')._v_attrs['has_finished'] = exit_flag
return
frame_diffs[dd] = frame_diffs_d;
#%% try to run the aligment and return empty data if it fails
try:
is_stage_move, movesI, stage_locations = \
findStageMovement(frame_diffs, mediaTimes, locations, delay_frames, fps);
exit_flag = 1;
except:
exit_flag = 82;
warnings.warn('Returning all nan stage vector. Exiting with has_finished flag {}'.format(exit_flag))
with tables.File(skeletons_file, 'r+') as fid:
fid.get_node('/stage_movement')._v_attrs['has_finished'] = exit_flag
#%remove the if we want to create an empty
is_stage_move = np.ones(frame_diffs.size+1);
stage_locations = [];
movesI = [];
#%%
stage_vec_d, is_stage_move_d = shift2video_ref(is_stage_move, movesI, stage_locations, video_timestamp_ind)
#%% save stage data into the skeletons.hdf5
with tables.File(skeletons_file, 'r+') as fid:
g_stage_movement = fid.get_node('/stage_movement')
g_stage_movement.create_carray(g_stage_movement, 'frame_diffs', obj=frame_diffs_d)
g_stage_movement.create_carray(g_stage_movement, 'stage_vec', obj=stage_vec_d)
g_stage_movement.create_carray(g_stage_movement, 'is_stage_move', obj=is_stage_move_d)
g_stage_movement._v_atttrs['fps'] = fps
g_stage_movement._v_atttrs['delay_frames'] = delay_frames
g_stage_movement._v_atttrs['microns_per_pixel_scale'] = pixelPerMicronScale
g_stage_movement._v_atttrs['rotation_matrix'] = rotation_matrix
g_stage_movement._v_attrs['has_finished'] = 1
print_flush('Finished.')
def fitness1(x):
return -0.75 / (1 + x ** 2) - (0.65 * x * numpy.atan(1 / x)) + 0.65
def guoy_phase (z, z0):
return atan(z/z0)
def multipath_moore(surff, H, AntennaType, np):
"""% [dsigmaL1,dsigmaL2,dsigmaLc]=multipath_herring(elev, rough, H, Grate, np)
% Multipath model based on Tom Herring's code
% uses Fresnel Equations and takes into account antenna gain for direct and
% reflected signals
% input H is H above reflector in m
% input elev is elevation angle of satellite (vectorised)
% input surff is the surface reflection quality (0<surff<=1)
% input Grate is how quick the gain goes to zero (modified dipole)
% input np is the refractive index of the reflecting surface
% The dielectric constant is simply the square of the refractive index in a non-magnetic medium
%np = sqrt(4); % dieltric constant dry sand (3-5); %concrete is 4;
%water is sqrt(88-80.1-55.3-34.5) (0-20-100-200 degC) wikipedia
"""
[backL1, backL2] = interpolateBackLobeGain_504()
vel_light = 299792458.0
fL1 = 10.23e6*77.*2.
fL2 = 10.23e6*60.*2.
wL1 = vel_light/fL1
wL2 = vel_light/fL2
lcl1 = 1./(1.-(fL2/fL1)**2)
lcl2 = -(fL2/fL1)/(1.-(fL2/fL1)**2)
# Set refractive index of media which determines the refrection
# coefficients. na is air, np is material below the antenna
# The dielectric constant is simply the square of the refractive index in a non-magnetic medium
n1 = 1.0 # refractive coefficient of air
np = np.sqrt(4) # dieletric costant of dry sand (3-5), concrete = 4
#water is sqrt(88-80.1-55.3-34.5) (0-20-100-200 degC) wikipedia
#
# For an AT504 mean = 1.1
#
Grate_L1_504 = 1.1044
Grate_L2_504 = 1.0931
GRID = 0.1
#GRID = 0.5
azs = np.linspace(0,360,(360./GRID) +1 )
i = 1
for az in azs:
j = 1
zds = np.linspace(0,90,(90./GRID)+1 )
for zd in zds :
e = 90. - zd
eRad = e*np.pi/180.
zRad = zd*np.pi/180.
# had a bug in previous version N = floor(el*100 + 1) (when incrementing by 0.01 -> produced
# a counter that repeated values, so now just use n and perform a fliplr
n = np.floor(zd*10+1)
#
# NB this is using the noazi values
# Now compute the height of the reflector taking into account the antenna PCV
#
htL1 = H # + antL1.neu(1,3)/1000 + pcvL1(i,j)/1000 * sin(eRad);
htL2 = H # + antL2.neu(1,3)/1000 + pcvL2(i,j)/1000 * sin(eRad);
Ra = ( n1*np.cos(zRad) - np.sqrt( np**2 - (n1 * np.sin(zRad))**2 )/
n1*np.cos(zRad) + np.sqrt( np**2 - (n1 * np.sin(zRad))**2 ) )
# Reflected gain from antenna
L1_back = surff * backL1(n) * Ra;
L2_back = surff * backL2(n) * Ra;
#
# The direct Gain from the antenna for L1 => gDL1 = cos(z/G)
# Where Grate is the rate of change of the antenna gain with zenith angle
#
gDL1_504 = np.cos( zRad / Grate_L1_504 );
gDL2_504 = np.cos( zRad / Grate_L2_504 );
#
# Reflected Gain rate
# gR = cos(90/G)(1 - sin(elev))
#
dRL1_back = ( wL1/(2.*np.pi) *
atan((L1_back*np.sin(4.*np.pi*(htL1/wL1)*np.sin(eRad))) /
(gDL1_504 + L1_back * np.cos(4*np.pi*(htL1/wL1)*np.sin(eRad))))
)
dRL2_back = ( wL2/(2.*np.pi) *
np.atan((L2_back*np.sin(4*np.pi*(htL2/wL2)*np.sin(eRad))) /
(gDL2_504 + L2_back * np.cos(4*np.pi*(htL2/wL2)*np.sin(eRad))))
)
#dR(j) = (lcl1 * dRL1_back) + (lcl2 * dRL2_back) ;
dR(j) = (2.5457 * dRL1_back) - (1.545 * dRL2_back)
j = j+1
# Make the vector a function of elevation, rather than zenith for use in the simulation
dsigmaLc(i,:) = fliplr(dR) # ./1000;
#dsigmaLc(i,:) = dR;
i = i+1
return dsigmaLc
def spheroidalCF2(r, psize, axrat):
"""Spheroidal nanoparticle characteristic function.
Form factor for ellipsoid with radii (psize/2, psize/2, axrat*psize/2)
r -- distance of interaction
psize -- The equatorial diameter
axrat -- The ratio of axis lengths
From Lei et al., Phys. Rev. B, 80, 024118 (2009)
"""
pelpt = 1.0 * axrat
if psize <= 0 or pelpt <= 0:
return numpy.zeros_like(r)
# to simplify the equations
v = pelpt
d = 1.0 * psize
d2 = d*d
v2 = v*v
if v == 1:
return sphericalCF(r, psize)
rx = r
if v < 1:
r = rx[rx <= v*psize]
r2 = r*r
f1 = 1 - 3*r/(4*d*v)*(1-r2/(4*d2)*(1+2.0/(3*v2))) \
- 3*r/(4*d)*(1-r2/(4*d2))*v/sqrt(1-v2)*atanh(sqrt(1-v2))
r = rx[numpy.logical_and(rx > v*psize, rx <= psize)]
r2 = r*r
f2 = (3*d/(8*r)*(1+r2/(2*d2))*sqrt(1-r2/d2) \
- 3*r/(4*d)*(1-r2/(4*d2))*atanh(sqrt(1-r2/d2)) \
) * v/sqrt(1-v2)
r = rx[rx > psize]
f3 = numpy.zeros_like(r)
f = numpy.concatenate((f1,f2,f3))
elif v > 1:
r = rx[rx <= psize]
r2 = r*r
f1 = 1 - 3*r/(4*d*v)*(1-r2/(4*d2)*(1+2.0/(3*v2))) \
- 3*r/(4*d)*(1-r2/(4*d2))*v/sqrt(v2-1)*atan(sqrt(v2-1))
r = rx[numpy.logical_and(rx > psize, rx <= v*psize)]
r2 = r*r
f2 = 1 - 3*r/(4*d*v)*(1-r2/(4*d2)*(1+2.0/(3*v2))) \
- 3.0/8*(1+r2/(2*d2))*sqrt(1-d2/r2)*v/sqrt(v2-1) \
- 3*r/(4*d)*(1-r2/(4*d2))*v/sqrt(v2-1) \
* (atan(sqrt(v2-1)) - atan(sqrt(r2/d2-1)))
r = rx[rx > v*psize]
f3 = numpy.zeros_like(r)
f = numpy.concatenate((f1,f2,f3))
return f
#this function takes arrays, along with a central point-for example tagged from a picture, and returns x,y,z or lat lon coordinates #equations based off of "Robot Navigation", 2011 by Gerald Cook shp = arr.shape xs = np.zeros(shp) ys = np.zeros(shp) N = shp[0] M = shp[1] #convert central point to meters c_index = [shp[0]/2 - 1,shp[1]/2 -1] #convert to cartesian for i in range(shp[0]) #rows of array ys[i][:] = float(L* (N+1.0-2.0*i)/(N-1.0) * np.tan(VFOV/2)) for j in range(shp[1]) xs[:][j] = float(L* (2*j - M -1.0)/(M-1.0) * np.tan(HFOV/2)) #azimuth and zenith az = np.atan(-1*xs/L) zen = np.atan(np.divide(ys, np.sqrt(L*L + np.square(xs)))) #camera coordinates xc = Z*np.tan(az) yc = Z*np.tan(zen) #world coordinates, 2x2 rotation matrix * [xc,yc], rotates by yaw xw = xc*np.cos(theta) - yc*np.sin(theta) yw = xc*np.sin(theta) + yc*np.cos(theta) #global coordinates in meters R = 6371000 #average radius of earth, equatorial radius is 6378137 meters lat_rad = center_lat*math.pi/180.0 lon_rad = center_lon*math.pi/180.0 xg = np.add(R*np.cos(lat_rad), xw) yg = np.add(R*np.cos(lat_rad)*np.sin(lon_rad),yw) zg = R*np.sin(lat_rad)+Z if met_or_lon == 0 #users
