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