/*
 * (C) 2003-2006 Gabest
 * (C) 2006-2014 see Authors.txt
 *
 * This file is part of MPC-BE.
 *
 * MPC-BE 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.
 *
 * MPC-BE 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/>.
 *
 */

#include "stdafx.h"
#include "CoordGeom.h"

//
// Vector
//

Vector::Vector(float x, float y, float z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

void Vector::Set(float x, float y, float z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

float Vector::Length()
{
	return(sqrt(x * x + y * y + z * z));
}

float Vector::Sum()
{
	return(x + y + z);
}

float Vector::CrossSum()
{
	return(x*y + x*z + y*z);
}

Vector Vector::Cross()
{
	return(Vector(x*y, x*z, y*z));
}

Vector Vector::Pow(float exp)
{
	return(exp == 0 ? Vector(1, 1, 1) : exp == 1 ? *this : Vector(pow(x, exp), pow(y, exp), pow(z, exp)));
}

Vector Vector::Unit()
{
	float l = Length();
	if (!l || l == 1) {
		return(*this);
	}
	return(*this * (1 / l));
}

Vector& Vector::Unitalize()
{
	return(*this = Unit());
}

Vector Vector::Normal(Vector& a, Vector& b)
{
	return((a - *this) % (b - a));
}

float Vector::Angle(Vector& a, Vector& b)
{
	return(((a - *this).Unit()).Angle((b - *this).Unit()));
}

float Vector::Angle(Vector& a)
{
	float angle = *this | a;
	return((angle > 1) ? 0 : (angle < -1) ? (float)M_PI : acos(angle));
}

void Vector::Angle(float& u, float& v)
{
	Vector n = Unit();

	u = asin(n.y);

	if (IsZero(n.z)) {
		v = (float)M_PI_2 * Sgn(n.x);
	} else if (n.z > 0) {
		v = atan(n.x / n.z);
	} else if (n.z < 0) {
		v = IsZero(n.x) ? (float)M_PI : ((float)M_PI * Sgn(n.x) + atan(n.x / n.z));
	}
}

Vector Vector::Angle()
{
	Vector ret;
	Angle(ret.x, ret.y);
	ret.z = 0;
	return(ret);
}

Vector& Vector::Min(Vector& a)
{
	x = (x < a.x) ? x : a.x;
	y = (y < a.y) ? y : a.y;
	z = (z < a.z) ? z : a.z;
	return(*this);
}

Vector& Vector::Max(Vector& a)
{
	x = (x > a.x) ? x : a.x;
	y = (y > a.y) ? y : a.y;
	z = (z > a.z) ? z : a.z;
	return(*this);
}

Vector Vector::Abs()
{
	return(Vector(fabs(x), fabs(y), fabs(z)));
}

Vector Vector::Reflect(Vector& n)
{
	return(n * ((-*this) | n) * 2 - (-*this));
}

Vector Vector::Refract(Vector& N, float nFront, float nBack, float* nOut)
{
	Vector D = -*this;

	float N_dot_D = (N | D);
	float n = N_dot_D >= 0 ? (nFront / nBack) : (nBack / nFront);

	Vector cos_D = N * N_dot_D;
	Vector sin_T = (cos_D - D) * n;

	float len_sin_T = sin_T | sin_T;

	if (len_sin_T > 1) {
		if (nOut) {
			*nOut = N_dot_D >= 0 ? nFront : nBack;
		}
		return((*this).Reflect(N));
	}

	float N_dot_T = (float)sqrt(1.0 - len_sin_T);
	if (N_dot_D < 0) {
		N_dot_T = -N_dot_T;
	}

	if (nOut) {
		*nOut = N_dot_D >= 0 ? nBack : nFront;
	}

	return(sin_T - (N * N_dot_T));
}

Vector Vector::Refract2(Vector& N, float nFrom, float nTo, float* nOut)
{
	Vector D = -*this;

	float N_dot_D = (N | D);
	float n = nFrom / nTo;

	Vector cos_D = N * N_dot_D;
	Vector sin_T = (cos_D - D) * n;

	float len_sin_T = sin_T | sin_T;

	if (len_sin_T > 1) {
		if (nOut) {
			*nOut = nFrom;
		}
		return((*this).Reflect(N));
	}

	float N_dot_T = (float)sqrt(1.0 - len_sin_T);
	if (N_dot_D < 0) {
		N_dot_T = -N_dot_T;
	}

	if (nOut) {
		*nOut = nTo;
	}

	return(sin_T - (N * N_dot_T));
}

float Vector::operator | (Vector& v)
{
	return(x * v.x + y * v.y + z * v.z);
}

Vector Vector::operator % (Vector& v)
{
	return(Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x));
}

float& Vector::operator [] (size_t i)
{
	return(!i ? x : (i == 1) ? y : z);
}

Vector Vector::operator - ()
{
	return(Vector(-x, -y, -z));
}

bool Vector::operator == (const Vector& v) const
{
	if (IsZero(x - v.x) && IsZero(y - v.y) && IsZero(z - v.z)) {
		return true;
	}
	return false;
}

bool Vector::operator != (const Vector& v) const
{
	return((*this == v) ? false : true);
}

Vector Vector::operator + (float d)
{
	return(Vector(x + d, y + d, z + d));
}

Vector Vector::operator + (Vector& v)
{
	return(Vector(x + v.x, y + v.y, z + v.z));
}

Vector Vector::operator - (float d)
{
	return(Vector(x - d, y - d, z - d));
}

Vector Vector::operator - (Vector& v)
{
	return(Vector(x - v.x, y - v.y, z - v.z));
}

Vector Vector::operator * (float d)
{
	return(Vector(x * d, y * d, z * d));
}

Vector Vector::operator * (Vector& v)
{
	return(Vector(x * v.x, y * v.y, z * v.z));
}

Vector Vector::operator / (float d)
{
	return(Vector(x / d, y / d, z / d));
}

Vector Vector::operator / (Vector& v)
{
	return(Vector(x / v.x, y / v.y, z / v.z));
}

Vector& Vector::operator += (float d)
{
	x += d;
	y += d;
	z += d;
	return(*this);
}

Vector& Vector::operator += (Vector& v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	return(*this);
}

Vector& Vector::operator -= (float d)
{
	x -= d;
	y -= d;
	z -= d;
	return(*this);
}

Vector& Vector::operator -= (Vector& v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	return(*this);
}

Vector& Vector::operator *= (float d)
{
	x *= d;
	y *= d;
	z *= d;
	return(*this);
}

Vector& Vector::operator *= (Vector& v)
{
	x *= v.x;
	y *= v.y;
	z *= v.z;
	return(*this);
}

Vector& Vector::operator /= (float d)
{
	x /= d;
	y /= d;
	z /= d;
	return(*this);
}

Vector& Vector::operator /= (Vector& v)
{
	x /= v.x;
	y /= v.y;
	z /= v.z;
	return(*this);
}

//
// Ray
//

Ray::Ray(Vector& p, Vector& d)
{
	this->p = p;
	this->d = d;
}

void Ray::Set(Vector& p, Vector& d)
{
	this->p = p;
	this->d = d;
}

float Ray::GetDistanceFrom(Ray& r)
{
	float t = (d | r.d);
	if (IsZero(t)) {
		return(-BIGNUMBER);    // plane is paralell to the ray, return -infinite
	}
	return(((r.p - p) | r.d) / t);
}

float Ray::GetDistanceFrom(Vector& v)
{
	float t = ((v - p) | d) / (d | d);
	return(((p + d*t) - v).Length());
}

Vector Ray::operator [] (float t)
{
	return(p + d*t);
}

//
// XForm
//

XForm::XForm(Ray& r, Vector& s, bool isWorldToLocal)
{
	Initalize(r, s, isWorldToLocal);
}

void XForm::Initalize()
{
	m.Initalize();
}

void XForm::Initalize(Ray& r, Vector& s, bool isWorldToLocal)
{
	Initalize();

	m_isWorldToLocal = isWorldToLocal;
	if (isWorldToLocal) {
		*this -= r.p;
		*this >>= r.d;
		*this /= s;

	} else {
		*this *= s;
		*this <<= r.d;
		*this += r.p;
	}
}

void XForm::operator *= (Vector& v)
{
	Matrix s;
	s.mat[0][0] = v.x;
	s.mat[1][1] = v.y;
	s.mat[2][2] = v.z;
	m *= s;
}

void XForm::operator += (Vector& v)
{
	Matrix t;
	t.mat[3][0] = v.x;
	t.mat[3][1] = v.y;
	t.mat[3][2] = v.z;
	m *= t;
}

void XForm::operator <<= (Vector& v)
{
	Matrix x;
	x.mat[1][1] = cos(v.x);
	x.mat[1][2] = -sin(v.x);
	x.mat[2][1] = sin(v.x);
	x.mat[2][2] = cos(v.x);

	Matrix y;
	y.mat[0][0] = cos(v.y);
	y.mat[0][2] = -sin(v.y);
	y.mat[2][0] = sin(v.y);
	y.mat[2][2] = cos(v.y);

	Matrix z;
	z.mat[0][0] = cos(v.z);
	z.mat[0][1] = -sin(v.z);
	z.mat[1][0] = sin(v.z);
	z.mat[1][1] = cos(v.z);

	m = m_isWorldToLocal ? (m * y * x * z) : (m * z * x * y);
}

void XForm::operator /= (Vector& v)
{
	Vector s;
	s.x = IsZero(v.x) ? 0 : 1 / v.x;
	s.y = IsZero(v.y) ? 0 : 1 / v.y;
	s.z = IsZero(v.z) ? 0 : 1 / v.z;
	*this *= s;
}

void XForm::operator -= (Vector& v)
{
	*this += -v;
}

void XForm::operator >>= (Vector& v)
{
	*this <<= -v;
}

Vector XForm::operator < (Vector& n)
{
	Vector ret;

	ret.x = n.x * m.mat[0][0] +
			n.y * m.mat[1][0] +
			n.z * m.mat[2][0];
	ret.y = n.x * m.mat[0][1] +
			n.y * m.mat[1][1] +
			n.z * m.mat[2][1];
	ret.z = n.x * m.mat[0][2] +
			n.y * m.mat[1][2] +
			n.z * m.mat[2][2];

	return(ret);
}

Vector XForm::operator << (Vector& v)
{
	Vector ret;

	ret.x = v.x * m.mat[0][0] +
			v.y * m.mat[1][0] +
			v.z * m.mat[2][0] +
			m.mat[3][0];
	ret.y = v.x * m.mat[0][1] +
			v.y * m.mat[1][1] +
			v.z * m.mat[2][1] +
			m.mat[3][1];
	ret.z = v.x * m.mat[0][2] +
			v.y * m.mat[1][2] +
			v.z * m.mat[2][2] +
			m.mat[3][2];

	return(ret);
}

Ray XForm::operator << (Ray& r)
{
	return(Ray(*this << r.p, *this < r.d));
}

//
// XForm::Matrix
//

XForm::Matrix::Matrix()
{
	Initalize();
}

void XForm::Matrix::Initalize()
{
	mat[0][0] = 1;
	mat[0][1] = 0;
	mat[0][2] = 0;
	mat[0][3] = 0;
	mat[1][0] = 0;
	mat[1][1] = 1;
	mat[1][2] = 0;
	mat[1][3] = 0;
	mat[2][0] = 0;
	mat[2][1] = 0;
	mat[2][2] = 1;
	mat[2][3] = 0;
	mat[3][0] = 0;
	mat[3][1] = 0;
	mat[3][2] = 0;
	mat[3][3] = 1;
}

XForm::Matrix XForm::Matrix::operator * (Matrix& m)
{
	Matrix ret;

	for (ptrdiff_t i = 0; i < 4; i++) {
		for (ptrdiff_t j = 0; j < 4; j++) {
			ret.mat[i][j] = mat[i][0] * m.mat[0][j] +
							mat[i][1] * m.mat[1][j] +
							mat[i][2] * m.mat[2][j] +
							mat[i][3] * m.mat[3][j];

			if (IsZero(ret.mat[i][j])) {
				ret.mat[i][j] = 0;
			}
		}
	}

	return(ret);
}

XForm::Matrix& XForm::Matrix::operator *= (XForm::Matrix& m)
{
	return(*this = *this * m);
}
