diff --git a/core/include/core/G3Map.h b/core/include/core/G3Map.h index 830e82d0..d63ecc52 100644 --- a/core/include/core/G3Map.h +++ b/core/include/core/G3Map.h @@ -3,7 +3,6 @@ #include #include -#include #include #include #include @@ -80,8 +79,6 @@ G3MAP_OF(std::string, G3VectorVectorString, G3MapVectorVectorString); G3MAP_OF(std::string, std::vector >, 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; \ diff --git a/core/include/core/G3Quat.h b/core/include/core/G3Quat.h index ced25269..cc8efffe 100644 --- a/core/include/core/G3Quat.h +++ b/core/include/core/G3Quat.h @@ -2,44 +2,104 @@ #define _CORE_G3QUAT_H #include +#include -#include -#include +class Quat +{ +public: + Quat() : a_(0), b_(0), c_(0), d_(0) {} + Quat(double a, double b, double c, double d) : + a_(a), b_(b), c_(c), d_(d) {} + Quat(const Quat &q) : a_(q.a_), b_(q.b_), c_(q.c_), d_(q.d_) {} + + double a() const { return a_; } + double b() const { return b_; } + double c() const { return c_; } + double d() const { return d_; } + + double real() const; + Quat unreal() const; + Quat conj() const; + double norm() const; + double vnorm() const; + double abs() const; + double dot3(const Quat &b) const; + Quat cross3(const Quat &b) const; + + Quat operator -() const; + Quat operator ~() const; + + Quat &operator +=(const Quat &); + Quat &operator -=(const Quat &); + Quat &operator *=(double); + Quat &operator *=(const Quat &); + Quat &operator /=(double); + Quat &operator /=(const Quat &); + + Quat operator +(const Quat &) const; + Quat operator -(const Quat &) const; + Quat operator *(double) const; + Quat operator *(const Quat &) const; + Quat operator /(double) const; + Quat operator /(const Quat &) const; + + bool operator ==(const Quat &) const; + bool operator !=(const Quat &) const; -typedef boost::math::quaternion quat; + template void serialize(A &ar, unsigned v); +private: + double a_, b_, c_, d_; +}; -namespace cereal -{ -// Define cereal serialization for the Quaternions -template -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::ostream& operator<<(std::ostream& os, const Quat &); + +namespace cereal { + template struct specialize {}; } -quat cross3(quat a, quat b); -double dot3(quat a, quat b); +CEREAL_CLASS_VERSION(Quat, 1); + +Quat operator *(double, const Quat &); +Quat operator /(double, const Quat &); + +inline double real(const Quat &q) { return q.real(); }; +inline Quat unreal(const Quat &q) { return q.unreal(); }; +inline Quat conj(const Quat &q) { return q.conj(); }; +inline double norm(const Quat &q) { return q.norm(); } +inline double vnorm(const Quat &q) { return q.vnorm(); } +inline double abs(const Quat &q) { return q.abs(); } -G3VECTOR_OF(quat, G3VectorQuat); +Quat pow(const Quat &, int); + +Quat cross3(const Quat &a, const Quat &b); +double dot3(const Quat &a, const Quat &b); + +// Frame object data wrapper + +class G3Quat : public G3FrameObject { +public: + Quat value; + + G3Quat() {} + G3Quat(const Quat &val) : value(val) {} + + template void serialize(A &ar, unsigned v); + std::string Description() const; + bool operator==(const G3Quat & other) const {return value == other.value;} +}; + +G3_POINTERS(G3Quat); +G3_SERIALIZABLE(G3Quat, 1); + +G3VECTOR_OF(Quat, G3VectorQuat); class G3TimestreamQuat : public G3VectorQuat { public: G3TimestreamQuat() : G3VectorQuat() {} - G3TimestreamQuat(std::vector::size_type s) : G3VectorQuat(s) {} - G3TimestreamQuat(std::vector::size_type s, - const quat &val) : G3VectorQuat(s, val) {} + G3TimestreamQuat(std::vector::size_type s) : G3VectorQuat(s) {} + G3TimestreamQuat(std::vector::size_type s, + const Quat &val) : G3VectorQuat(s, val) {} G3TimestreamQuat(const G3TimestreamQuat &r) : G3VectorQuat(r), start(r.start), stop(r.stop) {} G3TimestreamQuat(const G3VectorQuat &r) : G3VectorQuat(r) {} @@ -62,31 +122,24 @@ 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 Quat &); +G3VectorQuat operator / (const Quat &, const G3VectorQuat &); G3VectorQuat operator / (const G3VectorQuat &, const G3VectorQuat &); G3VectorQuat &operator /= (G3VectorQuat &, double); -G3VectorQuat &operator /= (G3VectorQuat &, const quat &); +G3VectorQuat &operator /= (G3VectorQuat &, const Quat &); 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 Quat &); +G3VectorQuat operator * (const Quat &, const G3VectorQuat &); +G3VectorQuat &operator *= (G3VectorQuat &, const Quat &); -G3VectorQuat pow(const G3VectorQuat &a, double b); G3VectorQuat pow(const G3VectorQuat &a, int b); G3TimestreamQuat operator ~ (const G3TimestreamQuat &); @@ -94,20 +147,22 @@ 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 Quat &); +G3TimestreamQuat operator / (const Quat &, const G3TimestreamQuat &); G3TimestreamQuat operator / (const G3TimestreamQuat &, const G3VectorQuat &); G3TimestreamQuat &operator /= (G3TimestreamQuat &, double); -G3TimestreamQuat &operator /= (G3TimestreamQuat &, const quat &); +G3TimestreamQuat &operator /= (G3TimestreamQuat &, const Quat &); 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 Quat &); +G3TimestreamQuat operator * (const Quat &, const G3TimestreamQuat &); +G3TimestreamQuat &operator *= (G3TimestreamQuat &, const Quat &); -G3TimestreamQuat pow(const G3TimestreamQuat &a, double b); G3TimestreamQuat pow(const G3TimestreamQuat &a, int b); +G3MAP_OF(std::string, G3VectorQuat, G3MapVectorQuat); +G3MAP_OF(std::string, Quat, G3MapQuat); + #endif diff --git a/core/python/__init__.py b/core/python/__init__.py index 86e4b6de..e7e26bb7 100644 --- a/core/python/__init__.py +++ b/core/python/__init__.py @@ -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 diff --git a/core/python/quatextensions.py b/core/python/quatextensions.py new file mode 100644 index 00000000..353c6fb9 --- /dev/null +++ b/core/python/quatextensions.py @@ -0,0 +1,42 @@ +import numpy as np +from . import Quat, G3VectorQuat, G3TimestreamQuat + +__all__ = [] + +quat_types = (Quat, G3VectorQuat, G3TimestreamQuat) + + +def quat_ufunc(self, ufunc, method, *inputs, **kwargs): + """Numpy ufunc interface for vectorized quaternion methods.""" + if ufunc.__name__ in ["isinf", "isnan", "isfinite"] and len(inputs) == 1: + v = getattr(ufunc, method)(np.asarray(inputs[0]), **kwargs) + if ufunc.__name__ == "isfinite": + return np.all(v, axis=-1) + return np.any(v, axis=-1) + if ufunc.__name__.startswith("logical"): + args = [] + for arg in inputs: + if isinstance(arg, quat_types): + arg = np.any(np.asarray(arg), axis=-1) + args.append(arg) + return getattr(ufunc, method)(*args, **kwargs) + if method != "__call__" or kwargs: + return NotImplemented + if len(inputs) == 1: + if ufunc.__name__ == "absolute": + return self.abs() + if ufunc.__name__ == "negative": + return self.__neg__() + if ufunc.__name__ == "conjugate": + return self.conj() + if ufunc.__name__ == "reciprocal": + return Quat(1, 0, 0, 0) / self + if len(inputs) == 2 and np.isscalar(inputs[1]): + if ufunc.__name__ == "power": + return self.__pow__(inputs[1]) + return NotImplemented + + +Quat.__array_ufunc__ = quat_ufunc +G3VectorQuat.__array_ufunc__ = quat_ufunc +G3TimestreamQuat.__array_ufunc__ = quat_ufunc diff --git a/core/src/G3Frame.cxx b/core/src/G3Frame.cxx index 8beb40bf..12100377 100644 --- a/core/src/G3Frame.cxx +++ b/core/src/G3Frame.cxx @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -493,6 +494,12 @@ static void g3frame_python_put(G3Frame &f, std::string name, bp::object obj) return; } + bp::extract extquat(obj); + if (extquat.check()) { + f.Put(name, boost::make_shared(extquat())); + return; + } + bp::extract extstr(obj); if (extstr.check()) f.Put(name, boost::make_shared(extstr())); @@ -520,6 +527,8 @@ static bp::object g3frame_python_get(G3Frame &f, std::string name) return bp::object(boost::dynamic_pointer_cast(element)->value); else if (!!boost::dynamic_pointer_cast(element)) return bp::object(boost::dynamic_pointer_cast(element)->value); + else if (!!boost::dynamic_pointer_cast(element)) + return bp::object(boost::dynamic_pointer_cast(element)->value); else return bp::object(boost::const_pointer_cast(element)); } diff --git a/core/src/G3Map.cxx b/core/src/G3Map.cxx index 389990ee..e17377a3 100644 --- a/core/src/G3Map.cxx +++ b/core/src/G3Map.cxx @@ -220,14 +220,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); @@ -245,8 +243,6 @@ PYBINDINGS("core") { register_g3map("G3MapInt", "Mapping from strings to ints."); register_g3map("G3MapString", "Mapping from strings to " "strings."); - register_g3map("G3MapQuat", "Mapping from strings to " - "quaternions."); register_g3map("G3MapVectorBool", "Mapping from " "strings to arrays of booleans."); register_g3map("G3MapVectorDouble", "Mapping from " @@ -261,8 +257,6 @@ PYBINDINGS("core") { "Mapping from strings to lists of lists of strings."); register_g3map("G3MapVectorTime", "Mapping from " "strings to lists of G3 time objects."); - register_g3map("G3MapVectorQuat", "Mapping from " - "strings to lists of quaternions."); // Special handling to get the object proxying right register_g3map("G3MapFrameObject", "Mapping " diff --git a/core/src/G3Quat.cxx b/core/src/G3Quat.cxx index 4f1261e0..1895b0c2 100644 --- a/core/src/G3Quat.cxx +++ b/core/src/G3Quat.cxx @@ -3,55 +3,335 @@ #include #include #include +#include // Quaternion utilities -quat -cross3(quat u, quat v) +std::ostream& +operator<<(std::ostream& os, const Quat &q) +{ + os << "(" << q.a() << ", " << q.b() << ", " << q.c() << ", " << q.d() << ")"; + return os; +} + +template +void +Quat::serialize(A &ar, const unsigned v) +{ + G3_CHECK_VERSION(v); + + ar & cereal::make_nvp("a", a_); + ar & cereal::make_nvp("b", b_); + ar & cereal::make_nvp("c", c_); + ar & cereal::make_nvp("d", d_); +} + +std::string +G3Quat::Description() const +{ + std::ostringstream oss; + oss << value; + return oss.str(); +} + +template +void +G3Quat::serialize(A &ar, unsigned v) +{ + G3_CHECK_VERSION(v); + + ar & cereal::make_nvp("G3FrameObject", + cereal::base_class(this)); + ar & cereal::make_nvp("value", value); +} + +double +Quat::real() const +{ + return a_; +} + +Quat +Quat::unreal() const +{ + if (!a_) + return *this; + return Quat(0, b_, c_, d_); +} + +Quat +Quat::conj() const +{ + return Quat(a_, -b_, -c_, -d_); +} + +double +Quat::norm() const +{ + return a_ * a_ + b_ * b_ + c_ * c_ + d_ * d_; +} + +double +Quat::vnorm() const +{ + return b_ * b_ + c_ * c_ + d_ * d_; +} + +double +Quat::abs() const +{ + return sqrt(norm()); +} + +Quat +Quat::operator -() const +{ + return Quat(-a_, -b_, -c_, -d_); +} + +Quat +Quat::operator ~() const +{ + return conj(); +} + +Quat & +Quat::operator +=(const Quat &rhs) +{ + a_ += rhs.a_; + b_ += rhs.b_; + c_ += rhs.c_; + d_ += rhs.d_; + return *this; +} + +Quat & +Quat::operator -=(const Quat &rhs) +{ + a_ -= rhs.a_; + b_ -= rhs.b_; + c_ -= rhs.c_; + d_ -= rhs.d_; + return *this; +} + +Quat & +Quat::operator *=(double rhs) +{ + a_ *= rhs; + b_ *= rhs; + c_ *= rhs; + d_ *= rhs; + return *this; +} + +Quat & +Quat::operator *=(const Quat &rhs) +{ + double a = a_ * rhs.a_ - b_ * rhs.b_ - c_ * rhs.c_ - d_ * rhs.d_; + double b = a_ * rhs.b_ + b_ * rhs.a_ + c_ * rhs.d_ - d_ * rhs.c_; + double c = a_ * rhs.c_ - b_ * rhs.d_ + c_ * rhs.a_ + d_ * rhs.b_; + double d = a_ * rhs.d_ + b_ * rhs.c_ - c_ * rhs.b_ + d_ * rhs.a_; + a_ = a; + b_ = b; + c_ = c; + d_ = d; + return *this; +} + +Quat & +Quat::operator /=(double rhs) +{ + a_ /= rhs; + b_ /= rhs; + c_ /= rhs; + d_ /= rhs; + return *this; +} + +Quat & +Quat::operator /=(const Quat &rhs) +{ + double n = rhs.norm(); + double a = a_ * rhs.a_ + b_ * rhs.b_ + c_ * rhs.c_ + d_ * rhs.d_; + double b = -a_ * rhs.b_ + b_ * rhs.a_ - c_ * rhs.d_ + d_ * rhs.c_; + double c = -a_ * rhs.c_ + b_ * rhs.d_ + c_ * rhs.a_ - d_ * rhs.b_; + double d = -a_ * rhs.d_ - b_ * rhs.c_ + c_ * rhs.b_ + d_ * rhs.a_; + a_ = a / n; + b_ = b / n; + c_ = c / n; + d_ = d / n; + return *this; +} + +Quat +Quat::operator +(const Quat &rhs) const +{ + return Quat(a_ + rhs.a_, b_ + rhs.b_, c_ + rhs.c_, d_ + rhs.d_); +} + +Quat +Quat::operator -(const Quat &rhs) const +{ + return Quat(a_ - rhs.a_, b_ - rhs.b_, c_ - rhs.c_, d_ - rhs.d_); +} + +Quat +Quat::operator *(double rhs) const +{ + return Quat(a_ * rhs, b_ * rhs, c_ * rhs, d_ * rhs); +} + +Quat +Quat::operator *(const Quat &rhs) const +{ + Quat out(*this); + out *= rhs; + return out; +} + +Quat +operator *(double a, const Quat &b) +{ + return b * a; +} + +Quat +Quat::operator /(double rhs) const +{ + return Quat(a_ / rhs, b_ / rhs, c_ / rhs, d_ / rhs); +} + +Quat +Quat::operator /(const Quat &rhs) const +{ + Quat out(*this); + out /= rhs; + return out; +} + +Quat +operator /(double a, const Quat &b) +{ + return Quat(a, 0, 0, 0) / b; +} + +bool +Quat::operator ==(const Quat &rhs) const +{ + return ((a_ == rhs.a_) && (b_ == rhs.b_) && + (c_ == rhs.c_) && (d_ == rhs.d_)); +} + +bool +Quat::operator !=(const Quat &rhs) const +{ + return !(*this == rhs); +} + +Quat +pow(const Quat &q, int n) +{ + if (n > 1) { + int m = (n >> 1); + Quat r = pow(q, m); + r *= r; + // n odd + if (n & 1) + r *= q; + return r; + } + + if (n == 1) + return q; + + if (n == 0) + return Quat(1, 0, 0, 0); + + // n < 0 + return pow(Quat(1, 0, 0, 0) / q, -n); +} + +Quat +Quat::cross3(const Quat &v) const { // Computes Euclidean cross product from the last three entries in the // quaternion - return quat( - 0, - u.R_component_3()*v.R_component_4() - (u.R_component_4()*v.R_component_3()), - u.R_component_4()*v.R_component_2() - (u.R_component_2()*v.R_component_4()), - u.R_component_2()*v.R_component_3() - (u.R_component_3()*v.R_component_2())); + return Quat(0, + c_ * v.d_ - d_ * v.c_, + d_ * v.b_ - b_ * v.d_, + b_ * v.c_ - c_ * v.b_); +} + +Quat +cross3(const Quat &u, const Quat &v) +{ + return u.cross3(v); } double -dot3(quat a, quat b) +Quat::dot3(const Quat &b) const { // Computes Euclidean dot product from the last three entries in the // quaternion - return (a.R_component_2()*b.R_component_2() + - a.R_component_3()*b.R_component_3() + - a.R_component_4()*b.R_component_4()); + return (b_ * b.b_ + c_ * b.c_ + d_ * b.d_); } -static double -_abs(const quat &a) +double +dot3(const Quat &a, const Quat &b) { - return sqrt(norm(a)); + return a.dot3(b); } static G3VectorDouble -_vabs(const G3VectorQuat &a) +vec_abs(const G3VectorQuat &a) { G3VectorDouble out(a.size()); for (unsigned i = 0; i < a.size(); i++) - out[i] = _abs(a[i]); + out[i] = abs(a[i]); return out; } -namespace boost { -namespace math { -quat -operator ~(quat a) +static G3VectorQuat +vec_neg(const G3VectorQuat &a) { - return conj(a); + return -1 * a; +} + +static G3VectorDouble +vec_real(const G3VectorQuat &a) +{ + G3VectorDouble out(a.size()); + for (unsigned i = 0; i < a.size(); i++) + out[i] = real(a[i]); + return out; +} + +static G3Timestream +ts_abs(const G3TimestreamQuat &a) +{ + G3Timestream out(a.size()); + out.start = a.start; out.stop = a.stop; + for (unsigned i = 0; i < a.size(); i++) + out[i] = abs(a[i]); + return out; +} + +static G3Timestream +ts_real(const G3TimestreamQuat &a) +{ + G3Timestream out(a.size()); + out.start = a.start; out.stop = a.stop; + for (unsigned i = 0; i < a.size(); i++) + out[i] = real(a[i]); + return out; +} + +static G3TimestreamQuat +ts_neg(const G3TimestreamQuat &a) +{ + return -1 * a; } -}; -}; G3VectorQuat operator ~(const G3VectorQuat &a) @@ -80,7 +360,7 @@ operator *(double b, const G3VectorQuat &a) G3VectorQuat & operator *=(G3VectorQuat &a, double b) { - for (quat &i: a) + for (auto &i: a) i *= b; return a; } @@ -104,7 +384,7 @@ operator /(double a, const G3VectorQuat &b) } G3VectorQuat -operator /(const G3VectorQuat &a, const quat &b) +operator /(const G3VectorQuat &a, const Quat &b) { G3VectorQuat out(a.size()); for (unsigned i = 0; i < a.size(); i++) @@ -113,7 +393,7 @@ operator /(const G3VectorQuat &a, const quat &b) } G3VectorQuat -operator /(const quat &a, const G3VectorQuat &b) +operator /(const Quat &a, const G3VectorQuat &b) { G3VectorQuat out(b.size()); for (unsigned i = 0; i < b.size(); i++) @@ -134,13 +414,13 @@ operator /(const G3VectorQuat &a, const G3VectorQuat &b) G3VectorQuat & operator /=(G3VectorQuat &a, double b) { - for (quat &i: a) + for (auto &i: a) i /= b; return a; } G3VectorQuat & -operator /=(G3VectorQuat &a, const quat &b) +operator /=(G3VectorQuat &a, const Quat &b) { for (unsigned i = 0; i < a.size(); i++) a[i] /= b; @@ -176,7 +456,7 @@ operator *=(G3VectorQuat &a, const G3VectorQuat &b) } G3VectorQuat -operator *(const G3VectorQuat &a, quat b) +operator *(const G3VectorQuat &a, const Quat &b) { G3VectorQuat out(a.size()); for (unsigned i = 0; i < a.size(); i++) @@ -185,7 +465,7 @@ operator *(const G3VectorQuat &a, quat b) } G3VectorQuat -operator *(quat b, const G3VectorQuat &a) +operator *(const Quat &b, const G3VectorQuat &a) { G3VectorQuat out(a.size()); for (unsigned i = 0; i < a.size(); i++) @@ -194,22 +474,13 @@ operator *(quat b, const G3VectorQuat &a) } G3VectorQuat & -operator *=(G3VectorQuat &a, quat b) +operator *=(G3VectorQuat &a, const Quat &b) { - for (quat &i: a) + for (auto &i: a) i *= b; return a; } -G3VectorQuat -pow(const G3VectorQuat &a, double b) -{ - G3VectorQuat out(a.size()); - for (unsigned i = 0; i < a.size(); i++) - out[i] = pow(a[i], b); - return out; -} - G3VectorQuat pow(const G3VectorQuat &a, int b) { @@ -280,7 +551,7 @@ operator *(double b, const G3TimestreamQuat &a) G3TimestreamQuat & operator *=(G3TimestreamQuat &a, double b) { - for (quat &i: a) + for (auto &i: a) i *= b; return a; } @@ -306,7 +577,7 @@ operator /(double a, const G3TimestreamQuat &b) } G3TimestreamQuat -operator /(const G3TimestreamQuat &a, const quat &b) +operator /(const G3TimestreamQuat &a, const Quat &b) { G3TimestreamQuat out(a.size()); out.start = a.start; out.stop = a.stop; @@ -316,7 +587,7 @@ operator /(const G3TimestreamQuat &a, const quat &b) } G3TimestreamQuat -operator /(const quat &a, const G3TimestreamQuat &b) +operator /(const Quat &a, const G3TimestreamQuat &b) { G3TimestreamQuat out(b.size()); out.start = b.start; out.stop = b.stop; @@ -339,13 +610,13 @@ operator /(const G3TimestreamQuat &a, const G3VectorQuat &b) G3TimestreamQuat & operator /=(G3TimestreamQuat &a, double b) { - for (quat &i: a) + for (auto &i: a) i /= b; return a; } G3TimestreamQuat & -operator /=(G3TimestreamQuat &a, const quat &b) +operator /=(G3TimestreamQuat &a, const Quat &b) { for (unsigned i = 0; i < a.size(); i++) a[i] /= b; @@ -382,7 +653,7 @@ operator *=(G3TimestreamQuat &a, const G3VectorQuat &b) } G3TimestreamQuat -operator *(const G3TimestreamQuat &a, quat b) +operator *(const G3TimestreamQuat &a, const Quat &b) { G3TimestreamQuat out(a.size()); out.start = a.start; out.stop = a.stop; @@ -392,7 +663,7 @@ operator *(const G3TimestreamQuat &a, quat b) } G3TimestreamQuat -operator *(quat b, const G3TimestreamQuat &a) +operator *(const Quat &b, const G3TimestreamQuat &a) { G3TimestreamQuat out(a.size()); out.start = a.start; out.stop = a.stop; @@ -402,23 +673,13 @@ operator *(quat b, const G3TimestreamQuat &a) } G3TimestreamQuat & -operator *=(G3TimestreamQuat &a, quat b) +operator *=(G3TimestreamQuat &a, const Quat &b) { - for (quat &i: a) + for (auto &i: a) i *= b; return a; } -G3TimestreamQuat -pow(const G3TimestreamQuat &a, double b) -{ - G3TimestreamQuat out(a.size()); - out.start = a.start; out.stop = a.stop; - for (unsigned i = 0; i < a.size(); i++) - out[i] = pow(a[i], b); - return out; -} - G3TimestreamQuat pow(const G3TimestreamQuat &a, int b) { @@ -430,10 +691,55 @@ pow(const G3TimestreamQuat &a, int b) } +G3_SERIALIZABLE_CODE(Quat); +G3_SERIALIZABLE_CODE(G3Quat); G3_SERIALIZABLE_CODE(G3VectorQuat); G3_SERIALIZABLE_CODE(G3TimestreamQuat); +G3_SERIALIZABLE_CODE(G3MapQuat); +G3_SERIALIZABLE_CODE(G3MapVectorQuat); namespace { +static int +Quat_getbuffer(PyObject *obj, Py_buffer *view, int flags) +{ + if (view == NULL) { + PyErr_SetString(PyExc_ValueError, "NULL view"); + return -1; + } + + view->shape = NULL; + + bp::handle<> self(bp::borrowed(obj)); + bp::object selfobj(self); + bp::extract ext(selfobj); + if (!ext.check()) { + PyErr_SetString(PyExc_ValueError, "Invalid quat"); + view->obj = NULL; + return -1; + } + Quat &q = ext(); + + view->obj = obj; + view->buf = (void*)&q; + view->len = 4 * sizeof(double); + view->readonly = 0; + view->itemsize = sizeof(double); + if (flags & PyBUF_FORMAT) + view->format = (char *)"d"; + else + view->format = NULL; + + view->ndim = 1; + view->internal = NULL; + view->shape = NULL; + view->strides = NULL; + view->suboffsets = NULL; + + Py_INCREF(obj); + + return 0; +} + static int G3VectorQuat_getbuffer(PyObject *obj, Py_buffer *view, int flags) { @@ -481,11 +787,12 @@ G3VectorQuat_getbuffer(PyObject *obj, Py_buffer *view, int flags) return 0; } +static PyBufferProcs quat_bufferprocs; static PyBufferProcs vectorquat_bufferprocs; static PyBufferProcs timestreamquat_bufferprocs; static std::string -quat_str(const quat &q) +quat_str(const Quat &q) { std::ostringstream oss; oss << q; @@ -493,14 +800,63 @@ quat_str(const quat &q) } static std::string -quat_repr(const quat &q) +quat_repr(const Quat &q) { std::ostringstream oss; - oss << "spt3g.core.quat" << q; + oss << "spt3g.core.Quat" << q; return oss.str(); } } +boost::shared_ptr +quat_container_from_object(boost::python::object v) +{ + // There's a chance this is actually a copy operation, so try that first + bp::extract extv(v); + if (extv.check()) + return boost::make_shared(extv()); + + Quat q; + + Py_buffer view; + if (PyObject_GetBuffer(v.ptr(), &view, + PyBUF_FORMAT | PyBUF_STRIDES) == -1) + goto slowpython; + +#define QELEM(t, i) *((t *)((char *)view.buf + i*view.strides[0])) +#define QUATI(t) Quat(QELEM(t, 0), QELEM(t, 1), QELEM(t, 2), QELEM(t, 3)) + + if (view.ndim != 1 || view.shape[0] != 4) { + PyBuffer_Release(&view); + goto slowpython; + } else if (strcmp(view.format, "d") == 0) { + q = QUATI(double); + } else if (strcmp(view.format, "f") == 0) { + q = QUATI(float); + } else if (strcmp(view.format, "i") == 0) { + q = QUATI(int); + } else if (strcmp(view.format, "l") == 0) { + q = QUATI(long); + } else { + PyBuffer_Release(&view); + goto slowpython; + } + PyBuffer_Release(&view); + return boost::make_shared(q); + +#undef QELEM +#undef QUATI + +slowpython: + PyErr_Clear(); + std::vector xv; + boost::python::container_utils::extend_container(xv, v); + if (xv.size() != 4) + throw std::runtime_error("Invalid quat"); + + return boost::make_shared(xv[0], xv[1], xv[2], xv[3]); +} + template boost::shared_ptr quat_vec_container_from_object(boost::python::object v) @@ -517,7 +873,7 @@ quat_vec_container_from_object(boost::python::object v) goto slowpython; #define QELEM(t, i, j) *((t *)((char *)view.buf + i*view.strides[0] + j*view.strides[1])) -#define QUATI(t, i) quat(QELEM(t, i, 0), QELEM(t, i, 1), QELEM(t, i, 2), QELEM(t, i, 3)) +#define QUATI(t, i) Quat(QELEM(t, i, 0), QELEM(t, i, 1), QELEM(t, i, 2), QELEM(t, i, 3)) x->resize(view.shape[0]); if (view.ndim != 2 || view.shape[1] != 4) { @@ -581,14 +937,23 @@ PYBINDINGS("core") { using namespace boost::python; - class_("quat", - "Representation of a quaternion. Data in a,b,c,d.", - init()) - .add_property("a", &quat::R_component_1) - .add_property("b", &quat::R_component_2) - .add_property("c", &quat::R_component_3) - .add_property("d", &quat::R_component_4) + object q = + class_ >("Quat", + "Representation of a quaternion. Data in a,b,c,d.", init<>()) + .def(init()) + .def(init( + "Create a quaternion from its four elements.", args("a", "b", "c", "d"))) + .def_pickle(g3frameobject_picklesuite()) + .def("__init__", make_constructor(quat_container_from_object, default_call_policies(), + (arg("data"))), "Create a quaternion from a numpy array") + .add_property("a", &Quat::a, "Scalar component") + .add_property("b", &Quat::b, "First vector component") + .add_property("c", &Quat::c, "Second vector component") + .add_property("d", &Quat::d, "Third vector component") + .add_property("real", &Quat::real, "The real (scalar) part of the quaternion") + .add_property("unreal", &Quat::unreal, "The unreal (vector) part of the quaternion") .def(~self) + .def(-self) .def(self == self) .def(self != self) .def(self + self) @@ -600,22 +965,35 @@ PYBINDINGS("core") .def(double() * self) .def(self *= self) .def(self *= double()) - .def(pow(self, double())) - .def(pow(self, long())) + .def(pow(self, int())) .def(self / self) .def(self / double()) .def(double() / self) .def(self /= self) .def(self /= double()) - .def("__abs__", _abs) + .def("__abs__", &Quat::abs) .def("__str__", quat_str) .def("__repr__", quat_repr) - .def("dot3", dot3, "Dot product of last three entries") - .def("cross3", cross3, "Cross product of last three entries") + .def("norm", &Quat::norm, "Return the Cayley norm of the quaternion") + .def("vnorm", &Quat::norm, "Return the Cayley norm of the unreal (vector) part of the quaternion") + .def("abs", &Quat::abs, "Return the Euclidean norm of the quaternion") + .def("dot3", &Quat::dot3, "Dot product of last three entries") + .def("cross3", &Quat::cross3, "Cross product of last three entries") ; - register_vector_of("Quat"); + PyTypeObject *qclass = (PyTypeObject *)q.ptr(); + quat_bufferprocs.bf_getbuffer = Quat_getbuffer; + qclass->tp_as_buffer = &quat_bufferprocs; +#if PY_MAJOR_VERSION < 3 + qclass->tp_flags |= Py_TPFLAGS_HAVE_NEWBUFFER; +#endif + + EXPORT_FRAMEOBJECT(G3Quat, init(), "Serializable quaternion") + .def_readwrite("value", &G3Quat::value) + ; + + register_vector_of("Quat"); object vq = - register_g3vector("G3VectorQuat", + register_g3vector("G3VectorQuat", "List of quaternions. Convertible to a 4xN numpy array. " "Arithmetic operations on this object are fast and provide " "results given proper quaternion math rather than " @@ -624,22 +1002,24 @@ PYBINDINGS("core") .def(self * double()) .def(double() * self) .def(self * self) - .def(self * quat()) - .def(quat() * self) + .def(self * Quat()) + .def(Quat() * self) .def(self *= double()) - .def(self *= quat()) + .def(self *= Quat()) .def(self *= self) .def(self / double()) .def(double() / self) .def(self /= double()) .def(self / self) .def(self /= self) - .def(self / quat()) - .def(self /= quat()) - .def(quat() / self) - .def(pow(self, double())) + .def(self / Quat()) + .def(self /= Quat()) + .def(Quat() / self) .def(pow(self, int())) - .def("__abs__", _vabs); + .def("__abs__", vec_abs) + .def("__neg__", vec_neg) + .def("abs", vec_abs, "Return the Euclidean norm of each quaternion") + .add_property("real", vec_real, "Return the real (scalar) part of each quaternion"); PyTypeObject *vqclass = (PyTypeObject *)vq.ptr(); vectorquat_bufferprocs.bf_getbuffer = G3VectorQuat_getbuffer; vqclass->tp_as_buffer = &vectorquat_bufferprocs; @@ -661,22 +1041,24 @@ PYBINDINGS("core") .def(self * double()) .def(double() * self) .def(self * G3VectorQuat()) - .def(self * quat()) - .def(quat() * self) + .def(self * Quat()) + .def(Quat() * self) .def(self *= double()) - .def(self *= quat()) + .def(self *= Quat()) .def(self *= G3VectorQuat()) .def(self / double()) .def(double() / self) .def(self /= double()) .def(self / G3VectorQuat()) .def(self /= G3VectorQuat()) - .def(self / quat()) - .def(self /= quat()) - .def(quat() / self) - .def(pow(self, double())) + .def(self / Quat()) + .def(self /= Quat()) + .def(Quat() / self) .def(pow(self, int())) - .def("__abs__", _vabs) + .def("__abs__", ts_abs) + .def("__neg__", ts_neg) + .def("abs", ts_abs, "Return the Euclidean norm of each quaternion") + .add_property("real", ts_real, "Return the real (scalar) part of each quaternion") .def_readwrite("start", &G3TimestreamQuat::start, "Time of the first sample in the time stream") .def_readwrite("stop", &G3TimestreamQuat::stop, @@ -697,4 +1079,9 @@ PYBINDINGS("core") register_pointer_conversions(); implicitly_convertible(); implicitly_convertible(); + + register_g3map("G3MapQuat", "Mapping from strings to " + "quaternions."); + register_g3map("G3MapVectorQuat", "Mapping from " + "strings to lists of quaternions."); } diff --git a/core/tests/quaternions.py b/core/tests/quaternions.py index d9734e71..522c0939 100755 --- a/core/tests/quaternions.py +++ b/core/tests/quaternions.py @@ -2,8 +2,10 @@ from spt3g import core import numpy as np -a = core.quat(2,3,4,5) +a = core.Quat(2,3,4,5) +assert(a.real == 2) +assert(a.real == np.real(a)) assert(a+a == 2*a) assert(a+a == a*2) assert(a*a == a**2) @@ -23,13 +25,16 @@ assert(b[2].c == 8) +assert((b.real == np.asarray([2, -46, 4])).all()) +assert((b.real == np.real(b)).all()) + c = np.asarray(b) assert(c.shape == (3,4)) -assert(core.quat(*c[0]) == a) -assert(core.quat(*c[1]) == b[1]) -assert(core.quat(*c[1]) != b[2]) +assert(core.Quat(*c[0]) == a) +assert(core.Quat(*c[1]) == b[1]) +assert(core.Quat(*c[1]) != b[2]) d = core.G3VectorQuat(c) @@ -48,9 +53,11 @@ assert((np.asarray(b*b) == np.asarray(core.G3VectorQuat([x**2 for x in b]))).all()) -assert(1./a == core.quat(1.,0,0,0)/a) +assert(1./a == core.Quat(1.,0,0,0)/a) assert(np.allclose(core.G3VectorQuat([1./a]), core.G3VectorQuat([(~a) / abs(a)**2]))) -assert((np.asarray(abs(b) - abs(~b)) == 0).all()) +assert(len(np.abs(b)) == len(b)) +assert((np.abs(b) == np.asarray(abs(b))).all()) +assert(((np.abs(b) - np.abs(~b)) == 0).all()) assert(a/b[0] == (a/b)[0]) assert(b[1]/a == (b/a)[1]) @@ -60,6 +67,12 @@ [0., 0., 0., 0., 0.], # c [18., -23., 5., 0., 0.]]) # d +# numpy slicing and conversions of single quaternions +assert(core.Quat(quats[0, :4]) == core.Quat(*quats[0, :4])) +assert((quats[0, :4] == np.asarray(core.Quat(*quats[0, :4]))).all()) +assert(core.Quat(quats[:, 0]) == core.Quat(*quats[:, 0])) +assert((quats[:, 0] == np.asarray(core.Quat(*quats[:, 0]))).all()) + try: q = core.G3VectorQuat(quats) except TypeError: @@ -70,24 +83,38 @@ # Non-trivial strides q = core.G3VectorQuat(quats[:,:4]) -assert(q[0] == core.quat(*quats[0,:4])) -assert(q[1] == core.quat(*quats[1,:4])) -assert(q[2] == core.quat(*quats[2,:4])) -assert(q[3] == core.quat(*quats[3,:4])) +assert(q[0] == core.Quat(*quats[0,:4])) +assert(q[1] == core.Quat(*quats[1,:4])) +assert(q[2] == core.Quat(*quats[2,:4])) +assert(q[3] == core.Quat(*quats[3,:4])) # When transposed, has right shape to convert q = core.G3VectorQuat(quats.T) # Strides, but simple ones -assert(q[0] == core.quat(1,0,0,18)) +assert(q[0] == core.Quat(*quats[:, 0])) +assert(q[1] == core.Quat(*quats[:, 1])) +assert(q[2] == core.Quat(*quats[:, 2])) +assert(q[3] == core.Quat(*quats[:, 3])) +assert(q[4] == core.Quat(*quats[:, 4])) # Trivial case, packed q = core.G3VectorQuat(quats.T.copy()) -assert(q[0] == core.quat(1,0,0,18)) +assert(q[0] == core.Quat(*quats[:, 0])) +assert(q[1] == core.Quat(*quats[:, 1])) +assert(q[2] == core.Quat(*quats[:, 2])) +assert(q[3] == core.Quat(*quats[:, 3])) +assert(q[4] == core.Quat(*quats[:, 4])) # Test conversion of integers qint = np.asarray(quats[:,:4], dtype='int64') q = core.G3VectorQuat(qint) -assert(q[0] == core.quat(1,2,3,4)) -assert(q[1] == core.quat(0,0,0,3)) -assert(q[3] == core.quat(18, -23, 5, 0)) +assert(q[0] == core.Quat(1,2,3,4)) +assert(q[1] == core.Quat(0,0,0,3)) +assert(q[3] == core.Quat(18, -23, 5, 0)) + +# Test serialization +frame = core.G3Frame() +q = core.Quat(1, 2, 3, 4) +frame["Quat"] = q +assert frame["Quat"] == q diff --git a/maps/include/maps/FlatSkyMap.h b/maps/include/maps/FlatSkyMap.h index 24ec7297..3758e51d 100644 --- a/maps/include/maps/FlatSkyMap.h +++ b/maps/include/maps/FlatSkyMap.h @@ -119,18 +119,18 @@ class FlatSkyMap : public G3FrameObject, public G3SkyMap { std::vector XYToAngle(double x, double y) const; size_t XYToPixel(double x, double y) const; std::vector PixelToXY(size_t pixel) const; - std::vector QuatToXY(quat q) const; - quat XYToQuat(double x, double y) const; - size_t QuatToPixel(quat q) const override; - quat PixelToQuat(size_t pixel) const override; + std::vector QuatToXY(const Quat &q) const; + Quat XYToQuat(double x, double y) const; + size_t QuatToPixel(const Quat &q) const override; + Quat PixelToQuat(size_t pixel) const override; std::vector PixelToAngleGrad(size_t pixel, double h=0.001) const; G3VectorQuat GetRebinQuats(size_t pixel, size_t scale) const override; - void GetInterpPixelsWeights(quat q, std::vector & pixels, + void GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const override; - std::vector QueryDisc(quat q, double radius) const override; + std::vector QueryDisc(const Quat &q, double radius) const override; G3SkyMapPtr Rebin(size_t scale, bool norm = true) const override; G3SkyMapPtr ExtractPatch(size_t x0, size_t y0, size_t width, size_t height, diff --git a/maps/include/maps/FlatSkyProjection.h b/maps/include/maps/FlatSkyProjection.h index 267bf4ee..a86647e4 100644 --- a/maps/include/maps/FlatSkyProjection.h +++ b/maps/include/maps/FlatSkyProjection.h @@ -91,10 +91,10 @@ class FlatSkyProjection : public G3FrameObject { std::vector AngleToXY(double alpha, double delta) const; std::vector PixelToAngle(size_t pixel) const; size_t AngleToPixel(double alpha, double delta) const; - std::vector QuatToXY(quat q) const; - quat XYToQuat(double x, double y) const; - size_t QuatToPixel(quat q) const; - quat PixelToQuat(size_t pixel) const; + std::vector QuatToXY(const Quat &q) const; + Quat XYToQuat(double x, double y) const; + size_t QuatToPixel(const Quat &q) const; + Quat PixelToQuat(size_t pixel) const; std::vector XYToAngleGrad(double x, double y, double h=0.001) const; std::vector PixelToAngleGrad(size_t pixel, double h=0.001) const; @@ -102,10 +102,10 @@ class FlatSkyProjection : public G3FrameObject { size_t RebinPixel(size_t pixel, size_t scale) const; G3VectorQuat GetRebinQuats(size_t pixel, size_t scale) const; - void GetInterpPixelsWeights(quat q, std::vector & pixels, + void GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const; - std::vector QueryDisc(quat q, double radius) const; + std::vector QueryDisc(const Quat &q, double radius) const; FlatSkyProjection Rebin(size_t scale, double x_center = 0.0 / 0.0, double y_center = 0.0 / 0.0) const; @@ -129,7 +129,7 @@ class FlatSkyProjection : public G3FrameObject { bool cyl_; double sindelta0_; double cosdelta0_; - quat q0_; + Quat q0_; SET_LOGGER("FlatSkyProjection"); }; diff --git a/maps/include/maps/G3SkyMap.h b/maps/include/maps/G3SkyMap.h index 8d918ba7..6f87b28a 100644 --- a/maps/include/maps/G3SkyMap.h +++ b/maps/include/maps/G3SkyMap.h @@ -175,8 +175,8 @@ class G3SkyMap { virtual std::vector PixelToAngle(size_t pixel) const; virtual size_t AngleToPixel(double alpha, double delta) const; - virtual quat PixelToQuat(size_t pixel) const = 0; - virtual size_t QuatToPixel(quat q) const = 0; + virtual Quat PixelToQuat(size_t pixel) const = 0; + virtual size_t QuatToPixel(const Quat &q) const = 0; // Rebinning and interpolation virtual void GetRebinAngles(size_t pixel, size_t scale, @@ -184,12 +184,12 @@ class G3SkyMap { virtual G3VectorQuat GetRebinQuats(size_t pixel, size_t scale) const = 0; virtual void GetInterpPixelsWeights(double alpha, double delta, std::vector & pixels, std::vector & weights) const; - virtual void GetInterpPixelsWeights(quat q, std::vector & pixels, + virtual void GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const = 0; double GetInterpPrecalc(const std::vector &pixels, const std::vector &weights) const; double GetInterpValue(double alpha, double delta) const; - double GetInterpValue(quat q) const; + double GetInterpValue(const Quat &q) const; std::vector GetInterpValues(const std::vector &alphas, const std::vector &deltas) const; std::vector GetInterpValues(const G3VectorQuat & quats) const; @@ -198,9 +198,9 @@ class G3SkyMap { /* Analogue to healpy.query_disc, returns list of pixels within a disc */ std::vector QueryDisc(double alpha, double delta, double radius) const; - virtual std::vector QueryDisc(quat q, double radius) const = 0; + virtual std::vector QueryDisc(const Quat &q, double radius) const = 0; std::vector QueryAlphaEllipse(double alpha, double delta, double a, double b) const; - std::vector QueryAlphaEllipse(quat q, double a, double b) const; + std::vector QueryAlphaEllipse(const Quat &q, double a, double b) const; virtual bool IsDense() const { throw std::runtime_error("Checking array density not implemented"); diff --git a/maps/include/maps/HealpixSkyMap.h b/maps/include/maps/HealpixSkyMap.h index 57b1753b..66730867 100644 --- a/maps/include/maps/HealpixSkyMap.h +++ b/maps/include/maps/HealpixSkyMap.h @@ -80,14 +80,14 @@ class HealpixSkyMap : public G3FrameObject, public G3SkyMap { size_t AngleToPixel(double alpha, double delta) const override; std::vector PixelToAngle(size_t pixel) const override; - size_t QuatToPixel(quat q) const override; - quat PixelToQuat(size_t pixel) const override; + size_t QuatToPixel(const Quat &q) const override; + Quat PixelToQuat(size_t pixel) const override; G3VectorQuat GetRebinQuats(size_t pixel, size_t scale) const override; - void GetInterpPixelsWeights(quat q, std::vector & pixels, + void GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const override; - std::vector QueryDisc(quat q, double radius) const override; + std::vector QueryDisc(const Quat &q, double radius) const override; G3SkyMapPtr Rebin(size_t scale, bool norm = true) const override; diff --git a/maps/include/maps/HealpixSkyMapInfo.h b/maps/include/maps/HealpixSkyMapInfo.h index ff57ba4e..6a5d49c4 100644 --- a/maps/include/maps/HealpixSkyMapInfo.h +++ b/maps/include/maps/HealpixSkyMapInfo.h @@ -40,17 +40,17 @@ class HealpixSkyMapInfo : public G3FrameObject { std::vector PixelToAngle(size_t pixel) const; size_t AngleToPixel(double alpha, double delta) const; - quat PixelToQuat(size_t pixel) const; - size_t QuatToPixel(quat q) const; + Quat PixelToQuat(size_t pixel) const; + size_t QuatToPixel(const Quat &q) const; size_t RebinPixel(size_t pixel, size_t scale) const; G3VectorQuat GetRebinQuats(size_t pixel, size_t scale) const; - void GetInterpPixelsWeights(quat q, std::vector & pixels, + void GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const; - std::vector QueryDisc(quat q, double radius) const; + std::vector QueryDisc(const Quat &q, double radius) const; private: // scheme diff --git a/maps/include/maps/pointing.h b/maps/include/maps/pointing.h index 87b27eca..402f2ebf 100644 --- a/maps/include/maps/pointing.h +++ b/maps/include/maps/pointing.h @@ -32,22 +32,22 @@ get_detector_rotation(double x_offset, double y_offset, // Compute a vector quaternion that is the boresight rotated // by the given x and y offsets. -quat offsets_to_quat(double x_offset, double y_offset); +Quat offsets_to_quat(double x_offset, double y_offset); // Conversion functions between sky coordinates and vector quaternions -void quat_to_ang(quat q, double &alpha, double &delta); -quat ang_to_quat(double alpha, double delta); +void quat_to_ang(const Quat &q, double &alpha, double &delta); +Quat ang_to_quat(double alpha, double delta); // Compute the angular separation between two vector quaternions -double quat_ang_sep(quat q0, quat q1); +double quat_ang_sep(const Quat &q0, const Quat &q1); // Compute a rotation quaternion that would rotate the boresight vector // to point in the given sky direction. -quat get_origin_rotator(double alpha, double delta); +Quat get_origin_rotator(double alpha, double delta); // Compute the quaternion for rotating FK5 J2000 coordinates to // Galactic J2000 coordinates -quat get_fk5_j2000_to_gal_quat(); +Quat get_fk5_j2000_to_gal_quat(); #endif diff --git a/maps/python/quathelpers.py b/maps/python/quathelpers.py index d2e884f4..c22cc867 100644 --- a/maps/python/quathelpers.py +++ b/maps/python/quathelpers.py @@ -1,4 +1,4 @@ -from ..core import G3Units, quat, G3VectorQuat, G3TimestreamQuat, usefulfunc, indexmod +from ..core import G3Units, Quat, G3VectorQuat, G3TimestreamQuat, usefulfunc, indexmod import numpy @@ -9,13 +9,13 @@ def quat_to_ang(q): vector of them) specified as a (longitude, latitude) pair. """ single = False - if isinstance(q, quat): - q = numpy.asarray(G3VectorQuat([q])) + if isinstance(q, Quat): + q = numpy.array(G3VectorQuat([q])) single = True elif isinstance(q, list): - q = numpy.asarray(G3VectorQuat(q)) + q = numpy.array(G3VectorQuat(q)) else: - q = numpy.asarray(q) + q = numpy.array(q) # Copied from C code d = q[:, 1] ** 2 + q[:, 2] ** 2 + q[:, 3] ** 2 @@ -40,21 +40,21 @@ def ang_to_quat(alpha, delta, start=None, stop=None): G3TimestreamQuat with start and stop times set to the provided values. """ - alpha = numpy.asarray(alpha) - delta = numpy.asarray(delta) + alpha = numpy.asarray(alpha) / G3Units.rad + delta = numpy.asarray(delta) / G3Units.rad # Copied from C code - c_delta = numpy.cos(delta / G3Units.rad) + c_delta = numpy.cos(delta) q = numpy.column_stack( ( 0 * c_delta, # 0s with the right shape - c_delta * numpy.cos(alpha / G3Units.rad), - c_delta * numpy.sin(alpha / G3Units.rad), - numpy.sin(delta / G3Units.rad), + c_delta * numpy.cos(alpha), + c_delta * numpy.sin(alpha), + numpy.sin(delta), ) ) if len(q) == 1: - return quat(q[0, 0], q[0, 1], q[0, 2], q[0, 3]) + return Quat(q[0, :]) else: if start is not None: out = G3TimestreamQuat(q) diff --git a/maps/src/FlatSkyMap.cxx b/maps/src/FlatSkyMap.cxx index 961bfe0f..d0ee5d91 100644 --- a/maps/src/FlatSkyMap.cxx +++ b/maps/src/FlatSkyMap.cxx @@ -723,24 +723,24 @@ FlatSkyMap::PixelToXY(size_t pixel) const { } std::vector -FlatSkyMap::QuatToXY(quat q) const +FlatSkyMap::QuatToXY(const Quat &q) const { return proj_info.QuatToXY(q); } size_t -FlatSkyMap::QuatToPixel(quat q) const +FlatSkyMap::QuatToPixel(const Quat &q) const { return proj_info.QuatToPixel(q); } -quat +Quat FlatSkyMap::XYToQuat(double x, double y) const { return proj_info.XYToQuat(x, y); } -quat +Quat FlatSkyMap::PixelToQuat(size_t pixel) const { return proj_info.PixelToQuat(pixel); @@ -769,14 +769,14 @@ FlatSkyMap::GetRebinQuats(size_t pixel, size_t scale) const } void -FlatSkyMap::GetInterpPixelsWeights(quat q, std::vector & pixels, +FlatSkyMap::GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const { proj_info.GetInterpPixelsWeights(q, pixels, weights); } std::vector -FlatSkyMap::QueryDisc(quat q, double radius) const +FlatSkyMap::QueryDisc(const Quat &q, double radius) const { return proj_info.QueryDisc(q, radius); } @@ -1214,6 +1214,32 @@ flatskymap_xy_to_pixels(const FlatSkyMap & skymap, const std::vector &x, return pixel; } +static boost::python::tuple +flatskymap_quats_to_xy(const FlatSkyMap & skymap, const G3VectorQuat &quats) +{ + std::vector x(quats.size()), y(quats.size()); + for (size_t i = 0; i < quats.size(); i++) { + auto xy = skymap.QuatToXY(quats[i]); + x[i] = xy[0]; + y[i] = xy[1]; + } + + return boost::python::make_tuple(x, y); +} + +static G3VectorQuat +flatskymap_xy_to_quats(const FlatSkyMap & skymap, const std::vector &x, + const std::vector &y) +{ + g3_assert(x.size() == y.size()); + + G3VectorQuat quats; + for (size_t i = 0; i < x.size(); i++) + quats.push_back(skymap.XYToQuat(x[i], y[i])); + + return quats; +} + G3_SPLIT_SERIALIZABLE_CODE(FlatSkyMap); @@ -1326,6 +1352,19 @@ PYBINDINGS("maps") (bp::arg("pixel")), "Compute the flat 2D coordinates of the input pixel indices (vectorized)") + .def("xy_to_quat", &FlatSkyMap::XYToQuat, + (bp::arg("x"), bp::arg("y")), + "Compute the quaternion rotation of the input flat 2D coordinates") + .def("xy_to_quat", flatskymap_xy_to_quats, + (bp::arg("x"), bp::arg("y")), + "Compute the quaternion rotations of the input flat 2D coordinates (vectorized)") + .def("quat_to_xy", &FlatSkyMap::QuatToXY, + (bp::arg("quat")), + "Compute the flat 2D coordinates of the input quaternion rotation") + .def("quat_to_xy", flatskymap_quats_to_xy, + (bp::arg("quat")), + "Compute the flat 2D coordinates of the input quaternion rotations (vectorized)") + .add_property("sparse", flatskymap_pysparsity_get, flatskymap_pysparsity_set, "True if the map is stored with column and row zero-suppression, False if " "every pixel is stored. Map sparsity can be changed by setting this to True " diff --git a/maps/src/FlatSkyProjection.cxx b/maps/src/FlatSkyProjection.cxx index bf4b99b1..81c1b64c 100644 --- a/maps/src/FlatSkyProjection.cxx +++ b/maps/src/FlatSkyProjection.cxx @@ -275,7 +275,7 @@ std::vector FlatSkyProjection::XYToAngle(double x, double y) const { if (!cyl_) { - quat q = XYToQuat(x, y); + auto q = XYToQuat(x, y); double alpha, delta; quat_to_ang(q, alpha, delta); return {alpha, delta}; @@ -322,7 +322,7 @@ std::vector FlatSkyProjection::AngleToXY(double alpha, double delta) const { if (!cyl_) { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); return QuatToXY(q); } @@ -375,7 +375,7 @@ FlatSkyProjection::AngleToXY(double alpha, double delta) const } std::vector -FlatSkyProjection::QuatToXY(quat q) const +FlatSkyProjection::QuatToXY(const Quat &q) const { if (cyl_) { double a, d; @@ -384,8 +384,8 @@ FlatSkyProjection::QuatToXY(quat q) const } // Rotate to projection center - quat qr = ~q0_ * q * q0_; - double cc = qr.R_component_2(); + auto qr = ~q0_ * q * q0_; + double cc = qr.b(); double k; switch(proj_) { @@ -414,8 +414,8 @@ FlatSkyProjection::QuatToXY(quat q) const break; } - double x = k * qr.R_component_3(); - double y = -k * qr.R_component_4(); + double x = k * qr.c(); + double y = -k * qr.d(); x = x0_ - x / x_res_; y = y0_ - y / y_res_; @@ -423,7 +423,7 @@ FlatSkyProjection::QuatToXY(quat q) const return {x, y}; } -quat +Quat FlatSkyProjection::XYToQuat(double x, double y) const { if (cyl_) { @@ -435,13 +435,13 @@ FlatSkyProjection::XYToQuat(double x, double y) const y = (y0_ - y) * y_res_ / rad; double rho = sqrt(x * x + y * y); - quat q; + Quat q; if (rho < 1e-8) { - q = quat(0, 1, 0, 0); + q = Quat(0, 1, 0, 0); } else if (proj_ == Proj2) { double cc = sqrt((1. - rho) * (1. + rho)); - q = quat(0, cc, x, -y); + q = Quat(0, cc, x, -y); } else { double c; @@ -465,24 +465,24 @@ FlatSkyProjection::XYToQuat(double x, double y) const double cc = COS(c); double sc = SIN(c) / rho; - q = quat(0, cc, x * sc, -y * sc); + q = Quat(0, cc, x * sc, -y * sc); } // Rotate from projection center return q0_ * q * ~q0_; } -quat +Quat FlatSkyProjection::PixelToQuat(size_t pixel) const { if (pixel < 0 || pixel >= xpix_ * ypix_) - return quat(0, 1, 0, 0); + return Quat(0, 1, 0, 0); std::vector xy = PixelToXY(pixel); return XYToQuat(xy[0], xy[1]); } size_t -FlatSkyProjection::QuatToPixel(quat q) const +FlatSkyProjection::QuatToPixel(const Quat &q) const { std::vector xy = QuatToXY(q); return XYToPixel(xy[0], xy[1]); @@ -507,7 +507,7 @@ FlatSkyProjection::AngleToPixel(double alpha, double delta) const G3VectorQuat FlatSkyProjection::GetRebinQuats(size_t pixel, size_t scale) const { - G3VectorQuat quats(scale * scale, quat(0, 1, 0, 0)); + G3VectorQuat quats(scale * scale, Quat(0, 1, 0, 0)); if (pixel < 0 || pixel >= xpix_ * ypix_) { log_debug("Point lies outside of pixel grid\n"); @@ -572,7 +572,7 @@ FlatSkyProjection::PixelToAngleGrad(size_t pixel, double h) const return XYToAngleGrad(xy[0], xy[1], h); } -void FlatSkyProjection::GetInterpPixelsWeights(quat q, +void FlatSkyProjection::GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const { std::vector xy = QuatToXY(q); @@ -598,15 +598,15 @@ void FlatSkyProjection::GetInterpPixelsWeights(quat q, } std::vector -FlatSkyProjection::QueryDisc(quat q, double radius) const +FlatSkyProjection::QueryDisc(const Quat &q, double radius) const { static const size_t npts = 72; double dd = -2.0 * radius / sqrt(2.0); - quat qd = get_origin_rotator(0, dd); - quat p = qd * q * ~qd; - double pva = q.R_component_2(); - double pvb = q.R_component_3(); - double pvc = q.R_component_4(); + auto qd = get_origin_rotator(0, dd); + auto p = qd * q * ~qd; + double pva = q.b(); + double pvb = q.c(); + double pvc = q.d(); ssize_t xmin = xpix_; ssize_t xmax = 0; @@ -620,7 +620,7 @@ FlatSkyProjection::QueryDisc(quat q, double radius) const double phi = i * step; double c = COS(phi); double s = SIN(phi); - quat qv = quat(c, pva * s, pvb * s, pvc * s); + Quat qv(c, pva * s, pvb * s, pvc * s); auto xy = QuatToXY(qv * p * ~qv); ssize_t fx = std::floor(xy[0]); ssize_t cx = std::ceil(xy[0]); @@ -644,7 +644,7 @@ FlatSkyProjection::QueryDisc(quat q, double radius) const size_t pixel = y * xpix_ + x; if (pixel > xpix_ * ypix_) continue; - quat qp = PixelToQuat(pixel); + auto qp = PixelToQuat(pixel); if (dot3(qp, q) > crad) pixels.push_back(pixel); } diff --git a/maps/src/G3SkyMap.cxx b/maps/src/G3SkyMap.cxx index 6022433c..dba033a0 100644 --- a/maps/src/G3SkyMap.cxx +++ b/maps/src/G3SkyMap.cxx @@ -187,7 +187,7 @@ G3SkyMap::PixelsToAngles(const std::vector & pixels, size_t G3SkyMap::AngleToPixel(double alpha, double delta) const { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); return QuatToPixel(q); } @@ -195,7 +195,7 @@ G3SkyMap::AngleToPixel(double alpha, double delta) const std::vector G3SkyMap::PixelToAngle(size_t pixel) const { - quat q = PixelToQuat(pixel); + auto q = PixelToQuat(pixel); double alpha, delta; quat_to_ang(q, alpha, delta); @@ -349,7 +349,7 @@ void G3SkyMap::GetInterpPixelsWeights(double alpha, double delta, std::vector & pixels, std::vector & weights) const { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); GetInterpPixelsWeights(q, pixels, weights); } @@ -383,7 +383,7 @@ G3SkyMap::GetInterpPrecalc(const std::vector & pix, double G3SkyMap::GetInterpValue(double alpha, double delta) const { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); return GetInterpValue(q); } @@ -401,7 +401,7 @@ G3SkyMap::GetInterpValues(const std::vector & alphas, } double -G3SkyMap::GetInterpValue(quat q) const +G3SkyMap::GetInterpValue(const Quat &q) const { std::vector pix; std::vector weight; @@ -423,32 +423,32 @@ G3SkyMap::GetInterpValues(const G3VectorQuat & quats) const std::vector G3SkyMap::QueryDisc(double alpha, double delta, double radius) const { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); return QueryDisc(q, radius); } std::vector G3SkyMap::QueryAlphaEllipse(double alpha ,double delta, double a, double b) const { - quat q = ang_to_quat(alpha, delta); + auto q = ang_to_quat(alpha, delta); return QueryAlphaEllipse(q, a, b); } std::vector -G3SkyMap::QueryAlphaEllipse(quat q, double a, double b) const +G3SkyMap::QueryAlphaEllipse(const Quat &q, double a, double b) const { double rmaj = a > b ? a : b; double rmin = a > b ? b : a; - double sd = q.R_component_4(); + double sd = q.d(); double cd = sqrt((1 - sd) * (1 + sd)); // focus distance from center double da = ACOS(COS(rmaj) / COS(rmin)) / cd; // focus locations - quat qda = get_origin_rotator(da, 0); - quat ql = qda * q * ~qda; - quat qr = ~qda * q * qda; + auto qda = get_origin_rotator(da, 0); + auto ql = qda * q * ~qda; + auto qr = ~qda * q * qda; // narrow search to pixels within the major disc auto disc = QueryDisc(q, rmaj); @@ -456,7 +456,7 @@ G3SkyMap::QueryAlphaEllipse(quat q, double a, double b) const // narrow further to locus of points within ellipse std::vector pixels; for (auto i: disc) { - quat qp = PixelToQuat(i); + auto qp = PixelToQuat(i); double d = quat_ang_sep(qp, ql) + quat_ang_sep(qp, qr); if (d < 2 * rmaj) pixels.push_back(i); @@ -1536,7 +1536,7 @@ PYBINDINGS("maps") { "a disc of the given radius at the given sky coordinates.") .def("query_disc", - (std::vector (G3SkyMap::*)(quat, double) const) + (std::vector (G3SkyMap::*)(const Quat &, double) const) &G3SkyMap::QueryDisc, (bp::arg("quat"), bp::arg("radius")), "Return a list of pixel indices whose centers are located within " @@ -1551,7 +1551,7 @@ PYBINDINGS("maps") { "delta sky coordinates, with semimajor and semiminor axes a and b.") .def("query_alpha_ellipse", - (std::vector (G3SkyMap::*)(quat, double, double) const) + (std::vector (G3SkyMap::*)(const Quat &, double, double) const) &G3SkyMap::QueryAlphaEllipse, (bp::arg("quat"), bp::arg("a"), bp::arg("b")), "Return a list of pixel indices whose centers are located within an " diff --git a/maps/src/HealpixSkyMap.cxx b/maps/src/HealpixSkyMap.cxx index 950f228e..f060a37d 100644 --- a/maps/src/HealpixSkyMap.cxx +++ b/maps/src/HealpixSkyMap.cxx @@ -1019,12 +1019,12 @@ HealpixSkyMap::PixelToAngle(size_t pixel) const } size_t -HealpixSkyMap::QuatToPixel(quat q) const +HealpixSkyMap::QuatToPixel(const Quat &q) const { return info_.QuatToPixel(q); } -quat +Quat HealpixSkyMap::PixelToQuat(size_t pixel) const { return info_.PixelToQuat(pixel); @@ -1037,14 +1037,14 @@ HealpixSkyMap::GetRebinQuats(size_t pixel, size_t scale) const } void -HealpixSkyMap::GetInterpPixelsWeights(quat q, std::vector & pixels, +HealpixSkyMap::GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const { info_.GetInterpPixelsWeights(q, pixels, weights); } std::vector -HealpixSkyMap::QueryDisc(quat q, double radius) const +HealpixSkyMap::QueryDisc(const Quat &q, double radius) const { return info_.QueryDisc(q, radius); } diff --git a/maps/src/HealpixSkyMapInfo.cxx b/maps/src/HealpixSkyMapInfo.cxx index 9b3c1cc8..1ebd0b1b 100644 --- a/maps/src/HealpixSkyMapInfo.cxx +++ b/maps/src/HealpixSkyMapInfo.cxx @@ -245,9 +245,9 @@ HealpixSkyMapInfo::AngleToPixel(double alpha, double delta) const } size_t -HealpixSkyMapInfo::QuatToPixel(quat q) const +HealpixSkyMapInfo::QuatToPixel(const Quat &q) const { - std::vector v = {q.R_component_2(), q.R_component_3(), q.R_component_4()}; + std::vector v = {q.b(), q.c(), q.d()}; ssize_t outpix; if (nested_) @@ -285,11 +285,11 @@ HealpixSkyMapInfo::PixelToAngle(size_t pixel) const return {alpha, delta}; } -quat +Quat HealpixSkyMapInfo::PixelToQuat(size_t pixel) const { if (pixel < 0 || pixel >= npix_) - return quat(0, 1, 0, 0); + return Quat(0, 1, 0, 0); std::vector v(3); if (nested_) @@ -297,7 +297,7 @@ HealpixSkyMapInfo::PixelToQuat(size_t pixel) const else pix2vec_ring64(nside_, pixel, &v[0]); - return quat(0, v[0], v[1], v[2]); + return Quat(0, v[0], v[1], v[2]); } size_t @@ -320,7 +320,7 @@ HealpixSkyMapInfo::GetRebinQuats(size_t pixel, size_t scale) const if (nside_ % scale != 0) log_fatal("Nside must be a multiple of rebinning scale"); - G3VectorQuat quats(scale * scale, quat(0, 1, 0, 0)); + G3VectorQuat quats(scale * scale, Quat(0, 1, 0, 0)); if (pixel >= npix_) { quats.resize(0); @@ -339,7 +339,7 @@ HealpixSkyMapInfo::GetRebinQuats(size_t pixel, size_t scale) const for (size_t i = 0; i < (scale * scale); i++) { pix2vec_nest64(nside_rebin, pixmin + i, &v[0]); - quats[i] = quat(0, v[0], v[1], v[2]); + quats[i] = Quat(0, v[0], v[1], v[2]); } return quats; @@ -356,14 +356,14 @@ HealpixSkyMapInfo::RingAbove(double z) const } void -HealpixSkyMapInfo::GetInterpPixelsWeights(quat q, std::vector & pixels, +HealpixSkyMapInfo::GetInterpPixelsWeights(const Quat &q, std::vector & pixels, std::vector & weights) const { pixels = std::vector(4, (size_t) -1); weights = std::vector(4, 0); - double z = q.R_component_4() / sqrt(dot3(q, q)); - double phi = atan2(q.R_component_3(), q.R_component_2()); + double z = q.d() / sqrt(dot3(q, q)); + double phi = atan2(q.c(), q.b()); if (phi < 0) phi += twopi; @@ -446,7 +446,7 @@ HealpixSkyMapInfo::GetInterpPixelsWeights(quat q, std::vector & pixels, } std::vector -HealpixSkyMapInfo::QueryDisc(quat q, double radius) const +HealpixSkyMapInfo::QueryDisc(const Quat &q, double radius) const { size_t n = 0; auto pixels = std::vector(n); @@ -461,11 +461,11 @@ HealpixSkyMapInfo::QueryDisc(quat q, double radius) const double cosrad = cos(radius); double sinrad = sin(radius); - double z = q.R_component_4() / sqrt(dot3(q, q)); + double z = q.d() / sqrt(dot3(q, q)); double theta = acos(z); double s = sqrt((1 - z) * (1 + z)); double xa = 1.0 / s; - double phi = atan2(q.R_component_3(), q.R_component_2()); + double phi = atan2(q.c(), q.b()); if (phi < 0) phi += twopi; diff --git a/maps/src/maputils.cxx b/maps/src/maputils.cxx index 8d71174f..d4a4e34c 100644 --- a/maps/src/maputils.cxx +++ b/maps/src/maputils.cxx @@ -282,7 +282,7 @@ void FlattenPol(FlatSkyMapPtr Q, FlatSkyMapPtr U, G3SkyMapWeightsPtr W, double h void ReprojMap(G3SkyMapConstPtr in_map, G3SkyMapPtr out_map, int rebin, bool interp) { bool rotate = false; // no transform - quat q_rot; // quaternion for rotating from output to input coordinate system + Quat q_rot; // quaternion for rotating from output to input coordinate system if (in_map->coord_ref != out_map->coord_ref && in_map->coord_ref != MapCoordReference::Local && out_map->coord_ref != MapCoordReference::Local) { @@ -330,7 +330,7 @@ void ReprojMap(G3SkyMapConstPtr in_map, G3SkyMapPtr out_map, int rebin, bool int } else { for (size_t i = 0; i < out_map->size(); i++) { double val = 0; - quat q = out_map->PixelToQuat(i); + auto q = out_map->PixelToQuat(i); if (rotate) q = q_rot * q * ~q_rot; if (interp) diff --git a/maps/src/pointing.cxx b/maps/src/pointing.cxx index 00cc75dd..86c1051a 100644 --- a/maps/src/pointing.cxx +++ b/maps/src/pointing.cxx @@ -15,13 +15,6 @@ #define ASIN asin #define ATAN2 atan2 -#define QNORM(q) \ - { \ - double n = dot3(q, q); \ - if (fabs(n - 1.0) > 1e-6) \ - q /= sqrt(n); \ - } - /* * Quaternions cannot represent parity flips. Since celestial coordinates * and az-el coordinates by construction have a different parity, we can't use @@ -32,43 +25,51 @@ * the z coordinate = -sin(elevation) = sin(declination) */ -static quat -project_on_plane(quat plane_normal, quat point) +static Quat +unit_vector(const Quat &q) +{ + double n = q.vnorm(); + if (fabs(n - 1.0) > 1e-6) + return q / sqrt(n); + return q; +} + +static Quat +project_on_plane(const Quat &plane_normal, const Quat &point) { // Projects the quaternion onto a plane with unit normal plane_normal // The plane is defined as going through the origin // with normal = plane_normal - quat out_q(point); + Quat out_q(point); //ensure unit vec - QNORM(plane_normal); - out_q -= plane_normal * dot3(plane_normal, point); - QNORM(out_q); - return out_q; + auto un = unit_vector(plane_normal); + out_q -= un * dot3(un, point); + return unit_vector(out_q); } -quat +Quat ang_to_quat(double alpha, double delta) { double c_delta = cos(delta / G3Units::rad); - return quat(0, + return Quat(0, c_delta * cos(alpha/G3Units::rad), c_delta * sin(alpha/G3Units::rad), sin(delta / G3Units::rad)); } void -quat_to_ang(quat q, double &alpha, double &delta) +quat_to_ang(const Quat &q, double &alpha, double &delta) { - QNORM(q); - delta = ASIN(q.R_component_4()) * G3Units::rad; - alpha = ATAN2(q.R_component_3(), q.R_component_2())*G3Units::rad; + auto uq = unit_vector(q); + delta = ASIN(uq.d()) * G3Units::rad; + alpha = ATAN2(uq.c(), uq.b()) * G3Units::rad; if (alpha < 0) alpha += 360 * G3Units::deg; } static boost::python::tuple -py_quat_to_ang(quat q) +py_quat_to_ang(const Quat &q) { double a,d; quat_to_ang(q, a, d); @@ -77,12 +78,12 @@ py_quat_to_ang(quat q) } double -quat_ang_sep(quat a, quat b) +quat_ang_sep(const Quat &a, const Quat &b) { - QNORM(a); - QNORM(b); + auto ua = unit_vector(a); + auto ub = unit_vector(b); - double d = dot3(a, b); + double d = dot3(ua, ub); if (d > 1) return 0; if (d < -1) @@ -90,39 +91,36 @@ quat_ang_sep(quat a, quat b) return acos(d) * G3Units::rad; } -static quat -coord_quat_to_delta_hat(quat q) +static Quat +coord_quat_to_delta_hat(const Quat &q) { // computes the delta hat vector for a given point on the unit sphere // specified by q // // (The delta hat is equal to -alpha hat) - QNORM(q); - double st = sqrt(1 - (q.R_component_4()*q.R_component_4())); - quat u= quat(0, - -1 * (q.R_component_2() * q.R_component_4())/st, - -1 * (q.R_component_3() * q.R_component_4())/st, - st); - QNORM(u); - return u; + auto uq = unit_vector(q); + double st = sqrt(1 - uq.d() * uq.d()); + double ct = -1.0 * uq.d() / st; + Quat qd(0, uq.b() * ct, uq.c() * ct, st); + return unit_vector(qd); } static double -get_rot_ang(quat start_q, quat trans) +get_rot_ang(const Quat &start_q, const Quat &trans) { // delta is the physicist spherical coordinates delta // Computes delta hat for the start q applies trans to it // and then computes the angle between that and end_q's delta hat. - quat t = trans * coord_quat_to_delta_hat(start_q) * ~trans; - quat end_q = trans * start_q * ~trans; - quat t_p = coord_quat_to_delta_hat(end_q); + auto t = trans * coord_quat_to_delta_hat(start_q) * ~trans; + auto end_q = trans * start_q * ~trans; + auto t_p = coord_quat_to_delta_hat(end_q); double sf = (dot3(end_q, cross3(t, t_p)) < 0) ? -1 : 1; return sf * quat_ang_sep(t, t_p); } -static quat +static Quat get_transform_quat(double as_0, double ds_0, double ae_0, double de_0, double as_1, double ds_1, double ae_1, double de_1) { @@ -139,51 +137,49 @@ get_transform_quat(double as_0, double ds_0, double ae_0, double de_0, * */ - quat asds_0 = ang_to_quat(as_0, ds_0); - quat asds_1 = ang_to_quat(as_1, ds_1); - quat aede_0 = ang_to_quat(ae_0, de_0); - quat aede_1 = ang_to_quat(ae_1, de_1); + auto asds_0 = ang_to_quat(as_0, ds_0); + auto asds_1 = ang_to_quat(as_1, ds_1); + auto aede_0 = ang_to_quat(ae_0, de_0); + auto aede_1 = ang_to_quat(ae_1, de_1); - quat tquat = cross3(asds_0, aede_0); - double mag = sqrt(dot3(tquat, tquat)); + auto tquat = cross3(asds_0, aede_0); + double mag = sqrt(tquat.vnorm()); double ang = quat_ang_sep(asds_0, aede_0); tquat *= sin(ang/2.0) / mag; - tquat += quat(cos(ang/2.0),0,0,0); + tquat += Quat(cos(ang/2.0), 0, 0, 0); // trans_asds_1 and aede_1 should now be the same up to a rotation // around aede_0 - quat trans_asds_1 = tquat * asds_1 * ~tquat; + auto trans_asds_1 = tquat * asds_1 * ~tquat; // Project them on to a plane and find the angle between the two vectors // using (ae_0, de_0) as the normal since we are rotating around that // vector. - quat p_asds1 = project_on_plane(aede_0, trans_asds_1); - quat p_aede1 = project_on_plane(aede_0, aede_1); + auto p_asds1 = project_on_plane(aede_0, trans_asds_1); + auto p_aede1 = project_on_plane(aede_0, aede_1); double rot_ang = quat_ang_sep(p_asds1, p_aede1); double sf = (dot3(aede_0, cross3(p_asds1, p_aede1)) < 0) ? -1 : 1; rot_ang *= sf; double sin_rot_ang_ov_2 = sin(rot_ang/2.0); - quat rot_quat = quat(cos(rot_ang/2.0), - sin_rot_ang_ov_2 * aede_0.R_component_2(), - sin_rot_ang_ov_2 * aede_0.R_component_3(), - sin_rot_ang_ov_2 * aede_0.R_component_4()); - quat final_trans = rot_quat * tquat; - - return final_trans; + Quat rot_quat(cos(rot_ang/2.0), + sin_rot_ang_ov_2 * aede_0.b(), + sin_rot_ang_ov_2 * aede_0.c(), + sin_rot_ang_ov_2 * aede_0.d()); + return rot_quat * tquat; } -quat +Quat get_fk5_j2000_to_gal_quat() { // returns the quaternion that rotates FK5 J2000 to galactic J2000 coordinates // return get_transform_quat(0,0, 1.6814025470759737, -1.050488399695429, // 0,-0.7853981633974483, 5.750520098164818, -1.2109809382060603); - return quat(0.4889475076,-0.483210684,0.1962537583,0.699229742); + return Quat(0.4889475076,-0.483210684,0.1962537583,0.699229742); } -quat +Quat offsets_to_quat(double x_offset, double y_offset) { // Rotates the point (1,0,0) by the rotation matrix for the y_offset @@ -195,13 +191,13 @@ offsets_to_quat(double x_offset, double y_offset) return ang_to_quat(x_offset, -y_offset); } -quat +Quat get_origin_rotator(double alpha, double delta) { // Rotates the point (1,0,0) to the point specified by alpha and // delta via a rotation about the y axis and then the z axis - return (quat(cos(alpha/2.0), 0, 0, sin(alpha/2.0)) * - quat(cos(delta/2.0), 0, -sin(delta/2.0), 0)); + return (Quat(cos(alpha/2.0), 0, 0, sin(alpha/2.0)) * + Quat(cos(delta/2.0), 0, -sin(delta/2.0), 0)); } static G3TimestreamQuat @@ -212,7 +208,7 @@ get_origin_rotator_timestream(const G3Timestream &alpha, const G3Timestream &del // for why it's -el see the comment at the top of this document g3_assert(alpha.size() == delta.size()); - G3TimestreamQuat trans_quats(alpha.size(), quat(1,0,0,0)); + G3TimestreamQuat trans_quats(alpha.size(), Quat(1,0,0,0)); trans_quats.start = alpha.start; trans_quats.stop = alpha.stop; if (coord_sys == Local) @@ -244,7 +240,7 @@ get_boresight_rotator_timestream(const G3Timestream &az_0, const G3Timestream &e g3_assert(az_0.size() == dec_1.size()); g3_assert(az_0.size() == ra_0.size()); g3_assert(az_0.size() == ra_1.size()); - G3TimestreamQuat trans_quats(ra_0.size(), quat(1,0,0,0)); + G3TimestreamQuat trans_quats(ra_0.size(), Quat(1,0,0,0)); trans_quats.start = az_0.start; trans_quats.stop = az_0.stop; @@ -264,18 +260,17 @@ G3VectorQuat get_detector_pointing_quats(double x_offset, double y_offset, const G3VectorQuat &trans_quat, MapCoordReference coord_sys) { - quat q_off = offsets_to_quat(x_offset, y_offset); + auto q_off = offsets_to_quat(x_offset, y_offset); size_t nsamp = trans_quat.size(); - G3VectorQuat det_quats(nsamp, quat(0, 1, 0, 0)); + G3VectorQuat det_quats(nsamp, Quat(0, 1, 0, 0)); for (size_t i = 0; i < nsamp; i++) det_quats[i] = trans_quat[i] * q_off * ~trans_quat[i]; if (coord_sys == Local) { for (size_t i = 0; i < nsamp; i++) { - const quat &q = det_quats[i]; - det_quats[i] = quat(q.R_component_1(), q.R_component_2(), - q.R_component_3(), -q.R_component_4()); + const auto &q = det_quats[i]; + det_quats[i] = Quat(q.a(), q.b(), q.c(), -q.d()); } } @@ -286,16 +281,15 @@ std::vector get_detector_pointing_pixels(double x_offset, double y_offset, const G3VectorQuat &trans_quat, G3SkyMapConstPtr skymap) { - quat q_off = offsets_to_quat(x_offset, y_offset); + auto q_off = offsets_to_quat(x_offset, y_offset); size_t nsamp = trans_quat.size(); std::vector pixels(nsamp, (size_t) -1); - quat q; + Quat q; if (skymap->coord_ref == Local) { for (size_t i = 0; i < nsamp; i++) { q = trans_quat[i] * q_off * ~trans_quat[i]; - q = quat(q.R_component_1(), q.R_component_2(), - q.R_component_3(), -q.R_component_4()); + q = Quat(q.a(), q.b(), q.c(), -q.d()); pixels[i] = skymap->QuatToPixel(q); } } else { @@ -317,7 +311,7 @@ get_detector_pointing(double x_offset, double y_offset, // trans_quat with a given coordinate system coord_sys, // computes the individual detector pointing coordinates. - quat det_pos = offsets_to_quat(x_offset, y_offset); + auto det_pos = offsets_to_quat(x_offset, y_offset); delta.resize(trans_quat.size()); alpha.resize(trans_quat.size()); @@ -332,7 +326,7 @@ get_detector_pointing(double x_offset, double y_offset, for (size_t i = 0; i < alpha.size(); i++) { //uses an inverse that assumes we are on the unit sphere - quat q = trans_quat[i] * det_pos * ~trans_quat[i]; + auto q = trans_quat[i] * det_pos * ~trans_quat[i]; quat_to_ang(q, alpha[i], delta[i]); } if (coord_sys == Local) { @@ -349,7 +343,7 @@ get_detector_rotation(double x_offset, double y_offset, // Computes the polarization angle rotation that occurs under the // transform trans_quat and stores it in rot. std::vector rot(trans_quat.size(), 0); - quat det_pos = offsets_to_quat(x_offset, y_offset); + auto det_pos = offsets_to_quat(x_offset, y_offset); for (size_t i = 0; i < rot.size(); i++) rot[i] = get_rot_ang(det_pos, trans_quat[i]); diff --git a/maps/tests/quatangtest.py b/maps/tests/quatangtest.py index 67b77d50..2178e30c 100755 --- a/maps/tests/quatangtest.py +++ b/maps/tests/quatangtest.py @@ -2,8 +2,8 @@ from spt3g import core, maps import numpy as np -a = core.quat(2,3,4,5) -a2 = core.quat(2,1,8,5) +a = core.Quat(2,3,4,5) +a2 = core.Quat(2,1,8,5) b = core.G3VectorQuat([a, a**2, 2*a, a2]) assert(np.allclose(maps.quat_to_ang(a), maps.c_quat_to_ang_(a)))