def test_mdot():
a = 1.0 * np.arange(18).reshape(3, 3, 2)
b = -a
t = np.concatenate(
[np.dot(a[..., i], b[..., i])[:, :, None] for i in range(2)], axis=2)
tm = mdot(a, b)
assert_allclose(t, tm)
t1 = np.concatenate(
[np.dot(a[..., i], b[:, 0, 0][:, None])[:, :, None] for i in range(2)],
axis=2)
tm1 = mdot(a, b[:, 0, 0].reshape(-1, 1))
assert_allclose(t1, tm1)
tt0 = mdot(a[..., 0], b[..., 0])
assert_allclose(t[..., 0], tt0)
tt0 = mdot(a[..., 0], b[:, :1, 0])
assert_allclose(t[:, :1, 0], tt0)
tt0 = mdot(a[..., 0], b[:, :2, 0][:, None])
assert_allclose(t[:, :2, 0][:, None], tt0)
def R_EL2n_E(R_EL):
"""
Returns n-vector from the rotation matrix R_EL.
Parameters
----------
R_EL: 3 x 3 x n array
Rotation matrix (direction cosine matrix) [no unit]
Returns
-------
n_E: 3 x n array
n-vector(s) [no unit] decomposed in E.
Notes
-----
n-vector is found from the rotation matrix (direction cosine matrix) R_EL.
See also
--------
R_EN2n_E, n_E_and_wa2R_EL, n_E2R_EN
"""
# n-vector equals minus the last column of R_EL and R_EN, see Section 5.5
# in Gade (2010)
n_E = mdot(R_EL, np.vstack((0, 0, -1)))
return n_E.reshape(3, -1)
def n_E2R_EN(n_E, R_Ee=None):
"""
Returns the rotation matrix R_EN from n-vector.
Parameters
----------
n_E: 3 x n array
n-vector [no unit] decomposed in E
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
R_EN: 3 x 3 x n array
The resulting rotation matrix [no unit] (direction cosine matrix).
See also
--------
R_EN2n_E, n_E_and_wa2R_EL, R_EL2n_E
"""
if R_Ee is None:
R_Ee = E_rotation()
# n_E = np.atleast_2d(n_E)
# _nvector_check_length(n_E)
# n_E = unit(np.matmul(R_Ee, n_E))
n_e = change_axes_to_E(n_E, R_Ee)
# N coordinate frame (North-East-Down) is defined in Table 2 in Gade (2010)
# Find z-axis of N (Nz):
Nz_e = -n_e # z-axis of N (down) points opposite to n-vector
# Find y-axis of N (East)(remember that N is singular at Poles)
# Equation (9) in Gade (2010):
# Ny points perpendicular to the plane
Ny_e_direction = np.cross([[1], [0], [0]], n_e, axis=0)
# formed by n-vector and Earth's spin axis
on_poles = np.flatnonzero(norm(Ny_e_direction, axis=0) == 0)
Ny_e = unit(Ny_e_direction)
Ny_e[:, on_poles] = array([[0], [1], [0]]) # selected y-axis direction
# Find x-axis of N (North):
Nx_e = np.cross(Ny_e, Nz_e, axis=0) # Final axis found by right hand rule
# Form R_EN from the unit vectors:
# R_EN = dot(R_Ee.T, np.hstack((Nx_e, Ny_e, Nz_e)))
Nxyz_e = np.hstack((Nx_e[:, None, ...], Ny_e[:, None, ...], Nz_e[:, None,
...]))
R_EN = mdot(np.swapaxes(R_Ee, 1, 0), Nxyz_e)
return np.squeeze(R_EN)
def n_E_and_wa2R_EL(n_E, wander_azimuth, R_Ee=None):
"""
Returns rotation matrix R_EL from n-vector and wander azimuth angle.
Parameters
----------
n_E: 3 x n array
n-vector [no unit] decomposed in E
wander_azimuth: real scalar or array of length n
Angle [rad] between L's x-axis and north, positive about L's z-axis.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
R_EL: 3 x 3 x n array
The resulting rotation matrix. [no unit]
Notes
-----
Calculates the rotation matrix (direction cosine matrix) R_EL using
n-vector (n_E) and the wander azimuth angle. When wander_azimuth=0, we
have that N=L. (See Table 2 in Gade (2010) for details)
See also
--------
R_EL2n_E, R_EN2n_E, n_E2R_EN
"""
if R_Ee is None:
R_Ee = E_rotation()
latitude, longitude = n_E2lat_lon(n_E, R_Ee)
# Longitude, -latitude, and wander azimuth are the x-y-z Euler angles (about
# new axes) for R_EL.
# Reference: See start of Section 5.2 in Gade (2010):
R_EL = mdot(R_Ee.T, xyz2R(longitude, -latitude, wander_azimuth))
return np.squeeze(R_EL)
def n_EA_E_and_p_AB_N2n_EB_E(n_EA_E, p_AB_N, z_EA=0, a=6378137, f=1.0 / 298.257223563, R_Ee=None):
"""
Returns position B from position A and delta vector decomposed in N.
Parameters
----------
n_EA_E: 3 x k array
n-vector(s) [no unit] of position A, decomposed in E.
p_AB_N: 3 x m array
Cartesian position vector(s) [m] from A to B, decomposed in N.
z_EA: 1 x n array
Depth(s) [m] of system A, relative to the ellipsoid. (z_EA = -height)
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
n_EB_E: 3 x max(k,m,n) array
n-vector(s) [no unit] of position B, decomposed in E.
z_EB: 1 x max(k,m,n) array
Depth(s) [m] of system B, relative to the ellipsoid.
(z_EB = -height)
Notes
-----
The n-vector for position A (n_EA_E) and the delta vector from position
A to position B decomposed in N (p_AB_N) are given. The output is the
n-vector of position B (`n_EB_E`) and depth of B (`z_EB`).
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
The default ellipsoid model used is WGS-84, but other ellipsoids/spheres
might be specified.
The shape of the output `n_EB_E` and `z_EB` is the broadcasted shapes of
`n_EA_E`, `p_AB_N` and `z_EA`.
Examples
--------
{super}
See also
--------
n_EA_E_and_n_EB_E2p_AB_N,
n_EA_E_and_p_AB_E2n_EB_E,
n_E2R_EN
"""
if R_Ee is None:
R_Ee = E_rotation()
n_EA_E, p_AB_N = np.atleast_2d(n_EA_E, p_AB_N)
R_EN = n_E2R_EN(n_EA_E, R_Ee=R_Ee)
# p_AB_E = dot(R_EN, p_AB_N)
p_AB_E = mdot(R_EN, p_AB_N[:, None, ...]).reshape(3, -1)
return n_EA_E_and_p_AB_E2n_EB_E(n_EA_E, p_AB_E, z_EA, a=a, f=f, R_Ee=R_Ee)
def n_EA_E_and_n_EB_E2p_AB_N(n_EA_E, n_EB_E, z_EA=0, z_EB=0, a=6378137,
f=1.0 / 298.257223563, R_Ee=None):
"""
Returns the delta vector from position A to B decomposed in N.
Parameters
----------
n_EA_E, n_EB_E: 3 x j and 3 x k arrays
n-vector(s) [no unit] of position A and B, decomposed in E.
z_EA, z_EB: 3 x m and 3 x n arrays
Depth(s) [m] of system A and B, relative to the ellipsoid.
(z_EA = -height, z_EB = -height)
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
p_AB_N: 3 x max(j,k,m,n) array
Cartesian position vector(s) [m] from A to B, decomposed in N.
Notes
-----
The n-vectors for positions A (`n_EA_E`) and B (`n_EB_E`) are given. The
output is the delta vector from A to B decomposed in N (`p_AB_N`).
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
The default ellipsoid model used is WGS-84, but other ellipsoids/spheres
might be specified.
The shape of the output p_AB_N is the broadcasted shapes of `n_EA_E`, `n_EB_E`,
`z_EA` and `z_EB`.
Examples
--------
{super}
See also
--------
n_EA_E_and_p_AB_E2n_EB_E, p_EB_E2n_EB_E, n_EB_E2p_EB_E, n_EA_E_and_n_EB_E2p_AB_E
"""
p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB, a, f, R_Ee)
R_EN = n_E2R_EN(n_EA_E, R_Ee=R_Ee)
# p_AB_N = dot(R_EN.T, p_AB_E)
p_AB_N = mdot(np.swapaxes(R_EN, 1, 0), p_AB_E[:, None, ...]).reshape(3, -1)
# (Note the transpose of R_EN: The "closest-rule" says that when
# decomposing, the frame in the subscript of the rotation matrix that
# is closest to the vector, should equal the frame where the vector is
# decomposed. Thus the calculation np.dot(R_NE, p_AB_E) is correct,
# since the vector is decomposed in E, and E is closest to the vector.
# In the example we only had R_EN, and thus we must transpose it:
# R_EN'=R_NE)
return p_AB_N