Basis.h

//-*- C++ -*-
//-*- coding: utf-8 -*-
//
// Author: Bryan V. Riel, Joshua Cohen
// Copyright 2017-2018
 
#pragma once
 
#include "forward.h"
 
#include <isce3/except/Error.h>
 
#include "Common.h"
#include "DenseMatrix.h"
#include "Vector.h"
 
namespace isce3 { namespace core {

class Basis {
 
public:
    CUDA_HOSTDEV
    Basis() : _x0 {1, 0, 0}, _x1 {0, 1, 0}, _x2 {0, 0, 1} {}

    CUDA_HOST
    Basis(const Vec3& x0, const Vec3& x1, const Vec3& x2, double tol = 1e-8)
        : _x0(x0), _x1(x1), _x2(x2)
    {
        auto b = toRotationMatrix();
        // Cast to array for elementwise abs() method.
        Eigen::Array33d resid = b.transpose().dot(b) - Mat3::Identity();
        if (not(resid.abs() < tol).all()) {
            // throw means this ctor is CUDA_HOST only.
            throw isce3::except::InvalidArgument(
                    ISCE_SRCINFO(), "Basis is not unitary/orthogonal");
        }
    }

    CUDA_HOSTDEV explicit Basis(const Vec3& p, const Vec3& v)
    {
        const Vec3 n = -p.normalized();
        const Vec3 c = n.cross(v).normalized();
        const Vec3 t = c.cross(n).normalized();
        _x0 = t;
        _x1 = c;
        _x2 = n;
    }

    CUDA_HOSTDEV const Vec3& x0() const { return _x0; }

    CUDA_HOSTDEV const Vec3& x1() const { return _x1; }

    CUDA_HOSTDEV const Vec3& x2() const { return _x2; }

    CUDA_HOSTDEV void x0(const Vec3& x0) { _x0 = x0; }

    CUDA_HOSTDEV void x1(const Vec3& x1) { _x1 = x1; }

    CUDA_HOSTDEV void x2(const Vec3& x2) { _x2 = x2; }

    CUDA_HOSTDEV inline Vec3 project(const Vec3& vec) const
    {
        return Vec3 {_x0.dot(vec), _x1.dot(vec), _x2.dot(vec)};
    };

    inline void combine(const Vec3& vec, Vec3& res) const
    {
        for (int ii = 0; ii < 3; ii++) {
            res[ii] = vec[0] * _x0[ii] + vec[1] * _x1[ii] + vec[2] * _x2[ii];
        }
    };
 
    // NOTE explicit operator Mat3() doesn't work due to Eigen weirdness,
    // at least with GCC {8,9,10} and -std=c++17.
    CUDA_HOSTDEV Mat3 toRotationMatrix() const
    {
        Mat3 out;
        out.col(0) = _x0;
        out.col(1) = _x1;
        out.col(2) = _x2;
        return out;
    }
 
private:
    Vec3 _x0;
    Vec3 _x1;
    Vec3 _x2;
};

Vec3 velocityECI(const Vec3& position, const Vec3& velocityECF);

Basis geodeticTCN(const Vec3& x, const Vec3& v, const Ellipsoid& ellipsoid);

EulerAngles factoredYawPitchRoll(const Quaternion& q, const Vec3& x,
                                 const Vec3& v, const Ellipsoid& ellipsoid);

EulerAngles factoredYawPitchRoll(const Quaternion& q, const Vec3& x,
                                 const Vec3& v);
 
}} // namespace isce3::core