// Residual - Virtual machine to run LucasArts' 3D adventure games
// Copyright (C) 2003-2005 The ScummVM-Residual Team (www.scummvm.org)
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library 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
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA

#include "matrix3.h"

#include <math.h>

void Matrix3::setAsIdentity() {
	_right.set(1.f, 0.f, 0.f);
	_up.set(0.f, 1.f, 0.f);
	_at.set(0.f, 0.f, 0.f);
}

void Matrix3::buildFromPitchYawRoll(float pitch, float yaw, float roll) {
	Matrix3 temp1, temp2;

	temp1.constructAroundPitch(pitch);
	constructAroundRoll(roll);

	(*this) *= temp1;

	temp2.constructAroundYaw(yaw);

	(*this) *= temp2;
}

#define MYPI 3.14159265358979323846
#define DEGTORAD(a) (a * MYPI / 180.0) 
#define RADTODEG(a) (a * 180.0 / MYPI) 

float RadianToDegree(float rad) {
	return (float)RADTODEG(rad);
}

float DegreeToRadian(float degrees) {
	return (float)DEGTORAD(degrees);
}

// right
void Matrix3::constructAroundPitch(float pitch) {
	float cosa;
	float sina;

	cosa = (float)cos(DegreeToRadian(pitch));
	sina = (float)sin(DegreeToRadian(pitch));

	_right.set(1.f, 0.f, 0.f);
	_up.set(0.f, cosa, -sina);
	_at.set(0.f, sina, cosa);
}

// up
void Matrix3::constructAroundYaw(float yaw) {
	float cosa;
	float sina;

	cosa = (float)cos(DegreeToRadian(yaw));
	sina = (float)sin(DegreeToRadian(yaw));

	_right.set(cosa, 0.f, sina);
	_up.set(0.f, 1.f, 0.f);
	_at.set(-sina, 0.f, cosa);
}

// at
void Matrix3::constructAroundRoll(float roll) {
	float cosa;
	float sina;

	cosa = (float)cos(DegreeToRadian(roll));
	sina = (float)sin(DegreeToRadian(roll));

	_right.set(cosa, -sina, 0.f);
	_up.set(sina, cosa, 0.f);
	_at.set(0.f, 0.f, 1.f);
}

/*
0 1 2 3
4 5 6 7
8 9 10 11
*/

// WARNING: Still buggy in some occasions.
void Matrix3::getPitchYawRoll(float* pPitch, float* pYaw, float* pRoll) {
	float D;
	float C;
	float ftrx;
	float ftry;
	float angle_x;
	float angle_y;
	float angle_z;

	angle_y = D = asin(_right.z());        /* Calculate Y-axis angle */
	C			= cos(angle_y);
	angle_y		= RadianToDegree(angle_y);

	if (fabs( C ) > 0.005) {            /* Gimball lock? */
		ftrx		=  _at.z() / C;           /* No, so get X-axis angle */
		ftry		= -_up.z() / C;

		angle_x		= RadianToDegree(atan2(ftry, ftrx));

		ftrx		=  _right.x() / C;            /* Get Z-axis angle */
		ftry		= -_right.y() / C;

		angle_z		= RadianToDegree(atan2(ftry, ftrx));
	} else {                                 /* Gimball lock has occurred */
		angle_x		= 0;                      /* Set X-axis angle to zqero */

		ftrx		= _up.y();                 /* And calculate Z-axis angle */
		ftry		= _up.x();

		angle_z  = RadianToDegree(atan2(ftry, ftrx));
	}

	/* return only positive angles in [0,360] */
	if (angle_x < 0) angle_x += 360;
	if (angle_y < 0) angle_y += 360;
	if (angle_z < 0) angle_z += 360;

	if (pPitch)
		*pPitch = angle_x;

	if (pYaw)
		*pYaw = angle_y;

	if (pRoll)
		*pRoll = angle_z;
}

float Matrix3::getPitch() {
	float pitch;

	getPitchYawRoll(&pitch, 0, 0);

	return pitch;
}

float Matrix3::getYaw() {
	float yaw;

	getPitchYawRoll(0, &yaw, 0);

	return yaw;
}

float Matrix3::getRoll() {
	float roll;

	getPitchYawRoll(0, 0, &roll);

	return roll;
}

void Matrix3::transform(Vector3d* v) {
	float x;
	float y;
	float z;

	x = v->dotProduct(_right.x(), _up.x(), _at.x());
	y = v->dotProduct(_right.x(), _up.x(), _at.x());
	z = v->dotProduct(_right.x(), _up.x(), _at.x());

	v->set(x, y, z);
}