Uni/CGII
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Least squares function

Browse source
This commit is contained in:
hodasemi 2018-06-20 11:18:17 +02:00
parent a3ef9b44fc
commit f931a943ea
3 changed files with 832 additions and 564 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

80
CGII/src/AtomicTransform.cpp
View file

@ -102,30 +102,72 @@ float angle(const Vec3 &v1, const Vec3 &v2)
return std::acos(d / (v1_length * v2_length));
}
/*
fn angle(v1
: cgmath::Vector2<f32>)
->f32
float a(float rcos, float x)
{
let dot = v1.x * DEFAULT_DIRECTION.x + v1.y * DEFAULT_DIRECTION.y;
return rcos + x * x * (1 - rcos);
}
let v1_len = ((v1.x * v1.x) + (v1.y * v1.y)).sqrt();
let v2_len = ((DEFAULT_DIRECTION.x * DEFAULT_DIRECTION.x) + (DEFAULT_DIRECTION.y * DEFAULT_DIRECTION.y))
.sqrt();
let angle = (dot / (v1_len * v2_len)).acos();
if
v1.x < 0.0
float b(float rcos, float rsin, float x, float y, float z)
{
-angle
return -z * rsin + y * x * (1 - rcos);
}
else
float c(float rcos, float rsin, float x, float y, float z)
{
angle
return y * rsin + z * x * (1 - rcos);
}
float d(float rcos, float rsin, float x, float y, float z)
{
return z * rsin + x * y * (1 - rcos);
}
float e(float rcos, float y)
{
return rcos + y * y * (1 - rcos);
}
float f(float rcos, float rsin, float x, float y, float z)
{
return -x * rsin + z * y * (1 - rcos);
}
float g(float rcos, float rsin, float x, float y, float z)
{
return -y * rsin + x * z * (1 - rcos);
}
float h(float rcos, float rsin, float x, float y, float z)
{
return x * rsin + y * z * (1 - rcos);
}
float j(float rcos, float z)
{
rcos + z *z *(1 - rcos);
}
float AtomicRotationTransform::least_squares(const Vec3 &local_vector, const Vec3 &target, float r)
{
float angler = r * PI / 180.0f;
float rcos = cos(angler);
float rsin = sin(angler);
float x = axis.x();
float y = axis.y();
float z = axis.z();
float xs = a(rcos, x) * local_vector.x() + b(rcos, rsin, x, y, z) * local_vector.y() + c(rcos, rsin, x, y, z) * local_vector.z();
float ys = d(rcos, rsin, x, y, z) * local_vector.x() + e(rcos, y) * local_vector.y() + f(rcos, rsin, x, y, z) * local_vector.z();
float zs = g(rcos, rsin, x, y, z) * local_vector.x() + h(rcos, rsin, x, y, z) * local_vector.y() + j(rcos, z);
float x_diff = xs - target.x();
float y_diff = ys - target.y();
float z_diff = zs - target.z();
float squares = x_diff * x_diff + y_diff * y_diff + z_diff * z_diff;
}
*/
void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
{
@ -133,7 +175,9 @@ void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec
// optimize this that: target = this->calculate_matrix() * local_vector;
double result = angle(local_vector, target);
float first_guess = angle(local_vector, target);
double result = least_squares(local_vector, target, first_guess);
if (inverse)
result = -result;

3
CGII/src/AtomicTransform.h
View file

@ -90,6 +90,9 @@ public:
virtual void drawIndicator(float size);
virtual void drawActualIndicator(float size);
private:
virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
protected:
Vec3 axis;
};

523
framework/include/cgv/math/transformations.h
View file

@ -2,18 +2,25 @@
#include "vec.h"
#include "mat.h"
namespace cgv {
namespace math {
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;
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;
}
@ -22,25 +29,33 @@ 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;
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(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, 0) = 0;
m(2, 1) = (T)sin((double)angler);
m(2, 2) = (T)cos((double)angler);
return m;
@ -52,13 +67,16 @@ 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, 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(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;
}
@ -69,10 +87,14 @@ 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(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;
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;
}
@ -102,21 +124,17 @@ const mat<T> rotate_33(const T &dirx, const T &diry, const T&dirz,
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)
@ -136,18 +154,14 @@ const mat<T> rotate_euler_33(const T& yaw, const T& pitch,const T& roll)
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;
}
@ -160,17 +174,14 @@ const mat<T> star(const vec<T>& v)
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;
}
@ -200,28 +211,38 @@ const mat<T> rotate_rodrigues_33(const T&rx, const T& ry, const T&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;
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;
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;
}
@ -230,14 +251,19 @@ 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;
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,
@ -245,9 +271,15 @@ const mat<T> shear_33(const T &syx, const T &szx,
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;
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;
}
@ -257,10 +289,22 @@ 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;
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;
}
@ -269,10 +313,22 @@ 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;
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;
}
@ -281,10 +337,22 @@ 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;
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;
}
@ -300,27 +368,47 @@ 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;
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;
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;
}
@ -330,12 +418,22 @@ 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;
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;
}
@ -346,15 +444,24 @@ 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(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;
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)
@ -368,12 +475,10 @@ const mat<T> rotate_44(const T &dirx, const T &diry, const T&dirz,
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);
@ -388,7 +493,6 @@ const mat<T> rotate_44(const T &dirx, const T &diry, const T&dirz,
return m;
}
template <typename T>
const mat<T> rotate_33(const cgv::math::vec<T> &dir, const T &angle)
{
@ -409,7 +513,6 @@ const mat<T> rotate_44(const cgv::math::vec<T>& dir, const T& angle)
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)
@ -445,29 +548,51 @@ const mat<T> rotate_euler_44(const T& yaw, const T& pitch,const T& roll)
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;
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;
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;
}
@ -476,14 +601,25 @@ 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;
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,
@ -491,17 +627,25 @@ const mat<T> shear_44(const T &syx, const T &szx,
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;
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,
@ -510,11 +654,22 @@ const mat<T> perspective_44(const T& fovy, const T&aspect, const T& znear,
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(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;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = -1;
m(3, 3) = 0;
return m;
}
@ -526,14 +681,25 @@ mat<T> viewport_44(const T& xoff, const T yoff, const T& width,
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;
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,
@ -550,10 +716,22 @@ const mat<T> look_at_44(const T &eyex, const T &eyey, const T &eyez,
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(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;
}
@ -562,17 +740,28 @@ 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(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;
}
@ -590,10 +779,22 @@ const mat<T> frustrum_44(const T& left, const T&right,
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;
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;
}
@ -610,14 +811,25 @@ const mat<T> ortho_44(const T& left, const T&right,
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;
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,
@ -626,7 +838,6 @@ const mat<T> ortho2d_44(const T& left, const T&right,
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)
@ -642,10 +853,22 @@ const mat<T> pick_44(const T& x,const T& y,const T& width, const T& height,int v
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;
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;
}
@ -676,14 +899,15 @@ void decompose_R_2_RxRyRz(const mat<T>& R33,T &angle_x, T& angle_y, T& angle_z)
}
/* 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;
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)
{
@ -700,10 +924,7 @@ const mat<T> extract_frustrum_planes(const mat<T>& modelviewprojection)
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