Quaternion Cpp

#include "Quaternion.h"
#include <cmath>

// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
// http://www.flipcode.com/documents/matrfaq.html
// http://db-in.com/blog/2011/04/cameras-on-opengl-es-2-x/
// http://wiki.beyondunreal.com/Legacy:Quaternion
// http://clb.demon.fi/MathGeoLib/docs/float3x3.cpp_code.html#612
// http://clb.demon.fi/MathGeoLib/docs/Quat_summary.php
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
// http://www.koders.com/cpp/fid6E231B19C37F95D54405AEADD2C7CE1E84C5CEF1.aspx
// http://forums.create.msdn.com/forums/t/4574.aspx

const Quaternion Quaternion::identity(1.0f, 0.0f, 0.0f, 0.0f);

Quaternion::Quaternion() : w(0.0f), x(0.0f), y(0.0f), z(0.0f) { }
Quaternion::Quaternion(float W, float X, float Y, float Z) : w(W), x(X), y(Y), z(Z) { }
Quaternion::~Quaternion() { }

Quaternion::Quaternion(const Quaternion& q) {
  w = q.w;
	x = q.x;
	y = q.y;
	z = q.z;
}

Quaternion& Quaternion::operator=(const Quaternion& q) {
	if (this == &q) return *this;
	w = q.w;
	x = q.x;
	y = q.y;
	z = q.z;
	return *this;
}

Matrix Quaternion::ToMatrix(Quaternion& q) {
	float sqw = q.w*q.w;
    float sqx = q.x*q.x;
    float sqy = q.y*q.y;
    float sqz = q.z*q.z;
    float invs = 1.0f / (sqx + sqy + sqz + sqw);

	Matrix matrix = Matrix::identity;

	matrix.mat[0] = ( sqx - sqy - sqz + sqw) * invs;
    matrix.mat[5] = (-sqx + sqy - sqz + sqw) * invs;
    matrix.mat[10] = (-sqx - sqy + sqz + sqw) * invs;
    
    float tmp1 = q.x*q.y;
    float tmp2 = q.z*q.w;
    matrix.mat[4] = 2.0 * (tmp1 + tmp2) * invs;
    matrix.mat[1] = 2.0 * (tmp1 - tmp2) * invs;
    
    tmp1 = q.x*q.z;
    tmp2 = q.y*q.w;
    matrix.mat[8] = 2.0 * (tmp1 - tmp2) * invs;
    matrix.mat[2] = 2.0 * (tmp1 + tmp2) * invs;

    tmp1 = q.y*q.z;
    tmp2 = q.x*q.w;
    matrix.mat[9] = 2.0 * (tmp1 + tmp2) * invs;
    matrix.mat[6] = 2.0 * (tmp1 - tmp2) * invs; 

	return matrix;
}

bool Quaternion::operator==(Quaternion& rhs) {
	return (w == rhs.w && x == rhs.x && y == rhs.y && z == rhs.z);
}

bool Quaternion::operator!=(Quaternion& rhs) {
	return !(w == rhs.w && x == rhs.x && y == rhs.y && z == rhs.z);
}

Quaternion& Quaternion::operator*=(Quaternion& rhs) {
	Quaternion q;
	
	q.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
	q.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
	q.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x;
	q.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w;

	*this = q;
	return *this;
}

Quaternion& Quaternion::operator*=(const Quaternion& rhs) {
	Quaternion q;
	
	q.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
	q.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
	q.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x;
	q.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w;

	*this = q;
	return *this;
}

Quaternion Quaternion::operator*(Quaternion& rhs) {
	Quaternion q;

	q.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
	q.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
	q.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x;
	q.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w;

	return q;
}

const Quaternion Quaternion::operator*(const Quaternion& rhs) {
	Quaternion q;

	q.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
	q.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
	q.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x;
	q.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w;

	return q;
}

Quaternion Quaternion::Normalized() {
	float mag = sqrtf(w * w + x * x + y * y + z * z);
	return Quaternion(w / mag, x / mag, y / mag, z / mag);
}

Quaternion Quaternion::Conjugate() {
	return Quaternion(w, -x, -y, -z);
}

void Quaternion::Normalize() {
	float mag = sqrtf(w * w + x * x + y * y + z * z);

	w /= mag;
	x /= mag;
	y /= mag;
	z /= mag;
}

Vector Quaternion::operator*(Vector& rhs) {
	return Quaternion::ToMatrix(*this) * rhs;
}

const Vector operator*(Vector& v, const Quaternion& m) {
	return Quaternion::ToMatrix((Quaternion)m) * v;
}

Vector operator*(Vector& v, Quaternion& m) {
	return Quaternion::ToMatrix(m) * v;
}

const Vector Quaternion::operator*(const Vector& rhs) {
	return Quaternion::ToMatrix(*this) * rhs;
}

Quaternion Quaternion::AngleAxis(float angle, Vector& axis) {
	Vector vn = axis.Normalized();

	angle *=  0.0174532925f; // To radians!
	angle *= 0.5f;	
	float sinAngle = sin(angle);
	
	return Quaternion(cos(angle), vn.x * sinAngle, vn.y * sinAngle, vn.z * sinAngle);
}

Quaternion Quaternion::Euler(float X, float Y, float Z) {
	X *= 0.0174532925f; // To radians!
	Y *= 0.0174532925f; // To radians!
	Z *= 0.0174532925f; // To radians!

	X *= 0.5f;
	Y *= 0.5f;
	Z *= 0.5f;

	float sinx = sinf(X);
	float siny = sinf(Y);
	float sinz = sinf(Z);
	float cosx = cosf(X);
	float cosy = cosf(Y);
	float cosz = cosf(Z);

	Quaternion q;

	q.w = cosx * cosy * cosz + sinx * siny * sinz;
	q.x = sinx * cosy * cosz + cosx * siny * sinz;
	q.y = cosx * siny * cosz - sinx * cosy * sinz;
	q.z = cosx * cosy * sinz - sinx * siny * cosz;

	return q;
}

void Quaternion::SetEuler(float X, float Y, float Z) {
	X *= 0.0174532925f; // To radians!
	Y *= 0.0174532925f; // To radians!
	Z *= 0.0174532925f; // To radians!

	X *= 0.5f;
	Y *= 0.5f;
	Z *= 0.5f;

	float sinx = sinf(X);
	float siny = sinf(Y);
	float sinz = sinf(Z);
	float cosx = cosf(X);
	float cosy = cosf(Y);
	float cosz = cosf(Z);

	w = cosx * cosy * cosz + sinx * siny * sinz;
	x = sinx * cosy * cosz + cosx * siny * sinz;
	y = cosx * siny * cosz - sinx * cosy * sinz;
	z = cosx * cosy * sinz - sinx * siny * cosz;
}

void Quaternion::ToAngleAxis(float* angle, Vector* axis) {
	*angle = acosf(w);
	float sinz = sinf(*angle);

	if (fabsf(sinz) > 1e-4f) {
		sinz = 1.0f / sinz;

		axis->x = x * sinz;
		axis->y = y * sinz;
		axis->z = z * sinz;
		
		*angle *= 2.0f * 57.2957795f;
		if (*angle > 180.0f)
			*angle = 360.0f - *angle;
	} else {
		*angle = 0.0f;
		axis->x = 1.0f;
		axis->y = 0.0f;
		axis->z = 0.0f;
	}
}

Quaternion Quaternion::Inverse(Quaternion& rotation) {
	return Quaternion(rotation.w, -1.0f * rotation.x, -1.0f * rotation.y, -1.0f * rotation.z);
}

float Quaternion::Dot(Quaternion& a, Quaternion& b) {
	return (a.w * b.w + a.x * b.x * a.y * b.y + a.z * b.z);
}

float Quaternion::Dot(Quaternion& b) {
	return (w * b.w + x * b.x * y * b.y + z * b.z);
}

float Quaternion::Angle(Quaternion& a, Quaternion& b) {
	float degrees = acosf((b * Quaternion::Inverse(a)).w) * 2.0f * 57.2957795f;
	if (degrees > 180.0f)
		return 360.0f - degrees;
	return degrees;
}

Quaternion operator*(float f, Quaternion& m) {
	return Quaternion(m.w * f, m.x * f, m.y * f, m.z * f);
}

const Quaternion operator*(float f, const Quaternion& m) {
	return Quaternion(m.w * f, m.x * f, m.y * f, m.z * f);
}

Quaternion Quaternion::operator*(float& f) {
	return Quaternion(w * f, x * f, y * f, z * f);
}

const Quaternion Quaternion::operator*(const float& f) {
	return Quaternion(w * f, x * f, y * f, z * f);
}

Quaternion Quaternion::operator+(Quaternion& rhs) {
	return Quaternion(w + rhs.w, x + rhs.x, y + rhs.y, z + rhs.z);
}

const Quaternion Quaternion::operator+(const Quaternion& rhs) {
	return Quaternion(w + rhs.w, x + rhs.x, y + rhs.y, z + rhs.z);
}

Quaternion Quaternion::Lerp(Quaternion& from, Quaternion& to, float t) {
	Quaternion src = from * (1.0f - t);
	Quaternion dst = to * t;

	Quaternion q = src + dst;
	return q;
}

Quaternion Quaternion::Slerp(Quaternion& from, Quaternion& to, float t) {
	float cosTheta = Quaternion::Dot(from, to);
	Quaternion temp(to);

	if (cosTheta < 0.0f) {
		cosTheta *= -1.0f;
		temp = temp * -1.0f;
	}

	float theta = acosf(cosTheta);
	float sinTheta = 1.0f / sinf(theta);

	return sinTheta * (
		((Quaternion)(from * sinf(theta * (1.0f - t)))) + 
		((Quaternion)(temp * sinf(t * theta)))
		);
}

#define m00 right.x
#define m01 up.x
#define m02 forward.x
#define m10 right.y
#define m11 up.y
#define m12 forward.y
#define m20 right.z
#define m21 up.z
#define m22 forward.z

Quaternion Quaternion::LookRotation(Vector& lookAt, Vector& upDirection) {
	Vector forward = lookAt; Vector up = upDirection;
	Vector::OrthoNormalize(&forward, &up);
	Vector right = Vector::Cross(up, forward);

	Quaternion ret;
	ret.w = sqrtf(1.0f + m00 + m11 + m22) * 0.5f;
	float w4_recip = 1.0f / (4.0f * ret.w);
	ret.x = (m21 - m12) * w4_recip;
	ret.y = (m02 - m20) * w4_recip;
	ret.z = (m10 - m01) * w4_recip;

	return ret;
}

Quaternion Quaternion::LookRotation(Vector& lookAt) {
	Vector up = (Vector)Vector::up;
	Vector forward = lookAt;
	Vector::OrthoNormalize(&forward, &up);
	Vector right = Vector::Cross(up, forward);

	Quaternion ret;
	ret.w = sqrtf(1.0f + m00 + m11 + m22) * 0.5f;
	float w4_recip = 1.0f / (4.0f * ret.w);
	ret.x = (m21 - m12) * w4_recip;
	ret.y = (m02 - m20) * w4_recip;
	ret.z = (m10 - m01) * w4_recip;

	return ret;
}

void Quaternion::SetLookRotation(Vector& lookAt) {
	Vector up = (Vector)Vector::up;
	Vector forward = lookAt;
	Vector::OrthoNormalize(&forward, &up);
	Vector right = Vector::Cross(up, forward);

	w = sqrtf(1.0f + m00 + m11 + m22) * 0.5f;
	float w4_recip = 1.0f / (4.0f * w);
	x = (m21 - m12) * w4_recip;
	y = (m02 - m20) * w4_recip;
	z = (m10 - m01) * w4_recip;
}

void Quaternion::SetLookRotation(Vector& lookAt, Vector& upDirection) {
	Vector forward = lookAt; Vector up = upDirection;
	Vector::OrthoNormalize(&forward, &up);
	Vector right = Vector::Cross(up, forward);

	w = sqrtf(1.0f + m00 + m11 + m22) * 0.5f;
	float w4_recip = 1.0f / (4.0f * w);
	x = (m21 - m12) * w4_recip;
	y = (m02 - m20) * w4_recip;
	z = (m10 - m01) * w4_recip;
}

#undef m00
#undef m01
#undef m02
#undef m10
#undef m11
#undef m12
#undef m20
#undef m21
#undef m22

Quaternion H

#ifndef _H_QUATERNION_
#define _H_QUATERNION_

#ifndef _H_MATRIX_
#include "Matrix.h"
#endif

class Quaternion {
public:
  float w, x, y, z;
public:
	Quaternion();
	Quaternion(float W, float X, float Y, float Z);
	Quaternion(const Quaternion& q);
	~Quaternion();
	Quaternion& operator=(const Quaternion&);

	bool operator==(Quaternion& rhs);
	bool operator!=(Quaternion& rhs);
	
	Quaternion& operator*=(Quaternion& rhs);
	Quaternion& operator*=(const Quaternion& rhs);
	Quaternion operator*(Quaternion& rhs);
	const Quaternion operator*(const Quaternion& rhs);
	Quaternion operator*(float& rhs);
	const Quaternion operator*(const float& rhs);
	Quaternion operator+(Quaternion& rhs);
	const Quaternion operator+(const Quaternion& rhs);
	Vector operator*(Vector& rhs);
	const Vector operator*(const Vector& rhs);

	Quaternion Normalized();
	void Normalize();
	Quaternion Conjugate(); // Same as inverse

	void ToAngleAxis(float* angle, Vector* axis);
	void SetEuler(float X, float Y, float Z);
	float Dot(Quaternion& b);
	void SetLookRotation(Vector& lookAt);
	void SetLookRotation(Vector& lookAt, Vector& upDirection);

	static Quaternion LookRotation(Vector& lookAt);
	static Quaternion LookRotation(Vector& lookAt, Vector& upDirection);
	static Quaternion Slerp(Quaternion& from, Quaternion& to, float t);
	static Quaternion Lerp(Quaternion& from, Quaternion& to, float t);
	static float Angle(Quaternion& a, Quaternion& b);
	static float Dot(Quaternion& a, Quaternion& b);
	static Quaternion AngleAxis(float angle, Vector& axis);
	static Quaternion Inverse(Quaternion& rotation);
	static Quaternion Euler(float X, float Y, float Z);
	static Matrix ToMatrix(Quaternion& q);

	static const Quaternion identity;
};

const Vector operator*(Vector& v, const Quaternion& m);
Vector operator*(Vector& v, Quaternion& m);
Quaternion operator*(float f, Quaternion& m);
const Quaternion operator*(float f, const Quaternion& m);

#endif