/* 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/>.
 *
 */

////////////////////////////////////////////////////////////////////////////////
//	XMath 1.0
//	Vector - Matrix library for 3D visualisation and
//	physics modeling.
//
//	(C) 1999 KDV Games
//	Author: Alexandre Kotliar
//
//
////////////////////////////////////////////////////////////////////////////////
#include "qdengine/xmath.h"


namespace QDEngine {


///////////////////////////////////////////////////////////////////////////////
//
//	Constants
//
///////////////////////////////////////////////////////////////////////////////

const Vect3f Vect3f::ZERO(0, 0, 0);
const QuatF QuatF::ID(1, 0, 0, 0);
const QuatD QuatD::ID(1, 0, 0, 0);

///////////////////////////////////////////////////////////////////////////////
//
//  class Vect2f
//
///////////////////////////////////////////////////////////////////////////////

float Vect2f::angle(const Vect2f &other) const {
	return acosf(dot(other) * invSqrtFast(norm2() * other.norm2() * 1.0000001f));
}

///////////////////////////////////////////////////////////////////////////////
//
//  class Vect3f
//
///////////////////////////////////////////////////////////////////////////////

float Vect3f::angle(const Vect3f &other) const {
	return acosf(dot(other) * invSqrtFast(norm2() * other.norm2() * 1.0000001f));
}

///////////////////////////////////////////////////////////////////////////////
//
//  class Mat3d
//
///////////////////////////////////////////////////////////////////////////////

Mat3d &Mat3d::set(const Vect3d &axis, double angle, int normalizeAxis) {
	QuatD q;

	q.set(angle, axis, normalizeAxis);
	set(q);
	return *this;
}

Mat3d &Mat3d::set(const QuatD &q) {
	xx = 2.0 * (q.s_ * q.s_ + q.x_ * q.x_ - 0.5);
	yy = 2.0 * (q.s_ * q.s_ + q.y_ * q.y_ - 0.5);
	zz = 2.0 * (q.s_ * q.s_ + q.z_ * q.z_ - 0.5);

	xy = 2.0 * (q.y_ * q.x_ - q.z_ * q.s_);
	yx = 2.0 * (q.x_ * q.y_ + q.z_ * q.s_);


	yz = 2.0 * (q.z_ * q.y_ - q.x_ * q.s_);
	zy = 2.0 * (q.y_ * q.z_ + q.x_ * q.s_);

	zx = 2.0 * (q.x_ * q.z_ - q.y_ * q.s_);
	xz = 2.0 * (q.z_ * q.x_ + q.y_ * q.s_);

	return *this;
}

Mat3d &Mat3d::set(const Vect3d &x_from, const Vect3d &y_from, const Vect3d &z_from,
                  const Vect3d &x_to, const Vect3d &y_to, const Vect3d &z_to) {
	xx = dot(x_from, x_to);
	xy = dot(y_from, x_to);
	xz = dot(z_from, x_to);

	yx = dot(x_from, y_to);
	yy = dot(y_from, y_to);
	yz = dot(z_from, y_to);

	zx = dot(x_from, z_to);
	zy = dot(y_from, z_to);
	zz = dot(z_from, z_to);
	return *this;
}


int Mat3d::invert(const Mat3d &M) {
	double D, oneOverDet;

	if (fabs(D = M.det()) < 1.0e-12) return 1; // not invertible
	oneOverDet = 1 / D;

	xx = (M.yy * M.zz - M.yz * M.zy) * oneOverDet;
	xy = (M.xz * M.zy - M.xy * M.zz) * oneOverDet;
	xz = (M.xy * M.yz - M.xz * M.yy) * oneOverDet;
	yx = (M.yz * M.zx - M.yx * M.zz) * oneOverDet;
	yy = (M.xx * M.zz - M.xz * M.zx) * oneOverDet;
	yz = (M.xz * M.yx - M.xx * M.yz) * oneOverDet;
	zx = (M.yx * M.zy - M.yy * M.zx) * oneOverDet;
	zy = (M.xy * M.zx - M.xx * M.zy) * oneOverDet;
	zz = (M.xx * M.yy - M.xy * M.yx) * oneOverDet;
	return 0;
}



int Mat3d::invert() {
	double D, oneOverDet;
	double oxy, oyz, ozx, oyx, ozy, oxz, oxx, oyy;

	if (fabs(D = det()) < 1.0e-12) return 1; // not invertible
	oneOverDet = 1 / D;

	oxx = xx;
	oyy = yy;
	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = (oxz * ozy - zz * oxy) * oneOverDet;
	yz = (oxz * oyx - xx * oyz) * oneOverDet;
	zx = (oyx * ozy - yy * ozx) * oneOverDet;
	yx = (oyz * ozx - oyx * zz) * oneOverDet;
	zy = (oxy * ozx - ozy * xx) * oneOverDet;
	xz = (oxy * oyz - oxz * yy) * oneOverDet;
	xx = (oyy * zz - oyz * ozy) * oneOverDet;
	yy = (oxx * zz - oxz * ozx) * oneOverDet;
	zz = (oxx * oyy - oxy * oyx) * oneOverDet;
	return 0;
}


Mat3d &Mat3d::mult(const Mat3d &M, const Mat3d &N) {
	xx = M.xx * N.xx + M.xy * N.yx + M.xz * N.zx;
	xy = M.xx * N.xy + M.xy * N.yy + M.xz * N.zy;
	xz = M.xx * N.xz + M.xy * N.yz + M.xz * N.zz;
	yx = M.yx * N.xx + M.yy * N.yx + M.yz * N.zx;
	yy = M.yx * N.xy + M.yy * N.yy + M.yz * N.zy;
	yz = M.yx * N.xz + M.yy * N.yz + M.yz * N.zz;
	zx = M.zx * N.xx + M.zy * N.yx + M.zz * N.zx;
	zy = M.zx * N.xy + M.zy * N.yy + M.zz * N.zy;
	zz = M.zx * N.xz + M.zy * N.yz + M.zz * N.zz;

	return *this;
}


Mat3d &Mat3d::premult(const Mat3d &M) {
	double oxy, oyz, ozx, oyx, ozy, oxz;

	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = M.xx * oxy + M.xy * yy  + M.xz * ozy;
	xz = M.xx * oxz + M.xy * oyz + M.xz * zz;
	yx = M.yx * xx  + M.yy * oyx + M.yz * ozx;
	yz = M.yx * oxz + M.yy * oyz + M.yz * zz;
	zx = M.zx * xx  + M.zy * oyx + M.zz * ozx;
	zy = M.zx * oxy + M.zy * yy  + M.zz * ozy;

	xx = M.xx * xx  + M.xy * oyx + M.xz * ozx;
	yy = M.yx * oxy + M.yy * yy  + M.yz * ozy;
	zz = M.zx * oxz + M.zy * oyz + M.zz * zz;

	return *this;
}


Mat3d &Mat3d::postmult(const Mat3d &M) {
	double oxy, oyz, ozx, oyx, ozy, oxz;

	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = xx *  M.xy + oxy * M.yy + oxz * M.zy;
	xz = xx *  M.xz + oxy * M.yz + oxz * M.zz;
	yx = oyx * M.xx + yy  * M.yx + oyz * M.zx;
	yz = oyx * M.xz + yy  * M.yz + oyz * M.zz;
	zx = ozx * M.xx + ozy * M.yx + zz  * M.zx;
	zy = ozx * M.xy + ozy * M.yy + zz  * M.zy;

	xx = xx  * M.xx + oxy * M.yx + oxz * M.zx;
	yy = oyx * M.xy + yy  * M.yy + oyz * M.zy;
	zz = ozx * M.xz + ozy * M.yz + zz  * M.zz;

	return *this;
}



///////////////////////////////////////////////////////////////////////////////
//
//  class Mat3f
//
///////////////////////////////////////////////////////////////////////////////

Mat3f &Mat3f::set(const Vect3f &axis, float angle, int normalizeAxis) {
	QuatF q;

	q.set(angle, axis, normalizeAxis);
	set(q);
	return *this;
}

Mat3f &Mat3f::set(const QuatF &q) {
	xx = 2.f * (q.s_ * q.s_ + q.x_ * q.x_ - 0.5f);
	yy = 2.f * (q.s_ * q.s_ + q.y_ * q.y_ - 0.5f);
	zz = 2.f * (q.s_ * q.s_ + q.z_ * q.z_ - 0.5f);

	xy = 2.f * (q.y_ * q.x_ - q.z_ * q.s_);
	yx = 2.f * (q.x_ * q.y_ + q.z_ * q.s_);


	yz = 2.f * (q.z_ * q.y_ - q.x_ * q.s_);
	zy = 2.f * (q.y_ * q.z_ + q.x_ * q.s_);

	zx = 2.f * (q.x_ * q.z_ - q.y_ * q.s_);
	xz = 2.f * (q.z_ * q.x_ + q.y_ * q.s_);

	return *this;
}

Mat3f &Mat3f::set(const Vect3f &x_from, const Vect3f &y_from, const Vect3f &z_from,
                  const Vect3f &x_to, const Vect3f &y_to, const Vect3f &z_to) {
	xx = dot(x_from, x_to);
	xy = dot(y_from, x_to);
	xz = dot(z_from, x_to);

	yx = dot(x_from, y_to);
	yy = dot(y_from, y_to);
	yz = dot(z_from, y_to);

	zx = dot(x_from, z_to);
	zy = dot(y_from, z_to);
	zz = dot(z_from, z_to);
	return *this;
}

int Mat3f::invert(const Mat3f &M) {
	float D, oneOverDet;

	if (fabs(D = M.det()) < 1.0e-12) return 1; // not invertible
	oneOverDet = 1 / D;

	xx = (M.yy * M.zz - M.yz * M.zy) * oneOverDet;
	xy = (M.xz * M.zy - M.xy * M.zz) * oneOverDet;
	xz = (M.xy * M.yz - M.xz * M.yy) * oneOverDet;
	yx = (M.yz * M.zx - M.yx * M.zz) * oneOverDet;
	yy = (M.xx * M.zz - M.xz * M.zx) * oneOverDet;
	yz = (M.xz * M.yx - M.xx * M.yz) * oneOverDet;
	zx = (M.yx * M.zy - M.yy * M.zx) * oneOverDet;
	zy = (M.xy * M.zx - M.xx * M.zy) * oneOverDet;
	zz = (M.xx * M.yy - M.xy * M.yx) * oneOverDet;
	return 0;
}



int Mat3f::invert() {
	float D, oneOverDet;
	float oxy, oyz, ozx, oyx, ozy, oxz, oxx, oyy;

	if (fabs(D = det()) < 1.0e-12) return 1; // not invertible
	oneOverDet = 1 / D;

	oxx = xx;
	oyy = yy;
	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = (oxz * ozy - zz * oxy) * oneOverDet;
	yz = (oxz * oyx - xx * oyz) * oneOverDet;
	zx = (oyx * ozy - yy * ozx) * oneOverDet;
	yx = (oyz * ozx - oyx * zz) * oneOverDet;
	zy = (oxy * ozx - ozy * xx) * oneOverDet;
	xz = (oxy * oyz - oxz * yy) * oneOverDet;
	xx = (oyy * zz - oyz * ozy) * oneOverDet;
	yy = (oxx * zz - oxz * ozx) * oneOverDet;
	zz = (oxx * oyy - oxy * oyx) * oneOverDet;
	return 0;
}


Mat3f &Mat3f::mult(const Mat3f &M, const Mat3f &N) {
	xx = M.xx * N.xx + M.xy * N.yx + M.xz * N.zx;
	xy = M.xx * N.xy + M.xy * N.yy + M.xz * N.zy;
	xz = M.xx * N.xz + M.xy * N.yz + M.xz * N.zz;
	yx = M.yx * N.xx + M.yy * N.yx + M.yz * N.zx;
	yy = M.yx * N.xy + M.yy * N.yy + M.yz * N.zy;
	yz = M.yx * N.xz + M.yy * N.yz + M.yz * N.zz;
	zx = M.zx * N.xx + M.zy * N.yx + M.zz * N.zx;
	zy = M.zx * N.xy + M.zy * N.yy + M.zz * N.zy;
	zz = M.zx * N.xz + M.zy * N.yz + M.zz * N.zz;

	return *this;
}


Mat3f &Mat3f::premult(const Mat3f &M) {
	float oxy, oyz, ozx, oyx, ozy, oxz;

	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = M.xx * oxy + M.xy * yy  + M.xz * ozy;
	xz = M.xx * oxz + M.xy * oyz + M.xz * zz;
	yx = M.yx * xx  + M.yy * oyx + M.yz * ozx;
	yz = M.yx * oxz + M.yy * oyz + M.yz * zz;
	zx = M.zx * xx  + M.zy * oyx + M.zz * ozx;
	zy = M.zx * oxy + M.zy * yy  + M.zz * ozy;

	xx = M.xx * xx  + M.xy * oyx + M.xz * ozx;
	yy = M.yx * oxy + M.yy * yy  + M.yz * ozy;
	zz = M.zx * oxz + M.zy * oyz + M.zz * zz;

	return *this;
}


Mat3f &Mat3f::postmult(const Mat3f &M) {
	float oxy, oyz, ozx, oyx, ozy, oxz;

	oxy = xy;
	oyx = yx;
	oyz = yz;
	ozy = zy;
	ozx = zx;
	oxz = xz;

	xy = xx *  M.xy + oxy * M.yy + oxz * M.zy;
	xz = xx *  M.xz + oxy * M.yz + oxz * M.zz;
	yx = oyx * M.xx + yy  * M.yx + oyz * M.zx;
	yz = oyx * M.xz + yy  * M.yz + oyz * M.zz;
	zx = ozx * M.xx + ozy * M.yx + zz  * M.zx;
	zy = ozx * M.xy + ozy * M.yy + zz  * M.zy;

	xx = xx  * M.xx + oxy * M.yx + oxz * M.zx;
	yy = oyx * M.xy + yy  * M.yy + oyz * M.zy;
	zz = ozx * M.xz + ozy * M.yz + zz  * M.zz;

	return *this;
}

///////////////////////////////////////////////////////////////////////////////
//
//  class MatXd
//
///////////////////////////////////////////////////////////////////////////////
MatXd &MatXd::mult(const MatXd &M, const MatXd &N) {
	// multiply rotation matrices
	R.xx = M.R.xx * N.R.xx + M.R.xy * N.R.yx + M.R.xz * N.R.zx;
	R.xy = M.R.xx * N.R.xy + M.R.xy * N.R.yy + M.R.xz * N.R.zy;
	R.xz = M.R.xx * N.R.xz + M.R.xy * N.R.yz + M.R.xz * N.R.zz;
	R.yx = M.R.yx * N.R.xx + M.R.yy * N.R.yx + M.R.yz * N.R.zx;
	R.yy = M.R.yx * N.R.xy + M.R.yy * N.R.yy + M.R.yz * N.R.zy;
	R.yz = M.R.yx * N.R.xz + M.R.yy * N.R.yz + M.R.yz * N.R.zz;
	R.zx = M.R.zx * N.R.xx + M.R.zy * N.R.yx + M.R.zz * N.R.zx;
	R.zy = M.R.zx * N.R.xy + M.R.zy * N.R.yy + M.R.zz * N.R.zy;
	R.zz = M.R.zx * N.R.xz + M.R.zy * N.R.yz + M.R.zz * N.R.zz;

	// d = M.R * N.d + M.d
	d.x = M.R.xx * N.d.x + M.R.xy * N.d.y + M.R.xz * N.d.z + M.d.x;
	d.y = M.R.yx * N.d.x + M.R.yy * N.d.y + M.R.yz * N.d.z + M.d.y;
	d.z = M.R.zx * N.d.x + M.R.zy * N.d.y + M.R.zz * N.d.z + M.d.z;

	return *this;
}


MatXd &MatXd::premult(const MatXd &M) {
	double oxy, oyz, ozx, oyx, ozy, oxz, odx, ody;


	// multiply rotation matrices
	oxy = R.xy;
	oyx = R.yx;
	oyz = R.yz;
	ozy = R.zy;
	ozx = R.zx;
	oxz = R.xz;

	R.xy = M.R.xx * oxy  + M.R.xy * R.yy + M.R.xz * ozy;
	R.xz = M.R.xx * oxz  + M.R.xy * oyz  + M.R.xz * R.zz;
	R.yx = M.R.yx * R.xx + M.R.yy * oyx  + M.R.yz * ozx;
	R.yz = M.R.yx * oxz  + M.R.yy * oyz  + M.R.yz * R.zz;
	R.zx = M.R.zx * R.xx + M.R.zy * oyx  + M.R.zz * ozx;
	R.zy = M.R.zx * oxy  + M.R.zy * R.yy + M.R.zz * ozy;

	R.xx = M.R.xx * R.xx + M.R.xy * oyx  + M.R.xz * ozx;
	R.yy = M.R.yx * oxy  + M.R.yy * R.yy + M.R.yz * ozy;
	R.zz = M.R.zx * oxz  + M.R.zy * oyz  + M.R.zz * R.zz;

	// d = M.R * d + M.d
	odx = d.x;
	ody = d.y;
	d.x = M.R.xx * odx + M.R.xy * ody + M.R.xz * d.z + M.d.x;
	d.y = M.R.yx * odx + M.R.yy * ody + M.R.yz * d.z + M.d.y;
	d.z = M.R.zx * odx + M.R.zy * ody + M.R.zz * d.z + M.d.z;

	return *this;
}


MatXd &MatXd::postmult(const MatXd &M) {
	double oxy, oyz, ozx, oyx, ozy, oxz;
	Vect3d v;

	// d = R * M.d + d
	d.x += R.xx * M.d.x + R.xy * M.d.y + R.xz * M.d.z;
	d.y += R.yx * M.d.x + R.yy * M.d.y + R.yz * M.d.z;
	d.z += R.zx * M.d.x + R.zy * M.d.y + R.zz * M.d.z;

	// multiply rotation matrices
	oxy = R.xy;
	oyx = R.yx;
	oyz = R.yz;
	ozy = R.zy;
	ozx = R.zx;
	oxz = R.xz;
	R.xy = R.xx * M.R.xy + oxy  * M.R.yy + oxz  * M.R.zy;
	R.xz = R.xx * M.R.xz + oxy  * M.R.yz + oxz  * M.R.zz;
	R.yx = oyx  * M.R.xx + R.yy * M.R.yx + oyz  * M.R.zx;
	R.yz = oyx  * M.R.xz + R.yy * M.R.yz + oyz  * M.R.zz;
	R.zx = ozx  * M.R.xx + ozy  * M.R.yx + R.zz * M.R.zx;
	R.zy = ozx  * M.R.xy + ozy  * M.R.yy + R.zz * M.R.zy;

	R.xx = R.xx * M.R.xx + oxy  * M.R.yx + oxz  * M.R.zx;
	R.yy = oyx  * M.R.xy + R.yy * M.R.yy + oyz  * M.R.zy;
	R.zz = ozx  * M.R.xz + ozy  * M.R.yz + R.zz * M.R.zz;

	return *this;
}


MatXd &MatXd::invert(const MatXd &M) {
	// invert the rotation part by transposing it
	R.xx = M.R.xx;
	R.xy = M.R.yx;
	R.xz = M.R.zx;
	R.yx = M.R.xy;
	R.yy = M.R.yy;
	R.yz = M.R.zy;
	R.zx = M.R.xz;
	R.zy = M.R.yz;
	R.zz = M.R.zz;

	// new displacement vector given by:  d' = -(R^-1) * d
	d.x = - (R.xx * M.d.x + R.xy * M.d.y + R.xz * M.d.z);
	d.y = - (R.yx * M.d.x + R.yy * M.d.y + R.yz * M.d.z);
	d.z = - (R.zx * M.d.x + R.zy * M.d.y + R.zz * M.d.z);

	return *this;
}



MatXd &MatXd::invert() {
	double tmp, odx, ody;

	// invert the rotation part by transposing it
	tmp  = R.xy;
	R.xy = R.yx;
	R.yx = tmp;

	tmp  = R.yz;
	R.yz = R.zy;
	R.zy = tmp;

	tmp  = R.zx;
	R.zx = R.xz;
	R.xz = tmp;

	// new displacement vector given by:  d' = -(R^T) * d
	odx = d.x;
	ody = d.y;
	d.x = - (R.xx * odx + R.xy * ody + R.xz * d.z);
	d.y = - (R.yx * odx + R.yy * ody + R.yz * d.z);
	d.z = - (R.zx * odx + R.zy * ody + R.zz * d.z);

	return *this;
}


MatXd &MatXd::Invert(const MatXd &M) {
	// invert the rotation part
	R.invert(M.R);

	// new displacement vector given by:    d' = -(R^-1) * d
	R.xform(M.d, d);
	d.negate();

	return *this;
}

MatXd &MatXd::Invert() {
	// invert the rotation part
	R.invert();

	// new displacement vector given by:    d' = -(R^-1) * d
	R.xform(d);
	d.negate();

	return *this;
}



///////////////////////////////////////////////////////////////////////////////
//
//  class MatXf
//
///////////////////////////////////////////////////////////////////////////////

MatXf &MatXf::mult(const MatXf &M, const MatXf &N) {
	// multiply rotation matrices
	R.xx = M.R.xx * N.R.xx + M.R.xy * N.R.yx + M.R.xz * N.R.zx;
	R.xy = M.R.xx * N.R.xy + M.R.xy * N.R.yy + M.R.xz * N.R.zy;
	R.xz = M.R.xx * N.R.xz + M.R.xy * N.R.yz + M.R.xz * N.R.zz;
	R.yx = M.R.yx * N.R.xx + M.R.yy * N.R.yx + M.R.yz * N.R.zx;
	R.yy = M.R.yx * N.R.xy + M.R.yy * N.R.yy + M.R.yz * N.R.zy;
	R.yz = M.R.yx * N.R.xz + M.R.yy * N.R.yz + M.R.yz * N.R.zz;
	R.zx = M.R.zx * N.R.xx + M.R.zy * N.R.yx + M.R.zz * N.R.zx;
	R.zy = M.R.zx * N.R.xy + M.R.zy * N.R.yy + M.R.zz * N.R.zy;
	R.zz = M.R.zx * N.R.xz + M.R.zy * N.R.yz + M.R.zz * N.R.zz;

	// d = M.R * N.d + M.d
	d.x = M.R.xx * N.d.x + M.R.xy * N.d.y + M.R.xz * N.d.z + M.d.x;
	d.y = M.R.yx * N.d.x + M.R.yy * N.d.y + M.R.yz * N.d.z + M.d.y;
	d.z = M.R.zx * N.d.x + M.R.zy * N.d.y + M.R.zz * N.d.z + M.d.z;

	return *this;
}


MatXf &MatXf::premult(const MatXf &M) {
	float oxy, oyz, ozx, oyx, ozy, oxz, odx, ody;


	// multiply rotation matrices
	oxy = R.xy;
	oyx = R.yx;
	oyz = R.yz;
	ozy = R.zy;
	ozx = R.zx;
	oxz = R.xz;

	R.xy = M.R.xx * oxy  + M.R.xy * R.yy + M.R.xz * ozy;
	R.xz = M.R.xx * oxz  + M.R.xy * oyz  + M.R.xz * R.zz;
	R.yx = M.R.yx * R.xx + M.R.yy * oyx  + M.R.yz * ozx;
	R.yz = M.R.yx * oxz  + M.R.yy * oyz  + M.R.yz * R.zz;
	R.zx = M.R.zx * R.xx + M.R.zy * oyx  + M.R.zz * ozx;
	R.zy = M.R.zx * oxy  + M.R.zy * R.yy + M.R.zz * ozy;

	R.xx = M.R.xx * R.xx + M.R.xy * oyx  + M.R.xz * ozx;
	R.yy = M.R.yx * oxy  + M.R.yy * R.yy + M.R.yz * ozy;
	R.zz = M.R.zx * oxz  + M.R.zy * oyz  + M.R.zz * R.zz;

	// d = M.R * d + M.d
	odx = d.x;
	ody = d.y;
	d.x = M.R.xx * odx + M.R.xy * ody + M.R.xz * d.z + M.d.x;
	d.y = M.R.yx * odx + M.R.yy * ody + M.R.yz * d.z + M.d.y;
	d.z = M.R.zx * odx + M.R.zy * ody + M.R.zz * d.z + M.d.z;

	return *this;
}


MatXf &MatXf::postmult(const MatXf &M) {
	float oxy, oyz, ozx, oyx, ozy, oxz;
	Vect3d v;

	// d = R * M.d + d
	d.x += R.xx * M.d.x + R.xy * M.d.y + R.xz * M.d.z;
	d.y += R.yx * M.d.x + R.yy * M.d.y + R.yz * M.d.z;
	d.z += R.zx * M.d.x + R.zy * M.d.y + R.zz * M.d.z;

	// multiply rotation matrices
	oxy = R.xy;
	oyx = R.yx;
	oyz = R.yz;
	ozy = R.zy;
	ozx = R.zx;
	oxz = R.xz;
	R.xy = R.xx * M.R.xy + oxy  * M.R.yy + oxz  * M.R.zy;
	R.xz = R.xx * M.R.xz + oxy  * M.R.yz + oxz  * M.R.zz;
	R.yx = oyx  * M.R.xx + R.yy * M.R.yx + oyz  * M.R.zx;
	R.yz = oyx  * M.R.xz + R.yy * M.R.yz + oyz  * M.R.zz;
	R.zx = ozx  * M.R.xx + ozy  * M.R.yx + R.zz * M.R.zx;
	R.zy = ozx  * M.R.xy + ozy  * M.R.yy + R.zz * M.R.zy;

	R.xx = R.xx * M.R.xx + oxy  * M.R.yx + oxz  * M.R.zx;
	R.yy = oyx  * M.R.xy + R.yy * M.R.yy + oyz  * M.R.zy;
	R.zz = ozx  * M.R.xz + ozy  * M.R.yz + R.zz * M.R.zz;

	return *this;
}


MatXf &MatXf::invert(const MatXf &M) {
	// invert the rotation part by transposing it
	R.xx = M.R.xx;
	R.xy = M.R.yx;
	R.xz = M.R.zx;
	R.yx = M.R.xy;
	R.yy = M.R.yy;
	R.yz = M.R.zy;
	R.zx = M.R.xz;
	R.zy = M.R.yz;
	R.zz = M.R.zz;

	// new displacement vector given by:  d' = -(R^-1) * d
	d.x = - (R.xx * M.d.x + R.xy * M.d.y + R.xz * M.d.z);
	d.y = - (R.yx * M.d.x + R.yy * M.d.y + R.yz * M.d.z);
	d.z = - (R.zx * M.d.x + R.zy * M.d.y + R.zz * M.d.z);

	return *this;
}



MatXf &MatXf::invert() {
	float tmp, odx, ody;

	// invert the rotation part by transposing it
	tmp  = R.xy;
	R.xy = R.yx;
	R.yx = tmp;

	tmp  = R.yz;
	R.yz = R.zy;
	R.zy = tmp;

	tmp  = R.zx;
	R.zx = R.xz;
	R.xz = tmp;

	// new displacement vector given by:  d' = -(R^T) * d
	odx = d.x;
	ody = d.y;
	d.x = - (R.xx * odx + R.xy * ody + R.xz * d.z);
	d.y = - (R.yx * odx + R.yy * ody + R.yz * d.z);
	d.z = - (R.zx * odx + R.zy * ody + R.zz * d.z);

	return *this;
}

MatXf &MatXf::Invert(const MatXf &M) {
	// invert the rotation part
	R.invert(M.R);

	// new displacement vector given by:    d' = -(R^-1) * d
	R.xform(M.d, d);
	d.negate();

	return *this;
}

MatXf &MatXf::Invert() {
	// invert the rotation part
	R.invert();

	// new displacement vector given by:    d' = -(R^-1) * d
	R.xform(d);
	d.negate();

	return *this;
}


///////////////////////////////////////////////////////////////////////////////
//
//  class QuatD
//
///////////////////////////////////////////////////////////////////////////////


QuatD &QuatD::set(double angle, const Vect3d &axis, int normalizeAxis) {
	double theta = 0.5 * angle;
	double sine = sin(theta);
	s_ = cos(theta);

	if (normalizeAxis) {
		double n2;
		if ((n2 = axis.norm2()) < DBL_EPS) {
			*this = QuatD::ID;
			return *this;
		}
		sine /= sqrt(n2);
	}

	x_ = axis.x * sine;
	y_ = axis.y * sine;
	z_ = axis.z * sine;

	return *this;
}


QuatD &QuatD::set(const Mat3d &R) {
	double qs2, qx2, qy2, qz2;  // squared magniudes of quaternion components
	double tmp;
	int i;

	// first compute squared magnitudes of quaternion components - at least one
	// will be greater than 0 since quaternion is unit magnitude

	qs2 = 0.25 * (R.xx + R.yy + R.zz + 1);
	qx2 = qs2 - 0.5 * (R.yy + R.zz);
	qy2 = qs2 - 0.5 * (R.zz + R.xx);
	qz2 = qs2 - 0.5 * (R.xx + R.yy);


	// find maximum magnitude component
	i = (qs2 > qx2) ?
	    ((qs2 > qy2) ? ((qs2 > qz2) ? 0 : 3) : ((qy2 > qz2) ? 2 : 3)) :
	    ((qx2 > qy2) ? ((qx2 > qz2) ? 1 : 3) : ((qy2 > qz2) ? 2 : 3));

	// compute signed quaternion components using numerically stable method
	switch (i) {
	case 0:
		s_ = sqrt(qs2);
		tmp = 0.25 / s_;
		x_ = (R.zy - R.yz) * tmp;
		y_ = (R.xz - R.zx) * tmp;
		z_ = (R.yx - R.xy) * tmp;
		break;
	case 1:
		x_ = sqrt(qx2);
		tmp = 0.25 / x_;
		s_ = (R.zy - R.yz) * tmp;
		y_ = (R.xy + R.yx) * tmp;
		z_ = (R.xz + R.zx) * tmp;
		break;
	case 2:
		y_ = sqrt(qy2);
		tmp = 0.25 / y_;
		s_ = (R.xz - R.zx) * tmp;
		z_ = (R.yz + R.zy) * tmp;
		x_ = (R.yx + R.xy) * tmp;
		break;
	case 3:
		z_ = sqrt(qz2);
		tmp = 0.25 / z_;
		s_ = (R.yx - R.xy) * tmp;
		x_ = (R.zx + R.xz) * tmp;
		y_ = (R.zy + R.yz) * tmp;
		break;
	}
	// for consistency, force positive scalar component [ (s; v) = (-s; -v) ]
	if (s_ < 0) {
		s_ = -s_;
		x_ = -x_;
		y_ = -y_;
		z_ = -z_;
	}
	// normalize, just to be safe
	tmp = 1.0 / sqrt(s_ * s_ + x_ * x_ + y_ * y_ + z_ * z_);
	s_ *= tmp;
	x_ *= tmp;
	y_ *= tmp;
	z_ *= tmp;

	return *this;
}


QuatD &QuatD::mult(const QuatD &p, const QuatD &q) {
	s_ = p.s_ * q.s_ - (p.x_ * q.x_ + p.y_ * q.y_ + p.z_ * q.z_);
	x_ = p.s_ * q.x_ +  q.s_ * p.x_ + p.y_ * q.z_ - p.z_ * q.y_;
	y_ = p.s_ * q.y_ +  q.s_ * p.y_ + p.z_ * q.x_ - p.x_ * q.z_;
	z_ = p.s_ * q.z_ +  q.s_ * p.z_ + p.x_ * q.y_ - p.y_ * q.x_;

	return *this;
}


QuatD &QuatD::premult(const QuatD &q) {
	double ox, oy, oz;

	ox = x_;
	oy = y_;
	oz = z_;

	x_ = q.s_ * ox +  s_ * q.x_ + q.y_ * oz - q.z_ * oy;
	y_ = q.s_ * oy +  s_ * q.y_ + q.z_ * ox - q.x_ * oz;
	z_ = q.s_ * oz +  s_ * q.z_ + q.x_ * oy - q.y_ * ox;
	s_ = q.s_ * s_ - (q.x_ * ox + q.y_ * oy + q.z_ * oz);

	return *this;
}


QuatD &QuatD::postmult(const QuatD &q) {
	double ox, oy, oz;

	ox = x_;
	oy = y_;
	oz = z_;

	x_ = s_ * q.x_ +  ox * q.s_ + oy * q.z_ - oz * q.y_;
	y_ = s_ * q.y_ +  oy * q.s_ + oz * q.x_ - ox * q.z_;
	z_ = s_ * q.z_ +  oz * q.s_ + ox * q.y_ - oy * q.x_;
	s_ = s_ * q.s_ - (ox * q.x_ + oy * q.y_ + oz * q.z_);

	return *this;
}


// The QuatD transformation routines use 19 multiplies and 12 adds
// (counting the multiplications by 2.0).  See Eqn (20) of "A
// Comparison of Transforms and Quaternions in Robotics," Funda and
// Paul, Proceedings of International Conference on Robotics and
// Automation, 1988, p. 886-991.

Vect3d &QuatD::xform(const Vect3d &v, Vect3d &xv) const {
	const Vect3d *u;
	Vect3d uv, uuv;


	u = (const Vect3d *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.0 * s_);
	uuv.scale(2.0);
	xv.add(v, uv);
	xv.add(uuv);
	return xv;
}


Vect3d &QuatD::xform(Vect3d &v) const {
	const Vect3d *u;
	Vect3d uv, uuv;

	u = (const Vect3d *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.0 * s_);
	uuv.scale(2.0);
	v.add(uv);
	v.add(uuv);
	return v;
}


Vect3d &QuatD::invXform(const Vect3d &v, Vect3d &xv) const {
	const Vect3d *u;
	Vect3d uv, uuv;

	u = (const Vect3d *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.0 * -s_);
	uuv.scale(2.0);
	xv.add(v, uv);
	xv.add(uuv);
	return xv;
}


Vect3d &QuatD::invXform(Vect3d &v) const {
	const Vect3d *u;
	Vect3d uv, uuv;

	u = (const Vect3d *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.0 * -s_);
	uuv.scale(2.0);
	v.add(uv);
	v.add(uuv);
	return v;
}


///////////////////////////////////////////////////////////////////////////////
//
//  class QuatF
//
///////////////////////////////////////////////////////////////////////////////


QuatF &QuatF::set(float angle, const Vect3f &axis, int normalizeAxis) {
	float theta = 0.5f * angle;
	float sine = sinf(theta);
	s_ = cosf(theta);

	if (normalizeAxis) {
		float n2;
		if ((n2 = axis.norm2()) < FLT_EPS) {
			*this = QuatD::ID;
			return *this;
		}
		sine *= invSqrtFast(n2);
	}

	x_ = axis.x * sine;
	y_ = axis.y * sine;
	z_ = axis.z * sine;

	return *this;
}

QuatF &QuatF::set(const Mat3f &R) {
	float qs2, qx2, qy2, qz2;  // squared magniudes of quaternion components
	float tmp;
	int i;

	// first compute squared magnitudes of quaternion components - at least one
	// will be greater than 0 since quaternion is unit magnitude

	qs2 = 0.25f * (R.xx + R.yy + R.zz + 1);
	qx2 = qs2 - 0.5f * (R.yy + R.zz);
	qy2 = qs2 - 0.5f * (R.zz + R.xx);
	qz2 = qs2 - 0.5f * (R.xx + R.yy);


	// find maximum magnitude component
	i = (qs2 > qx2) ?
	    ((qs2 > qy2) ? ((qs2 > qz2) ? 0 : 3) : ((qy2 > qz2) ? 2 : 3)) :
	    ((qx2 > qy2) ? ((qx2 > qz2) ? 1 : 3) : ((qy2 > qz2) ? 2 : 3));

	// compute signed quaternion components using numerically stable method
	switch (i) {
	case 0:
		s_ = sqrtf(qs2);
		tmp = 0.25f / s_;
		x_ = (R.zy - R.yz) * tmp;
		y_ = (R.xz - R.zx) * tmp;
		z_ = (R.yx - R.xy) * tmp;
		break;
	case 1:
		x_ = sqrtf(qx2);
		tmp = 0.25f / x_;
		s_ = (R.zy - R.yz) * tmp;
		y_ = (R.xy + R.yx) * tmp;
		z_ = (R.xz + R.zx) * tmp;
		break;
	case 2:
		y_ = sqrtf(qy2);
		tmp = 0.25f / y_;
		s_ = (R.xz - R.zx) * tmp;
		z_ = (R.yz + R.zy) * tmp;
		x_ = (R.yx + R.xy) * tmp;
		break;
	case 3:
		z_ = sqrtf(qz2);
		tmp = 0.25f / z_;
		s_ = (R.yx - R.xy) * tmp;
		x_ = (R.zx + R.xz) * tmp;
		y_ = (R.zy + R.yz) * tmp;
		break;
	}
	// for consistency, force positive scalar component [ (s; v) = (-s; -v) ]
	if (s_ < 0) {
		s_ = -s_;
		x_ = -x_;
		y_ = -y_;
		z_ = -z_;
	}
	// normalize, just to be safe
	tmp = invSqrtFast(s_ * s_ + x_ * x_ + y_ * y_ + z_ * z_);
	s_ *= tmp;
	x_ *= tmp;
	y_ *= tmp;
	z_ *= tmp;

	return *this;
}

QuatF &QuatF::mult(const QuatF &p, const QuatF &q) {
	s_ = p.s_ * q.s_ - (p.x_ * q.x_ + p.y_ * q.y_ + p.z_ * q.z_);
	x_ = p.s_ * q.x_ +  q.s_ * p.x_ + p.y_ * q.z_ - p.z_ * q.y_;
	y_ = p.s_ * q.y_ +  q.s_ * p.y_ + p.z_ * q.x_ - p.x_ * q.z_;
	z_ = p.s_ * q.z_ +  q.s_ * p.z_ + p.x_ * q.y_ - p.y_ * q.x_;

	return *this;
}


QuatF &QuatF::premult(const QuatF &q) {
	float ox, oy, oz;

	ox = x_;
	oy = y_;
	oz = z_;

	x_ = q.s_ * ox +  s_ * q.x_ + q.y_ * oz - q.z_ * oy;
	y_ = q.s_ * oy +  s_ * q.y_ + q.z_ * ox - q.x_ * oz;
	z_ = q.s_ * oz +  s_ * q.z_ + q.x_ * oy - q.y_ * ox;
	s_ = q.s_ * s_ - (q.x_ * ox + q.y_ * oy + q.z_ * oz);

	return *this;
}


QuatF &QuatF::postmult(const QuatF &q) {
	float ox, oy, oz;

	ox = x_;
	oy = y_;
	oz = z_;

	x_ = s_ * q.x_ +  ox * q.s_ + oy * q.z_ - oz * q.y_;
	y_ = s_ * q.y_ +  oy * q.s_ + oz * q.x_ - ox * q.z_;
	z_ = s_ * q.z_ +  oz * q.s_ + ox * q.y_ - oy * q.x_;
	s_ = s_ * q.s_ - (ox * q.x_ + oy * q.y_ + oz * q.z_);

	return *this;
}

// The QuatF transformation routines use 19 multiplies and 12 adds
// (counting the multiplications by 2.0).  See Eqn (20) of "A
// Comparison of Transforms and Quaternions in Robotics," Funda and
// Paul, Proceedings of International Conference on Robotics and
// Automation, 1988, p. 886-991.

Vect3f &QuatF::xform(const Vect3f &v, Vect3f &xv) const {
	const Vect3f *u;
	Vect3f uv, uuv;

	u = (const Vect3f *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.f * s_);
	uuv.scale(2.f);
	xv.add(v, uv);
	xv.add(uuv);
	return xv;
}

Vect3f &QuatF::xform(Vect3f &v) const {
	const Vect3f *u;
	Vect3f uv, uuv;

	u = (const Vect3f *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.f * s_);
	uuv.scale(2.f);
	v.add(uv);
	v.add(uuv);
	return v;
}

Vect3f &QuatF::invXform(const Vect3f &v, Vect3f &xv) const {
	const Vect3f *u;
	Vect3f uv, uuv;

	u = (const Vect3f *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.f * -s_);
	uuv.scale(2.f);
	xv.add(v, uv);
	xv.add(uuv);
	return xv;
}


Vect3f &QuatF::invXform(Vect3f &v) const {
	const Vect3f *u;
	Vect3f uv, uuv;

	u = (const Vect3f *) &x_;
	uv.cross(*u, v);
	uuv.cross(*u, uv);
	uv.scale(2.f * -s_);
	uuv.scale(2.f);
	v.add(uv);
	v.add(uuv);
	return v;
}

//////////////////////////////////////////////////////////////////////////////////
//           QuatD definitions
//////////////////////////////////////////////////////////////////////////////////
QuatD Slerp(const QuatD &A, const QuatD &B, double t) {
	double theta = acos(dot(A, B));
	double sin_theta = sin(theta);
	if (fabs(sin_theta) < DBL_EPS)
		return A;
	sin_theta = 1 / sin_theta;
	return A * (sin(theta * (1 - t)) * sin_theta) + B * (sin(theta * t) * sin_theta);
}

void Mat3f::makeRotationZ() {
	float cos = 0.0f;
	float sin;
	if (yy > FLT_EPS) {
		float tan = xy / yy;
		cos = invSqrtFast(tan * tan + 1.0f);
		sin = cos * tan;
	} else
		sin = xy > 0.0f ? 1.0f : -1.0f;
	xx = cos;
	xy = -sin;
	xz = 0.0f;
	yx = sin;
	yy = cos;
	yz = 0.0f;
	zx = 0.0f;
	zy = 0.0f;
	zz = 1.0f;
}
} // namespace QDEngine