planar.hpp

Go to the documentation of this file.

/* -*- mode: C++; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2022, Colorado School of Mines
 * All rights reserved.
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */
 
#ifndef AMINO_PLANAR_HPP
#define AMINO_PLANAR_HPP
 
#include <complex>
 
namespace amino {
 
struct BaseCmplx : ::std::complex<double> {
    typedef ::std::complex<double> type;
 
    BaseCmplx() {}
 
    BaseCmplx(double real, double imag) : type(real, imag) {}
 
    BaseCmplx(type c) : type(c) {}
 
    BaseCmplx(const aa_tf_cmplx& v) : type(from(v)) {}
 
    static aa_tf_cmplx c_cmplx(type c) {
        return *reinterpret_cast<const aa_tf_cmplx*>(&c);
    }
 
    aa_tf_cmplx c_cmplx() const {
        return c_cmplx(*this);
    }
 
    operator aa_tf_cmplx() const { return c_cmplx(); }
 
    static double c_real (const aa_tf_cmplx& v) {
        return reinterpret_cast<const type*>(&v)->real();
    }
 
    static double c_imag (const aa_tf_cmplx& v) {
        return reinterpret_cast<const type*>(&v)->imag();
    }
 
    static type from(const aa_tf_cmplx& v) {
        return type(c_real(v), c_imag(v));
    }
 
protected:
    double& real_ref() { return reinterpret_cast<double(&)[2]>(*this)[0]; }
 
    double& imag_ref() { return reinterpret_cast<double(&)[2]>(*this)[1]; }
};
 
struct Vec2 : BaseCmplx {
    Vec2() {}
 
    Vec2(double x, double y) : BaseCmplx(x, y) {}
 
    Vec2(aa_tf_vec2 v) : BaseCmplx(v) {}
 
    double x() const { return real(); }
 
    double y() const { return imag(); }
 
    double& x() { return real_ref(); }
 
    double& y() { return imag_ref(); }
};
 
struct AngleP {
    AngleP() {}
 
    AngleP(double theta) : angle_(theta) {}
 
    AngleP(aa_tf_cmplx c) : angle_(aa_tf_cmplx2angle(c)) {}
 
    AngleP(BaseCmplx::type c) : angle_(atan2(c.imag(), c.real())) {}
 
    AngleP(const aa_tf_rotmatp* R) : angle_(aa_tf_rotmatp2angle(R->data)) {}
 
    AngleP(const aa_tf_rotmatp& R) : angle_(aa_tf_rotmatp2angle(R.data)) {}
 
    double angle() const { return angle_; };
 
    double& angle() { return angle_; };
 
   private:
    double angle_;
};
 
struct Cmplx : BaseCmplx {
    Cmplx() {}
 
    Cmplx(double real, double imag) : BaseCmplx(real, imag) {}
 
    Cmplx(type c) : BaseCmplx(c) {}
 
    Cmplx(aa_tf_cmplx c) : BaseCmplx(c) {}
 
    Cmplx(const aa_tf_rotmatp& R) : BaseCmplx(from(R)) {}
 
    Cmplx(const aa_tf_rotmatp* R) : BaseCmplx(from(R)) {}
 
    Cmplx(const AngleP& a) : BaseCmplx(from(a)) {}
 
    Cmplx(const AngleP* a) : BaseCmplx(from(a)) {}
 
    static Cmplx from_angle(double theta) {
        return aa_tf_angle2cmplx(theta);
    }
 
    static Cmplx from_rotmatp(const double *R) {
        return aa_tf_rotmatp2cmplx(R);
    }
 
    static Cmplx from(const aa_tf_rotmatp* R) { return from_rotmatp(R->data); }
    static Cmplx from(const aa_tf_rotmatp& R) { return from_rotmatp(R.data); }
    static Cmplx from(const AngleP& a) { return from_angle(a.angle()); }
    static Cmplx from(const AngleP* a) { return from_angle(a->angle()); }
 
    static aa_tf_cmplx ident() { return 1; }
 
    double angle() const { return aa_tf_cmplx2angle(*this); }
 
    double uln() const { return aa_tf_culn(*this); }
 
    Cmplx conj() const { return aa_tf_cconj(*this); }
};
 
struct RotMatP : aa_tf_rotmatp {
    RotMatP () : aa_tf_rotmatp(ident())  {}
 
    RotMatP(const aa_tf_rotmatp& R) : aa_tf_rotmatp(R) {}
    RotMatP(const aa_tf_rotmatp* R) : aa_tf_rotmatp(*R) {}
    RotMatP(double r11, double r12, double r21, double r22)
        : aa_tf_rotmatp(from_rotmatp(r11, r12, r21, r22))
    {}
 
    RotMatP(Cmplx::type c) : aa_tf_rotmatp(from(c)) {}
    RotMatP(aa_tf_cmplx c) : aa_tf_rotmatp(from(c)) {}
    RotMatP(double angle) : aa_tf_rotmatp(from(angle)) {}
    RotMatP(const AngleP &angle) : aa_tf_rotmatp(from(angle)) {}
    RotMatP(const AngleP *angle) : aa_tf_rotmatp(from(angle)) {}
 
    static aa_tf_rotmatp from_angle(double theta) {
        aa_tf_rotmatp R;
        aa_tf_angle2rotmatp(theta, R.data);
        return R;
    }
 
    static aa_tf_rotmatp
    from_rotmatp (double r11, double r12, double r21, double r22)
    {
        aa_tf_rotmatp R;
        R.data[0] = r11;
        R.data[1] = r21;
        R.data[2] = r12;
        R.data[3] = r22;
        return R;
    }
 
    static aa_tf_rotmatp
    from_rotmatp (const double *R) {
        return from_rotmatp(R[0], R[2], R[1], R[3]);
    }
 
    static aa_tf_rotmatp
    from_cmplx (aa_tf_cmplx c) {
        aa_tf_rotmatp R;
        aa_tf_cmplx2rotmatp(c, R.data);
        return R;
    }
 
    static aa_tf_rotmatp
    from_cmplx (Cmplx::type c) {
        return from_cmplx(Cmplx::c_cmplx(c));
    }
 
    static aa_tf_rotmatp from(aa_tf_cmplx c) { return from_cmplx(c); }
    static aa_tf_rotmatp from(Cmplx::type c) { return from_cmplx(c); }
    static aa_tf_rotmatp from(double angle) { return from_angle(angle); }
    static aa_tf_rotmatp from(const AngleP& angle) {
        return from_angle(angle.angle());
    }
    static aa_tf_rotmatp from(const AngleP* angle) { return from_angle(angle->angle()); }
 
    static aa_tf_rotmatp ident() {
        static const double R[] = AA_TF_ROTMATP_IDENT_INITIALIZER;
        return from_rotmatp(R);
    }
 
    double angle() const { return aa_tf_rotmatp2angle(this->data); }
 
    double uln() const { return aa_tf_rotmatp_uln(this->data); }
 
    aa_tf_rotmatp inv() const {
        aa_tf_rotmatp Ri;
        aa_tf_rotmatp_inv2(this->data, Ri.data);
        return Ri;
    }
};
 
struct CmplxTran : aa_tf_cv {
    CmplxTran() : aa_tf_cv(ident()) {}
    CmplxTran(aa_tf_cmplx c_, aa_tf_vec2 v_) : aa_tf_cv(from(c_, v_)) {}
    CmplxTran(Cmplx::type c_, aa_tf_vec2 v_) : aa_tf_cv(from(c_, v_)) {}
    CmplxTran(double angle, aa_tf_vec2 v_) : aa_tf_cv(from(angle, v_)) {}
    CmplxTran(const AngleP &angle, aa_tf_vec2 v_) : aa_tf_cv(from(angle, v_)) {}
    CmplxTran(const AngleP *angle, aa_tf_vec2 v_) : aa_tf_cv(from(angle, v_)) {}
    CmplxTran(const RotMatP &R, aa_tf_vec2 v_) : aa_tf_cv(from(R, v_)) {}
    CmplxTran(const RotMatP *R, aa_tf_vec2 v_) : aa_tf_cv(from(R, v_)) {}
    CmplxTran(const aa_tf_cv &cv) : aa_tf_cv(from(cv)) {}
    CmplxTran(const aa_tf_cv *cv) : aa_tf_cv(from(cv)) {}
    CmplxTran(const struct aa_tf_tfmatp &T) : aa_tf_cv(from(T)) {}
    CmplxTran(const struct aa_tf_tfmatp *T) : aa_tf_cv(from(T)) {}
 
    static aa_tf_cv ident() {
        static const double data[] = AA_TF_COTR_IDENT_INITIALIZER;
        return from_cv(data);
    }
 
    static aa_tf_cv from_cv(const double *ptr) {
        aa_tf_cv e;
        e.data[0] = ptr[0];
        e.data[1] = ptr[1];
        e.data[2] = ptr[2];
        e.data[3] = ptr[3];
        return e;
    }
 
    static aa_tf_cv from(aa_tf_cmplx c, aa_tf_vec2 v)
    {
        aa_tf_cv e;
        e.c = c;
        e.v = v;
        return e;
    }
 
    static aa_tf_cv from(Cmplx::type c, aa_tf_vec2 v) {
        return from(Cmplx::c_cmplx(c), v);
    }
 
    static aa_tf_cv from_tfmatp(const double *T) {
        aa_tf_cv e;
        aa_tf_tfmatp2cv(T, &e.c, &e.v);
        return e;
    }
 
    static aa_tf_cv from(double angle, aa_tf_vec2 v) {
        return from(Cmplx::from_angle(angle), v);
    }
    static aa_tf_cv from(const AngleP &angle, aa_tf_vec2 v) {
        return from(angle.angle(), v);
    }
    static aa_tf_cv from(const AngleP *angle, aa_tf_vec2 v) {
        return from(angle->angle(), v);
    }
 
    static aa_tf_cv from(const aa_tf_cv& cv) { return from(cv.c, cv.v); }
    static aa_tf_cv from(const aa_tf_cv* cv) { return from(*cv); }
    static aa_tf_cv from(const aa_tf_tfmatp& T) { return from_tfmatp(T.data); }
    static aa_tf_cv from(const aa_tf_tfmatp* T) { return from(*T); }
 
    static aa_tf_cv from(const aa_tf_rotmatp *R, aa_tf_vec2 v) {
        aa_tf_cv e;
        e.c = aa_tf_rotmatp2cmplx(R->data);
        e.v = v;
        return e;
    }
 
    static aa_tf_cv from(const aa_tf_rotmatp &R, aa_tf_vec2 v) {
        return from(&R, v);
    }
 
    aa_tf_cv inv() const {
        aa_tf_cv e;
        aa_tf_cv_inv(this->c, this->v, &e.c, &e.v);
        return e;
    }
};
 
struct TfMatP : aa_tf_tfmatp {
    TfMatP() : aa_tf_tfmatp(ident()) {}
 
    TfMatP(const aa_tf_rotmatp* R_, aa_tf_vec2 v_) : aa_tf_tfmatp(from(R_, v_))
    {}
    TfMatP(const aa_tf_rotmatp& R_, aa_tf_vec2 v_) : aa_tf_tfmatp(from(R_, v_))
    {}
    TfMatP(aa_tf_cmplx c, aa_tf_vec2 v_) : aa_tf_tfmatp(from(c,v_)) {}
    TfMatP(Cmplx::type c, aa_tf_vec2 v_) : aa_tf_tfmatp(from(c,v_)) {}
 
    TfMatP(double angle, aa_tf_vec2 v_) : aa_tf_tfmatp(from(angle,v_)) {}
    TfMatP(const AngleP &angle, aa_tf_vec2 v_) : aa_tf_tfmatp(from(angle,v_)) {}
    TfMatP(const AngleP *angle, aa_tf_vec2 v_) : aa_tf_tfmatp(from(angle,v_)) {}
 
    TfMatP(const aa_tf_tfmatp *T) : aa_tf_tfmatp(*T) {}
    TfMatP(const aa_tf_tfmatp &T) : aa_tf_tfmatp(T) {}
    TfMatP(const aa_tf_cv* cv) : aa_tf_tfmatp(from(cv)) {}
    TfMatP(const aa_tf_cv& cv) : aa_tf_tfmatp(from(cv)) {}
 
    static aa_tf_tfmatp ident() {
        static const double data[] = AA_TF_TFMATP_IDENT_INITIALIZER;
        return from_tfmatp(data);
    }
 
    static aa_tf_tfmatp from_tfmatp(const double * ptr) {
        aa_tf_tfmatp T;
        memcpy(T.data, ptr, sizeof(T));
        return T;
    }
 
    static aa_tf_tfmatp from_cv(aa_tf_cmplx c, aa_tf_vec2 v) {
        aa_tf_tfmatp T;
        aa_tf_cmplx2rotmatp(c, T.R.data);
 
        T.v = v;
        return T;
    }
 
    static aa_tf_tfmatp from_cv(Cmplx::type c, aa_tf_vec2 v) {
        return from_cv(Cmplx::c_cmplx(c), v);
    }
 
 
    static aa_tf_tfmatp from(const aa_tf_tfmatp* T) {
        return from_tfmatp(T->data);
    }
 
    static aa_tf_tfmatp from(const aa_tf_tfmatp& T) {
        return from_tfmatp(T.data);
    }
 
    static aa_tf_tfmatp from(const aa_tf_rotmatp &R, aa_tf_vec2 v) {
        aa_tf_tfmatp T;
        T.R = R;
        T.v = v;
        return T;
    }
 
    static aa_tf_tfmatp from(const aa_tf_rotmatp *R, aa_tf_vec2 v) {
        return from(*R, v);
    }
 
    static aa_tf_tfmatp from(aa_tf_cmplx c, aa_tf_vec2 v) {
        return from_cv(c, v);
    }
 
    static aa_tf_tfmatp from(Cmplx::type c, aa_tf_vec2 v) {
        return from_cv(c, v);
    }
 
    static aa_tf_tfmatp from(double angle, aa_tf_vec2 v) {
        return from_cv(Cmplx::from_angle(angle), v);
    }
 
    static aa_tf_tfmatp from(const AngleP &angle, aa_tf_vec2 v) {
        return from(angle.angle(), v);
    }
 
    static aa_tf_tfmatp from(const AngleP* angle, aa_tf_vec2 v) {
        return from(*angle, v);
    }
 
    static aa_tf_tfmatp from(const aa_tf_cv& cv) { return from(cv.c, cv.v); }
 
    static aa_tf_tfmatp from(const aa_tf_cv* cv) { return from(*cv); }
 
    aa_tf_tfmatp inv() const {
        aa_tf_tfmatp T;
        aa_tf_tfmatp_inv2(this->data, T.data);
        return T;
    }
};
 
static inline aa_tf_vec2
operator*(const Cmplx& c, const Vec2& v)
{
    return aa_tf_crot(c,v);
}
 
static inline aa_tf_vec2
operator*(const aa_tf_rotmatp& R, const Vec2& v)
{
    return aa_tf_rotmatp_rot(R.data,v);
}
 
static inline aa_tf_rotmatp
operator*(const aa_tf_rotmatp& R1, const aa_tf_rotmatp& R2)
{
    aa_tf_rotmatp R12;
    aa_tf_rotmatp_mul(R1.data,R2.data, R12.data);
    return R12;
}
 
static inline aa_tf_vec2
operator*(const aa_tf_tfmatp& T, const aa_tf_vec2& p)
{
    return aa_tf_tfmatp_tf(T.data, p);
}
 
static inline aa_tf_vec2
operator*(const aa_tf_cv& E, const aa_tf_vec2& p)
{
    return aa_tf_cv_tf(E.c, E.v, p);
}
 
 
static inline aa_tf_tfmatp
operator*(const aa_tf_tfmatp& T1, const aa_tf_tfmatp& T2)
{
    aa_tf_tfmatp T12;
    aa_tf_tfmatp_mul(T1.data, T2.data, T12.data);
    return T12;
}
 
static inline aa_tf_cv
operator*(const aa_tf_cv& E1, const aa_tf_cv& E2)
{
    aa_tf_cv E12;
    aa_tf_cv_mul(E1.c, E1.v, E2.c, E2.v, &E12.c, &E12.v);
    return E12;
}
 
}  // namespace amino
 
#endif /* AMINO_TFP_HPP */