/*
								+----------------------------------+
								|                                  |
								|    ***  Vector math v2.0  ***    |
								|                                  |
								|   Copyright © -tHE SWINe- 2005   |
								|                                  |
								|            Vector.cpp            |
								|                                  |
								+----------------------------------+
*/

/**
 *	@file Vector.cpp
 *	@author -tHE SWINe-
 *	@date 2005
 *	@brief vector math primitives
 *
 *	@date 2006-05-16
 *
 *	passed code revision
 *
 *	optimized some parts of code
 *	all integers storing just true of false were replaced with bool
 *
 *	@date 2006-07-02
 *
 *	passed code revision
 *
 *	added swizzle operators to vector classes
 *	added redlect / refract / fresnel functions to Vector3<T> class
 *	renamed Vector3<T>::Len to Vector3<T>::f_Length, Vector3<T>::Len2 to Vector3<T>::f_Length2
 *	added some Matrix3f methods, renamed Matrix3f::Init() to Matrix3f::Identity()
 *	renamed Planef to Plane3f
 *	renamed Pi to f_pi
 *	general cleanup of code
 *
 *	@date 2006-07-17
 *
 *	renamed CPolygon::r_Vertex(int) to CPolygon::r_t_Vertex(int)
 *	renamed CPolygon::r_Normal() to CPolygon::r_t_Normal()
 *
 *	@date 2006-07-20
 *
 *	added CPolygon::b_MT_RayHit_CullFrontfaces and CPolygon::b_MT_RayHit_CullBackFaces
 *
 *	@date 2006-07-24
 *
 *	renamed TVertexStruct to TVertex3f
 *	added class CVectorMath with ray distances and bounding box / line (ray) intersection tests
 *
 *	@date 2007-01-23
 *
 *	fixed std::vector handling in template CPolygon<TVertex3f> (not-enough-memory checks)
 *	remade TVertex3f base class and CPolygon<TVertex3f> a bit (added + and * operators,
 *	renamed Lerp() to t_Lerp())
 *
 *	@date 2007-03-07
 *
 *	added complete swizzling functions set to vectors
 *
 *	@date 2007-06-04
 *
 *	added missing const keyword to input swizzle functions
 *
 *	@date 2007-08-03
 *
 *	added quaternion class with basic quaternion operations
 *
 *	@date 2007-10-27
 *
 *	added convenience vector / constant operators and component-wise vector * vector
 *	and vector / vector operators to all vector templates
 *	added Vector4f Matrix4f::operator *(const Vector4f &) function for natural matrix-vertex
 *	multiplication as alternative to v_Transform_Pos() and v_Transform_Dir()
 *
 *	@date 2007-11-26
 *
 *	added Matrix4f::f_Subdet, Matrix4f::f_Determinant, Matrix4f::FullInvert and
 *	Matrix4f::t_FullInverse
 *	added convenience vector + constant and vector - constant operators to all vector templates
 *
 *	@date 2008-08-21
 *
 *	fixed Matrix4f::Scale(), added non-Vector3f variants of Matrix4f::Scale(),
 *	Matrix4f::Translate() and Matrix4f::Rotate()
 *
 *	@date 2008-08-23
 *
 *	documented Matrix4f class, added functions for generating transformation
 *	matrices (not applying transformation on the matrix), namely Matrix4f::Translation(),
 *	Matrix4f::Scaling(), Matrix4f::RotationX(), Matrix4f::RotationY(), Matrix4f::RotationZ(),
 *	and Matrix4f::Rotation(). added component-wise matrix by scalar multiplication (overloads
 *	of Matrix4f::operator *() and Matrix4f::operator *=()) and finaly added transposition
 *	function Matrix4f::Transposition()
 *
 *	@date 2009-05-04
 *
 *	fixed mixed windows / linux line endings
 *
 *	@date 2009-05-12
 *
 *	fixed out-of-bounds index in TObjectOrientedBox::b_Intersect_Ray()
 *
 *	@date 2009-05-23
 *
 *	removed all instances of std::vector::reserve and replaced them by stl_ut::Reserve_*
 *
 */

/*#include "NewFix.h"

#include "CallStack.h"
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <algorithm>*/
//#include "StlUtils.h"
#include "Vector.h"

const double f_pi = 3.1415926535897932384626433832795028841971691075;
const float f_epsilon = .001f;
const float f_edge_epsilon = .05f;

/*
 *        === Plane3f ===
 */

#if defined(_MSC_VER) && !defined(__MWERKS__) && _MSC_VER <= 1200
const plane::from_position_normal_tag_t plane::from_position_normal;
const plane::from_position_edge_vecs_tag_t plane::from_position_edge_vecs;
const plane::from_triangle_tag_t plane::from_triangle;
const quat::from_vector_w_tag_t quat::from_vector_and_w;
const quat::from_axis_angle_tag_t quat::from_axis_angle;
const quat::rotation_from_vector_to_vector_tag_t quat::rotation_from_vector_to_vector;
// MSVC 60 can't initialize empty structures
#else // _MSC_VER && !__MWERKS__ && _MSC_VER <= 1200
const plane::from_position_normal_tag_t plane::from_position_normal = {};
const plane::from_position_edge_vecs_tag_t plane::from_position_edge_vecs = {};
const plane::from_triangle_tag_t plane::from_triangle = {};
const quat::from_vector_w_tag_t quat::from_vector_and_w = {};
const quat::from_axis_angle_tag_t quat::from_axis_angle = {};
const quat::rotation_from_vector_to_vector_tag_t quat::rotation_from_vector_to_vector = {};
#endif // _MSC_VER && !__MWERKS__ && _MSC_VER <= 1200

#if 0

Plane3f::Plane3f(ConstArgType() r_v_norm, float _f_dist)
	:v_normal(r_v_norm), f_dist(_f_dist)
{}

Plane3f::Plane3f(ConstArgType() r_v_pos, ConstArgType() r_v_norm)
{
	v_normal = r_v_norm;
	//_ASSERTE(v_normal.f_Length() > f_epsilon);
	f_dist = -r_v_pos.f_Dot(v_normal);
}
	
Plane3f::Plane3f(ConstArgType() r_v_u, ConstArgType() r_v_v, ConstArgType() r_v_pos)
{
	v_normal = r_v_u.v_Cross(r_v_v);
	//_ASSERTE(v_normal.f_Length() > f_epsilon);
	v_normal.Normalize();
	f_dist = -r_v_pos.f_Dot(v_normal);
}

EPlanePos Plane3f::n_Vector_Pos(ConstArgType() r_v_vec, float f_epsilon_ex) const
{
	float f;
	return ((f = f_Vector_Dist(r_v_vec)) > f_epsilon_ex)? plane_Front : (f < -f_epsilon_ex)? plane_Back : plane_Onplane;
}

EPlanePos Plane3f::n_Tri_Pos(ConstArgType() v_a,
	ConstArgType() v_b, ConstArgType() v_c, float f_epsilon_ex) const
{
	int b_front = false, b_back = false;

	switch(n_Vector_Pos(v_a, f_epsilon_ex)) {
	case plane_Front:
		b_front = true;
		break;
	case plane_Back:
		b_back = true;
		break;
	default:
		break;
	}
	switch(n_Vector_Pos(v_b, f_epsilon_ex)) {
	case plane_Front:
		b_front = true;
		break;
	case plane_Back:
		b_back = true;
		break;
	default:
		break;
	}
	switch(n_Vector_Pos(v_c, f_epsilon_ex)) {
	case plane_Front:
		if(!b_back)
			return plane_Front;
		return plane_Split;
	case plane_Back:
		if(!b_front)
			return plane_Back;
		return plane_Split;
	default:
		break;
	}

	if(!b_front) {
		if(!b_back)
			return plane_Onplane;
		return plane_Back;
	} else if(!b_back)
		return plane_Front;
	return plane_Split;
}

float Plane3f::f_Vector_Dist(ConstArgType() r_v_vec) const
{
	return v_normal.x * r_v_vec.x + v_normal.y * r_v_vec.y + v_normal.z * r_v_vec.z + f_dist;
}

float Plane3f::f_Vector_Dot(ConstArgType() r_v_vec) const
{
	return v_normal.f_Dot(r_v_vec);
}

bool Plane3f::Intersect_Ray(Vector3f &r_v_intersection, ConstArgType() r_v_org, ConstArgType() r_v_dir) const
{
	float f_t;
	if((f_t = f_Vector_Dot(r_v_dir)) == .0f)
		return false;
	r_v_intersection = r_v_org - r_v_dir * (f_Vector_Dist(r_v_org) / f_t);
	return true;
}

bool Plane3f::Intersect_Ray_t(float &r_f_intersection_t, ConstArgType() r_v_org, ConstArgType() r_v_dir) const
{
	float f_t;
	if((f_t = f_Vector_Dot(r_v_dir)) == .0f)
		return false;
	r_f_intersection_t = -f_Vector_Dist(r_v_org) / f_t;
	return true;
}

/*Vector3f Plane3f::v_Intersect_Ray(ConstArgType() v_org, ConstArgType() v_dir) const
{
	float f_t;

	if((f_t = f_Vector_Dot(v_dir)) == .0f) {
		//_ASSERTE(0);
		throw CRayIsParallel_Exception();
		return v_org;
	}

	f_t = f_Vector_Dist(v_org) / f_t;
	// vzdálenost

	return v_org - v_dir * f_t;
}

float Plane3f::f_Intersect_Ray_t(ConstArgType() v_org, ConstArgType() v_dir) const
{
	float f_t;

	if((f_t = f_Vector_Dot(v_dir)) == .0f) {
		//_ASSERTE(0);
		throw CRayIsParallel_Exception();
		return 0;
	}

	return -f_Vector_Dist(v_org) / f_t;
	// vzdálenost
}*/ // deprecated

bool Plane3f::operator ==(ConstArgType(Plane3f) t_plane) const
{
	if(fabs(fabs(f_Vector_Dot(t_plane.v_normal)) - 1.0f) < f_epsilon &&
	   fabs(f_dist - t_plane.f_dist) < f_epsilon)
		return true;
	return false;
}

#endif // 0

/*
 *        === ~Plane3f ===
 */

/*
 *        === Matrix4f ===
 */

#if 0

void Matrix4f::Translation(ConstArgType(Vector3<_TyScalar>)) r_v_translate)
{
	for(int j = 0; j < 4; ++ j) {		
		for(int i = 0; i < 3; ++ i)
			f[i][j] = (_TyScalar)(i == j);
	}
	f[3][0] = r_v_translate.x;
    f[3][1] = r_v_translate.y;
    f[3][2] = r_v_translate.z;
	f[3][3] = 1;
}

void Matrix4f::Translation(_TyScalar f_translate_x, _TyScalar f_translate_y, _TyScalar f_translate_z)
{
	for(int j = 0; j < 4; ++ j) {		
		for(int i = 0; i < 3; ++ i)
			f[i][j] = (_TyScalar)(i == j);
	}
	f[3][0] = f_translate_x;
    f[3][1] = f_translate_y;
    f[3][2] = f_translate_z;
	f[3][3] = 1;
}

void Matrix4f::Scaling(_TyScalar f_scale_x, _TyScalar f_scale_y, _TyScalar f_scale_z)
{
    Identity();
    f[0][0] = f_scale_x;
    f[1][1] = f_scale_y;
    f[2][2] = f_scale_z;
}

void Matrix4f::Scaling(ConstArgType(Vector3<_TyScalar>)) r_v_scale)
{
    Identity();
    f[0][0] = r_v_scale.x;
    f[1][1] = r_v_scale.y;
    f[2][2] = r_v_scale.z;
}

void Matrix4f::RotationX(_TyScalar f_angle)
{
    Identity();
	_TyScalar f_sin = _TyScalar(sin(f_angle));
	_TyScalar f_cos = _TyScalar(cos(f_angle));
    f[2][1] = -f_sin;
    f[2][2] = f_cos;
    f[1][1] = f_cos;
    f[1][2] = f_sin;
}

void Matrix4f::RotationY(_TyScalar f_angle)
{
    Identity();
	_TyScalar f_sin = _TyScalar(sin(f_angle));
	_TyScalar f_cos = _TyScalar(cos(f_angle));
    f[0][0] = f_cos;
    f[0][2] = -f_sin;
    f[2][2] = f_cos;
    f[2][0] = f_sin;
}

void Matrix4f::RotationZ(_TyScalar f_angle)
{
    Identity();
	_TyScalar f_sin = _TyScalar(sin(f_angle));
	_TyScalar f_cos = _TyScalar(cos(f_angle));
    f[0][0] = f_cos;
    f[0][1] = f_sin;
    f[1][1] = f_cos;
    f[1][0] = -f_sin;
}

void Matrix4f::Rotation(_TyScalar f_angle, _TyScalar f_axis_x, _TyScalar f_axis_y, _TyScalar f_axis_z)
{
	// formula for rotation matrix around arbitrary axis:
	// R = uuT + cos(f_angle) * (I - uuT) + sin(f_angle)S

	_TyScalar f_cos = _TyScalar(cos(f_angle));
	_TyScalar f_o_m_cos = 1 - f_cos;
	_TyScalar f_axis_x_o_m_cos = f_axis_x * f_o_m_cos;
	_TyScalar f_axis_y_o_m_cos = f_axis_y * f_o_m_cos;
	_TyScalar f_axis_z_o_m_cos = f_axis_z * f_o_m_cos;
    f[0][0] = f_axis_x * f_axis_x_o_m_cos + f_cos;
    f[0][1] = f_axis_x * f_axis_y_o_m_cos;
    f[0][2] = f_axis_x * f_axis_z_o_m_cos;
	f[0][3] = 0;
    f[1][0] = f_axis_y * f_axis_x_o_m_cos;
    f[1][1] = f_axis_y * f_axis_y_o_m_cos + f_cos;
    f[1][2] = f_axis_y * f_axis_z_o_m_cos;
	f[1][3] = 0;
    f[2][0] = f_axis_z * f_axis_x_o_m_cos;
    f[2][1] = f_axis_z * f_axis_y_o_m_cos;
    f[2][2] = f_axis_z * f_axis_z_o_m_cos + f_cos;
	f[2][3] = 0;
	f[3][0] = 0;
	f[3][1] = 0;
	f[3][2] = 0;
	f[3][3] = 1;
	// R = uu^T * (1 - cos(f_angle)) + cos(f_angle) * I + ...

	_TyScalar f_sin = _TyScalar(sin(f_angle));
	f_axis_x *= f_sin;
	f_axis_y *= f_sin;
	f_axis_z *= f_sin;
	f[1][0] -= f_axis_z;
	f[0][1] += f_axis_z;
	f[2][0] += f_axis_y;
	f[0][2] -= f_axis_y;
	f[2][1] -= f_axis_x;
	f[1][2] += f_axis_x;
	// ...  + sin(f_angle)S
}

void Matrix4f::Rotation(_TyScalar f_angle, ConstArgType(Vector3<_TyScalar>)) r_v_axis)
{
	// formula for rotation matrix around arbitrary axis:
	// R = uuT + cos(f_angle) * (I - uuT) + sin(f_angle)S

	_TyScalar f_cos = _TyScalar(cos(f_angle));
	Vector3<_TyScalar>) v_u_o_m_cos(r_v_axis * (1 - f_cos));
	for(int i = 0; i < 3; ++ i) {
        for(int j = 0; j < 3; ++ j)
            f[i][j] = r_v_axis[i] * v_u_o_m_cos[j] + ((i == j)? f_cos : 0);
		f[i][3] = 0;
		f[3][i] = 0;
    }
	f[3][3] = 1;
	// R = uu^T * (1 - cos(f_angle)) + cos(f_angle) * I + ...

	Vector3<_TyScalar>) v_s(r_v_axis * _TyScalar(sin(f_angle)));
	f[1][0] -= v_s.z;
	f[0][1] += v_s.z;
	f[2][0] += v_s.y;
	f[0][2] -= v_s.y;
	f[2][1] -= v_s.x;
	f[1][2] += v_s.x;
	// ...  + sin(f_angle)S
}

void Matrix4f::Translate(_TyScalar f_translate_x, _TyScalar f_translate_y, _TyScalar f_translate_z)
{
	Matrix4f t_tmp;
    t_tmp.Translation(f_translate_x, f_translate_y, f_translate_z);
	*this *= t_tmp;
}

void Matrix4f::Translate(ConstArgType(Vector3<_TyScalar>)) r_v_translate)
{
	Matrix4f t_tmp;
    t_tmp.Translation(r_v_translate);
	*this *= t_tmp;
}

void Matrix4f::Scale(_TyScalar f_scale)
{
	Matrix4f t_tmp;
	t_tmp.Scaling(f_scale);
	*this *= t_tmp;
}

void Matrix4f::Scale(_TyScalar f_scale_x, _TyScalar f_scale_y, _TyScalar f_scale_z)
{
	Matrix4f t_tmp;
	t_tmp.Scaling(f_scale_x, f_scale_y, f_scale_z);
	*this *= t_tmp;
}

void Matrix4f::Scale(ConstArgType(Vector3<_TyScalar>)) r_v_scale)
{
	Matrix4f t_tmp;
	t_tmp.Scaling(r_v_scale);
	*this *= t_tmp;
}

void Matrix4f::RotateX(_TyScalar f_angle)
{
	Matrix4f t_tmp;
    t_tmp.RotationX(f_angle);
	*this *= t_tmp;
}

void Matrix4f::RotateY(_TyScalar f_angle)
{
	Matrix4f t_tmp;
    t_tmp.RotationY(f_angle);
	*this *= t_tmp;
}

void Matrix4f::RotateZ(_TyScalar f_angle)
{
	Matrix4f t_tmp;
    t_tmp.RotationZ(f_angle);
	*this *= t_tmp;
}

void Matrix4f::Rotate(_TyScalar f_angle, _TyScalar f_axis_x, _TyScalar f_axis_y, _TyScalar f_axis_z)
{
	Matrix4f t_tmp;
    t_tmp.Rotation(f_angle, f_axis_x, f_axis_y, f_axis_z);
	*this *= t_tmp;
}

void Matrix4f::Rotate(_TyScalar f_angle, ConstArgType(Vector3<_TyScalar>)) r_v_axis)
{
	Matrix4f t_tmp;
    t_tmp.Rotation(f_angle, r_v_axis);
	*this *= t_tmp;
}

/*
 *	Matrix4f Matrix4f::&operator *=(ConstArgType(Matrix4f) r_t_mat)
 *		- multiplies this matrix by r_t_mat and returns reference to this
 */
NonConstResultType(Matrix4f) Matrix4f::operator *=(ConstArgType(Matrix4f) r_t_mat)
{
	_TyScalar f_temp_1_0 = f[0][0] * r_t_mat.f[1][0] + f[1][0] * r_t_mat.f[1][1] +
		f[2][0] * r_t_mat.f[1][2] + f[3][0] * r_t_mat.f[1][3];
	_TyScalar f_temp_2_0 = f[0][0] * r_t_mat.f[2][0] + f[1][0] * r_t_mat.f[2][1] +
		f[2][0] * r_t_mat.f[2][2] + f[3][0] * r_t_mat.f[2][3];
	_TyScalar f_temp_3_0 = f[0][0] * r_t_mat.f[3][0] + f[1][0] * r_t_mat.f[3][1] +
		f[2][0] * r_t_mat.f[3][2] + f[3][0] * r_t_mat.f[3][3];
	f[0][0] = f[0][0] * r_t_mat.f[0][0] + f[1][0] * r_t_mat.f[0][1] +
		f[2][0] * r_t_mat.f[0][2] + f[3][0] * r_t_mat.f[0][3];
	f[1][0] = f_temp_1_0;
	f[2][0] = f_temp_2_0;
	f[3][0] = f_temp_3_0;
	_TyScalar f_temp_1_1 = f[0][1] * r_t_mat.f[1][0] + f[1][1] * r_t_mat.f[1][1] +
		f[2][1] * r_t_mat.f[1][2] + f[3][1] * r_t_mat.f[1][3];
	_TyScalar f_temp_2_1 = f[0][1] * r_t_mat.f[2][0] + f[1][1] * r_t_mat.f[2][1] +
		f[2][1] * r_t_mat.f[2][2] + f[3][1] * r_t_mat.f[2][3];
	_TyScalar f_temp_3_1 = f[0][1] * r_t_mat.f[3][0] + f[1][1] * r_t_mat.f[3][1] +
		f[2][1] * r_t_mat.f[3][2] + f[3][1] * r_t_mat.f[3][3];
	f[0][1] = f[0][1] * r_t_mat.f[0][0] + f[1][1] * r_t_mat.f[0][1] +
		f[2][1] * r_t_mat.f[0][2] + f[3][1] * r_t_mat.f[0][3];
	f[1][1] = f_temp_1_1;
	f[2][1] = f_temp_2_1;
	f[3][1] = f_temp_3_1;
	_TyScalar f_temp_1_2 = f[0][2] * r_t_mat.f[1][0] + f[1][2] * r_t_mat.f[1][1] +
		f[2][2] * r_t_mat.f[1][2] + f[3][2] * r_t_mat.f[1][3];
	_TyScalar f_temp_2_2 = f[0][2] * r_t_mat.f[2][0] + f[1][2] * r_t_mat.f[2][1] +
		f[2][2] * r_t_mat.f[2][2] + f[3][2] * r_t_mat.f[2][3];
	_TyScalar f_temp_3_2 = f[0][2] * r_t_mat.f[3][0] + f[1][2] * r_t_mat.f[3][1] +
		f[2][2] * r_t_mat.f[3][2] + f[3][2] * r_t_mat.f[3][3];
	f[0][2] = f[0][2] * r_t_mat.f[0][0] + f[1][2] * r_t_mat.f[0][1] +
		f[2][2] * r_t_mat.f[0][2] + f[3][2] * r_t_mat.f[0][3];
	f[1][2] = f_temp_1_2;
	f[2][2] = f_temp_2_2;
	f[3][2] = f_temp_3_2;
	_TyScalar f_temp_1_3 = f[0][3] * r_t_mat.f[1][0] + f[1][3] * r_t_mat.f[1][1] +
		f[2][3] * r_t_mat.f[1][2] + f[3][3] * r_t_mat.f[1][3];
	_TyScalar f_temp_2_3 = f[0][3] * r_t_mat.f[2][0] + f[1][3] * r_t_mat.f[2][1] +
		f[2][3] * r_t_mat.f[2][2] + f[3][3] * r_t_mat.f[2][3];
	_TyScalar f_temp_3_3 = f[0][3] * r_t_mat.f[3][0] + f[1][3] * r_t_mat.f[3][1] +
		f[2][3] * r_t_mat.f[3][2] + f[3][3] * r_t_mat.f[3][3];
	f[0][3] = f[0][3] * r_t_mat.f[0][0] + f[1][3] * r_t_mat.f[0][1] +
		f[2][3] * r_t_mat.f[0][2] + f[3][3] * r_t_mat.f[0][3];
	f[1][3] = f_temp_1_3;
	f[2][3] = f_temp_2_3;
	f[3][3] = f_temp_3_3;
	// somewhat better optimized, requires less copying

	return *this;
}

/*
 *	Vector4<_TyScalar>) Matrix4f::operator *(ConstArgType(Vector4<_TyScalar>)) r_v_vec) const
 *		- vector-matrix multiplication
 *		- returns this * r_v_vec
 */
Vector4<_TyScalar>) Matrix4f::operator *(ConstArgType(Vector4<_TyScalar>)) r_v_vec) const
{
	return Vector4<_TyScalar>)(r_v_vec.x * f[0][0] + r_v_vec.y * f[1][0] +
					r_v_vec.z * f[2][0] + r_v_vec.w * f[3][0],
				    r_v_vec.x * f[0][1] + r_v_vec.y * f[1][1] +
					r_v_vec.z * f[2][1] + r_v_vec.w * f[3][1],
				    r_v_vec.x * f[0][2] + r_v_vec.y * f[1][2] +
					r_v_vec.z * f[2][2] + r_v_vec.w * f[3][2],
				    r_v_vec.x * f[0][3] + r_v_vec.y * f[1][3] +
					r_v_vec.z * f[2][3] + r_v_vec.w * f[3][3]);
}

/*
 *	Vector3<_TyScalar>) Matrix4f::v_Transform_Pos(ConstArgType(Vector3<_TyScalar>)) r_v_vec) const
 *		- transforms position r_v_vec by this matrix
 *		- equivalent to multiplying this matrix by Vector4<_TyScalar>)(r_v_vec, 1)
 */
Vector3<_TyScalar>) Matrix4f::v_Transform_Pos(ConstArgType(Vector3<_TyScalar>)) r_v_vec) const
{
	return Vector3<_TyScalar>)(r_v_vec.x * f[0][0] + r_v_vec.y * f[1][0] + r_v_vec.z * f[2][0] + f[3][0],
				    r_v_vec.x * f[0][1] + r_v_vec.y * f[1][1] + r_v_vec.z * f[2][1] + f[3][1],
				    r_v_vec.x * f[0][2] + r_v_vec.y * f[1][2] + r_v_vec.z * f[2][2] + f[3][2]);
}

/*
 *	Vector3<_TyScalar>) Matrix4f::v_Transform_Dir(ConstArgType(Vector3<_TyScalar>)) r_v_vec) const
 *		- transforms direction r_v_vec by this matrix
 *		- equivalent to multiplying this matrix by Vector4<_TyScalar>)(r_v_vec, 0)
 */
Vector3<_TyScalar>) Matrix4f::v_Transform_Dir(ConstArgType(Vector3<_TyScalar>)) r_v_vec) const
{
	return Vector3<_TyScalar>)(r_v_vec.x * f[0][0] + r_v_vec.y * f[1][0] + r_v_vec.z * f[2][0],
				    r_v_vec.x * f[0][1] + r_v_vec.y * f[1][1] + r_v_vec.z * f[2][1],
				    r_v_vec.x * f[0][2] + r_v_vec.y * f[1][2] + r_v_vec.z * f[2][2]);
}

Plane3f Matrix4f::t_Transform_Plane(ConstArgType(Plane3f) r_t_plane) const
{
#if 0
	_TyScalar k = -r_t_plane.f_dist / r_t_plane.v_normal.f_Length2();
	Vector3<_TyScalar>) v_pos(v_Transform_Pos(r_t_plane.v_normal * k));
	// calculate a point on the plane in the target space

	return Plane3f(v_pos, v_Transform_Dir(r_t_plane.v_normal));
	// new plane with the target normal and with the point on it
#else // 0
	_TyScalar k = -r_t_plane.f_dist / r_t_plane.v_normal.f_Length2(); // the same as above
	Vector3<_TyScalar>) v_new_normal(v_Transform_Dir(r_t_plane.v_normal)); // 3 MUL + 3 ADD shorter than the corresponding line above
	_TyScalar f_new_dist = -v_new_normal.f_Dot(v_new_normal * k + v_Offset()); // 6 MUL + 3 ADD shorter than the corresponding line above
	// this version saves 9 MUL + 6 ADD, and is algebraically equivalent

	return Plane3f(v_new_normal, f_new_dist);

	// and how was it derived:
	//
	// let n = v_old_normal
	// k = -f_old_dist / n.f_Length2()
	// v_pos = vec3(dot(v_right, n * k), dot(v_up, n * k), dot(v_dir, n * k)) + v_off
	// v_new_normal = vec3(dot(v_right, n), dot(v_up, n), dot(v_dir, n))
	// f_new_dist = -dot(v_pos, v_new_normal)
	// f_new_dist = -dot(vec3(dot(v_right, n * k), dot(v_up, n * k), dot(v_dir, n * k)) + v_off, v_new_normal)
	// f_new_dist = -dot(vec3(dot(v_right, n * k), dot(v_up, n * k), dot(v_dir, n * k)), v_new_normal) - dot(v_off, v_new_normal)
	// f_new_dist = -dot(vec3(dot(v_right, n * k), dot(v_up, n * k), dot(v_dir, n * k)), vec3(dot(v_right, n), dot(v_up, n), dot(v_dir, n))) - dot(v_off, v_new_normal)
	// f_new_dist = -vec3(dot(v_right, n * k) * dot(v_right, n), dot(v_up, n * k) * dot(v_up, n), dot(v_dir, n * k) * dot(v_dir, n)) - dot(v_off, v_new_normal)
	// f_new_dist = -vec3(dot(v_right, n) * dot(v_right, n) * k, dot(v_up, n) * dot(v_up, n) * k, dot(v_dir, n) * dot(v_dir, n) * k) - dot(v_off, v_new_normal)
	// f_new_dist = -dot(v_new_normal, v_new_normal) * k - dot(v_off, v_new_normal)
	// f_new_dist = -dot(v_new_normal, v_new_normal * k + v_off)
#endif // 0
}

void Matrix4f::Get_FrustumPlanes(Plane3f &r_t_left, Plane3f &r_t_right, Plane3f &r_t_bottom,
	Plane3f &r_t_top, Plane3f &r_t_near, Plane3f &r_t_far, bool b_normalize) const
{
	{
		r_t_left.v_normal.x =	f[0][3] - f[0][0];
		r_t_left.v_normal.y =	f[1][3] - f[1][0];
		r_t_left.v_normal.z =	f[2][3] - f[2][0];
		r_t_left.f_dist =		f[3][3] - f[3][0];

		r_t_right.v_normal.x =	f[0][3] + f[0][0];
		r_t_right.v_normal.y =	f[1][3] + f[1][0];
		r_t_right.v_normal.z =	f[2][3] + f[2][0];
		r_t_right.f_dist =		f[3][3] + f[3][0];

		r_t_bottom.v_normal.x =	f[0][3] - f[0][1];
		r_t_bottom.v_normal.y = f[1][3] - f[1][1];
		r_t_bottom.v_normal.z = f[2][3] - f[2][1];
		r_t_bottom.f_dist =		f[3][3] - f[3][1];

		r_t_top.v_normal.x =	f[0][3] + f[0][1];
		r_t_top.v_normal.y =	f[1][3] + f[1][1];
		r_t_top.v_normal.z =	f[2][3] + f[2][1];
		r_t_top.f_dist =		f[3][3] + f[3][1];

		r_t_near.v_normal.x =	f[0][3] + f[0][2];
		r_t_near.v_normal.y =	f[1][3] + f[1][2];
		r_t_near.v_normal.z =	f[2][3] + f[2][2];
		r_t_near.f_dist =		f[3][3] + f[3][2];

		r_t_far.v_normal.x =	f[0][3] - f[0][2];
		r_t_far.v_normal.y =	f[1][3] - f[1][2];
		r_t_far.v_normal.z =	f[2][3] - f[2][2];
		r_t_far.f_dist =		f[3][3] - f[3][2];
	}
	// extract the planes

	if(b_normalize) {
		r_t_left.Normalize();
		r_t_right.Normalize();
		r_t_bottom.Normalize();
		r_t_top.Normalize();
		r_t_near.Normalize();
		r_t_far.Normalize();
	}
	// normalize the planes, if requested
}

void Matrix4f::FastInverseTo(Matrix4f &r_dest) const
{
	_ASSERTE(&r_dest != this); // use FastInvert() instead

	_ASSERTE(f[0][3] == 0 && f[1][3] == 0 && f[2][3] == 0 && f[3][3] == 1);
	// bottom row must be equal to 0 0 0 1

    _TyScalar f_det = f[0][0] * f[1][1] * f[2][2] +
            f[1][0] * f[2][1] * f[0][2] +
            f[0][1] * f[1][2] * f[2][0] -
            f[0][2] * f[1][1] * f[2][0] -
            f[0][1] * f[1][0] * f[2][2] -
            f[0][0] * f[1][2] * f[2][1];

    r_dest.f[0][0] = f[1][1] * f[2][2] - f[1][2] * f[2][1];
    r_dest.f[1][0] =-f[1][0] * f[2][2] + f[1][2] * f[2][0];
    r_dest.f[2][0] = f[1][0] * f[2][1] - f[1][1] * f[2][0];
    r_dest.f[3][0] =-f[1][0] * f[2][1] * f[3][2] -
		f[1][1] * f[2][2] * f[3][0] -
		f[2][0] * f[3][1] * f[1][2] +
		f[1][2] * f[2][1] * f[3][0] +
		f[1][1] * f[2][0] * f[3][2] +
		f[2][2] * f[1][0] * f[3][1];
    r_dest.f[0][1] =-f[0][1] * f[2][2] + f[0][2] * f[2][1];
    r_dest.f[1][1] = f[0][0] * f[2][2] - f[0][2] * f[2][0];
    r_dest.f[2][1] =-f[0][0] * f[2][1] + f[0][1] * f[2][0];
	r_dest.f[3][1] = f[0][0] * f[2][1] * f[3][2] +
		f[0][1] * f[2][2] * f[3][0] +
		f[2][0] * f[3][1] * f[0][2] -
		f[0][2] * f[2][1] * f[3][0] -
		f[0][1] * f[2][0] * f[3][2] -
		f[2][2] * f[0][0] * f[3][1];
    r_dest.f[0][2] = f[0][1] * f[1][2] - f[0][2] * f[1][1];
    r_dest.f[1][2] =-f[0][0] * f[1][2] + f[0][2] * f[1][0];
    r_dest.f[2][2] = f[0][0] * f[1][1] - f[0][1] * f[1][0];
    r_dest.f[3][2] =-f[0][0] * f[1][1] * f[3][2] -
		f[0][1] * f[1][2] * f[3][0] -
		f[1][0] * f[3][1] * f[0][2] +
		f[0][2] * f[1][1] * f[3][0] +
		f[0][1] * f[1][0] * f[3][2] +
		f[1][2] * f[0][0] * f[3][1];

	f_det = 1.0f / f_det;
    for(int j = 0; j < 4; ++ j) {
        for(int i = 0; i < 3; ++ i)
            r_dest.f[j][i] *= f_det;
	}

    r_dest.f[0][3] = 0;
    r_dest.f[1][3] = 0;
    r_dest.f[2][3] = 0;
    r_dest.f[3][3] = 1;
}

_TyScalar Matrix4f::f_Subdet(int n_col, int n_row) const
{
	int i0 = (n_row <= 0) + 0, i1 = (n_row <= 1) + 1, i2 = (n_row <= 2) + 2;
	int j0 = (n_col <= 0) + 0, j1 = (n_col <= 1) + 1, j2 = (n_col <= 2) + 2;

	return (f[j0][i0] * f[j1][i1] * f[j2][i2] +
		   f[j1][i0] * f[j2][i1] * f[j0][i2] +
		   f[j0][i1] * f[j1][i2] * f[j2][i0]) -
		   (f[j0][i2] * f[j1][i1] * f[j2][i0] +
		   f[j0][i1] * f[j1][i0] * f[j2][i2] +
		   f[j0][i0] * f[j1][i2] * f[j2][i1]);
}

_TyScalar Matrix4f::f_Determinant() const
{
	_TyScalar f_result = 0;
	_TyScalar f_sign = 1;
	for(int i = 0; i < n_column_num; ++ i, f_sign *= -1) {
		if(f[i][n_row_num - 1]) // last row is sometimes zero-prone
			f_result += f_sign * f_Subdet(i, n_row_num - 1) * f[i][n_row_num - 1];
	}
	return f_result;
}

void Matrix4f::FullInverseTo(Matrix4f &r_dest) const
{
	_ASSERTE(&r_dest != this); // use FullInvert() instead

	_TyScalar f_inv_det = 1 / f_Determinant();
	for(int i = 0; i < n_column_num; ++ i) {
		for(int j = 0; j < n_row_num; ++ j)
			r_dest.f[j][i] = (1 - 2 * ((i + j) & 1)) * f_Subdet(i, j) * f_inv_det;
	}
}

void Matrix4f::FullInverseNoTransposeTo(Matrix4f &r_dest) const
{
	_ASSERTE(&r_dest != this); // use FullInvert() instead

	_TyScalar f_inv_det = 1 / f_Determinant();
	for(int i = 0; i < n_column_num; ++ i) {
		for(int j = 0; j < n_row_num; ++ j)
			r_dest.f[i][j] = (1 - 2 * ((i + j) & 1)) * f_Subdet(i, j) * f_inv_det;
	}
}

void Matrix4f::Transpose()
{
	_TyScalar f_tmp0 = f[0][1];
    f[0][1] = f[1][0];
    f[1][0] = f_tmp0;
	_TyScalar f_tmp1 = f[0][2];
    f[0][2] = f[2][0];
    f[2][0] = f_tmp1;
	_TyScalar f_tmp2 = f[0][3];
    f[0][3] = f[3][0];
    f[3][0] = f_tmp2;
	_TyScalar f_tmp3 = f[1][2];
    f[1][2] = f[2][1];
    f[2][1] = f_tmp3;
	_TyScalar f_tmp4 = f[1][3];
    f[1][3] = f[3][1];
    f[3][1] = f_tmp4;
	_TyScalar f_tmp5 = f[2][3];
    f[2][3] = f[3][2];
    f[3][2] = f_tmp5;
}

#endif // 0

/*
 *        === ~Matrix4f ===
 */
