CGII/framework/include/cgv/math/transformations.h

#pragma once
#include "vec.h"
#include "mat.h"

namespace cgv
{
namespace math
{

///creates a 3x3 scale matrix
template <typename T>
const mat<T> scale_33(const T &sx, const T &sy, const T &sz)
{
	mat<T> m(3, 3);
	m(0, 0) = sx;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(1, 0) = 0;
	m(1, 1) = sy;
	m(1, 2) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = sz;
	return m;
}

///creates a 3x3 uniform scale matrix
template <typename T>
const mat<T> scale_33(const T &s)
{
	mat<T> m(3, 3);
	m(0, 0) = s;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(1, 0) = 0;
	m(1, 1) = s;
	m(1, 2) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = s;

	return m;
}

///creates a 3x3 rotation matrix around the x axis, the angle is in degree
template <typename T>
const mat<T> rotatex_33(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(3, 3);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(1, 0) = 0;
	m(1, 1) = (T)cos((double)angler);
	m(1, 2) = -(T)sin((double)angler);
	m(2, 0) = 0;
	m(2, 1) = (T)sin((double)angler);
	m(2, 2) = (T)cos((double)angler);

	return m;
}

///creates a 3x3 rotation matrix around the y axis, the angle is in degree
template <typename T>
const mat<T> rotatey_33(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(3, 3);
	m(0, 0) = (T)cos((double)angler);
	m(0, 1) = 0;
	m(0, 2) = (T)sin((double)angler);
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(2, 0) = -(T)sin((double)angler);
	m(2, 1) = 0;
	m(2, 2) = (T)cos((double)angler);

	return m;
}

///creates a 3x3 rotation matrix around the z axis, the angle is in degree
template <typename T>
const mat<T> rotatez_33(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(3, 3);
	m(0, 0) = (T)cos((double)angler);
	m(0, 1) = -(T)sin((double)angler);
	m(0, 2) = 0;
	m(1, 0) = (T)sin((double)angler);
	m(1, 1) = (T)cos((double)angler);
	m(1, 2) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	return m;
}

///creates a 2x2 rotation matrix around the z axis, the angle is in degree
template <typename T>
const mat<T> rotate_22(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(2, 2);
	m(0, 0) = (T)cos((double)angler);
	m(0, 1) = -(T)sin((double)angler);
	m(1, 0) = (T)sin((double)angler);
	m(1, 1) = (T)cos((double)angler);
	return m;
}

template <typename T>
const mat<T> rotate_33(const T &dirx, const T &diry, const T &dirz,
					   const T &angle)
{

	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(3, 3);
	T rcos = cos(angler);
	T rsin = sin(angler);
	m(0, 0) = rcos + dirx * dirx * ((T)1 - rcos);
	m(0, 1) = -dirz * rsin + diry * dirx * ((T)1 - rcos);
	m(0, 2) = diry * rsin + dirz * dirx * ((T)1 - rcos);

	m(1, 0) = dirz * rsin + dirx * diry * ((T)1 - rcos);
	m(1, 1) = rcos + diry * diry * ((T)1 - rcos);
	m(1, 2) = -dirx * rsin + dirz * diry * ((T)1 - rcos);

	m(2, 0) = -diry * rsin + dirx * dirz * ((T)1 - rcos);
	m(2, 1) = dirx * rsin + diry * dirz * ((T)1 - rcos);
	m(2, 2) = rcos + dirz * dirz * ((T)1 - rcos);

	return m;
}

///creates a 3x3 euler rotation matrix from yaw, pitch and roll given in degree
template <typename T>
const mat<T> rotate_euler_33(const T &yaw, const T &pitch, const T &roll)
{
	T yawd = (T)(yaw * 3.14159 / 180.0);
	T pitchd = (T)(pitch * 3.14159 / 180.0);
	T rolld = (T)(roll * 3.14159 / 180.0);
	T cosy = (T)cos(yawd);
	T siny = (T)sin(yawd);
	T cosp = (T)cos(pitchd);
	T sinp = (T)sin(pitchd);
	T cosr = (T)cos(rolld);
	T sinr = (T)sin(rolld);
	mat<T> m(3, 3);

	m(0, 0) = cosr * cosy - sinr * sinp * siny;
	m(0, 1) = -sinr * cosp;
	m(0, 2) = cosr * siny + sinr * sinp * cosy;

	m(1, 0) = sinr * cosy + cosr * sinp * siny;
	m(1, 1) = cosr * cosp;
	m(1, 2) = sinr * siny - cosr * sinp * cosy;

	m(2, 0) = -cosp * siny;
	m(2, 1) = sinp;
	m(2, 2) = cosp * cosy;

	return m;
}

template <typename T>
const mat<T> star(const vec<T> &v)
{
	mat<T> m(3, 3);

	m(0, 0) = 0;
	m(0, 1) = -v(2);
	m(0, 2) = v(1);

	m(1, 0) = v(2);
	m(1, 1) = 0;
	m(1, 2) = -v(0);

	m(2, 0) = -v(1);
	m(2, 1) = v(0);
	m(2, 2) = 0;

	return m;
}

///creates a 3x3 rotation matrix from a rodrigues vector
template <typename T>
const mat<T> rotate_rodrigues_33(const vec<T> &r)
{
	T theta = length(r);
	if (theta == 0)
		return identity<T>(3);

	vec<T> rn = r / theta;
	T cos_theta = cos(theta);
	T sin_theta = sin(theta);

	return cos_theta * identity<T>(3) + ((T)1 - cos_theta) * dyad(rn, rn) + sin_theta * star(rn);
}

template <typename T>
const mat<T> rotate_rodrigues_33(const T &rx, const T &ry, const T &rz)
{
	vec<T> r(3);
	r(0) = rx;
	r(1) = ry;
	r(2) = rz;

	return rotate_rodrigues_33(r);
}

///creates a homogen 3x3 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxy_33(const T &shx, const T &shy)
{
	mat<T> m(3, 3);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = shx;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = shy;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;

	return m;
}

///creates a homogen 3x3 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxz_33(const T &shx, const T &shz)
{
	mat<T> m(3, 3);
	m(0, 0) = 1;
	m(0, 1) = shx;
	m(0, 2) = 0;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(2, 0) = 0;
	m(2, 1) = shz;
	m(2, 2) = 1;
	return m;
}

///creates a homogen 3x3 shear matrix with given shears shy in y direction, and shz in z direction
template <typename T>
const mat<T> shearyz_33(const T &shy, const T &shz)
{
	mat<T> m(3, 3);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(1, 0) = shy;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(2, 0) = shz;
	m(2, 1) = 0;
	m(2, 2) = 1;

	return m;
}

///creates a homogen 3x3 shear matrix
template <typename T>
const mat<T> shear_33(const T &syx, const T &szx,
					  const T &sxy, const T &szy,
					  const T &sxz, const T &syz)
{
	mat<T> m(3, 3);
	m(0, 0) = 1;
	m(0, 1) = syx;
	m(0, 2) = szx;
	m(1, 0) = sxy;
	m(1, 1) = 1;
	m(1, 2) = szy;
	m(2, 0) = sxz;
	m(2, 1) = syz;
	m(2, 2) = 1;

	return m;
}

///creates a 4x4 translation matrix
template <typename T>
const mat<T> translate_44(const T &x, const T &y, const T &z)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = x;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(1, 3) = y;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = z;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a 4x4 translation matrix
template <typename T>
const mat<T> translate_44(const vec<T> &v)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = v(0);
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(1, 3) = v(1);
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = v(2);
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a 4x4 scale matrix
template <typename T>
const mat<T> scale_44(const T &sx, const T &sy, const T &sz)
{
	mat<T> m(4, 4);
	m(0, 0) = sx;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = sy;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = sz;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

template <typename T>
const mat<T> scale_44(const cgv::math::vec<T> s)
{
	assert(s.size() == 3);
	return scale_44(s(0), s(1), s(2));
}

///creates a 4x4 uniform scale matrix
template <typename T>
const mat<T> scale_44(const T &s)
{
	mat<T> m(4, 4);
	m(0, 0) = s;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = s;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = s;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a 4x4 rotation matrix around the x axis, the angle is in degree
template <typename T>
const mat<T> rotatex_44(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = (T)cos((double)angler);
	m(1, 2) = -(T)sin((double)angler);
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = (T)sin((double)angler);
	m(2, 2) = (T)cos((double)angler);
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a 4x4 rotation matrix around the y axis, the angle is in degree
template <typename T>
const mat<T> rotatey_44(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(4, 4);
	m(0, 0) = (T)cos((double)angler);
	m(0, 1) = 0;
	m(0, 2) = (T)sin((double)angler);
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = -(T)sin((double)angler);
	m(2, 1) = 0;
	m(2, 2) = (T)cos((double)angler);
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a 4x4 rotation matrix around the z axis, the angle is in degree
template <typename T>
const mat<T> rotatez_44(const T &angle)
{
	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(4, 4);
	m(0, 0) = (T)cos((double)angler);
	m(0, 1) = -(T)sin((double)angler);
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = (T)sin((double)angler);
	m(1, 1) = (T)cos((double)angler);
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

template <typename T>
const mat<T> rotate_44(const T &dirx, const T &diry, const T &dirz,
					   const T &angle)
{

	T angler = angle * (T)3.14159 / (T)180.0;
	mat<T> m(4, 4);
	T rcos = cos(angler);
	T rsin = sin(angler);
	m(0, 0) = rcos + dirx * dirx * ((T)1 - rcos);
	m(0, 1) = -dirz * rsin + diry * dirx * ((T)1 - rcos);
	m(0, 2) = diry * rsin + dirz * dirx * ((T)1 - rcos);

	m(1, 0) = dirz * rsin + dirx * diry * ((T)1 - rcos);
	m(1, 1) = rcos + diry * diry * ((T)1 - rcos);
	m(1, 2) = -dirx * rsin + dirz * diry * ((T)1 - rcos);

	m(2, 0) = -diry * rsin + dirx * dirz * ((T)1 - rcos);
	m(2, 1) = dirx * rsin + diry * dirz * ((T)1 - rcos);
	m(2, 2) = rcos + dirz * dirz * ((T)1 - rcos);

	m(3, 0) = (T)0;
	m(3, 1) = (T)0;
	m(3, 2) = (T)0;
	m(0, 3) = (T)0;
	m(1, 3) = (T)0;
	m(2, 3) = (T)0;
	m(3, 3) = (T)1;
	return m;
}

template <typename T>
const mat<T> rotate_33(const cgv::math::vec<T> &dir, const T &angle)
{

	assert(dir.size() == 3);
	cgv::math::vec<T> vdir = dir;
	vdir.normalize();
	return rotate_33<T>(vdir(0), vdir(1), vdir(2), angle);
}

template <typename T>
const mat<T> rotate_44(const cgv::math::vec<T> &dir, const T &angle)
{

	assert(dir.size() == 3);
	cgv::math::vec<T> vdir = dir;
	vdir.normalize();
	return rotate_44<T>(vdir(0), vdir(1), vdir(2), angle);
}

///creates a 4x4 euler rotation matrix from yaw, pitch and roll given in degree
template <typename T>
const mat<T> rotate_euler_44(const T &yaw, const T &pitch, const T &roll)
{
	T yawd = yaw * 3.14159 / 180.0;
	T pitchd = pitch * 3.14159 / 180.0;
	T rolld = roll * 3.14159 / 180.0;
	T cosy = (T)cos(yawd);
	T siny = (T)sin(yawd);
	T cosp = (T)cos(pitchd);
	T sinp = (T)sin(pitchd);
	T cosr = (T)cos(rolld);
	T sinr = (T)sin(rolld);
	mat<T> m(4, 4);

	m(0, 0) = cosr * cosy - sinr * sinp * siny;
	m(0, 1) = -sinr * cosp;
	m(0, 2) = cosr * siny + sinr * sinp * cosy;
	m(0, 3) = 0;

	m(1, 0) = sinr * cosy + cosr * sinp * siny;
	m(1, 1) = cosr * cosp;
	m(1, 2) = sinr * siny - cosr * sinp * cosy;
	m(1, 3) = 0;

	m(2, 0) = -cosp * siny;
	m(2, 1) = sinp;
	m(2, 2) = cosp * cosy;
	m(2, 3) = 0;

	m(3, 0) = m(3, 1) = m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a homogen 4x4 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxy_44(const T &shx, const T &shy)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = shx;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = shy;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a homogen 4x4 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxz_44(const T &shx, const T &shz)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = shx;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = shz;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a homogen 4x4 shear matrix with given shears shy in y direction, and shz in z direction
template <typename T>
const mat<T> shearyz_44(const T &shy, const T &shz)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = shy;
	m(1, 1) = 1;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = shz;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a homogen 4x4 shear matrix
template <typename T>
const mat<T> shear_44(const T &syx, const T &szx,
					  const T &sxy, const T &szy,
					  const T &sxz, const T &syz)
{
	mat<T> m(4, 4);
	m(0, 0) = 1;
	m(0, 1) = syx;
	m(0, 2) = szx;
	m(0, 3) = 0;
	m(1, 0) = sxy;
	m(1, 1) = 1;
	m(1, 2) = szy;
	m(1, 3) = 0;
	m(2, 0) = sxz;
	m(2, 1) = syz;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a perspective transformation matrix in the same way as gluPerspective does
template <typename T>
const mat<T> perspective_44(const T &fovy, const T &aspect, const T &znear,
							const T &zfar)
{
	T fovyr = (T)(fovy * 3.14159 / 180.0);
	T f = (T)(cos(fovyr / 2.0f) / sin(fovyr / 2.0f));
	mat<T> m(4, 4);
	m(0, 0) = f / aspect;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = f;
	m(1, 2) = 0;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = (zfar + znear) / (znear - zfar);
	m(2, 3) = (2 * zfar * znear) / (znear - zfar);
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = -1;
	m(3, 3) = 0;
	return m;
}

///creates a viewport transformation matrix
template <typename T>
mat<T> viewport_44(const T &xoff, const T yoff, const T &width,
				   const T &height)
{
	mat<T> m(4, 4);
	T a = width / (T)2.0;
	T b = height / (T)2.0;
	m(0, 0) = a;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = xoff + (T)0.5;
	m(1, 0) = 0;
	m(1, 1) = b;
	m(1, 2) = 0;
	m(1, 3) = yoff + (T)0.5;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = (T)0.5;
	m(2, 3) = (T)0.5;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates a look at transformation matrix in the same way as gluLookAt does
template <typename T>
const mat<T> look_at_44(const T &eyex, const T &eyey, const T &eyez,
						const T &centerx, const T &centery, const T &centerz,
						const T &upx, const T &upy, const T &upz)
{
	vec<T> center(centerx, centery, centerz);
	vec<T> eye(eyex, eyey, eyez);
	vec<T> up(upx, upy, upz);

	vec<T> f = normalize(center - eye);
	up = normalize(up);
	vec<T> s = normalize(cross(f, up));
	vec<T> u = normalize(cross(s, f));

	mat<T> m(4, 4);
	m(0, 0) = s(0);
	m(0, 1) = s(1);
	m(0, 2) = s(2);
	m(0, 3) = 0;
	m(1, 0) = u(0);
	m(1, 1) = u(1);
	m(1, 2) = u(2);
	m(1, 3) = 0;
	m(2, 0) = -f(0);
	m(2, 1) = -f(1);
	m(2, 2) = -f(2);
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	m = m * translate_44(-eyex, -eyey, -eyez);
	return m;
}

template <typename T>
const mat<T> look_at_44(vec<T> eye, vec<T> center, vec<T> up)
{

	vec<T> f = normalize(center - eye);
	up = normalize(up);
	vec<T> s = normalize(cross(f, up));
	vec<T> u = normalize(cross(s, f));

	mat<T> m(4, 4);
	m(0, 0) = s(0);
	m(0, 1) = s(1);
	m(0, 2) = s(2);
	m(0, 3) = 0;
	m(1, 0) = u(0);
	m(1, 1) = u(1);
	m(1, 2) = u(2);
	m(1, 3) = 0;
	m(2, 0) = -f(0);
	m(2, 1) = -f(1);
	m(2, 2) = -f(2);
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	m = m * translate_44(-eye);
	return m;
}

///creates a frustrum projection matrix in the same way as glFrustum does
template <typename T>
const mat<T> frustrum_44(const T &left, const T &right,
						 const T &bottom, const T &top,
						 const T &znear, const T &zfar)
{
	T e = 2 * znear / (right - left);
	T f = 2 * znear / (top - bottom);
	T A = (right + left) / (right - left);
	T B = (top + bottom) / (top - bottom);
	T C = -(zfar + znear) / (zfar - znear);
	T D = -(2 * zfar * znear) / (zfar - znear);
	mat<T> m(4, 4);
	m(0, 0) = e;
	m(0, 1) = 0;
	m(0, 2) = A;
	m(0, 3) = 0;
	m(1, 0) = 0;
	m(1, 1) = f;
	m(1, 2) = B;
	m(1, 3) = 0;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = C;
	m(2, 3) = D;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = -1;
	m(3, 3) = 0;
	return m;
}

///creates an orthographic projection matrix in the same way as glOrtho does
template <typename T>
const mat<T> ortho_44(const T &left, const T &right,
					  const T &bottom, const T &top,
					  const T &znear, const T &zfar)
{
	T a = 2 / (right - left);
	T b = 2 / (top - bottom);
	T c = -2 / (zfar - znear);
	T tx = (right + left) / (right - left);
	T ty = (top + bottom) / (top - bottom);
	T tz = (zfar + znear) / (zfar - znear);
	mat<T> m(4, 4);
	m(0, 0) = a;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = -tx;
	m(1, 0) = 0;
	m(1, 1) = b;
	m(1, 2) = 0;
	m(1, 3) = -ty;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = c;
	m(2, 3) = -tz;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m;
}

///creates an orthographic projection matrix in the same way as glOrtho2d does
template <typename T>
const mat<T> ortho2d_44(const T &left, const T &right,
						const T &bottom, const T &top)
{
	return ortho_44<T>(left, right, bottom, top, (T)-1.0, (T)1.0);
}

///creates a picking matrix like gluPickMatrix with pixel (0,0) in the lower left corner if flipy=false
template <typename T>
const mat<T> pick_44(const T &x, const T &y, const T &width, const T &height, int viewport[4], const mat<double> &modelviewproj, bool flipy = true)
{
	mat<T> m(4, 4);
	T sx, sy;
	T tx, ty;
	sx = viewport[2] / width;
	sy = viewport[3] / height;
	tx = (T)(viewport[2] + 2.0 * (viewport[0] - x)) / width;
	if (flipy)
		ty = (T)(viewport[3] + 2.0 * (viewport[1] - (viewport[3] - y))) / height;
	else
		ty = (T)(viewport[3] + 2.0 * (viewport[1] - y)) / height;

	m(0, 0) = sx;
	m(0, 1) = 0;
	m(0, 2) = 0;
	m(0, 3) = tx;
	m(1, 0) = 0;
	m(1, 1) = sy;
	m(1, 2) = 0;
	m(1, 3) = ty;
	m(2, 0) = 0;
	m(2, 1) = 0;
	m(2, 2) = 1;
	m(2, 3) = 0;
	m(3, 0) = 0;
	m(3, 1) = 0;
	m(3, 2) = 0;
	m(3, 3) = 1;
	return m * modelviewproj;
}

//extract rotation angles from combined rotation matrix
template <typename T>
void decompose_R_2_RxRyRz(const mat<T> &R33, T &angle_x, T &angle_y, T &angle_z)
{

	angle_y = asin(R33(0, 2)); /* Calculate Y-axis angle */
	T C = cos(angle_y);
	T trx, atry;
	angle_y *= (T)(180 / 3.14159);
	if (std::abs(C) > (T)0.005) /* Gimbal lock? */
	{
		trx = R33(2, 2) / C; /* No, so get X-axis angle */
		atry = -R33(1, 2) / C;
		angle_x = atan2(atry, trx) * (T)(180 / 3.14159);
		trx = R33(0, 0) / C; /* Get Z-axis angle */
		atry = -R33(0, 1) / C;
		angle_z = atan2(atry, trx) * (T)(180 / 3.14159);
	}
	else /* Gimbal lock has occurred */
	{
		angle_x = 0;	 /* Set X-axis angle to zero */
		trx = R33(1, 1); /* And calculate Z-axis angle */
		atry = R33(1, 0);
		angle_z = atan2(atry, trx) * (T)(180 / 3.14159);
	}

	/* return only positive angles in [0,360] */
	if (angle_x < 0)
		angle_x += (T)360;
	if (angle_y < 0)
		angle_y += (T)360;
	if (angle_z < 0)
		angle_z += (T)360;
	//std::cout << angle_x << " "<< angle_y << " " <<angle_z <<std::endl;
}

template <typename T>
const mat<T> extract_frustrum_planes(const mat<T> &modelviewprojection)
{
	mat<T> frustrum_planes(4, 6);

	frustrum_planes.set_col(0, modelviewprojection.row(3) - modelviewprojection.row(0)); //right
	frustrum_planes.set_col(1, modelviewprojection.row(3) + modelviewprojection.row(0)); //left
	frustrum_planes.set_col(2, modelviewprojection.row(3) - modelviewprojection.row(1)); //top
	frustrum_planes.set_col(3, modelviewprojection.row(3) + modelviewprojection.row(1)); //bottom
	frustrum_planes.set_col(4, modelviewprojection.row(3) - modelviewprojection.row(2)); //far
	frustrum_planes.set_col(5, modelviewprojection.row(3) + modelviewprojection.row(2)); //near

	// Normalize all planes
	for (unsigned i = 0; i < 6; i++)
		frustrum_planes.set_col(i, frustrum_planes.col(i) / length(frustrum_planes.col(i).sub_vec(0, 3)));
	return frustrum_planes;
}

} // namespace math
} // namespace cgv