quat.h

Go to the documentation of this file.

#pragma once
#include "avect.h"
#include "matrix.h"
#include "vect.h"
#include <cmath>
#include <cassert>
 
class Quat2 {
public:
    Quat2(): ang_(0) { }
    Quat2(const double &a): ang_(a) { }
    ~Quat2() { }
    Quat2(const Quat2 &q): ang_(q.ang_) { }
    constexpr Quat2 &operator=(const Quat2 &in) { ang_ = in.ang_; return *this; }
    constexpr auto operator<=>(const Quat2 &in) const { return ang_ <=> in.ang_; }
 
    void setQuat(const double &a){ ang_ = a; }
 
    const double &ang() const { return ang_; }
 
    double &rang() { return ang_; }
 
    void reset() { ang_=0.0; }
    void ident() { ang_ = 0.0; }
    static BASE_EXPORT Quat2 identity() { Quat2 ret(0.0);  return ret;}
 
    BASE_EXPORT DMatrix<2,2> quatToMatrix() const;
    static BASE_EXPORT Quat2 fromVect(const DVect2 &v);
    BASE_EXPORT void fromMatrix(const DMatrix<2,2> &);
    BASE_EXPORT void fromAxisAngle(const DVect2 &,const double &);
    BASE_EXPORT void fromEuler(const DAVect2 &v) { *this = Quat2(v.z()) * dDegrad; }
    BASE_EXPORT void fromEulerZXZ(const DAVect2& v) { *this = Quat2(v.z()) * dDegrad; }
    BASE_EXPORT void incrementAxisAngle(const DVect2 &,const double &);
    BASE_EXPORT void spinAboutZ(const double &);
    BASE_EXPORT DVect2 rotate(const DVect2 &v) const;
    BASE_EXPORT void fromUnitAxes(const DVect2 &,const DVect2 &);
    BASE_EXPORT void increment(const DAVect2 &);
 
    void conj() { ang_*=-1; }
    Quat2 getConj() const { Quat2 q(-ang_); return q; }
    void normalize() {  }
 
    Quat2 operator *(const double &d) const { Quat2 q(ang_*d); return q; }
    Quat2 operator /(const double &d) const { Quat2 q(ang_/d); return q; }
    friend Quat2 operator *(const double &d,const Quat2 &p2) { Quat2 q(p2*d); return q;}
    Quat2 operator *(const Quat2 &v) const { Quat2 q(ang_+v.ang_); return q; }
    Quat2 operator +(const Quat2 &v) const { Quat2 q(ang_+v.ang_); return q; }
 
    const Quat2 & operator *=(const double &d) { ang_ *= d; return *this; }
    const Quat2 & operator /=(const double &d) { ang_ /= d; return *this; }
    const Quat2 & operator *=(const Quat2 &v) { ang_ += v.ang_; return *this; }
    const Quat2 & operator +=(const Quat2 &v) { ang_ += v.ang_; return *this; }
    bool operator ==(const Quat2 &v) const { return (ang_ == v.ang_); }
 
private:
    double ang_; //the angle of rotation
};
 
class Quat3 {
public:
    Quat3(): w_(0), i_(0), j_(0), k_(0) { }
    Quat3(const double &w1,const double &x,const double &y,const double &z) { w_ = w1; i_ = x; j_ = y; k_ = z; }
    ~Quat3() { }
    Quat3(const Quat3 &q) { w_ = q.w_; i_ = q.i_; j_ = q.j_; k_ = q.k_; }
    inline constexpr std::partial_ordering operator<=>(const Quat3 &in) const;
    constexpr const Quat3 &operator=(const Quat3 &q) { w_ = q.w_; i_ = q.i_; j_ = q.j_; k_ = q.k_; return *this; }
    void setQuat(const double &w1,const double &x,const double &y,const double &z){ w_ = w1; i_ = x; j_ = y; k_ = z; }
 
    const double &w() const { return w_; }
    const double &i() const { return i_; }
    const double &j() const { return j_; }
    const double &k() const { return k_; }
 
    double &rw() { return w_; }
    double &ri() { return i_; }
    double &rj() { return j_; }
    double &rk() { return k_; }
 
    void reset() { w_=0.0; i_=0.0; j_=0.0; k_=0.0; }
    void ident() { w_=1.0; i_=0.0; j_=0.0; k_=0.0; }
    static BASE_EXPORT Quat3 identity() { Quat3 ret(1.0,0.0,0.0,0.0);  return ret;}
    BASE_EXPORT bool isNull() const { return (abs(w_) > std::numeric_limits<double>::epsilon() || abs(i_) > std::numeric_limits<double>::epsilon() || abs(j_) > std::numeric_limits<double>::epsilon() || abs(k_) > std::numeric_limits<double>::epsilon()) ? false : true; }
    BASE_EXPORT DMatrix<3,3> quatToMatrix() const;
    BASE_EXPORT DVect3 quatToAAngle() const;
    static BASE_EXPORT Quat3 fromVect(const DVect3 &v);
    BASE_EXPORT void fromMatrix(const DMatrix<3,3> &);
    BASE_EXPORT void fromAxisAngle(const DVect3 &,const double &);
    BASE_EXPORT void fromEuler(const DVect3 &);
    BASE_EXPORT void fromEulerZXZ(const DVect3 &);
    BASE_EXPORT void incrementAxisAngle(const DVect3 &,const double &);
    BASE_EXPORT void spinAboutX(const double &);
    BASE_EXPORT void spinAboutY(const double &);
    BASE_EXPORT void spinAboutZ(const double &);
    BASE_EXPORT DVect3 rotate(const DVect3 &v) const;
    BASE_EXPORT DVect3 real() const { return DVect3(i_,j_,k_); }
 
    BASE_EXPORT void fromUnitAxes(const DVect3 &,const DVect3 &);
    BASE_EXPORT void increment(const DAVect3 &);
    BASE_EXPORT DVect3 e1() const;
    BASE_EXPORT DVect3 e2() const;
    BASE_EXPORT DVect3 e3() const;
    BASE_EXPORT DVect3 quatToEuler() const;
 
    void conj() { i_=-i_; j_=-j_; k_=-k_; }
    Quat3 getConj() const { Quat3 q(w_,-i_,-j_,-k_); return q; }
    BASE_EXPORT void normalize() { double m=1.0/std::sqrt(w_*w_+i_*i_+j_*j_+k_*k_); w_*=m; i_*=m; j_*=m; k_*=m; }
 
    Quat3 operator *(const double &d) const { Quat3 q(w_*d,i_*d,j_*d,k_*d); return q; }
    Quat3 operator /(const double &d) const { Quat3 q(w_/d,i_/d,j_/d,k_/d); return q; }
    friend Quat3 operator *(const double &d,const Quat3 &p2) {Quat3 q(p2*d); return q;}
    Quat3 operator *(const Quat3 &v) const { Quat3 q(w_*v.w_-i_*v.i_-j_*v.j_-k_*v.k_,w_*v.i_+i_*v.w_+j_*v.k_-k_*v.j_,w_*v.j_-i_*v.k_+j_*v.w_+k_*v.i_,w_*v.k_+i_*v.j_-j_*v.i_+k_*v.w_); return q; }
    Quat3 operator +(const Quat3 &v) const { Quat3 q(w_+v.w_,i_+v.i_,j_+v.j_,k_+v.k_); return q; }
    Quat3 friend operator *(const DVect3 &v,const Quat3 &q) {Quat3 q2(0.0,v.x(),v.y(),v.z()); return q2*q;}
    Quat3 operator *(const DVect3 &v) const {Quat3 q2(0.0,v.x(),v.y(),v.z()); return *this*q2;}
 
    const Quat3 & operator *=(const double &d) { w_ *= d; i_ *= d; j_ *= d; k_ *= d; return *this; }
    const Quat3 & operator /=(const double &d) { w_ /= d; i_ /= d; j_ /= d; k_ /= d; return *this; }
    const Quat3 & operator *=(const Quat3 &v) { double tw(w_*v.w_-i_*v.i_-j_*v.j_-k_*v.k_);
    double ti(w_*v.i_+i_*v.w_+j_*v.k_-k_*v.j_);
    double tj(w_*v.j_-i_*v.k_+j_*v.w_+k_*v.i_);
    double tk(w_*v.k_+i_*v.j_-j_*v.i_+k_*v.w_);
    //double tw(v.w_*w_-v.i_*i_-v.j_*j_-v.k_*k_);
    //double ti(v.w_*i_+v.i_*w_+v.j_*k_-v.k_*j_);
    //double tj(v.w_*j_-v.i_*k_+v.j_*w_+v.k_*i_);
    //double tk(v.w_*k_+v.i_*j_-v.j_*i_+v.k_*w_);
    w_ = tw; i_ = ti; j_ = tj; k_ = tk;
    return *this; }
    const Quat3 & operator +=(const Quat3 &v) { w_ += v.w_; i_ += v.i_; j_ += v.j_; k_ += v.k_; return *this; }
    bool operator ==(const Quat3 &v) const { return (w_ == v.w_ && i_==v.i_ && j_==v.j_ && k_==v.k_); }
 
private:
    double w_; //the real amplitude part
    double i_,j_,k_; //the imaginary parts
};
 
constexpr std::partial_ordering Quat3::operator<=>(const Quat3 &in) const {
    auto i = w_ <=> in.w_;
    if (i!=std::partial_ordering::equivalent) return i;
    i = i_ <=> in.i_;
    if (i!=std::partial_ordering::equivalent) return i;
    i = j_ <=> in.j_;
    if (i!=std::partial_ordering::equivalent) return i;
    return k_ <=> in.k_;
}
 
namespace base {
    BASE_EXPORT string ts(const Quat2 &q, int width=0, char notation = '\0', int precision = -1, char fill = ' ');
 
    BASE_EXPORT string ts(const Quat3 &q, int width=0, char notation = '\0', int precision = -1, char fill = ' ');
}