Program Listing for File navigation.hpp
↰ Return to documentation for file (src/navtk/navutils/navigation.hpp)
#pragma once
#include <memory>
#include <utility>
#include <navtk/factory.hpp>
#include <navtk/inspect.hpp>
#include <navtk/navutils/math.hpp>
#include <navtk/navutils/wgs84.hpp>
#include <navtk/tensors.hpp>
namespace navtk {
namespace navutils {
template <typename S, IfTensorOfDim<S, 0>* = nullptr>
double meridian_radius(const S& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS * (1 - ECCENTRICITY_SQUARED) /
pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 1.5);
}
template <typename B, IfTensorOfDim<B, 1>* = nullptr>
Vector meridian_radius(const B& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS * (1 - ECCENTRICITY_SQUARED) /
pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 1.5);
}
template <typename S, IfTensorOfDim<S, 0>* = nullptr>
double transverse_radius(const S& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS / pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 0.5);
}
template <typename B, IfTensorOfDim<B, 1>* = nullptr>
Vector transverse_radius(const B& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS / pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 0.5);
}
Matrix3 axis_angle_to_dcm(const Vector3& axis, double angle);
Matrix calc_van_loan(const Matrix& F, const Matrix& G, const Matrix& Q, double dt);
Matrix3 correct_dcm_with_tilt(const Matrix3& dcm, const Vector3& tilt);
template <typename S = Initializer<2, Scalar>, IfTensorOfDim<S, 2>* = nullptr>
Vector4 dcm_to_quat(const S& dcm) {
auto dcm_mat = to_matrix(dcm);
auto d0 = dcm_mat(0, 0);
auto d1 = dcm_mat(1, 1);
auto d2 = dcm_mat(2, 2);
// Book just says max, but alternative formulation uses squares, so probably max(abs)
auto pa = std::fabs(1 + d0 + d1 + d2);
auto pb = std::fabs(1 + d0 - d1 - d2);
auto pc = std::fabs(1 - d0 + d1 - d2);
auto pd = std::fabs(1 - d0 - d1 + d2);
Scalar q0, q1, q2, q3;
if (pa >= pb && pa >= pc && pa >= pd) {
q0 = 0.5 * sqrt(pa);
auto q0t4 = 4 * q0;
q1 = (dcm_mat(2, 1) - dcm_mat(1, 2)) / (q0t4);
q2 = (dcm_mat(0, 2) - dcm_mat(2, 0)) / (q0t4);
q3 = (dcm_mat(1, 0) - dcm_mat(0, 1)) / (q0t4);
} else if (pb >= pa && pb >= pc && pb >= pd) {
q1 = 0.5 * sqrt(pb);
auto q1t4 = 4 * q1;
q0 = (dcm_mat(2, 1) - dcm_mat(1, 2)) / (q1t4);
q2 = (dcm_mat(1, 0) + dcm_mat(0, 1)) / (q1t4);
q3 = (dcm_mat(0, 2) + dcm_mat(2, 0)) / (q1t4);
} else if (pc >= pa && pc >= pb && pc >= pd) {
q2 = 0.5 * sqrt(pc);
auto q2t4 = 4 * q2;
q0 = (dcm_mat(0, 2) - dcm_mat(2, 0)) / (q2t4);
q1 = (dcm_mat(1, 0) + dcm_mat(0, 1)) / (q2t4);
q3 = (dcm_mat(2, 1) + dcm_mat(1, 2)) / (q2t4);
} else {
q3 = 0.5 * sqrt(pd);
auto q3t4 = 4 * q3;
q0 = (dcm_mat(1, 0) - dcm_mat(0, 1)) / (q3t4);
q1 = (dcm_mat(0, 2) + dcm_mat(2, 0)) / (q3t4);
q2 = (dcm_mat(2, 1) + dcm_mat(1, 2)) / (q3t4);
}
// use of sign bit is because -0.0 and 0.0 are technically both <= 0 and neither are < 0.
if (std::signbit(q0)) {
return {-q0, -q1, -q2, -q3};
} else {
return {q0, q1, q2, q3};
}
}
template <typename B = Initializer<3, Scalar>, IfTensorOfDim<B, 3>* = nullptr>
Matrix dcm_to_quat(const B& dcm) {
auto dcm_tensor = to_tensor_3d(dcm);
const size_t N = dcm_tensor.shape()[0];
Matrix quats = empty(N, 4);
Scalar d0, d1, d2, pa, pb, pc, pd, q0, q1, q2, q3, q0t4, q1t4, q2t4, q3t4;
for (size_t i = 0; i < N; i++) {
d0 = dcm_tensor(i, 0, 0);
d1 = dcm_tensor(i, 1, 1);
d2 = dcm_tensor(i, 2, 2);
// Book just says max, but alternative formulation uses squares, so probably max(abs)
pa = std::fabs(1 + d0 + d1 + d2);
pb = std::fabs(1 + d0 - d1 - d2);
pc = std::fabs(1 - d0 + d1 - d2);
pd = std::fabs(1 - d0 - d1 + d2);
q0 = 0.0;
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
if (pa >= pb && pa >= pc && pa >= pd) {
q0 = 0.5 * sqrt(pa);
q0t4 = 4 * q0;
q1 = (dcm_tensor(i, 2, 1) - dcm_tensor(i, 1, 2)) / (q0t4);
q2 = (dcm_tensor(i, 0, 2) - dcm_tensor(i, 2, 0)) / (q0t4);
q3 = (dcm_tensor(i, 1, 0) - dcm_tensor(i, 0, 1)) / (q0t4);
} else if (pb >= pa && pb >= pc && pb >= pd) {
q1 = 0.5 * sqrt(pb);
q1t4 = 4 * q1;
q0 = (dcm_tensor(i, 2, 1) - dcm_tensor(i, 1, 2)) / (q1t4);
q2 = (dcm_tensor(i, 1, 0) + dcm_tensor(i, 0, 1)) / (q1t4);
q3 = (dcm_tensor(i, 0, 2) + dcm_tensor(i, 2, 0)) / (q1t4);
} else if (pc >= pa && pc >= pb && pc >= pd) {
q2 = 0.5 * sqrt(pc);
q2t4 = 4 * q2;
q0 = (dcm_tensor(i, 0, 2) - dcm_tensor(i, 2, 0)) / (q2t4);
q1 = (dcm_tensor(i, 1, 0) + dcm_tensor(i, 0, 1)) / (q2t4);
q3 = (dcm_tensor(i, 2, 1) + dcm_tensor(i, 1, 2)) / (q2t4);
} else {
q3 = 0.5 * sqrt(pd);
q3t4 = 4 * q3;
q0 = (dcm_tensor(i, 1, 0) - dcm_tensor(i, 0, 1)) / (q3t4);
q1 = (dcm_tensor(i, 0, 2) + dcm_tensor(i, 2, 0)) / (q3t4);
q2 = (dcm_tensor(i, 2, 1) + dcm_tensor(i, 1, 2)) / (q3t4);
}
// use of sign bit is because -0.0 and 0.0 are technically both <= 0.
if (std::signbit(q0)) {
quats(i, 0) = -q0;
quats(i, 1) = -q1;
quats(i, 2) = -q2;
quats(i, 3) = -q3;
} else {
quats(i, 0) = q0;
quats(i, 1) = q1;
quats(i, 2) = q2;
quats(i, 3) = q3;
}
}
return quats;
}
template <typename S = Initializer<2, Scalar>, IfTensorOfDim<S, 2>* = nullptr>
Vector3 dcm_to_rpy(const S& dcm) {
auto dcm_mat = to_matrix(dcm);
auto asin_arg = std::min(1.0, std::max(dcm_mat(2, 0), -1.0));
auto r = atan2(dcm_mat(2, 1), dcm_mat(2, 2));
auto p = -asin(asin_arg);
auto y = atan2(dcm_mat(1, 0), dcm_mat(0, 0));
if (asin_arg <= -1 + 1e-12) {
auto y_min_r = atan2(dcm_mat(1, 2) - dcm_mat(0, 1), dcm_mat(0, 2) + dcm_mat(1, 1));
y = y_min_r + r;
}
if (asin_arg >= 1 - 1e-12) {
auto y_pls_r = atan2(dcm_mat(1, 2) + dcm_mat(0, 1), dcm_mat(0, 2) - dcm_mat(1, 1)) + PI;
y = remainder((y_pls_r - r), 2.0 * PI);
}
return {r, p, y};
}
template <typename B = Initializer<3, Scalar>, IfTensorOfDim<B, 3>* = nullptr>
Matrix dcm_to_rpy(const B& dcm) {
auto dcm_tensor = to_tensor_3d(dcm);
const size_t N = dcm_tensor.shape()[0];
Matrix rpys = empty(N, 3);
Scalar asin_arg;
for (size_t i = 0; i < N; i++) {
asin_arg = std::min(1.0, std::max(dcm_tensor(i, 2, 0), -1.0));
rpys(i, 0) = atan2(dcm_tensor(i, 2, 1), dcm_tensor(i, 2, 2));
rpys(i, 1) = -asin(asin_arg);
rpys(i, 2) = atan2(dcm_tensor(i, 1, 0), dcm_tensor(i, 0, 0));
if (asin_arg <= -1 + 1e-12) {
auto y_min_r = atan2(dcm_tensor(i, 1, 2) - dcm_tensor(i, 0, 1),
dcm_tensor(i, 0, 2) + dcm_tensor(i, 1, 1));
rpys(i, 2) = y_min_r + rpys(i, 0);
}
if (asin_arg >= 1 - 1e-12) {
auto y_pls_r = atan2(dcm_tensor(i, 1, 2) + dcm_tensor(i, 0, 1),
dcm_tensor(i, 0, 2) - dcm_tensor(i, 1, 1)) +
PI;
rpys(i, 2) = remainder((y_pls_r - rpys(i, 0)), 2.0 * PI);
}
}
return rpys;
}
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double delta_lat_to_north(const S1& delta_lat, const S2& approx_lat, const S3& altitude) {
return (meridian_radius(approx_lat) + altitude) * delta_lat;
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector delta_lat_to_north(const B1& delta_lat, const B2& approx_lat, const B3& altitude) {
return (meridian_radius(approx_lat) + altitude) * delta_lat;
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto delta_lat_to_north(const A& delta_lat, const B& approx_lat) {
return delta_lat_to_north(delta_lat, approx_lat, 0.0);
}
#endif
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double delta_lon_to_east(const S1& delta_lon, const S2& approx_lat, const S3& altitude) {
return (transverse_radius(approx_lat) + altitude) * delta_lon * cos(approx_lat);
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector delta_lon_to_east(const B1& delta_lon, const B2& approx_lat, const B3& altitude) {
return (transverse_radius(approx_lat) + altitude) * delta_lon * cos(approx_lat);
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto delta_lon_to_east(const A& delta_lon, const B& approx_lat) {
return delta_lon_to_east(delta_lon, approx_lat, 0.0);
}
#endif
std::pair<Matrix, Matrix> discretize_first_order(const Matrix& f, const Matrix& q, double dt);
std::pair<Matrix, Matrix> discretize_second_order(const Matrix& f, const Matrix& q, double dt);
std::pair<Matrix, Matrix> discretize_van_loan(const Matrix& f, const Matrix& q, double dt);
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double east_to_delta_lon(const S1& east_distance, const S2& approx_lat, const S3& altitude) {
return east_distance / ((transverse_radius(approx_lat) + altitude) * cos(approx_lat));
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector east_to_delta_lon(const B1& east_distance, const B2& approx_lat, const B3& altitude) {
return east_distance / ((transverse_radius(approx_lat) + altitude) * cos(approx_lat));
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto east_to_delta_lon(const A& east_distance, const B& approx_lat) {
return east_to_delta_lon(east_distance, approx_lat, 0.0);
}
#endif
Matrix3 ecef_to_cen(const Vector3& Pe);
Vector3 ecef_to_llh(const Vector3& Pe);
Vector3 ecef_to_local_level(const Vector3& P0e, const Vector3& Pe);
Matrix3 llh_to_cen(const Vector3& Pwgs);
Vector3 llh_to_ecef(const Vector3& Pwgs);
Vector3 local_level_to_ecef(const Vector3& P0e, const Vector3& Pn);
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double north_to_delta_lat(const S1& north_distance, const S2& approx_lat, const S3& altitude) {
return north_distance / (meridian_radius(approx_lat) + altitude);
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector north_to_delta_lat(const B1& north_distance, const B2& approx_lat, const B3& altitude) {
return north_distance / (meridian_radius(approx_lat) + altitude);
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto north_to_delta_lat(const A& north_distance, const B& approx_lat) {
return north_to_delta_lat(north_distance, approx_lat, 0.0);
}
#endif
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 quat_to_dcm(const S& quat) {
auto quat_vec = to_vec(quat);
auto q0 = quat_vec(0);
auto q1 = quat_vec(1);
auto q2 = quat_vec(2);
auto q3 = quat_vec(3);
auto a2 = pow(q0, 2);
auto b2 = pow(q1, 2);
auto c2 = pow(q2, 2);
auto d2 = pow(q3, 2);
auto ab = q0 * q1;
auto ac = q0 * q2;
auto ad = q0 * q3;
auto bc = q1 * q2;
auto bd = q1 * q3;
auto cd = q2 * q3;
return {{a2 + b2 - c2 - d2, 2 * (bc - ad), 2 * (bd + ac)},
{2 * (bc + ad), a2 - b2 + c2 - d2, 2 * (cd - ab)},
{2 * (bd - ac), 2 * (cd + ab), a2 - b2 - c2 + d2}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> quat_to_dcm(const B& quat) {
auto quat_mat = to_matrix(quat);
const size_t N = quat_mat.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar q0, q1, q2, q3, a2, b2, c2, d2, ab, ac, ad, bc, bd, cd;
for (size_t i = 0; i < N; i++) {
q0 = quat_mat(i, 0);
q1 = quat_mat(i, 1);
q2 = quat_mat(i, 2);
q3 = quat_mat(i, 3);
a2 = pow(q0, 2);
b2 = pow(q1, 2);
c2 = pow(q2, 2);
d2 = pow(q3, 2);
ab = q0 * q1;
ac = q0 * q2;
ad = q0 * q3;
bc = q1 * q2;
bd = q1 * q3;
cd = q2 * q3;
dcms(i, 0, 0) = a2 + b2 - c2 - d2;
dcms(i, 0, 1) = 2 * (bc - ad);
dcms(i, 0, 2) = 2 * (bd + ac);
dcms(i, 1, 0) = 2 * (bc + ad);
dcms(i, 1, 1) = a2 - b2 + c2 - d2;
dcms(i, 1, 2) = 2 * (cd - ab);
dcms(i, 2, 0) = 2 * (bd - ac);
dcms(i, 2, 1) = 2 * (cd + ab);
dcms(i, 2, 2) = a2 - b2 - c2 + d2;
}
return dcms;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Vector3 quat_to_rpy(const S& quat) {
auto quat_vec = to_vec(quat);
auto q0 = quat_vec[0];
auto q1 = quat_vec[1];
auto q2 = quat_vec[2];
auto q3 = quat_vec[3];
auto roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)),
pitch = asin(std::min(std::max(2 * (q0 * q2 - q1 * q3), -1.0), 1.0)),
yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
return {roll, pitch, yaw};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix quat_to_rpy(const B& quat) {
auto quat_mat = to_matrix(quat);
const size_t N = quat_mat.shape()[0];
// allocate return Matrix
Matrix rpys = empty(N, 3);
Scalar q0, q1, q2, q3;
for (size_t i = 0; i < N; i++) {
q0 = quat_mat(i, 0);
q1 = quat_mat(i, 1);
q2 = quat_mat(i, 2);
q3 = quat_mat(i, 3);
// roll, pitch, and yaw
rpys(i, 0) = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
rpys(i, 1) = asin(std::min(std::max(2 * (q0 * q2 - q1 * q3), -1.0), 1.0));
rpys(i, 2) = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
}
return rpys;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 rpy_to_dcm(const S& rpy) {
auto rpy_vec = to_vec(rpy);
Scalar cph = cos(rpy_vec[0]);
Scalar sph = sin(rpy_vec[0]);
Scalar cth = cos(rpy_vec[1]);
Scalar sth = sin(rpy_vec[1]);
Scalar cps = cos(rpy_vec[2]);
Scalar sps = sin(rpy_vec[2]);
return Matrix{{cps * cth, -sps * cph + cps * sth * sph, sps * sph + cps * sth * cph},
{sps * cth, cps * cph + sps * sth * sph, -cps * sph + sps * sth * cph},
{-sth, cth * sph, cth * cph}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> rpy_to_dcm(const B& rpy) {
auto rpy_mat = to_matrix(rpy);
const size_t N = rpy_mat.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar cph, sph, cth, sth, cps, sps;
for (size_t i = 0; i < N; i++) {
// Negate angles as we are performing
// left-handed coordinate rotations
cph = cos(rpy_mat(i, 0));
sph = sin(rpy_mat(i, 0));
cth = cos(rpy_mat(i, 1));
sth = sin(rpy_mat(i, 1));
cps = cos(rpy_mat(i, 2));
sps = sin(rpy_mat(i, 2));
dcms(i, 0, 0) = cps * cth;
dcms(i, 0, 1) = -sps * cph + cps * sth * sph;
dcms(i, 0, 2) = sps * sph + cps * sth * cph;
dcms(i, 1, 0) = sps * cth;
dcms(i, 1, 1) = cps * cph + sps * sth * sph;
dcms(i, 1, 2) = -cps * sph + sps * sth * cph;
dcms(i, 2, 0) = -sth;
dcms(i, 2, 1) = cth * sph;
dcms(i, 2, 2) = cth * cph;
}
return dcms;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Vector4 rpy_to_quat(const S& rpy) {
auto rpy_vec = to_vec(rpy);
auto cr = cos(rpy_vec[0] / 2), cp = cos(rpy_vec[1] / 2), cy = cos(rpy_vec[2] / 2),
sr = sin(rpy_vec[0] / 2), sp = sin(rpy_vec[1] / 2), sy = sin(rpy_vec[2] / 2);
return {cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix rpy_to_quat(const B& rpy) {
auto rpy_mat = to_matrix(rpy);
const size_t N = rpy_mat.shape()[0];
// allocate return Matrix
Matrix quats = empty(N, 4);
Scalar cr, cp, cy, sr, sp, sy;
for (size_t i = 0; i < N; i++) {
cr = cos(rpy_mat(i, 0) / 2);
cp = cos(rpy_mat(i, 1) / 2);
cy = cos(rpy_mat(i, 2) / 2);
sr = sin(rpy_mat(i, 0) / 2);
sp = sin(rpy_mat(i, 1) / 2);
sy = sin(rpy_mat(i, 2) / 2);
quats(i, 0) = cr * cp * cy + sr * sp * sy;
quats(i, 1) = sr * cp * cy - cr * sp * sy;
quats(i, 2) = cr * sp * cy + sr * cp * sy;
quats(i, 3) = cr * cp * sy - sr * sp * cy;
}
return quats;
}
Matrix3 wander_to_C_enu_to_n(double wander);
Matrix3 wander_to_C_ned_to_n(double wander);
Matrix3 wander_to_C_ned_to_l(double wander);
Matrix3 lat_lon_wander_to_C_n_to_e(double lat, double lon, double wander = 0.0);
Vector3 C_n_to_e_to_lat_lon_wander(const Matrix& C_n_to_e);
double C_n_to_e_to_wander(const Matrix3& C_n_to_e);
std::pair<Matrix3, double> ecef_wander_to_C_n_to_e_h(const Vector3& ecef_pos, double wander = 0.0);
Vector3 C_n_to_e_h_to_llh(const Matrix3& C_n_to_e, double h);
Vector3 C_n_to_e_h_to_ecef(const Matrix3& C_n_to_e, double h);
Matrix3 C_ecef_to_e();
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 rot_vec_to_dcm(const S& phi) {
/*
// 'Normal' implementation. Long chains of xtensor related operations can cause large slowdowns,
// especially w/ ASAN testing, so actual implementation does math 'manually' to achieve speed.
double phi_mag = xt::norm_l2(phi)[0];
double phi_mag2 = phi_mag * phi_mag;
double phi_mag4 = phi_mag2 * phi_mag2;
double term1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
double term2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
auto phi_cross = skew(phi);
return eye(3) + term1 * phi_cross + term2 * dot(phi_cross, phi_cross);
*/
auto phi_vec = to_vec(phi);
auto p0 = phi_vec(0);
auto p1 = phi_vec(1);
auto p2 = phi_vec(2);
double phi_mag = sqrt(p0 * p0 + p1 * p1 + p2 * p2);
double phi_mag2 = phi_mag * phi_mag;
double phi_mag4 = phi_mag2 * phi_mag2;
double t1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
double t2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
return {{1.0 + t2 * (-p2 * p2 - p1 * p1), -t1 * p2 + t2 * p1 * p0, t1 * p1 + t2 * p0 * p2},
{t1 * p2 + t2 * p0 * p1, 1.0 + t2 * (-p2 * p2 - p0 * p0), -t1 * p0 + t2 * p1 * p2},
{-t1 * p1 + t2 * p0 * p2, t1 * p0 + t2 * p1 * p2, 1.0 + t2 * (-p1 * p1 - p0 * p0)}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> rot_vec_to_dcm(const B& phi) {
auto phi_mat = to_matrix(phi);
const size_t N = phi_mat.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar p0, p1, p2, phi_mag, phi_mag2, phi_mag4, t1, t2;
for (size_t i = 0; i < N; i++) {
p0 = phi_mat(i, 0);
p1 = phi_mat(i, 1);
p2 = phi_mat(i, 2);
phi_mag = sqrt(p0 * p0 + p1 * p1 + p2 * p2);
phi_mag2 = phi_mag * phi_mag;
phi_mag4 = phi_mag2 * phi_mag2;
t1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
t2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
dcms(i, 0, 0) = 1.0 + t2 * (-p2 * p2 - p1 * p1);
dcms(i, 0, 1) = -t1 * p2 + t2 * p1 * p0;
dcms(i, 0, 2) = t1 * p1 + t2 * p0 * p2;
dcms(i, 1, 0) = t1 * p2 + t2 * p0 * p1;
dcms(i, 1, 1) = 1.0 + t2 * (-p2 * p2 - p0 * p0);
dcms(i, 1, 2) = -t1 * p0 + t2 * p1 * p2;
dcms(i, 2, 0) = -t1 * p1 + t2 * p0 * p2;
dcms(i, 2, 1) = t1 * p0 + t2 * p1 * p2;
dcms(i, 2, 2) = 1.0 + t2 * (-p1 * p1 - p0 * p0);
}
return dcms;
}
std::pair<bool, double> geoid_minus_ellipsoid(double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
std::pair<bool, double> hae_to_msl(double hae,
double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
std::pair<bool, double> msl_to_hae(double msl,
double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
} // namespace navutils
} // namespace navtk