Escolar Documentos
Profissional Documentos
Cultura Documentos
#define QUATERNION_H
#include <iostream>
#include <math.h>
#include "tmatrix.h"
class Quaternion {
double mData[4];
public:
Quaternion() {
mData[0] = mData[1] = mData[2] = 0;
mData[3] = 1;
}
Quaternion(const TVector3& v, double w) {
mData[0] = v.element(0,0);
mData[1] = v.element(1,0);
mData[2] = v.element(2,0);
mData[3] = w;
}
Quaternion(const TVector4& v) {
mData[0] = v.element(0,0);
mData[1] = v.element(1,0);
mData[2] = v.element(2,0);
mData[3] = v.element(3,0);
}
Quaternion(const double* array) {
if (!array) {
UAV_EXCEPTION("Constructing quaternion from 0 array.");
}
for (uint32_t i = 0; i < 4; i++) {
mData[i] = array[i];
}
}
Quaternion(double x, double y, double z, double w) {
mData[0] = x;
mData[1] = y;
mData[2] = z;
mData[3] = w;
}
double
double
double
double
x()
y()
z()
w()
const
const
const
const
{
{
{
{
return
return
return
return
mData[0]; }
mData[1]; }
mData[2]; }
real(); }
+
+
+
-
w()*rhs.x(),
w()*rhs.y(),
w()*rhs.z(),
z()*rhs.z());
*
* Specifically this is the matrix such that:
*
* q.vector().transpose() * this->matrix() = (q *
* (*this)).vector().transpose() for any quaternion q.
*
* Note that this is @e NOT the rotation matrix that may be
* represented by a unit quaternion.
*/
TMatrix4 rightMatrix() const {
double m[16] = {
+w(), -z(), y(), -x(),
+z(), w(), -x(), -y(),
-y(), x(), w(), -z(),
+x(), y(), z(), w()
};
return TMatrix4(m);
}
/**
* @brief Returns this quaternion as a 4-vector.
*
* This is simply the vector [x y z w]<sup>T</sup>
*/
TVector4 vector() const { return TVector4(mData); }
/**
* @brief Returns the norm ("magnitude") of the quaternion.
* @return The 2-norm of [ w(), x(), y(), z() ]<sup>T</sup>.
*/
double norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+
mData[2]*mData[2]+mData[3]*mData[3]); }
/**
* @brief Computes the rotation matrix represented by a unit
* quaternion.
*
* @note This does not check that this quaternion is normalized.
* It formulaically returns the matrix, which will not be a
* rotation if the quaternion is non-unit.
*/
TMatrix3 rotationMatrix() const {
double m[9] = {
1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(),
2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(),
2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y()
};
return TMatrix3(m);
}
/**
* @brief Returns the scaled-axis representation of this
* quaternion rotation.
*/
TVector3 scaledAxis(void) const {
double w[3];
HeliMath::scaled_axis_from_quaternion(w, mData);
return TVector3(w);
}
/**
* @brief Sets quaternion to be same as rotation by scaled axis w.
*/
void scaledAxis(const TVector3& w) {
double theta = w.norm();
if (theta > 0.0001) {
double s = sin(theta / 2.0);
TVector3 W(w / theta * s);
mData[0] = W[0];
mData[1] = W[1];
mData[2] = W[2];
mData[3] = cos(theta / 2.0);
} else {
mData[0]=mData[1]=mData[2]=0;
mData[3]=1.0;
}
}
/**
* @brief Returns a vector rotated by this quaternion.
*
* Functionally equivalent to: (rotationMatrix() * v)
* or (q * Quaternion(0, v) * q.inverse()).
*
* @warning conjugate() is used instead of inverse() for better
* performance, when this quaternion must be normalized.
*/
TVector3 rotatedVector(const TVector3& v) const {
return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
}
/**
* @brief Computes the quaternion that is equivalent to a given
* euler angle rotation.
* @param euler A 3-vector in order: roll-pitch-yaw.
*/
void euler(const TVector3& euler) {
double c1 = cos(euler[2] * 0.5);
double c2 = cos(euler[1] * 0.5);
double c3 = cos(euler[0] * 0.5);
double s1 = sin(euler[2] * 0.5);
double s2 = sin(euler[1] * 0.5);
double s3 = sin(euler[0] * 0.5);
mData[0]
mData[1]
mData[2]
mData[3]
=
=
=
=
c1*c2*s3
c1*s2*c3
s1*c2*c3
c1*c2*c3
+
+
s1*s2*c3;
s1*c2*s3;
c1*s2*s3;
s1*s2*s3;
}
/** @brief Returns an equivalent euler angle representation of
* this quaternion.
* @return Euler angles in roll-pitch-yaw order.
*/
TVector3 euler(void) const {
TVector3 euler;
const static double PI_OVER_2 = M_PI * 0.5;
const static double EPSILON = 1e-10;
+
+
+
+
q1.mData[0]*st1,
q1.mData[1]*st1,
q1.mData[2]*st1,
q1.mData[3]*st1);
}
/**
* @brief Returns pointer to the internal array.
*
* Array is in order x,y,z,w.
*/
double* row(uint32_t i) { return mData + i; }
// Const version of the above.
const double* row(uint32_t i) const { return mData + i; }
};
/**
* @brief Global operator allowing left-multiply by scalar.
*/
Quaternion operator*(double s, const Quaternion& q);
#endif /* QUATERNION_H */