quaternion.h
#ifndef CMU462_QUATERNION_H
#define CMU462_QUATERNION_H
#include "CMU462.h"
#include "matrix3x3.h"
#include "matrix4x4.h"
#include <iosfwd>
namespace CMU462 {
class Quaternion : public Vector4D {
public:
Quaternion( ) : Vector4D( 0.0, 0.0, 0.0, 1.0 ) { }
Quaternion(const Vector3D& v, double w) : Vector4D(v.x, v.y, v.z, w) { }
Quaternion(const Vector4D& v) : Vector4D(v.x, v.y, v.z, v.w) { }
Quaternion(double x, double y, double z, double w) : Vector4D(x, y, z, w) { }
void from_axis_angle(const Vector3D& axis, double radians) {
radians /= 2;
const Vector3D& nAxis = axis.unit();
double sinTheta = sin(radians);
x = sinTheta * nAxis.x;
y = sinTheta * nAxis.y;
z = sinTheta * nAxis.z;
w = cos(radians);
this->normalize();
}
Vector3D complex() const { return Vector3D(x, y, z); }
void setComplex(const Vector3D& c)
{
x = c.x;
y = c.y;
z = c.z;
}
double real() const { return w; }
void setReal(double r) { w = r; }
Quaternion conjugate(void) const
{
return Quaternion(-complex(), real());
}
Quaternion inverse(void) const
{
return conjugate() / norm();
}
Quaternion product(const Quaternion& rhs) const {
return Quaternion(y*rhs.z - z*rhs.y + x*rhs.w + w*rhs.x,
z*rhs.x - x*rhs.z + y*rhs.w + w*rhs.y,
x*rhs.y - y*rhs.x + z*rhs.w + w*rhs.z,
w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z);
}
Quaternion operator*(const Quaternion& rhs) const {
return product(rhs);
}
Matrix4x4 matrix() const {
double m[16] = {
w, -z, y, x,
z, w, -x, y,
-y, x, w, z,
-x, -y, -z, w
};
return Matrix4x4(m);
}
Matrix4x4 rightMatrix() const {
double m[16] = {
+w, -z, y, -x,
+z, w, -x, -y,
-y, x, w, -z,
+x, y, z, w
};
return Matrix4x4(m);
}
Vector4D vector() const { return Vector4D(x, y, z, w); }
Matrix3x3 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 Matrix3x3(m);
}
Vector3D scaledAxis(void) const{
Quaternion q1 = (Quaternion)unit();
// Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
double angle = 2 * acos(q1.w);
// s must be positive, because q1 <= 1, due to normalization.
double s = sqrt(1-q1.w*q1.w);
// Avoid dividing by 0.
if (s < 0.001)
{
// if s close to zero then direction of axis not important
// if it is important that axis is normalised then replace with x=1; y=z=0;
return Vector3D(q1.x, q1.y, q1.z);
}
else
{
// normalise axis
return Vector3D(q1.x / s, q1.y / s, q1.z / s);
}
// NEVER getsgg HERE.
}
void scaledAxis(const Vector3D& vec_in)
{
double theta = vec_in.norm();
// Small magnitudes are handled via the default vector.
if (theta > 0.0001)
{
double s = sin(theta / 2.0);
Vector3D W(vec_in / theta * s);
x = W.x;
y = W.y;
z = W.z;
w = cos(theta / 2.0);
}
else
{
x = y = z = 0;
w = 1.0;
}
}
Vector3D rotatedVector(const Vector3D& v) const {
return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
}
void euler(const Vector3D& 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);
x = c1*c2*s3 - s1*s2*c3;
y = c1*s2*c3 + s1*c2*s3;
z = s1*c2*c3 - c1*s2*s3;
w = c1*c2*c3 + s1*s2*s3;
}
Vector3D euler(void) const
{
Vector3D euler;
const static double PI_OVER_2 = M_PI * 0.5;
double sqw, sqx, sqy, sqz;
// quick conversion to Euler angles to give tilt to user
sqw = w*w;
sqx = x*x;
sqy = y*y;
sqz = z*z;
euler[1] = asin(2.0 * (w*y - x*z));
if (PI_OVER_2 - fabs(euler[1]) > EPS_D) {
euler[2] = atan2(2.0 * (x*y + w*z),
sqx - sqy - sqz + sqw);
euler[0] = atan2(2.0 * (w*x + y*z),
sqw - sqx - sqy + sqz);
}
else
{
// compute heading from local 'down' vector
euler[2] = atan2(2*y*z - 2*x*w,
2*x*z + 2*y*w);
euler[0] = 0.0;
// If facing down, reverse yaw
if (euler[1] < 0)
{
euler[2] = M_PI - euler[2];
}
}
return euler;
}
void decoupleZ(Quaternion* Qxy, Quaternion* Qz) const {
Vector3D ztt(0,0,1);
Vector3D zbt = this->rotatedVector(ztt);
Vector3D axis_xy = cross(ztt, zbt);
double axis_norm = axis_xy.norm();
double axis_theta = acos(clamp(zbt.z, -1.0, 1.0));
if (axis_norm > 0.00001)
{
axis_xy = axis_xy * (axis_theta/axis_norm); // limit is *1
}
Qxy->scaledAxis(axis_xy);
*Qz = (Qxy->conjugate() * (*this));
}
Quaternion slerp(const Quaternion& q1, double t)
{
return slerp(*this, q1, t);
}
static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, double t) {
double omega = acos(clamp(q0.x*q1.x +
q0.y*q1.y +
q0.z*q1.z +
q0.w*q1.w, -1.0, 1.0));
if (fabs(omega) < 1e-10)
{
omega = 1e-10;
}
double som = sin(omega);
double st0 = sin((1-t) * omega) / som;
double st1 = sin(t * omega) / som;
return Quaternion(q0.x*st0 + q1.x*st1,
q0.y*st0 + q1.y*st1,
q0.z*st0 + q1.z*st1,
q0.w*st0 + q1.w*st1);
}
};
Quaternion operator*(double s, const Quaternion& q);
} // namespace CMU462
#endif /* CMU462_QUATERNION_H */
CMU462::Vector4D::unit
Vector4D unit(void) const
Returns unit vector.
Definition: vector4D.h:137
CMU462
Definition: CMU462.h:8
CMU462::Vector4D::norm
double norm(void) const
Returns Euclidean distance metric extended to 4 dimensions.
Definition: vector4D.h:123
CMU462::Vector4D::Vector4D
Vector4D()
Constructor.
Definition: vector4D.h:25
CMU462::Vector4D::normalize
void normalize(void)
Divides by Euclidean length.
Definition: vector4D.h:146