nuclear@1: #include "quat.h"
nuclear@1: #include "vmath.h"
nuclear@1: 
nuclear@1: Quaternion::Quaternion()
nuclear@1: {
nuclear@1: 	s = 1.0;
nuclear@1: 	v.x = v.y = v.z = 0.0;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion::Quaternion(scalar_t s, const Vector3 &v)
nuclear@1: {
nuclear@1: 	this->s = s;
nuclear@1: 	this->v = v;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z)
nuclear@1: {
nuclear@1: 	v.x = x;
nuclear@1: 	v.y = y;
nuclear@1: 	v.z = z;
nuclear@1: 	this->s = s;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion::Quaternion(const Vector3 &axis, scalar_t angle)
nuclear@1: {
nuclear@1: 	set_rotation(axis, angle);
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion::Quaternion(const quat_t &quat)
nuclear@1: {
nuclear@1: 	v.x = quat.x;
nuclear@1: 	v.y = quat.y;
nuclear@1: 	v.z = quat.z;
nuclear@1: 	s = quat.w;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion Quaternion::operator +(const Quaternion &quat) const
nuclear@1: {
nuclear@1: 	return Quaternion(s + quat.s, v + quat.v);
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion Quaternion::operator -(const Quaternion &quat) const
nuclear@1: {
nuclear@1: 	return Quaternion(s - quat.s, v - quat.v);
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion Quaternion::operator -() const
nuclear@1: {
nuclear@1: 	return Quaternion(-s, -v);
nuclear@1: }
nuclear@1: 
nuclear@1: /** Quaternion Multiplication:
nuclear@1:  * Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
nuclear@1:  */
nuclear@1: Quaternion Quaternion::operator *(const Quaternion &quat) const
nuclear@1: {
nuclear@1: 	Quaternion newq;
nuclear@1: 	newq.s = s * quat.s - dot_product(v, quat.v);
nuclear@1: 	newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
nuclear@1: 	return newq;
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::operator +=(const Quaternion &quat)
nuclear@1: {
nuclear@1: 	*this = Quaternion(s + quat.s, v + quat.v);
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::operator -=(const Quaternion &quat)
nuclear@1: {
nuclear@1: 	*this = Quaternion(s - quat.s, v - quat.v);
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::operator *=(const Quaternion &quat)
nuclear@1: {
nuclear@1: 	*this = *this * quat;
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::reset_identity()
nuclear@1: {
nuclear@1: 	s = 1.0;
nuclear@1: 	v.x = v.y = v.z = 0.0;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion Quaternion::conjugate() const
nuclear@1: {
nuclear@1: 	return Quaternion(s, -v);
nuclear@1: }
nuclear@1: 
nuclear@1: scalar_t Quaternion::length() const
nuclear@1: {
nuclear@1: 	return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@1: }
nuclear@1: 
nuclear@1: /** Q * ~Q = ||Q||^2 */
nuclear@1: scalar_t Quaternion::length_sq() const
nuclear@1: {
nuclear@1: 	return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::normalize()
nuclear@1: {
nuclear@1: 	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@1: 	v.x /= len;
nuclear@1: 	v.y /= len;
nuclear@1: 	v.z /= len;
nuclear@1: 	s /= len;
nuclear@1: }
nuclear@1: 
nuclear@1: Quaternion Quaternion::normalized() const
nuclear@1: {
nuclear@1: 	Quaternion nq = *this;
nuclear@1: 	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@1: 	nq.v.x /= len;
nuclear@1: 	nq.v.y /= len;
nuclear@1: 	nq.v.z /= len;
nuclear@1: 	nq.s /= len;
nuclear@1: 	return nq;
nuclear@1: }
nuclear@1: 
nuclear@1: /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
nuclear@1: Quaternion Quaternion::inverse() const
nuclear@1: {
nuclear@1: 	Quaternion inv = conjugate();
nuclear@1: 	scalar_t lensq = length_sq();
nuclear@1: 	inv.v /= lensq;
nuclear@1: 	inv.s /= lensq;
nuclear@1: 
nuclear@1: 	return inv;
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle)
nuclear@1: {
nuclear@1: 	scalar_t half_angle = angle / 2.0;
nuclear@1: 	s = cos(half_angle);
nuclear@1: 	v = axis * sin(half_angle);
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::rotate(const Vector3 &axis, scalar_t angle)
nuclear@1: {
nuclear@1: 	Quaternion q;
nuclear@1: 	scalar_t half_angle = angle / 2.0;
nuclear@1: 	q.s = cos(half_angle);
nuclear@1: 	q.v = axis * sin(half_angle);
nuclear@1: 
nuclear@1: 	*this *= q;
nuclear@1: }
nuclear@1: 
nuclear@1: void Quaternion::rotate(const Quaternion &q)
nuclear@1: {
nuclear@1: 	*this = q * *this * q.conjugate();
nuclear@1: }
nuclear@1: 
nuclear@1: Matrix3x3 Quaternion::get_rotation_matrix() const
nuclear@1: {
nuclear@1: 	return Matrix3x3(
nuclear@1: 			1.0 - 2.0 * v.y*v.y - 2.0 * v.z*v.z,	2.0 * v.x * v.y - 2.0 * s * v.z,		2.0 * v.z * v.x + 2.0 * s * v.y,
nuclear@1: 			2.0 * v.x * v.y + 2.0 * s * v.z,		1.0 - 2.0 * v.x*v.x - 2.0 * v.z*v.z,	2.0 * v.y * v.z - 2.0 * s * v.x,
nuclear@1: 			2.0 * v.z * v.x - 2.0 * s * v.y,		2.0 * v.y * v.z + 2.0 * s * v.x,		1.0 - 2.0 * v.x*v.x - 2.0 * v.y*v.y);
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: /** Spherical linear interpolation (slerp) */
nuclear@1: Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t)
nuclear@1: {
nuclear@1: 	Quaternion q1;
nuclear@1: 	scalar_t dot = q1.s * q2.s + q1.v.x * q2.v.x + q1.v.y * q2.v.y + q1.v.z * q2.v.z;
nuclear@1: 
nuclear@1: 	if(dot < 0.0) {
nuclear@1: 		/* make sure we interpolate across the shortest arc */
nuclear@1: 		q1 = -quat1;
nuclear@1: 		dot = -dot;
nuclear@1: 	} else {
nuclear@1: 		q1 = quat1;
nuclear@1: 	}
nuclear@1: 
nuclear@1: 	/* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
nuclear@1: 	 * floating point imprecisions
nuclear@1: 	 */
nuclear@1: 	if(dot < -1.0) dot = -1.0;
nuclear@1: 	if(dot > 1.0) dot = 1.0;
nuclear@1: 
nuclear@1: 	scalar_t angle = acos(dot);
nuclear@1: 	scalar_t a, b;
nuclear@1: 
nuclear@1: 	scalar_t sin_angle = sin(angle);
nuclear@1: 	if(fabs(sin_angle) < SMALL_NUMBER) {
nuclear@1: 		/* for very small angles or completely opposite orientations
nuclear@1: 		 * use linear interpolation to avoid div/zero (in the first case it makes sense,
nuclear@1: 		 * the second case is pretty much undefined anyway I guess ...
nuclear@1: 		 */
nuclear@1: 		a = 1.0f - t;
nuclear@1: 		b = t;
nuclear@1: 	} else {
nuclear@1: 		a = sin((1.0f - t) * angle) / sin_angle;
nuclear@1: 		b = sin(t * angle) / sin_angle;
nuclear@1: 	}
nuclear@1: 
nuclear@1: 	scalar_t x = q1.v.x * a + q2.v.x * b;
nuclear@1: 	scalar_t y = q1.v.y * a + q2.v.y * b;
nuclear@1: 	scalar_t z = q1.v.z * a + q2.v.z * b;
nuclear@1: 	scalar_t s = q1.s * a + q2.s * b;
nuclear@1: 
nuclear@1: 	return Quaternion(s, Vector3(x, y, z));
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: std::ostream &operator <<(std::ostream &out, const Quaternion &q)
nuclear@1: {
nuclear@1: 	out << "(" << q.s << ", " << q.v << ")";
nuclear@1: 	return out;
nuclear@1: }