/* ScummVM - Graphic Adventure Engine
 *
 * ScummVM is the legal property of its developers, whose names
 * are too numerous to list here. Please refer to the COPYRIGHT
 * file distributed with this source distribution.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/*
 * Copyright (C) 2006-2010 - Frictional Games
 *
 * This file is part of HPL1 Engine.
 */

#include "hpl1/engine/math/Quaternion.h"

#include "hpl1/engine/math/Math.h"
#include "hpl1/engine/system/low_level_system.h"

namespace hpl {

//////////////////////////////////////////////////////////////////////////
// CONSTRUCTORS
//////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------

cQuaternion::cQuaternion() {
}

//-----------------------------------------------------------------------

cQuaternion::cQuaternion(float afAngle, const cVector3f &avAxis) {
	FromAngleAxis(afAngle, avAxis);
}

//-----------------------------------------------------------------------

//////////////////////////////////////////////////////////////////////////
// PUBLIC METHODS
//////////////////////////////////////////////////////////////////////////

const cQuaternion cQuaternion::Identity = cQuaternion(1.0f, 0.0f, 0.0f, 0.0f);

//-----------------------------------------------------------------------

void cQuaternion::Normalise() {
	float fLen = w * w + v.x * v.x + v.y * v.y + v.z * v.z;
	float fFactor = 1.0f / sqrt(fLen);
	v = v * fFactor;
	w = w * fFactor;
}

//-----------------------------------------------------------------------

void cQuaternion::ToRotationMatrix(cMatrixf &a_mtxDest) const {
	cMatrixf mtxA;

	float fTx = 2.0f * v.x;
	float fTy = 2.0f * v.y;
	float fTz = 2.0f * v.z;
	float fTwx = fTx * w;
	float fTwy = fTy * w;
	float fTwz = fTz * w;
	float fTxx = fTx * v.x;
	float fTxy = fTy * v.x;
	float fTxz = fTz * v.x;
	float fTyy = fTy * v.y;
	float fTyz = fTz * v.y;
	float fTzz = fTz * v.z;

	a_mtxDest.m[0][0] = 1.0f - (fTyy + fTzz);
	a_mtxDest.m[0][1] = fTxy - fTwz;
	a_mtxDest.m[0][2] = fTxz + fTwy;
	a_mtxDest.m[1][0] = fTxy + fTwz;
	a_mtxDest.m[1][1] = 1.0f - (fTxx + fTzz);
	a_mtxDest.m[1][2] = fTyz - fTwx;
	a_mtxDest.m[2][0] = fTxz - fTwy;
	a_mtxDest.m[2][1] = fTyz + fTwx;
	a_mtxDest.m[2][2] = 1.0f - (fTxx + fTyy);
}

//-----------------------------------------------------------------------

void cQuaternion::FromRotationMatrix(const cMatrix<float> &a_mtxRot) {
	float fTrace = a_mtxRot.m[0][0] + a_mtxRot.m[1][1] + a_mtxRot.m[2][2];
	float fRoot;

	if (fTrace > 0.0) {
		// |w| > 1/2, may as well choose w > 1/2
		fRoot = sqrt(fTrace + 1.0f); // 2w
		w = 0.5f * fRoot;
		fRoot = 0.5f / fRoot; // 1/(4w)
		v.x = (a_mtxRot.m[2][1] - a_mtxRot.m[1][2]) * fRoot;
		v.y = (a_mtxRot.m[0][2] - a_mtxRot.m[2][0]) * fRoot;
		v.z = (a_mtxRot.m[1][0] - a_mtxRot.m[0][1]) * fRoot;
	} else {
		// |w| <= 1/2
		static size_t s_iNext[3] = {1, 2, 0};
		size_t i = 0;
		if (a_mtxRot.m[1][1] > a_mtxRot.m[0][0])
			i = 1;
		if (a_mtxRot.m[2][2] > a_mtxRot.m[i][i])
			i = 2;
		size_t j = s_iNext[i];
		size_t k = s_iNext[j];

		fRoot = sqrt(a_mtxRot.m[i][i] - a_mtxRot.m[j][j] - a_mtxRot.m[k][k] + 1.0f);
		float *apkQuat[3] = {&v.x, &v.y, &v.z};
		*apkQuat[i] = 0.5f * fRoot;
		fRoot = 0.5f / fRoot;
		w = (a_mtxRot.m[k][j] - a_mtxRot.m[j][k]) * fRoot;
		*apkQuat[j] = (a_mtxRot.m[j][i] + a_mtxRot.m[i][j]) * fRoot;
		*apkQuat[k] = (a_mtxRot.m[k][i] + a_mtxRot.m[i][k]) * fRoot;
	}
}

//-----------------------------------------------------------------------

void cQuaternion::FromAngleAxis(float afAngle, const cVector3f &avAxis) {
	// assert:  axis[] is unit length
	//
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

	float fHalfAngle = 0.5f * afAngle;
	float fSin = sin(fHalfAngle);
	w = cos(fHalfAngle);
	v.x = fSin * avAxis.x;
	v.y = fSin * avAxis.y;
	v.z = fSin * avAxis.z;
}

//-----------------------------------------------------------------------

cQuaternion cQuaternion::operator+(const cQuaternion &aqB) const {
	cQuaternion qOut;
	qOut.v = v + aqB.v;
	qOut.w = w + aqB.w;

	return qOut;
}
//-----------------------------------------------------------------------

cQuaternion cQuaternion::operator-(const cQuaternion &aqB) const {
	cQuaternion qOut;
	qOut.v = v - aqB.v;
	qOut.w = w - aqB.w;

	return qOut;
}
//-----------------------------------------------------------------------
cQuaternion cQuaternion::operator*(const cQuaternion &aqB) const {
	cQuaternion qOut;

	qOut.w = w * aqB.w - v.x * aqB.v.x - v.y * aqB.v.y - v.z * aqB.v.z;
	qOut.v.x = w * aqB.v.x + v.x * aqB.w + v.y * aqB.v.z - v.z * aqB.v.y;
	qOut.v.y = w * aqB.v.y + v.y * aqB.w + v.z * aqB.v.x - v.x * aqB.v.z;
	qOut.v.z = w * aqB.v.z + v.z * aqB.w + v.x * aqB.v.y - v.y * aqB.v.x;

	return qOut;
}
//-----------------------------------------------------------------------
cQuaternion cQuaternion::operator*(float afScalar) const {
	cQuaternion qOut;
	qOut.v = v * afScalar;
	qOut.w = w * afScalar;

	return qOut;
}

//-----------------------------------------------------------------------
} // namespace hpl