#include "../include/xcv_license.h" //"xcv_license.h" //PCH
#include "../include/xcv_algebra3.h" //"algebra3.h"
	
namespace XCV_ALGEBRA3_NAMESPACE //Widgets95
{//-.
//<-'

	/*
#ifdef VEC_ERROR_FATAL
#ifndef VEC_ERROR
#define VEC_ERROR(E) { printf( "VERROR %s\n", E ); exit(1); }
#endif
#else
#ifndef VEC_ERROR
#define VEC_ERROR(E) { printf( "VERROR %s\n", E ); }
#endif
#endif*/

	enum{ VX, VY, VZ, VW }; //REMOVE ME

/****************************************************************
 *                                                              *
 *          vec2 Member functions                               *
 *                                                              *
 ****************************************************************/

/******************** vec2 CONSTRUCTORS ********************/

vec2::vec2()
{
	n[VX] = n[VY] = 0;
}

vec2::vec2(fp_t x, fp_t y)
{
	n[VX] = x; n[VY] = y;
}

vec2::vec2(const vec2 &v)
{
	n[VX] = v[0]; n[VY] = v[1];
}

vec2::vec2(const vec3 &v) // it is up to caller to avoid divide-by-zero
{
	n[VX] = v[0]/v[2];
	n[VY] = v[1]/v[2];
}

vec2::vec2(const vec3 &v, int dropAxis)
{
	switch(dropAxis)
	{
	case VX: n[VX] = v[1]; n[VY] = v[2]; break;
	case VY: n[VX] = v[0]; n[VY] = v[2]; break;
	default: n[VX] = v[0]; n[VY] = v[1]; break;
	}
}

/******************** vec2 ASSIGNMENT OPERATORS ******************/

vec2 &vec2::operator=(const vec2 &v)
{
	n[VX] = v[0]; n[VY] = v[1]; return *this;
}

vec2 &vec2::operator+=(const vec2 &v)
{
	n[VX] += v[0]; n[VY] += v[1]; return *this;
}

vec2 &vec2::operator-=(const vec2 &v)
{
	n[VX] -= v[0]; n[VY] -= v[1]; return *this;
}

vec2 &vec2::operator*=(fp_t d)
{
	n[VX] *= d; n[VY] *= d; return *this;
}

vec2 &vec2::operator/=(fp_t d)
{
	fp_t d_inv = 1/d;
	n[VX] *= d_inv;
	n[VY] *= d_inv; return *this;
}

fp_t &vec2::operator[](int i)
{
//	if(i<VX||i>VY)
	//VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec2 [] operator: illegal access");
	return n[i];
}

const fp_t &vec2::operator[](int i)const
{
//	if(i<VX||i>VY)
	//VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec2 [] operator: illegal access");
	return n[i];
}

/******************** vec2 SPECIAL FUNCTIONS ********************/

fp_t vec2::length()const
{
	return sqrt(length2());
}

fp_t vec2::length2()const
{
	return n[VX]*n[VX]+n[VY]*n[VY];
}

vec2 &vec2::normalize() // it is up to caller to avoid divide-by-zero
{
	return *this /= length(); 
}

void vec2::set(fp_t x, fp_t y)
{
	n[VX] = x; n[VY] = y;
}

/******************** vec2 FRIENDS *****************************/

vec2 operator-(const vec2 &a)
{
	return vec2(-a[0],-a[1]);
}

vec2 operator+(const vec2 &a, const vec2& b)
{
	return vec2(a[0]+b[0],a[1]+b[1]);
}

vec2 operator-(const vec2 &a, const vec2& b)
{
	return vec2(a[0]-b[0],a[1]-b[1]);
}

vec2 operator*(const vec2 &a, fp_t d)
{
	return vec2(d*a[0],d*a[1]);
}

vec2 operator*(fp_t d, const vec2 &a)
{
	return a*d;
}

//2021: Reversing to match GLSL (what about mat*mat?)
//vec2 operator*(const mat3 &a, const vec2 &v)
vec2 operator*(const vec2 &v, const mat3 &a)
{
	return vec2
	(a.v[0][0]*v[0]+a.v[0][1]*v[1]+a.v[0][2]
	,a.v[1][0]*v[0]+a.v[1][1]*v[1]+a.v[1][2]);
}
//vec2 operator*(const vec2 &v, const mat3 &a)
vec2 operator*(const mat3 &a, const vec2 &v)
{
	//2021: This version matches OpenGL results ... it should be default?
	//return a.transpose()*v;

	return vec2
	(a.v[0][0]*v[0]+a.v[1][0]*v[1]+a.v[2][0]
	,a.v[0][1]*v[0]+a.v[1][1]*v[1]+a.v[2][1]);
}

//2021: Reversing to match GLSL (what about mat*mat?)
//vec3 operator*(const mat3 &a, const vec3 &v)
vec3 operator*(const vec3 &v, const mat3 &a)
{
	return vec3
	(a.v[0][0]*v[0]+a.v[0][1]*v[1]+a.v[0][2]*v[2]
	,a.v[1][0]*v[0]+a.v[1][1]*v[1]+a.v[1][2]*v[2]
	,a.v[2][0]*v[0]+a.v[2][1]*v[1]+a.v[2][2]*v[2]);
}

//vec3 operator*(const vec3 &v, const mat3 &a)
vec3 operator*(const mat3 &a, const vec3 &v)
{
	//2021: This version matches OpenGL results ... it should be default?
	//return a.transpose()*v;

	return vec3
	(a.v[0][0]*v[0]+a.v[1][0]*v[1]+a.v[2][0]*v[2]
	,a.v[0][1]*v[0]+a.v[1][1]*v[1]+a.v[2][1]*v[2]
	,a.v[0][2]*v[0]+a.v[1][2]*v[1]+a.v[2][2]*v[2]);
}

fp_t operator*(const vec2 &a, const vec2 &b)
{
	return a[0]*b[0]+a[1]*b[1];
}

vec2 operator/(const vec2 &a, fp_t d)
{
	fp_t d_inv = 1/d;
	return vec2(a[0]*d_inv,a[1]*d_inv);
}

vec3 operator%(const vec2 &a, const vec2 &b) //^
{
	return vec3(0,0,a[0]*b[1]-b[0]*a[1]);
}

int operator==(const vec2 &a, const vec2 &b)
{
	return(a[0]==b[0]) && (a[1]==b[1]);
}

int operator!=(const vec2 &a, const vec2 &b)
{
	return !(a==b);
}

/****************************************************************
 *                                                              *
 *          vec3 Member functions                               *
 *                                                              *
 ****************************************************************/

// CONSTRUCTORS

vec3::vec3()
{
	n[VX] = n[VY] = n[VZ] = 0;
}

vec3::vec3(fp_t x, fp_t y, fp_t z)
{
	n[VX] = x; n[VY] = y; n[VZ] = z;
}

vec3::vec3(const vec3 &v)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v[2];
}

vec3::vec3(const vec2 &v)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = 1;
}

vec3::vec3(const vec2 &v, fp_t d)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = d;
}

vec3::vec3(const vec4 &v) // it is up to caller to avoid divide-by-zero
{
	n[VX] = v[0]/v.n[VW];
	n[VY] = v[1]/v.n[VW];
	n[VZ] = v[2]/v.n[VW];
}

vec3::vec3(const vec4 &v, int dropAxis)
{
	switch(dropAxis)
	{
	case VX: n[VX] = v[1]; n[VY] = v[2]; n[VZ] = v.n[VW]; break;
	case VY: n[VX] = v[0]; n[VY] = v[2]; n[VZ] = v.n[VW]; break;
	case VZ: n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v.n[VW]; break;
	default: n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v[2]; break;
	}
}


// ASSIGNMENT OPERATORS

vec3 &vec3::operator=(const vec3 &v)
{
	n[VX] = v[0];
	n[VY] = v[1];
	n[VZ] = v[2]; return *this;
}

vec3 &vec3::operator+=(const vec3 &v)
{
	n[VX] += v[0];
	n[VY] += v[1];
	n[VZ] += v[2]; return *this;
}

vec3 &vec3::operator-=(const vec3& v)
{
	n[VX] -= v[0];
	n[VY] -= v[1];
	n[VZ] -= v[2]; return *this;
}

vec3 &vec3::operator*=(fp_t d)
{
	n[VX] *= d;
	n[VY] *= d;
	n[VZ] *= d; return *this;
}

vec3 &vec3::operator/=(fp_t d)
{
	fp_t d_inv = 1/d;
	n[VX] *= d_inv;
	n[VY] *= d_inv;
	n[VZ] *= d_inv; return *this;
}

fp_t &vec3::operator[](int i)
{
//	if(i<VX||i>VZ)
	//VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec3 [] operator: illegal access");
	return n[i];
}

const fp_t &vec3::operator[](int i)const
{
//	if(i<VX||i>VZ)
	//VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec3 [] operator: illegal access");
	return n[i];
}

// SPECIAL FUNCTIONS

fp_t vec3::length()const
{
	return sqrt(length2());
}

fp_t vec3::length2()const
{
	return n[VX]*n[VX]+n[VY]*n[VY]+n[VZ]*n[VZ];
}

vec3 &vec3::normalize() // it is up to caller to avoid divide-by-zero
{
	*this /= length(); return *this;
}

vec3 &vec3::homogenize() // it is up to caller to avoid divide-by-zero
{
	n[VX] /= n[VZ];
	n[VY] /= n[VZ]; n[VZ] = 1; return *this;
}

void vec3::set(fp_t x, fp_t y, fp_t z) // set vector
{
	n[VX] = x; n[VY] = y; n[VZ] = z;
}

// FRIENDS

vec3 operator-(const vec3 &a)
{
	return vec3(-a[0],-a[1],-a[2]);
}

vec3 operator+(const vec3 &a, const vec3 &b)
{
	return vec3(a[0]+b[0],a[1]+b[1],a[2]+b[2]);
}

vec3 operator-(const vec3 &a, const vec3 &b)
{
	return vec3(a[0]-b[0],a[1]-b[1],a[2]-b[2]);
}

vec3 operator*(const vec3 &a, fp_t d)
{
	return vec3(d*a[0],d*a[1],d*a[2]);
}

vec3 operator*(fp_t d, const vec3 &a)
{
	return a*d;
}

vec3 operator*(const mat4 &a, const vec3 &v)
{
	return a*vec4(v);
}

vec3 operator*(const vec3 &v, const mat4 &a)
{
	//2021: This version matches OpenGL results ... it should be default?
	//return a.transpose()*v;

	return vec4(v)*a;
}

fp_t operator*(const vec3 &a, const vec3 &b)
{
	return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}

vec3 operator/(const vec3 &a, fp_t d)
{
	fp_t d_inv = 1/d;
	return vec3(a[0]*d_inv,a[1]*d_inv,a[2]*d_inv);
}

vec3 operator%(const vec3 &a, const vec3 &b)
{
	return vec3(
	a[1]*b[2]-a[2]*b[1],
	a[2]*b[0]-a[0]*b[2],
	a[0]*b[1]-a[1]*b[0]);
}

int operator==(const vec3 &a,const vec3 &b)
{
	return a[0]==b[0]&&a[1]==b[1]&&a[2]==b[2];
}

int operator!=(const vec3 &a,const vec3 &b)
{
	return !(a==b);
}

/****************************************************************
 *                                                              *
 *          vec4 Member functions                               *
 *                                                              *
 ****************************************************************/

// CONSTRUCTORS

vec4::vec4()
{
	n[VX] = n[VY] = n[VZ] = 0;
	n[VW] = 1;
}

vec4::vec4(fp_t x, fp_t y, fp_t z, fp_t w)
{
	n[VX] = x; n[VY] = y; n[VZ] = z; n[VW] = w;
}

vec4::vec4(const vec4 &v)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v[2]; n[VW] = v.n[VW];
}

vec4::vec4(const vec3 &v)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v[2]; n[VW] = 1;
}

vec4::vec4(const vec3 &v,fp_t d)
{
	n[VX] = v[0]; n[VY] = v[1]; n[VZ] = v[2]; n[VW] = d;
}

// ASSIGNMENT OPERATORS

vec4 &vec4::operator=(const vec4 &v)
{
	n[VX] = v[0];
	n[VY] = v[1];
	n[VZ] = v[2];
	n[VW] = v.n[VW]; return *this;
}

vec4 &vec4::operator+=(const vec4 &v)
{
	n[VX] += v[0];
	n[VY] += v[1];
	n[VZ] += v[2];
	n[VW] += v.n[VW]; return *this;
}

vec4 &vec4::operator-=(const vec4 &v)
{
	n[VX] -= v[0];
	n[VY] -= v[1];
	n[VZ] -= v[2];
	n[VW] -= v.n[VW]; return *this;
}

vec4 &vec4::operator*=(fp_t d)
{
	n[VX] *= d;
	n[VY] *= d;
	n[VZ] *= d;
	n[VW] *= d; return *this;
}

vec4 &vec4::operator/=(fp_t d)
{
	fp_t d_inv = 1/d;
	n[VX] *= d_inv;
	n[VY] *= d_inv;
	n[VZ] *= d_inv;
	n[VW] *= d_inv; return *this;
}

fp_t &vec4::operator[](int i)
{
//	if(i<VX||i>VW)
	//VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec4 [] operator: illegal access");
	return n[i];
}

const fp_t &vec4::operator[](int i)const
{
//	if(i<VX||i>VW)
	//VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("vec4 [] operator: illegal access");
	return n[i];
}

// SPECIAL FUNCTIONS

fp_t vec4::length()const
{
	return sqrt(length2());
}

fp_t vec4::length2()const
{
	return n[VX]*n[VX]+n[VY]*n[VY]+n[VZ]*n[VZ]+n[VW]*n[VW];
}

vec4 &vec4::normalize() // it is up to caller to avoid divide-by-zero
{
	return *this /= length();
}

vec4 &vec4::homogenize() // it is up to caller to avoid divide-by-zero
{
	n[VX] /= n[VW];
	n[VY] /= n[VW];
	n[VZ] /= n[VW]; n[VW] = 1; return *this;
}

void vec4::set(fp_t x, fp_t y, fp_t z, fp_t a)
{
	n[0] = x; n[1] = y; n[2] = z; n[3] = a;
}


// FRIENDS

vec4 operator-(const vec4 &a)
{
	return vec4(-a[0],-a[1],-a[2],-a.n[VW]);
}

vec4 operator+(const vec4 &a, const vec4 &b)
{
	return vec4(
	a[0]+b[0],
	a[1]+b[1],
	a[2]+b[2],
	a.n[VW]+b.n[VW]);
}

vec4 operator-(const vec4 &a, const vec4 &b)
{
	return vec4(
	a[0]-b[0],
	a[1]-b[1],
	a[2]-b[2],
	a.n[VW]-b.n[VW]);
}

vec4 operator*(const vec4 &a, fp_t d)
{
	return vec4(d*a[0],d*a[1],d*a[2],d*a.n[VW]);
}

vec4 operator*(fp_t d, const vec4 &a)
{
	return a*d;
}

//2021: Reversing to match GLSL (what about mat*mat?)
//vec4 operator*(const mat4 &a, const vec4 &v)
vec4 operator*(const vec4 &v, const mat4 &a)
{
	/*Interesting ???
	#define ROWCOL(i) \
    a.v[i].n[0]*v[0] + \
    a.v[i].n[1]*v[1] + \
    a.v[i].n[2]*v[2] + \
    a.v[i].n[3]*v.n[VW]
	return vec4(ROWCOL(0),ROWCOL(1),ROWCOL(2),ROWCOL(3));
	#undef ROWCOL*/

	return vec4
	(a.v[0][0]*v[0]+a.v[0][1]*v[1]+a.v[0][2]*v[2]+a.v[0][3]*v[3]
	,a.v[1][0]*v[0]+a.v[1][1]*v[1]+a.v[1][2]*v[2]+a.v[1][3]*v[3]
	,a.v[2][0]*v[0]+a.v[2][1]*v[1]+a.v[2][2]*v[2]+a.v[2][3]*v[3]
	,a.v[3][0]*v[0]+a.v[3][1]*v[1]+a.v[3][2]*v[2]+a.v[3][3]*v[3]);
}

//vec4 operator*(const vec4 &v, const mat4 &a)
vec4 operator*(const mat4 &a, const vec4 &v)
{
	//2021: This version matches OpenGL results ... it should be default?
	//return a.transpose()*v;

	return vec4
	(a.v[0][0]*v[0]+a.v[1][0]*v[1]+a.v[2][0]*v[2]+a.v[3][0]*v[3]
	,a.v[0][1]*v[0]+a.v[1][1]*v[1]+a.v[2][1]*v[2]+a.v[3][1]*v[3]
	,a.v[0][2]*v[0]+a.v[1][2]*v[1]+a.v[2][2]*v[2]+a.v[3][2]*v[3]
	,a.v[0][3]*v[0]+a.v[1][3]*v[1]+a.v[2][3]*v[2]+a.v[3][3]*v[3]);
}

fp_t operator*(const vec4 &a, const vec4 &b)
{
	return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a.n[VW]*b.n[VW];
}

vec4 operator/(const vec4 &a, fp_t d)
{
	fp_t d_inv = 1/d;
	return vec4(a[0]*d_inv,a[1]*d_inv,a[2]*d_inv,a.n[VW]*d_inv);
}

int operator==(const vec4 &a, const vec4 &b)
{
	return a[0]==b[0]&&a[1]==b[1]&&a[2]==b[2]&&a.n[VW]==b.n[VW];
}

int operator!=(const vec4 &a, const vec4 &b)
{
	return !(a==b);
}

/****************************************************************
 *                                                              *
 *          mat3 member functions                               *
 *                                                              *
 ****************************************************************/

// CONSTRUCTORS

mat3::mat3()
{
	*this = identity2D();
}

mat3::mat3(const vec3 &v0, const vec3 &v1, const vec3 &v2)
{
	set(v0,v1,v2);
}

mat3::mat3(const mat3 &m)
{
	v[0] = m.v[0];
	v[1] = m.v[1];
	v[2] = m.v[2];
}

/*2021: Is this stuff ever useful???
mat3 &mat3::operator=(const mat3 &m)
{
	v[0] = m.v[0];
	v[1] = m.v[1];
	v[2] = m.v[2]; return *this;
}
mat3 &mat3::operator+=(const mat3& m)
{
	v[0] += m.v[0];
	v[1] += m.v[1];
	v[2] += m.v[2]; return *this;
}
mat3 &mat3::operator-=(const mat3& m)
{
	v[0] -= m.v[0];
	v[1] -= m.v[1];
	v[2] -= m.v[2]; return *this;
}
mat3 &mat3::operator*=(fp_t d)
{
	v[0] *= d;
	v[1] *= d;
	v[2] *= d; return *this;
}
mat3 &mat3::operator/=(fp_t d)
{
	v[0] /= d;
	v[1] /= d;
	v[2] /= d; return *this;
}*/

vec3 &mat3::operator[](int i)
{
//	if(i<VX||i>VZ)
	//VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("mat3 [] operator: illegal access");
	return v[i];
}

const vec3 &mat3::operator[](int i)const
{
//	if(i<VX||i>VZ)
	//VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("mat3 [] operator: illegal access");
	return v[i];
}

void mat3::set(const vec3 &v0, const vec3 &v1, const vec3 &v2)
{
	v[0] = v0; v[1] = v1; v[2] = v2;
}

// SPECIAL FUNCTIONS

mat3 mat3::transpose()const
{
	return mat3(
	vec3(v[0][0],v[1][0],v[2][0]),
	vec3(v[0][1],v[1][1],v[2][1]),
	vec3(v[0][2],v[1][2],v[2][2]));
}

mat3 mat3::inverse()const // Gauss-Jordan elimination with partial pivoting
{
	mat3 a(*this);// As a evolves from original mat into identity
	mat3 b(identity2D());// b evolves from identity into inverse(a)
	int i,j,i1;

	// Loop over cols of a from left to right, eliminating above and below diag
	for(j=0;j<3;j++) // Find largest pivot in column j among rows j..2
	{
		i1 = j;// Row with largest pivot candidate
		for(i=j+1;i<3;i++)
		if(fabs(a.v[i].n[j])>fabs(a.v[i1].n[j]))
		i1 = i;

		// Swap rows i1 and j in a and b to put pivot on diagonal
		std::swap(a.v[i1],a.v[j]);
		std::swap(b.v[i1],b.v[j]);

		// Scale row j to have a unit diagonal
//		if(a.v[j].n[j]==0)
//		VEC_ERROR("mat3::inverse: singular matrix; can't invert\n");

		b.v[j] /= a.v[j].n[j];
		a.v[j] /= a.v[j].n[j];

		// Eliminate off-diagonal elems in col j of a, doing identical ops to b
		for(i=0;i<3;i++) if(i!=j)
		{
			b.v[i] -= a.v[i].n[j]*b.v[j];
			a.v[i] -= a.v[i].n[j]*a.v[j];
		}
	}

	return b;
}

/*2021: Is this stuff ever useful?
mat3 operator-(const mat3 &a)
{
	return mat3(-a.v[0],-a.v[1],-a.v[2]);
}
mat3 operator+(const mat3 &a, const mat3 &b)
{
	return mat3(a.v[0]+b.v[0],a.v[1]+b.v[1],a.v[2]+b.v[2]);
}
mat3 operator-(const mat3 &a, const mat3 &b)
{
	return mat3(a.v[0]-b.v[0],a.v[1]-b.v[1],a.v[2]-b.v[2]);
}
mat3 operator*(const mat3 &a, fp_t d)
{
	return mat3(a.v[0]*d,a.v[1]*d,a.v[2]*d);
}
mat3 operator*(fp_t d, const mat3 &a)
{
	return a*d;
}
mat3 operator/(const mat3 &a, fp_t d)
{
	return mat3(a.v[0]/d,a.v[1]/d,a.v[2]/d);
}
int operator==(const mat3 &a, const mat3 &b)
{
	return
	a.v[0]==b.v[0] &&
	a.v[1]==b.v[1] &&
	a.v[2]==b.v[2];
}
int operator!=(const mat3 &a, const mat3 &b)
{
	return !(a==b);
}*/

//2021: Reversed to match OpenGL.
//mat3 operator*(const mat3 &a, const mat3 &b)
mat3 mat3::operator*(const mat3 &a)
{
	auto &b = *this; //Reversing to match OpenGL.

	#define ROWCOL(i, j) \
	a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]

	return mat3(
	vec3(ROWCOL(0,0),ROWCOL(0,1),ROWCOL(0,2)),
	vec3(ROWCOL(1,0),ROWCOL(1,1),ROWCOL(1,2)),
	vec3(ROWCOL(2,0),ROWCOL(2,1),ROWCOL(2,2)));

	#undef ROWCOL
}

/****************************************************************
 *                                                              *
 *          mat4 member functions                               *
 *                                                              *
 ****************************************************************/

// CONSTRUCTORS

mat4::mat4()
{
	*this = identity3D();
}

mat4::mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3)
{
	v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3;
}

mat4::mat4(const mat4 &m)
{
	v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
}

mat4::mat4(
fp_t a00, fp_t a01, fp_t a02, fp_t a03, 
fp_t a10, fp_t a11, fp_t a12, fp_t a13, 
fp_t a20, fp_t a21, fp_t a22, fp_t a23, 
fp_t a30, fp_t a31, fp_t a32, fp_t a33)
{
	v[0][0] = a00; v[0][1] = a01; v[0][2] = a02; v[0][3] = a03;
	v[1][0] = a10; v[1][1] = a11; v[1][2] = a12; v[1][3] = a13;
	v[2][0] = a20; v[2][1] = a21; v[2][2] = a22; v[2][3] = a23;
	v[3][0] = a30; v[3][1] = a31; v[3][2] = a32; v[3][3] = a33;
}

void mat4::ortho(fp_t left, fp_t right, 
fp_t bottom, fp_t top, fp_t znear, fp_t zfar)
{
	v[0][0] = 2/(right-left);
	v[0][1] = 0;
	v[0][2] = 0;
	v[0][3] = 0;
	v[1][0] = 0;
	v[1][1] = 2/(top-bottom);
	v[1][2] = 0;
	v[1][3] = 0;
	v[2][0] = 0;
	v[2][1] = 0;
	v[2][2] = -2/(zfar-znear);
	v[2][3] = 0;
	v[3][0] = -(right+left)/(right-left);
	v[3][1] = -(top+bottom)/(top-bottom);			
	v[3][2] = -(zfar+znear)/(zfar-znear);
	v[3][3] = 1;
}
void mat4::frustum(fp_t left, fp_t right, 
fp_t bottom, fp_t top, fp_t znear, fp_t zfar)
{
	v[0][0] = (znear*2)/(right-left);
	v[0][1] = 0;
	v[0][2] = 0;
	v[0][3] = 0;
	v[1][0] = 0;
	v[1][1] = (znear*2)/(top-bottom);
	v[1][2] = 0;
	v[1][3] = 0;
	v[2][0] = (right+left)/(right-left);
	v[2][1] = (top+bottom)/(top-bottom);
	//hmm: would prefer to go with OpenGL here???
	v[2][2] = -(zfar+znear)/(zfar-znear); //OpenGL
	//v[2][2] = zfar/(znear-zfar); //Direct3D
	//NOTE: D3D may be 1 here?
	v[2][3] = -1;
	v[3][0] = 0;
	v[3][1] = 0;			
	v[3][2] = -(zfar*znear*2)/(zfar-znear); //OpenGL
	//v[3][2] = zfar*znear/(znear-zfar); //Direct3D
	v[3][3] = 0;
}

/*2021: Is this stuff ever useful?
mat4 &mat4::operator=(const mat4 &m)
{
	v[0] = m.v[0];
	v[1] = m.v[1];
	v[2] = m.v[2];
	v[3] = m.v[3]; return *this;
}
mat4 &mat4::operator+=(const mat4 &m)
{
	v[0] += m.v[0];
	v[1] += m.v[1];
	v[2] += m.v[2];
	v[3] += m.v[3]; return *this;
}
mat4 &mat4::operator-=(const mat4 &m)
{
	v[0] -= m.v[0];
	v[1] -= m.v[1];
	v[2] -= m.v[2];
	v[3] -= m.v[3]; return *this;
}
mat4 &mat4::operator*=(fp_t d)
{
	v[0] *= d;
	v[1] *= d;
	v[2] *= d;
	v[3] *= d; return *this;
}
mat4 &mat4::operator/=(fp_t d)
{
	v[0] /= d;
	v[1] /= d;
	v[2] /= d;
	v[3] /= d; return *this;
}*/

vec4 &mat4::operator[](int i)
{
//	if(i<VX||i>VW)
	//VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("mat4 [] operator: illegal access");
	return v[i];
}

const vec4 &mat4::operator[](int i)const
{
//	if(i<VX||i>VW)
	//VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
//	VEC_ERROR("mat4 [] operator: illegal access");
	return v[i];
}

// SPECIAL FUNCTIONS;

mat4 mat4::transpose()const
{
	return mat4(
	vec4(v[0][0],v[1][0],v[2][0],v[3][0]),
	vec4(v[0][1],v[1][1],v[2][1],v[3][1]),
	vec4(v[0][2],v[1][2],v[2][2],v[3][2]),
	vec4(v[0][3],v[1][3],v[2][3],v[3][3]));
}

mat4 mat4::inverse()const // Gauss-Jordan elimination with partial pivoting
{
	mat4 a(*this);// As a evolves from original mat into identity
	mat4 b(identity3D());// b evolves from identity into inverse(a)
	int i,j,i1;

	// Loop over cols of a from left to right, eliminating above and below diag
	for(j=0;j<4;j++) // Find largest pivot in column j among rows j..3
	{
		i1 = j;// Row with largest pivot candidate
		for(i=j+1;i<4;i++)
		if(fabs(a.v[i].n[j])>fabs(a.v[i1].n[j]))
		i1 = i;

		// Swap rows i1 and j in a and b to put pivot on diagonal
		std::swap(a.v[i1],a.v[j]);
		std::swap(b.v[i1],b.v[j]);

		// Scale row j to have a unit diagonal
//		if(a.v[j].n[j]==0)
//		VEC_ERROR("mat4::inverse: singular matrix; can't invert\n");

		b.v[j] /= a.v[j].n[j];
		a.v[j] /= a.v[j].n[j];

		// Eliminate off-diagonal elems in col j of a, doing identical ops to b
		for(i=0;i<4;i++) if(i!=j)
		{
			b.v[i] -= a.v[i].n[j]*b.v[j];
			a.v[i] -= a.v[i].n[j]*a.v[j];
		}
	}

	return b;
}

void mat4::swap_rows(int i, int j)
{
	vec4 t; t = v[i]; v[i] = v[j]; v[j] = t;
}

void mat4::swap_cols(int i, int j)
{
	fp_t t; int k; for(k=0;k<4;k++)
	{
		t = v[k][i]; v[k][i] = v[k][j]; v[k][j] = t;
	}
}

/*2021: Is this stuff ever useful???
mat4 operator-(const mat4 &a)
{
	return mat4(-a.v[0],-a.v[1],-a.v[2],-a.v[3]);
}
mat4 operator+(const mat4 &a, const mat4 &b)
{
	return mat4(
	a.v[0]+b.v[0],
	a.v[1]+b.v[1],
	a.v[2]+b.v[2],
	a.v[3]+b.v[3]);
}
//2021/TODO? I have a feeling a/b should be swapped since
//most of these classes hadn't followed OpenGL convention.
mat4 operator-(const mat4 &a, const mat4 &b)
{
	return mat4(
	a.v[0]-b.v[0],
	a.v[1]-b.v[1],
	a.v[2]-b.v[2],
	a.v[3]-b.v[3]);
}
mat4 operator*(const mat4 &a, fp_t d)
{
	return mat4(a.v[0]*d,a.v[1]*d,a.v[2]*d,a.v[3]*d);
}
mat4 operator*(fp_t d, const mat4 &a)
{
	return a*d;
}
mat4 operator/(const mat4 &a, fp_t d)
{
	return mat4(a.v[0]/d,a.v[1]/d,a.v[2]/d,a.v[3]/d);
}
int operator==(const mat4 &a, const mat4 &b)
{
	return
	a.v[0]==b.v[0] &&
	a.v[1]==b.v[1] &&
	a.v[2]==b.v[2] &&
	a.v[3]==b.v[3];
}

int operator!=(const mat4 &a, const mat4 &b)
{
	return !(a==b);
}*/

//2021: Reversing to match OpenGL.
//mat4 operator*(const mat4 &a, const mat4 &b)
mat4 mat4::operator*(const mat4 &a)
{
	auto &b = *this; //Reversing to match OpenGL.

	#define ROWCOL(i, j) \
	a.v[i].n[0]*b.v[0][j] + \
	a.v[i].n[1]*b.v[1][j] + \
	a.v[i].n[2]*b.v[2][j] + \
	a.v[i].n[3]*b.v[3][j]

	return mat4(
	vec4(ROWCOL(0,0),ROWCOL(0,1),ROWCOL(0,2),ROWCOL(0,3)),
	vec4(ROWCOL(1,0),ROWCOL(1,1),ROWCOL(1,2),ROWCOL(1,3)),
	vec4(ROWCOL(2,0),ROWCOL(2,1),ROWCOL(2,2),ROWCOL(2,3)),
	vec4(ROWCOL(3,0),ROWCOL(3,1),ROWCOL(3,2),ROWCOL(3,3)));

	#undef ROWCOL
}

/****************************************************************
 *                                                              *
 *         2D functions and 3D functions                        *
 *                                                              *
 ****************************************************************/

mat3 identity2D()
{
	return mat3(vec3(1,0,0),vec3(0,1,0),vec3(0,0,1));
}

mat3 translation2D(const vec2 &v)
{
	/*2021: This isn't OpenGL.
	return mat3(vec3(1,0,v[VX]),vec3(0,1,v[VY]),vec3(0,0,1));*/
	return mat3(vec3(1,0,0),vec3(0,1,0),vec3(v[0],v[1],1));
}

mat3 rotation2D(const vec2 &Center, fp_t angleDeg)
{
	//UNTESTED: Only changing to match rotation3D.
	//2021: glRotatef seems to go in the opposite
	//direction from this in my work on GL->ANGLE.
	//fp_t angleRad = angleDeg*(fp_t)M_PI/180;
	fp_t angleRad = angleDeg*(fp_t)M_PI/-180;
	fp_t c = cos(angleRad);
	fp_t s = sin(angleRad);

	return mat3(
	vec3(c,-s,Center[VX] * (1-c)+Center[VY]*s),
	vec3(s,c,Center[VY] * (1-c)-Center[VX]*s),
	vec3(0,0,1));
}

mat3 scaling2D(const vec2 &scaleVector)
{
	return mat3(
	vec3(scaleVector[VX],0,0),
	vec3(0,scaleVector[VY],0),
	vec3(0,0,1));
}

mat4 identity3D()
{
	return mat4(
	vec4(1,0,0,0),
	vec4(0,1,0,0),
	vec4(0,0,1,0),
	vec4(0,0,0,1));
}

mat4 translation3D(const vec3 &v)
{
	/*2021: This isn't OpenGL.
	return mat4(
	vec4(1,0,0,v[VX]),
	vec4(0,1,0,v[VY]),
	vec4(0,0,1,v[VZ]),
	vec4(0,0,0,1));*/
	return mat4(
	vec4(1,0,0,0),
	vec4(0,1,0,0),
	vec4(0,0,1,0),
	vec4(v[0],v[1],v[2],1));
}

mat4 rotation3D(const vec3 &Axis, fp_t angleDeg)
{
	//2021: glRotatef seems to go in the opposite
	//direction from this in my work on GL->ANGLE.
	//fp_t angleRad = angleDeg*(fp_t)M_PI/180;
	fp_t angleRad = angleDeg*(fp_t)M_PI/-180;
	fp_t c = cos(angleRad);
	fp_t s = sin(angleRad);
	fp_t t = 1-c;

	vec3 axis(Axis); axis.normalize();

	return mat4(
	vec4(t*axis[VX]*axis[VX]+c,
	t*axis[VX]*axis[VY]-s*axis[VZ],
	t*axis[VX]*axis[VZ]+s*axis[VY],0),
	vec4(t*axis[VX]*axis[VY]+s*axis[VZ],
	t*axis[VY]*axis[VY]+c,
	t*axis[VY]*axis[VZ]-s*axis[VX],0),
	vec4(t*axis[VX]*axis[VZ]-s*axis[VY],
	t*axis[VY]*axis[VZ]+s*axis[VX],
	t*axis[VZ]*axis[VZ]+c,0),
	vec4(0,0,0,1));
}

mat4 scaling3D(const vec3 &scaleVector)
{
	return mat4(
	vec4(scaleVector[VX],0,0,0),
	vec4(0,scaleVector[VY],0,0),
	vec4(0,0,scaleVector[VZ],0),
	vec4(0,0,0,1));
}

mat4 perspective3D(fp_t d)
{
	return mat4(vec4(1,0,0,0),vec4(0,1,0,0),vec4(0,0,1,0),
	vec4(0,0,1/d,0));
}

/*
void vec3::print(FILE *file,const char *name)const // print vector to a file
{
	fprintf(file,"%s: <%f, %f, %f>\n",name,n[VX],n[VY],n[VZ]);
}
void vec4::print(FILE *file, const char *name)const // print vector to a file
{
	fprintf(file,"%s: <%f, %f, %f, %f>\n",name,n[VX],n[VY],n[VZ],n[VW]);
}
void mat3::print(FILE *file, const char *name)const
{
	int i,j;

	fprintf(stderr,"%s:\n",name);

	for(i=0;i<3;i++)
	{
		fprintf(stderr,"   ");
		for(j=0;j<3;j++)
		{
			fprintf(stderr,"%f  ",v[i][j]);
		}
		fprintf(stderr,"\n");
	}
}
void mat4::print(FILE *file, const char *name)const
{
	int i,j;

	fprintf(stderr,"%s:\n",name);

	for(i=0;i<4;i++)
	{
		fprintf(stderr,"   ");
		for(j=0;j<4;j++)
		{
			fprintf(stderr,"%f  ",v[i][j]);
		}
		fprintf(stderr,"\n");
	}
}

ostream &operator<<(ostream &s, vec2& v)
{
	return s << "| " << v[0] << ' ' << v[1] << " |"; 
}
istream &operator>>(istream &s, vec2& v)
{
	vec2 v_tmp;
	char c = ' ';

	while(isspace(c)) s>>c;
	// The vectors can be formatted either as x y or | x y |
	if(c=='|')
	{
		s>>v_tmp[VX]>>v_tmp[VY];

		while(s>>c&&isspace(c));

		if(c!='|');//s.set(_bad); //???
	}
	else
	{
		s.putback(c);
		s>>v_tmp[VX]>>v_tmp[VY];
	}
	if(s) v = v_tmp; return s;
}
ostream &operator<<(ostream &s, vec3& v)
{
	return s << "| " << v[0] << ' ' << v[1] << ' ' << v[2] << " |"; 
}

istream &operator>>(istream &s, vec3& v) 
{
	vec3 v_tmp;
	char c = ' ';

	while(isspace(c)) s>>c;

	// The vectors can be formatted either as x y z or | x y z |
	if(c=='|')
	{
		s>>v_tmp[VX]>>v_tmp[VY]>>v_tmp[VZ];
		while(s>>c&&isspace(c));
		//if(c!='|'); //s.set(_bad); //???
	}
	else
	{
		s.putback(c);
		s>>v_tmp[VX]>>v_tmp[VY]>>v_tmp[VZ];
	}
	if(s) v = v_tmp; return s;
}
ostream &operator<<(ostream &s, vec4& v)
{
	return s << "| " << v[0] << ' ' << v[1] << ' ' << v[2] << ' ' << v.n[VW] << " |"; 
}
istream &operator>>(istream &s, vec4& v) 
{
	vec4 v_tmp; char c = ' ';

	while(isspace(c)) s>>c;

	// The vectors can be formatted either as x y z w or | x y z w |
	if(c=='|')
	{
		s>>v_tmp[VX]>>v_tmp[VY]>>v_tmp[VZ]>>v_tmp[VW];
		while(s>>c&&isspace(c));
		//if(c!='|'); //s.set(_bad); //???
	}
	else
	{
		s.putback(c);
		s>>v_tmp[VX]>>v_tmp[VY]>>v_tmp[VZ]>>v_tmp[VW];
	}
	if(s) v = v_tmp; return s;
}
ostream &operator<<(ostream &s, mat3& m)
{
	return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ]; 
}

istream &operator>>(istream &s, mat3& m) 
{
	mat3 m_tmp; s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ];

	if(s) m = m_tmp; return s;
}
ostream &operator<<(ostream &s, mat4& m)
{
	return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ] << '\n' << m.v[VW];
}

istream &operator>>(istream &s, mat4& m)
{
	mat4 m_tmp; s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ] >> m_tmp[VW];

	if(s) m = m_tmp; return s;
}*/

//---.
}//<-'