97 lines
2.7 KiB
C
97 lines
2.7 KiB
C
|
#ifndef GEOMETRY_UTILS_QUATERNION_H
|
||
|
|
#define GEOMETRY_UTILS_QUATERNION_H
|
||
|
|
|
||
|
|
#include <string>
|
||
|
|
#include <boost/array.hpp>
|
||
|
|
#include <Eigen/Geometry>
|
||
|
|
#include "GeometryUtilsMath.h"
|
||
|
|
|
||
|
|
namespace geometry_utils {
|
||
|
|
|
||
|
|
template <typename T>
|
||
|
|
struct QuaternionBase : VectorNBase<T, 4> {
|
||
|
|
QuaternionBase() : VectorNBase<T, 4>() {
|
||
|
|
this->data.assign(0);
|
||
|
|
this->data[0] = 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
QuaternionBase(T val) : VectorNBase<T, 4>(val) {}
|
||
|
|
QuaternionBase(const QuaternionBase& in) : VectorNBase<T, 4>(in.data) {}
|
||
|
|
QuaternionBase(const boost::array<T, 4>& in) : VectorNBase<T, 4>(in) {}
|
||
|
|
QuaternionBase(T (&in)[4]) : VectorNBase<T, 4>(in) {}
|
||
|
|
QuaternionBase(const Eigen::Quaternion<T>& in) {
|
||
|
|
this->data[0] = in.w();
|
||
|
|
this->data[1] = in.x();
|
||
|
|
this->data[2] = in.y();
|
||
|
|
this->data[3] = in.z();
|
||
|
|
}
|
||
|
|
QuaternionBase(const VectorNBase<T, 4>& in) : VectorNBase<T, 4>(in) {}
|
||
|
|
|
||
|
|
QuaternionBase(T w, T x, T y, T z) {
|
||
|
|
this->data[0] = w;
|
||
|
|
this->data[1] = x;
|
||
|
|
this->data[2] = y;
|
||
|
|
this->data[3] = z;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline QuaternionBase operator*(const QuaternionBase& rhs) const {
|
||
|
|
T a1 = this->data[0];
|
||
|
|
T b1 = this->data[1];
|
||
|
|
T c1 = this->data[2];
|
||
|
|
T d1 = this->data[3];
|
||
|
|
T a2 = rhs.data[0];
|
||
|
|
T b2 = rhs.data[1];
|
||
|
|
T c2 = rhs.data[2];
|
||
|
|
T d2 = rhs.data[3];
|
||
|
|
return QuaternionBase(a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2,
|
||
|
|
a2 * b1 + a1 * b2 - c2 * d1 + c1 * d2,
|
||
|
|
a2 * c1 + a1 * c2 + b2 * d1 - b1 * d2,
|
||
|
|
-(b2 * c1) + b1 * c2 + a2 * d1 + a1 * d2);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline T& W() { return this->data[0]; }
|
||
|
|
inline const T& W() const { return this->data[0]; }
|
||
|
|
|
||
|
|
inline T& X() { return this->data[1]; }
|
||
|
|
inline const T& X() const { return this->data[1]; }
|
||
|
|
|
||
|
|
inline T& Y() { return this->data[2]; }
|
||
|
|
inline const T& Y() const { return this->data[2]; }
|
||
|
|
|
||
|
|
inline T& Z() { return this->data[3]; }
|
||
|
|
inline const T& Z() const { return this->data[3]; }
|
||
|
|
|
||
|
|
inline QuaternionBase Conj() const {
|
||
|
|
return QuaternionBase(this->data[0], -this->data[1], -this->data[2],
|
||
|
|
-this->data[3]);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline QuaternionBase Error(const QuaternionBase& q) const {
|
||
|
|
return q * (*this).Conj();
|
||
|
|
}
|
||
|
|
|
||
|
|
inline QuaternionBase AxisAngle() const {
|
||
|
|
QuaternionBase q((*this));
|
||
|
|
if (q.W() > 1)
|
||
|
|
q = q.Normalize();
|
||
|
|
T den = math::sqrt(1 - q.W() * q.W());
|
||
|
|
if (den < 1e-6)
|
||
|
|
den = 1;
|
||
|
|
return QuaternionBase(2 * math::acos(q.W()), q.X() / den, q.Y() / den,
|
||
|
|
q.Z() / den);
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
typedef QuaternionBase<float> Quaternionf;
|
||
|
|
typedef QuaternionBase<float> Quatf;
|
||
|
|
|
||
|
|
typedef QuaternionBase<double> Quaterniond;
|
||
|
|
typedef QuaternionBase<double> Quatd;
|
||
|
|
|
||
|
|
typedef QuaternionBase<double> Quaternion;
|
||
|
|
typedef QuaternionBase<double> Quat;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|