Least squares function

CGII/src/AtomicTransform.cpp

@@ -102,30 +102,72 @@ float angle(const Vec3 &v1, const Vec3 &v2)
 	return std::acos(d / (v1_length * v2_length));
 }
 
-/*
+float a(float rcos, float x)
-fn angle(v1
-		 : cgmath::Vector2<f32>)
-	->f32
 {
-	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();
+float b(float rcos, float rsin, float x, float y, float z)
-	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
 {
-			-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;

CGII/src/AtomicTransform.h

@@ -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;
 };

framework/include/cgv/math/transformations.h

@@ -2,18 +2,25 @@
 #include "vec.h"
 #include "mat.h"
 
-namespace cgv {
+namespace cgv
-namespace math {
+{
-
+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(0, 0) = sx;
-	m(1,0)=0; m(1,1)=sy; m(1,2)= 0; 
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= sz; 
+	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(0, 0) = s;
-	m(1,0)=0; m(1,1)=s; m(1,2)= 0; 
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= s; 
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= (T)cos((double)angler); 
+	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(1, 0) = 0;
-	m(2,0)=-(T)sin((double)angler); m(2,1)= 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(1, 1) = (T)cos((double)angler);
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; 
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= shy;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; 
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= 0;
+	m(0, 1) = shx;
-	m(2,0)=0; m(2,1)= shz; m(2,2)= 1; 
+	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(0, 0) = 1;
-	m(1,0)=shy; m(1,1)= 1; m(1,2)= 0; 
+	m(0, 1) = 0;
-	m(2,0)=shz; m(2,1)= 0; m(2,2)= 1;
+	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(0, 0) = 1;
-	m(1,0)=sxy; m(1,1)= 1; m(1,2)= szy; 
+	m(0, 1) = syx;
-	m(2,0)=sxz; m(2,1)= syz; m(2,2)= 1; 
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= y;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= z;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= v(1);
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= v(2);
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = sx;
-	m(1,0)=0; m(1,1)=sy; m(1,2)= 0; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= sz; m(2,3)= 0;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = s;
-	m(1,0)=0; m(1,1)=s; m(1,2)= 0; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= s; m(2,3)= 0;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= (T)cos((double)angler); 
+	m(0, 1) = 0;
-	m(1,2)= -(T)sin((double)angler); m(1,3)= 0;
+	m(0, 2) = 0;
-	m(2,0)=0; m(2,1)= (T)sin((double)angler); 
+	m(0, 3) = 0;
-	m(2,2)= (T)cos((double)angler); m(2,3)= 0;
+	m(1, 0) = 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) = -(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, 0) = (T)cos((double)angler);
-	m(0,2)= (T)sin((double)angler); m(0,3)= 0;
+	m(0, 1) = 0;
-	m(1,0)=0; m(1,1)=1; m(1,2)= 0; m(1,3)= 0;
+	m(0, 2) = (T)sin((double)angler);
-	m(2,0)=-(T)sin((double)angler); m(2,1)= 0; 
+	m(0, 3) = 0;
-	m(2,2)= (T)cos((double)angler); m(2,3)= 0;
+	m(1, 0) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(1, 1) = (T)cos((double)angler);
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
+	m(1, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= shy; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
+	m(0, 2) = shx;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= 0;
+	m(0, 1) = shx;
-	m(2,0)=0; m(2,1)= shz; m(2,2)= 1; m(2,3)= 0;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=shy; m(1,1)= 1; m(1,2)= 0; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=shz; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = 1;
-	m(1,0)=sxy; m(1,1)= 1; m(1,2)= szy; m(1,3)= 0;
+	m(0, 1) = syx;
-	m(2,0)=sxz; m(2,1)= syz; m(2,2)= 1; m(2,3)= 0;
+	m(0, 2) = szx;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = f / aspect;
-	m(1,0)=0; m(1,1)= f; m(1,2)= 0; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= (zfar+znear)/(znear-zfar); 
+	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(0, 0) = a;
-	m(1,0)=0; m(1,1)= b; m(1,2)= 0; m(1,3)= yoff+(T)0.5;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= (T)0.5; m(2,3)=(T)0.5 ;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = s(0);
-	m(1,0)=u(0); m(1,1)=u(1) ; m(1,2)= u(2); m(1,3)= 0;
+	m(0, 1) = s(1);
-	m(2,0)=-f(0); m(2,1)= -f(1); m(2,2)= -f(2); m(2,3)= 0;
+	m(0, 2) = s(2);
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = s(0);
-	m(1,0)=u(0); m(1,1)=u(1) ; m(1,2)= u(2); m(1,3)= 0;
+	m(0, 1) = s(1);
-	m(2,0)=-f(0); m(2,1)= -f(1); m(2,2)= -f(2); m(2,3)= 0;
+	m(0, 2) = s(2);
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = e;
-	m(1,0)=0; m(1,1)= f; m(1,2)= B; m(1,3)= 0;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= C; m(2,3)= D;
+	m(0, 2) = A;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= -1; m(3,3)= 0;
+	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(0, 0) = a;
-	m(1,0)=0; m(1,1)= b; m(1,2)= 0; m(1,3)= -ty;
+	m(0, 1) = 0;
-	m(2,0)=0; m(2,1)= 0; m(2,2)= c; m(2,3)= -tz;
+	m(0, 2) = 0;
-	m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
+	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(0, 0) = sx;
-	m(1,0) = 0; m(1,1) = sy; m(1,2) = 0; m(1,3) = ty;
+	m(0, 1) = 0;
-	m(2,0) = 0; m(2,1) = 0; m(2,2) = 1; m(2,3) = 0;
+	m(0, 2) = 0;
-	m(3,0) = 0; m(3,1) = 0; m(3,2) = 0; m(3,3) = 1;
+	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_x < 0)
-    if (angle_y < 0) angle_y += (T)360;
+		angle_x += (T)360;
-    if (angle_z < 0) angle_z += (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
-
-