709 lines
18 KiB
C++
709 lines
18 KiB
C++
#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 ­)
|
|
{
|
|
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­, 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 ­)
|
|
{
|
|
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­, 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;
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|