#include "quaternion.h"
#include "matrix.h"


Quaternion& Quaternion::CopyFrom(const Quaternion &Q) {

	_data[0] = Q._data[0];
	_data[1] = Q._data[1];
	_data[2] = Q._data[2];
	_data[3] = Q._data[3];

	return *this;
}

ostream& operator<<(ostream &co, const Quaternion &Q) {

	co << "[ " << Q._data[0]
	   << ", " << Q._data[1]
	   << ", " << Q._data[2]
	   << ", " << Q._data[3] << " ]";

	return co;
}

void Quaternion::Normalize() {

	real l = fsqrt(_data[0]*_data[0] +
				   _data[1]*_data[1] +
				   _data[2]*_data[2] +
				   _data[3]*_data[3]);

	if (l) {
		_data[0] /= l;
		_data[1] /= l;
		_data[2] /= l;
		_data[3] /= l;
	}
	else
		Zero();
}

void Quaternion::SetAxisAngle(const real axis[3], real angle) {

	float l = 1.0f / fsqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);

	l *= fsin(angle / 2.0f);

	_data[0] = axis[0] * l;
	_data[1] = axis[1] * l;
	_data[2] = axis[2] * l;
	_data[3] = fcos(angle / 2.0f);
}

void Add(Quaternion &q1, const Quaternion &q2, const Quaternion &q3) {

	Vec4 t1(q2._data[0], q2._data[1], q2._data[2]);
	Vec4 t2(q3._data[0], q3._data[1], q3._data[2]);

	Vec4 t3, tf;
	Vec4FastCross3(t3, t2, t1);

	// can be optimized with Vec4FastAddScale()
	//
	t1 *= q3._data[3];
	t2 *= q2._data[3];
	//
	Vec4FastAdd(tf, t1, t2);
	Vec4FastAdd(tf, tf, t3);
	//

	tf.set_w(q2._data[3] * q3._data[3] - 
				(q2._data[0] * q3._data[0] +
				 q2._data[1] * q3._data[1] +
				 q2._data[2] * q3._data[2]));

	q1._data[0] = tf.x();
	q1._data[1] = tf.y();
	q1._data[2] = tf.z();
	q1._data[3] = tf.w();

	q1.Normalize();
}

void Quaternion::BuildMatrix(Matrix M) const {

    M[0][0] = 1.0 - 2.0 * (_data[1] * _data[1] + _data[2] * _data[2]);
    M[0][1] = 2.0 * (_data[0] * _data[1] - _data[2] * _data[3]);
    M[0][2] = 2.0 * (_data[2] * _data[0] + _data[1] * _data[3]);
    M[0][3] = 0.0;

    M[1][0] = 2.0 * (_data[0] * _data[1] + _data[2] * _data[3]);
    M[1][1] = 1.0 - 2.0 * (_data[2] * _data[2] + _data[0] * _data[0]);
    M[1][2] = 2.0 * (_data[1] * _data[2] - _data[0] * _data[3]);
    M[1][3] = 0.0;

    M[2][0] = 2.0 * (_data[2] * _data[0] - _data[1] * _data[3]);
    M[2][1] = 2.0 * (_data[1] * _data[2] + _data[0] * _data[3]);
    M[2][2] = 1.0 - 2.0 * (_data[1] * _data[1] + _data[0] * _data[0]);
    M[2][3] = 0.0;

    M[3][0] = 0.0;
    M[3][1] = 0.0;
    M[3][2] = 0.0;
    M[3][3] = 1.0;
}