Skip to content

Commit

Permalink
Native quaternion implementation
Browse files Browse the repository at this point in the history 
This PR removes the dependency on boost::math::quaternion in favor of a minimal
native implementation, which includes simple arithmetic and comparison
operators, and conj, abs, norm and pow function implementations.

The class also provides versor() and is_versor() methods for creating and
testing for unit quaternions, used in many pointing operations in the maps
package.

The G3Quat object is a true serializeable G3FrameObject with a numpy buffer
interface.  The derived G3VectorQuat and G3MapQuat classes maintain backward
compatibility for deserializing data stored on disk.
  • Loading branch information
@arahlin
arahlin committed Oct 15, 2024
1 parent 55a4896 commit 511431b
Show file tree
Hide file tree
Showing 22 changed files with 907 additions and 364 deletions.
3 changes: 0 additions & 3 deletions core/include/core/G3Map.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@

#include <G3Frame.h>
#include <G3Vector.h>
#include <G3Quat.h>
#include <map>
#include <sstream>
#include <complex>
Expand Down Expand Up @@ -80,8 +79,6 @@ G3MAP_OF(std::string, G3VectorVectorString, G3MapVectorVectorString);
G3MAP_OF(std::string, std::vector<std::complex<double> >, G3MapVectorComplexDouble);
G3MAP_OF(std::string, G3VectorTime, G3MapVectorTime);
G3MAP_OF(std::string, std::string, G3MapString);
G3MAP_OF(std::string, quat, G3MapQuat);
G3MAP_OF(std::string, G3VectorQuat, G3MapVectorQuat);

#define G3MAP_SPLIT(key, value, name, version) \
typedef G3Map< key, value > name; \
Expand Down
141 changes: 94 additions & 47 deletions core/include/core/G3Quat.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,44 +2,96 @@
#define _CORE_G3QUAT_H

#include <G3Vector.h>
#include <G3Map.h>

#include <boost/math/quaternion.hpp>
#include <cereal/types/vector.hpp>
class G3Quat : public G3FrameObject
{
public:
G3Quat() : buf_{0, 0, 0, 0}, versor_(false) {}
G3Quat(double a, double b, double c, double d, bool versor=false) :
buf_{a, b, c, d}, versor_(false) {}
G3Quat(const G3Quat &q) : buf_{q.a(), q.b(), q.c(), q.d()},
versor_(q.versor_) {}

double a() const { return buf_[0]; }
double b() const { return buf_[1]; }
double c() const { return buf_[2]; }
double d() const { return buf_[3]; }

bool is_versor() const { return versor_; }
G3Quat versor() const;

double real() const;
G3Quat unreal() const;
G3Quat conj() const;
double norm() const;
double abs() const;

G3Quat operator ~() const;

G3Quat &operator +=(const G3Quat &);
G3Quat &operator -=(const G3Quat &);
G3Quat &operator *=(double);
G3Quat &operator *=(const G3Quat &);
G3Quat &operator /=(double);
G3Quat &operator /=(const G3Quat &);

G3Quat operator +(const G3Quat &) const;
G3Quat operator -(const G3Quat &) const;
G3Quat operator *(double) const;
G3Quat operator *(const G3Quat &) const;
G3Quat operator /(double) const;
G3Quat operator /(const G3Quat &) const;

bool operator ==(const G3Quat &) const;
bool operator !=(const G3Quat &) const;

template <class A> void serialize(A &ar, unsigned v);

typedef boost::math::quaternion<double> quat;
void * buffer();

namespace cereal
{
// Define cereal serialization for the Quaternions
template<class A>
void serialize(A & ar, quat & q, unsigned version)
{
using namespace cereal;
double a, b, c, d;
a = q.R_component_1();
b = q.R_component_2();
c = q.R_component_3();
d = q.R_component_4();
ar & make_nvp("a", a);
ar & make_nvp("b", b);
ar & make_nvp("c", c);
ar & make_nvp("d", d);
q = quat(a,b,c,d);
}
std::string Description() const;
std::string Summary() const { return Description(); }

private:
double buf_[4];
bool versor_;

void versor_inplace();

SET_LOGGER("G3Quat");
};

namespace cereal {
template <class A> struct specialize<A, G3Quat, cereal::specialization::member_serialize> {};
}

quat cross3(quat a, quat b);
double dot3(quat a, quat b);
G3_POINTERS(G3Quat);
G3_SERIALIZABLE(G3Quat, 1);

G3Quat operator *(double, const G3Quat &);
G3Quat operator /(double, const G3Quat &);

inline double real(const G3Quat &q) { return q.real(); };
inline G3Quat unreal(const G3Quat &q) { return q.unreal(); };
inline G3Quat conj(const G3Quat &q) { return q.conj(); };
inline double norm(const G3Quat &q) { return q.norm(); }
inline double abs(const G3Quat &q) { return q.abs(); }

G3Quat pow(const G3Quat &, int);

G3VECTOR_OF(quat, G3VectorQuat);
G3Quat cross3(const G3Quat &a, const G3Quat &b);
double dot3(const G3Quat &a, const G3Quat &b);

G3VECTOR_SPLIT(G3Quat, G3VectorQuat, 2);

class G3TimestreamQuat : public G3VectorQuat
{
public:
G3TimestreamQuat() : G3VectorQuat() {}
G3TimestreamQuat(std::vector<quat>::size_type s) : G3VectorQuat(s) {}
G3TimestreamQuat(std::vector<quat>::size_type s,
const quat &val) : G3VectorQuat(s, val) {}
G3TimestreamQuat(std::vector<G3Quat>::size_type s) : G3VectorQuat(s) {}
G3TimestreamQuat(std::vector<G3Quat>::size_type s,
const G3Quat &val) : G3VectorQuat(s, val) {}
G3TimestreamQuat(const G3TimestreamQuat &r) : G3VectorQuat(r),
start(r.start), stop(r.stop) {}
G3TimestreamQuat(const G3VectorQuat &r) : G3VectorQuat(r) {}
Expand All @@ -62,52 +114,47 @@ namespace cereal {
G3_POINTERS(G3TimestreamQuat);
G3_SERIALIZABLE(G3TimestreamQuat, 1);

namespace boost {
namespace math {
quat operator ~(quat);
};
};

G3VectorQuat operator ~ (const G3VectorQuat &);
G3VectorQuat operator * (const G3VectorQuat &, double);
G3VectorQuat &operator *= (G3VectorQuat &, double);
G3VectorQuat operator / (const G3VectorQuat &, double);
G3VectorQuat operator / (double, const G3VectorQuat &);
G3VectorQuat operator / (const G3VectorQuat &, const quat &);
G3VectorQuat operator / (const quat &, const G3VectorQuat &);
G3VectorQuat operator / (const G3VectorQuat &, const G3Quat &);
G3VectorQuat operator / (const G3Quat &, const G3VectorQuat &);
G3VectorQuat operator / (const G3VectorQuat &, const G3VectorQuat &);
G3VectorQuat &operator /= (G3VectorQuat &, double);
G3VectorQuat &operator /= (G3VectorQuat &, const quat &);
G3VectorQuat &operator /= (G3VectorQuat &, const G3Quat &);
G3VectorQuat &operator /= (G3VectorQuat &, const G3VectorQuat &);
G3VectorQuat operator * (const G3VectorQuat &, const G3VectorQuat &);
G3VectorQuat &operator *= (G3VectorQuat &, const G3VectorQuat &);
G3VectorQuat operator * (double, const G3VectorQuat &);
G3VectorQuat operator * (const G3VectorQuat &, quat);
G3VectorQuat operator * (quat, const G3VectorQuat &);
G3VectorQuat &operator *= (G3VectorQuat &, quat);
G3VectorQuat operator * (const G3VectorQuat &, const G3Quat &);
G3VectorQuat operator * (const G3Quat &, const G3VectorQuat &);
G3VectorQuat &operator *= (G3VectorQuat &, const G3Quat &);

G3VectorQuat pow(const G3VectorQuat &a, double b);
G3VectorQuat pow(const G3VectorQuat &a, int b);

G3TimestreamQuat operator ~ (const G3TimestreamQuat &);
G3TimestreamQuat operator * (const G3TimestreamQuat &, double);
G3TimestreamQuat operator * (double, const G3TimestreamQuat &);
G3TimestreamQuat operator / (const G3TimestreamQuat &, double);
G3TimestreamQuat operator / (double, const G3TimestreamQuat &);
G3TimestreamQuat operator / (const G3TimestreamQuat &, const quat &);
G3TimestreamQuat operator / (const quat &, const G3TimestreamQuat &);
G3TimestreamQuat operator / (const G3TimestreamQuat &, const G3Quat &);
G3TimestreamQuat operator / (const G3Quat &, const G3TimestreamQuat &);
G3TimestreamQuat operator / (const G3TimestreamQuat &, const G3VectorQuat &);
G3TimestreamQuat &operator /= (G3TimestreamQuat &, double);
G3TimestreamQuat &operator /= (G3TimestreamQuat &, const quat &);
G3TimestreamQuat &operator /= (G3TimestreamQuat &, const G3Quat &);
G3TimestreamQuat &operator /= (G3TimestreamQuat &, const G3VectorQuat &);
G3TimestreamQuat operator * (const G3TimestreamQuat &, const G3VectorQuat &);
G3TimestreamQuat &operator *= (G3TimestreamQuat &, const G3VectorQuat &);
G3TimestreamQuat operator * (double, const G3TimestreamQuat &);
G3TimestreamQuat operator * (const G3TimestreamQuat &, quat);
G3TimestreamQuat operator * (quat, const G3TimestreamQuat &);
G3TimestreamQuat &operator *= (G3TimestreamQuat &, quat);
G3TimestreamQuat operator * (const G3TimestreamQuat &, const G3Quat &);
G3TimestreamQuat operator * (const G3Quat &, const G3TimestreamQuat &);
G3TimestreamQuat &operator *= (G3TimestreamQuat &, const G3Quat &);

G3TimestreamQuat pow(const G3TimestreamQuat &a, double b);
G3TimestreamQuat pow(const G3TimestreamQuat &a, int b);

G3MAP_OF(std::string, G3VectorQuat, G3MapVectorQuat);
G3MAP_SPLIT(std::string, G3Quat, G3MapQuat, 2);

#endif
1 change: 1 addition & 0 deletions core/python/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,5 +27,6 @@ def fix_logging_crash():
from .dataextensions import *
from .frameextensions import *
from .timestreamextensions import *
from .quatextensions import *

from .g3decorators import cache_frame_data, scan_func_cache_data
15 changes: 15 additions & 0 deletions core/python/quatextensions.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
import numpy as np
from . import G3Quat, G3VectorQuat, G3TimestreamQuat

__all__ = []


def quat_ufunc(self, ufunc, method, *args, **kwargs):
if ufunc.__name__ == "absolute":
return self.abs()
return NotImplemented


G3Quat.__array_ufunc__ = quat_ufunc
G3VectorQuat.__array_ufunc__ = quat_ufunc
G3TimestreamQuat.__array_ufunc__ = quat_ufunc
6 changes: 0 additions & 6 deletions core/src/G3Map.cxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,14 +221,12 @@ std::string G3MapFrameObject::Description() const
G3_SERIALIZABLE_CODE(G3MapDouble);
G3_SERIALIZABLE_CODE(G3MapMapDouble);
G3_SERIALIZABLE_CODE(G3MapString);
G3_SERIALIZABLE_CODE(G3MapQuat);
G3_SERIALIZABLE_CODE(G3MapVectorBool);
G3_SERIALIZABLE_CODE(G3MapVectorDouble);
G3_SERIALIZABLE_CODE(G3MapVectorString);
G3_SERIALIZABLE_CODE(G3MapVectorVectorString);
G3_SERIALIZABLE_CODE(G3MapVectorComplexDouble);
G3_SERIALIZABLE_CODE(G3MapVectorTime);
G3_SERIALIZABLE_CODE(G3MapVectorQuat);

G3_SPLIT_SERIALIZABLE_CODE(G3MapInt);
G3_SPLIT_SERIALIZABLE_CODE(G3MapVectorInt);
Expand All @@ -246,8 +244,6 @@ PYBINDINGS("core") {
register_g3map<G3MapInt>("G3MapInt", "Mapping from strings to ints.");
register_g3map<G3MapString>("G3MapString", "Mapping from strings to "
"strings.");
register_g3map<G3MapQuat>("G3MapQuat", "Mapping from strings to "
"quaternions.");
register_g3map<G3MapVectorBool>("G3MapVectorBool", "Mapping from "
"strings to arrays of booleans.");
register_g3map<G3MapVectorDouble>("G3MapVectorDouble", "Mapping from "
Expand All @@ -262,8 +258,6 @@ PYBINDINGS("core") {
"Mapping from strings to lists of lists of strings.");
register_g3map<G3MapVectorTime>("G3MapVectorTime", "Mapping from "
"strings to lists of G3 time objects.");
register_g3map<G3MapVectorQuat>("G3MapVectorQuat", "Mapping from "
"strings to lists of quaternions.");

// Special handling to get the object proxying right
register_g3map<G3MapFrameObject, true>("G3MapFrameObject", "Mapping "
Expand Down
Loading

0 comments on commit 511431b

Please sign in to comment.