Add a Quaternion class

diff --git a/source/geometry/affinetransform.h b/source/geometry/affinetransform.h
index 1d48a5d84f0396b7538ff7b0c569864b798d41fd..f9660c60d1c7042a6e96f88cf3fdd8bc67820959 100644 (file)
--- a/source/geometry/affinetransform.h
+++ b/source/geometry/affinetransform.h
@@ -4,6 +4,7 @@
 #include <msp/linal/matrix.h>
 #include "angle.h"
 #include "boundingbox.h"
+#include "quaternion.h"
 #include "ray.h"
 
 namespace Msp {
@@ -41,6 +42,7 @@ protected:
 
 public:
        static AffineTransform<T, 3> rotation(Angle<T>, const LinAl::Vector<T, 3> &);
+       static AffineTransform<T, 3> rotation(const Quaternion<T> &);
 };
 
 
@@ -144,6 +146,17 @@ AffineTransform<T, 3> AffineTransformOps<T, 3>::rotation(Angle<T> angle, const L
        return r;
 }
 
+template<typename T>
+AffineTransform<T, 3> AffineTransformOps<T, 3>::rotation(const Quaternion<T> &quat)
+{
+       AffineTransform<T, 3> r;
+       LinAl::Matrix<T, 3, 3> m = quat.to_matrix();
+       for(unsigned i=0; i<3; ++i)
+               for(unsigned j=0; j<3; ++j)
+                       r.matrix(i, j) = m(i, j);
+       return r;
+}
+
 template<typename T, unsigned D>
 inline AffineTransform<T, D> &AffineTransform<T, D>::operator*=(const AffineTransform<T, D> &other)
 {

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
new file mode 100644 (file)
index 0000000..7c03612
--- /dev/null
+++ b/source/geometry/quaternion.h
@@ -0,0 +1,194 @@
+#ifndef MSP_MATH_QUATERNION_H_
+#define MSP_MATH_QUATERNION_H_
+
+#include <cmath>
+#include <msp/linal/matrix.h>
+#include <msp/linal/vector.h>
+#include "angle.h"
+
+namespace Msp {
+namespace Geometry {
+
+template<typename T>
+struct Quaternion
+{
+       T a, b, c, d;
+
+       Quaternion(): a(0), b(0), c(0), d(0) { }
+       Quaternion(T a_, T b_, T c_, T d_): a(a_), b(b_), c(c_), d(d_) { }
+
+       static Quaternion one() { return Quaternion(1, 0, 0, 0); }
+       static Quaternion rotation(Angle<T>, const LinAl::Vector<T, 3> &);
+
+       T scalar() const { return a; }
+       LinAl::Vector<T, 3> vector() const { return LinAl::Vector<T, 3>(b, c, d); }
+
+       Quaternion &operator+=(const Quaternion &);
+       Quaternion &operator-=(const Quaternion &);
+       Quaternion &operator*=(T);
+       Quaternion &operator*=(const Quaternion &);
+       Quaternion &operator/=(T);
+       Quaternion &operator/=(const Quaternion &);
+
+       T norm() const;
+       Quaternion &normalize();
+
+       Quaternion conjugate() const { return Quaternion(a, -b, -c, -d); }
+       Quaternion inverse() const;
+
+       LinAl::Vector<T, 3> rotate(const LinAl::Vector<T, 3> &) const;
+       LinAl::Matrix<T, 3, 3> to_matrix() const;
+};
+
+template<typename T>
+Quaternion<T> Quaternion<T>::rotation(Angle<T> angle, const LinAl::Vector<T, 3> &axis)
+{
+       float c = cos(angle/T(2));
+       float s = sin(angle/T(2));
+       return Quaternion<T>(c, s*axis.x, s*axis.y, s*axis.z);
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator+=(const Quaternion<T> &q)
+{
+       a += q.a;
+       b += q.b;
+       c += q.c;
+       d += q.d;
+       return *this;
+}
+
+template<typename T>
+Quaternion<T> operator+(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+       Quaternion<T> r(q1);
+       return r += q2;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator-=(const Quaternion<T> &q)
+{
+       a -= q.a;
+       b -= q.b;
+       c -= q.c;
+       d -= q.d;
+       return *this;
+}
+
+template<typename T>
+Quaternion<T> operator-(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+       Quaternion<T> r(q1);
+       return r -= q2;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator*=(T s)
+{
+       a *= s;
+       b *= s;
+       c *= s;
+       d *= s;
+       return *this;
+}
+
+template<typename T>
+Quaternion<T> operator*(const Quaternion<T> &q, T s)
+{
+       Quaternion<T> r(q);
+       return r *= s;
+}
+
+template<typename T>
+Quaternion<T> operator*(T s, const Quaternion<T> &q)
+{
+       return q*s;
+}
+
+template<typename T>
+Quaternion<T> operator*(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+       return Quaternion<T>(q1.a*q2.a-q1.b*q2.b-q1.c*q2.c-q1.d*q2.d,
+               q1.a*q2.b+q1.b*q2.a+q1.c*q2.d-q1.d*q2.c,
+               q1.a*q2.c+q1.c*q2.a-q1.b*q2.d+q1.d*q2.b,
+               q1.a*q2.d+q1.d*q2.a+q1.b*q2.c-q1.c*q2.b);
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator*=(const Quaternion<T> &q)
+{
+       return *this = *this*q;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator/=(T s)
+{
+       a /= s;
+       b /= s;
+       c /= s;
+       d /= s;
+       return *this;
+}
+
+template<typename T>
+Quaternion<T> operator/(const Quaternion<T> &q, T s)
+{
+       Quaternion<T> r(q);
+       return r /= s;
+}
+
+template<typename T>
+Quaternion<T> operator/(T s, const Quaternion<T> &q)
+{
+       return s*q.inverse();
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator/=(const Quaternion<T> &q)
+{
+       return *this *= q.inverse();
+}
+
+template<typename T>
+Quaternion<T> operator/(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+       Quaternion<T> r(q1);
+       return r /= q2;
+}
+
+template<typename T>
+T Quaternion<T>::norm() const
+{
+       using std::sqrt;
+       return sqrt(a*a+b*b+c*c+d*d);
+}
+
+template<typename T>
+Quaternion<T> Quaternion<T>::inverse() const
+{
+       return conjugate()/(a*a+b*b+c*c+d*d);
+}
+
+template<typename T>
+LinAl::Vector<T, 3> Quaternion<T>::rotate(const LinAl::Vector<T, 3> &v) const
+{
+       return (*this*Quaternion(0, v.x, v.y, v.z)*inverse()).vector();
+}
+
+template<typename T>
+LinAl::Matrix<T, 3, 3> Quaternion<T>::to_matrix() const
+{
+       T s = 2*(a*a+b*b+c*c+d*d);
+       LinAl::Vector<T, 3> columns[3] =
+       {
+               { 1-s*(c*c+d*d), s*(b*c+a*d), s*(b*d-a*c) },
+               { s*(b*c-a*d), 1-s*(b*b+d*d), s*(c*d+a*b) },
+               { s*(b*d+a*c), s*(c*d-a*b), 1-s*(b*b+c*c) }
+       };
+       return LinAl::Matrix<T, 3, 3>::from_columns(columns);
+}
+
+} // namespace Geometry
+} // namespace Msp
+
+#endif