/***************************************************************************
                           matrix3d.cpp
                           -------------------
    copyright            : (C) 2004 by Jos van den Oever
    email                : jos@vandenoever.info
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/
#include "matrix3d.h"
#include <cmath>

double Matrix3D::pi = M_PI;
double Matrix3D::twopi = 2*pi;

Matrix3D::Matrix3D() {
	angstep = 0.15;
	unit();
	lastanim = false;
	oldt = oldp = oldr = 0;
}
// Scale by f in all dimensions
void
Matrix3D::scale(double f) {
	s = f;
}
// Set the matrix to the unit matrix
void
Matrix3D::unit() {
	t = 0;
	p = 0;
	r = 0;
	s = 1;
	updateMatrix();
}
void
Matrix3D::rotX(double deg) {
	deg *= (pi / 180);
	double ct = cos(deg);
	double st = sin(deg);

	double Nyx = yx * ct + zx * st;
	double Nyy = yy * ct + zy * st;
	double Nyz = yz * ct + zz * st;

	double Nzx = zx * ct - yx * st;
	double Nzy = zy * ct - yy * st;
	double Nzz = zz * ct - yz * st;

	yx = Nyx;
	yy = Nyy;
	yz = Nyz;
	zx = Nzx;
	zy = Nzy;
	zz = Nzz;

	updateTPR();
}
void
Matrix3D::rotY(double deg) {
	deg *= (pi / 180);
	double ct = cos(deg);
	double st = sin(deg);

	double Nxx = (double) (xx * ct + zx * st);
	double Nxy = (double) (xy * ct + zy * st);
	double Nxz = (double) (xz * ct + zz * st);

	double Nzx = (double) (zx * ct - xx * st);
	double Nzy = (double) (zy * ct - xy * st);
	double Nzz = (double) (zz * ct - xz * st);

	xx = Nxx;
	xy = Nxy;
	xz = Nxz;
	zx = Nzx;
	zy = Nzy;
	zz = Nzz;

	updateTPR();
}
void
Matrix3D::rotZ(double deg) {
	deg *= (pi / 180);
	double ct = cos(deg);
	double st = sin(deg);

	double Nyx = yx * ct + xx * st;
	double Nyy = yy * ct + xy * st;
	double Nyz = yz * ct + xz * st;

	double Nxx = xx * ct - yx * st;
	double Nxy = xy * ct - yy * st;
	double Nxz = xz * ct - yz * st;

	yx = Nyx;
	yy = Nyy;
	yz = Nyz;
	xx = Nxx;
	xy = Nxy;
	xz = Nxz;

	updateTPR();
}
void
Matrix3D::setOrient(double nt, double np, double nr) {
	t = nt*twopi/360;
	p = np*twopi/360;
	r = nr*twopi/360;
	updateMatrix();
}
/* calculate a transformation matrix from the numbers theta, phi and rho */
void
Matrix3D::updateMatrix() {

	xx =  cos(t)*cos(p)*cos(r) - sin(p)*sin(r);
	xy =  cos(t)*sin(p)*cos(r) + cos(p)*sin(r);
	xz = -sin(t)*cos(r);

	yx = -cos(t)*cos(p)*sin(r) - sin(p)*cos(r);
	yy = -cos(t)*sin(p)*sin(r) + cos(p)*cos(r);
	yz =  sin(t)*sin(r);

	zx =  sin(t)*cos(p);
	zy =  sin(t)*sin(p);
	zz =  cos(t);
}
void
Matrix3D::updateTPR() {
	t = acos(zz);
	p = atan2(zy, zx);
	if (p < 0) p += twopi;
	r = atan2(yz, -xz);
	if (r < 0) r += twopi;
}
/* Transform nvert points from v into tv.  v contains the input coordinates in
    doubleing point. Three successive entries in the array constitute a point.
    tv ends up holding the transformed points as integers; three successive
    entries per point */
void
Matrix3D::transform(const double v[], double tv[], int nvert) const {
	for (int i = nvert * 3; (i -= 3) >= 0;) {
		double x = v[i];
		double y = v[i + 1];
		double z = v[i + 2];
		tv[i    ] = s * (x * xx + y * xy + z * xz);
		tv[i + 1] = s * (x * yx + y * yy + z * yz);
		tv[i + 2] = s * (x * zx + y * zy + z * zz);
	}
}
bool
Matrix3D::stepBack() {
	// calculate the total 'distance'
	double dt, dp, dr; // distance in for each parameter
	double d; // total distance
	double adt, adp, adr; // absolute distance for each parameter
	dt = t - oldt;
	adt = fabs(dt);
	dp = p - oldp;
	dp -= floor(dp/twopi)*twopi;
	if (dp > pi) {
		adp = twopi - dp;
	} else {
		adp = dp;
	}
	dr = r - oldr;
	dr -= floor(dr/twopi)*twopi;
	if (dr > pi) {
		adr = twopi - dr;
	} else {
		adr = dr;
	}
	d = adt + adp + adr;
	if (d < 1e-6) {
		lastanim = false;
		return false;
	}
	if (d < angstep || lastanim) { // if distance is smaller than stepsize,
		adt = adp = adr = 0; // step to the final position
	}
	if (d < angstep*2) {
		lastanim = true;
	}

	// move t
	if (adt == 0) {
		t = oldt;
	} else if (dt > 0) {
		t -= angstep * adt/d;
	} else if (dt < -angstep) {
		t += angstep * adt/d;
	}

	// move p and check that it is within bounds
	if (adp == 0) {
		p = oldp;
	} else if (dp <= pi) {
		p -= angstep * adp/d;
		if (p < 0) p += twopi;
	} else {
		p += angstep * adp/d;
		if (p >= twopi) p -= twopi;
	}

	// move r and check that it is within bounds
	if (adr == 0) {
		r = oldr;
	} else if (dr <= pi) {
		r -= angstep * adr/d;
		if (r < 0) r += twopi;
	} else if (dr < twopi-angstep) {
		r += angstep * adr/d;
		if (r >= twopi) r -= twopi;
	}
	updateMatrix();
	return true;
}
void
Matrix3D::savePosition() {
	oldt = t;
	oldp = p;
	oldr = r;
}
