diff --git a/CG2_SS18_04_IK.pdf b/CG2_SS18_04_IK.pdf
new file mode 100644
index 0000000..63ffa1a
Binary files /dev/null and b/CG2_SS18_04_IK.pdf differ
diff --git a/CGII/build/cmake/CMakeLists.txt b/CGII/build/cmake/CMakeLists.txt
index e90fb69..1144f37 100644
--- a/CGII/build/cmake/CMakeLists.txt
+++ b/CGII/build/cmake/CMakeLists.txt
@@ -33,8 +33,7 @@ cgv_add_module(CGII
 	../../src/Skeleton.cpp
 	../../src/SkeletonViewer.cpp
 	../../src/SkinnedMeshViewer.cpp
-	../../src/main.cpp
-	../../src/debughelpers.cpp)
+	../../src/main.cpp)
 
 # Set include directories
 include_directories(
diff --git a/CGII/src/AtomicTransform.cpp b/CGII/src/AtomicTransform.cpp
index 9cd619b..3115041 100644
--- a/CGII/src/AtomicTransform.cpp
+++ b/CGII/src/AtomicTransform.cpp
@@ -92,19 +92,92 @@ T clamp(T &v, T &c1, T &c2)
 	return v;
 }
 
+float angle(const Vec3 &v1, const Vec3 &v2)
+{
+	float d = dot(v1, v2);
+
+	float v1_length = length(v1);
+	float v2_length = length(v2);
+
+	return std::acos(d / (v1_length * v2_length));
+}
+
+float a(float rcos, float x)
+{
+	return rcos + x * x * (1 - rcos);
+}
+
+float b(float rcos, float rsin, float x, float y, float z)
+{
+	return -z * rsin + y * x * (1 - rcos);
+}
+
+float c(float rcos, float rsin, float x, float y, float z)
+{
+	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)
 {
 	/*Task: Implement parameter optimization*/
 
-	// target into local_target
-	// get dofs
-	// project target into plane decided by which dofs exist
-	// scal mult local_target and local_vector
+	// optimize this that: target = this->calculate_matrix() * local_vector;
 
-	// use only degrees defined by dofs
-	// if inverse == true use -angle with set_value()
+	float first_guess = angle(local_vector, target);
 
-	double result = 0.0;
+	double result = least_squares(local_vector, target, first_guess);
 
 	if (inverse)
 		result = -result;
diff --git a/CGII/src/AtomicTransform.h b/CGII/src/AtomicTransform.h
index 332f0d0..1edbf8c 100644
--- a/CGII/src/AtomicTransform.h
+++ b/CGII/src/AtomicTransform.h
@@ -1,5 +1,5 @@
-// This source code is property of the Computer Graphics and Visualization 
-// chair of the TU Dresden. Do not distribute! 
+// This source code is property of the Computer Graphics and Visualization
+// chair of the TU Dresden. Do not distribute!
 // Copyright (C) CGV TU Dresden - All Rights Reserved
 //
 #pragma once
@@ -14,30 +14,30 @@
 
 class Transform
 {
-public:
+  public:
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) = 0;
 };
 
 class StaticTransform : public Transform
 {
-public:
-	StaticTransform(const Mat4& t) : t(t) {}
+  public:
+	StaticTransform(const Mat4 &t) : t(t) {}
 
 	Mat4 calculate_matrix() { return t; }
-	void optimize_value(const Vec3& local_vector, const Vec3& target) { }
+	void optimize_value(const Vec3 &local_vector, const Vec3 &target) {}
 
-private:
+  private:
 	Mat4 t;
 };
 
 //Represents an arbitrary affine transform with exactly one scalar parameter
 class AtomicTransform : public cgv::gui::control_provider<double>, public Transform
 {
-public:
+  public:
 	//Sets the limits of the scalar parameter
 	void set_limits(double lower, double upper);
 
@@ -45,24 +45,24 @@ public:
 	const double get_upper_limit() const;
 
 	//Sets the current scalar parameter. Ignore ud.
-	virtual void set_value(const double& value, void* ud = nullptr);
+	virtual void set_value(const double &value, void *ud = nullptr);
 
 	//Gets the current scalar parameter. Ignore ud.
-	const double get_value(void* ud = nullptr) const;
+	const double get_value(void *ud = nullptr) const;
 
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false) = 0;
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) { optimize_value(local_vector, target, false); }
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) { optimize_value(local_vector, target, false); }
 
 	//Draws an indicator that visualizes the transform, including its limits.
 	virtual void drawIndicator(float size) = 0;
 	//Draws an indicator that visualizes the current scalar parameter.
 	virtual void drawActualIndicator(float size) = 0;
 
-	std::string get_name() const;	
+	std::string get_name() const;
 
 	//Signal that is raised whenever the scalar parameter changes
 	cgv::signal::signal<double> changed_signal;
@@ -73,7 +73,7 @@ public:
 	//Get the order in which the transform is specified in the animation file.
 	int get_index_in_amc() const { return index_in_amc; }
 
-protected:
+  protected:
 	double lower_limit, upper_limit;
 	double value;
 	std::string title;
@@ -82,79 +82,82 @@ protected:
 
 class AtomicRotationTransform : public AtomicTransform
 {
-public:
+  public:
 	AtomicRotationTransform(Vec3 axis);
 	virtual Mat4 calculate_matrix();
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
 	virtual void drawIndicator(float size);
 	virtual void drawActualIndicator(float size);
 
-protected:
+  private:
+	virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
+
+  protected:
 	Vec3 axis;
 };
 
 class AtomicXRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicXRotationTransform() : AtomicRotationTransform(Vec3(1, 0, 0)) { title = "X-Rotation"; }
 };
 
 class AtomicYRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicYRotationTransform() : AtomicRotationTransform(Vec3(0, 1, 0)) { title = "Y-Rotation"; }
 };
 
 class AtomicZRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicZRotationTransform() : AtomicRotationTransform(Vec3(0, 0, 1)) { title = "Z-Rotation"; }
 };
 
 class AtomicTranslationTransform : public AtomicTransform
 {
-public:
+  public:
 	AtomicTranslationTransform(int dim);
 	virtual Mat4 calculate_matrix();
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
-	virtual void drawIndicator(float size) { };
-	virtual void drawActualIndicator(float size) { };
+	virtual void drawIndicator(float size){};
+	virtual void drawActualIndicator(float size){};
 
-private:
+  private:
 	int dim;
 };
 
 class AtomicXTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicXTranslationTransform() : AtomicTranslationTransform(0) { title = "X-Translation"; }
 };
 
 class AtomicYTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicYTranslationTransform() : AtomicTranslationTransform(1) { title = "Y-Translation"; }
 };
 
 class AtomicZTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicZTranslationTransform() : AtomicTranslationTransform(2) { title = "Z-Translation"; }
 };
 
 class InverseTransform : public Transform
 {
-public:
-	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) { }
+  public:
+	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) {}
 
 	Mat4 calculate_matrix() { return cgv::math::inv(t->calculate_matrix()); }
-	void optimize_value(const Vec3& local_vector, const Vec3& target)
+	void optimize_value(const Vec3 &local_vector, const Vec3 &target)
 	{
 		t->optimize_value(local_vector, target, true);
 	}
 
-private:
+  private:
 	std::shared_ptr<AtomicTransform> t;
 };
\ No newline at end of file
diff --git a/CGII/src/IKViewer.cpp b/CGII/src/IKViewer.cpp
index c193d2c..4141d2d 100644
--- a/CGII/src/IKViewer.cpp
+++ b/CGII/src/IKViewer.cpp
@@ -5,7 +5,6 @@
 #include "IKViewer.h"
 
 #include "math_helper.h"
-#include "debughelpers.h"
 
 #include <unordered_set>
 
@@ -13,6 +12,11 @@
 #include <cgv/math/inv.h>
 #include <cgv/math/mat.h>
 
+Vec3 into_vec3(const Vec4 &v)
+{
+	return Vec3(v.x(), v.y(), v.z());
+}
+
 IKViewer::IKViewer(DataStore *data)
 	: node("IK Viewer"), data(data), modifying(false), target_position(0, 0, 0, 1), max_iterations(20)
 {
@@ -97,6 +101,8 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 		current_base_matrix = current_base_matrix * transform->calculate_matrix();
 	}
 
+	current_base_matrix = current_base_matrix * base->get_translation_transform_current_joint_to_next();
+
 	if (!endeffector)
 		return;
 
@@ -129,6 +135,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 	//TODO: check if common ancestor is visited twice
 
+	std::shared_ptr<Transform> static_trans = std::shared_ptr<Transform>(new StaticTransform(endeffector->get_translation_transform_current_joint_to_next()));
+	kinematic_chain.emplace_front(static_trans);
+
 	while (1)
 	{
 		if (state == 0)
@@ -164,16 +173,16 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 			Bone *bone = base_tree[base_index];
 
-			for (int i = bone->dof_count() - 1; i >= 0; i--)
+			std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
+			kinematic_chain.emplace_front(inverse_static);
+
+			for (int i = 0; i < bone->dof_count(); i++)
 			{
 				Transform *tmp = new InverseTransform(bone->get_dof(i));
 				std::shared_ptr<Transform> inverse_dof = std::shared_ptr<Transform>(tmp);
 				kinematic_chain.emplace_front(inverse_dof);
 			}
 
-			std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
-			kinematic_chain.emplace_front(inverse_static);
-
 			base_index--;
 		}
 		else
@@ -183,17 +192,19 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 		}
 	}
 
+	std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(base->get_translation_transform_current_joint_to_next())));
+	kinematic_chain.emplace_front(inverse_static);
+
 	current_endeffector_matrix.identity();
+	kinematic_vector.clear();
 
 	for (auto &transform : kinematic_chain)
 	{
+		kinematic_vector.emplace_back(transform);
 		current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
 	}
 
-	target_position = current_base_matrix * current_endeffector_matrix * endeffector->get_bone_local_tip_position();
-
-	print_vec3(target_position);
-	std::cout << std::endl;
+	target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
 }
 
 void IKViewer::optimize()
@@ -204,15 +215,40 @@ void IKViewer::optimize()
 	auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
 	float distance_threshold = 0.0001f * std::max({skeleton_size.x(), skeleton_size.y(), skeleton_size.z()});
 
-	//split the current matrix in:
-	//  before_dof -> dof -> after_dof
-
 	/*Task 3.3: Implement CCD	*/
 	for (int i = 0; i < max_iterations; i++)
 	{
-		for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); i++)
+		int kc_size = kinematic_vector.size();
+
+		// reverse iterate through kinematic chain
+		for (int j = kc_size - 1; j >= 0; j--)
 		{
-			//it->optimize_value(/*local_vector*/, target_position);
+			Mat4 before_dof = current_base_matrix;
+
+			for (int k = 0; k < j; k++)
+			{
+				before_dof = before_dof * kinematic_vector[k]->calculate_matrix();
+			}
+
+			Mat4 after_dof;
+			after_dof.identity();
+
+			int after_dof_index = j + 1;
+
+			for (int k = j + 1; k < kc_size; k++)
+			{
+				after_dof = after_dof * kinematic_vector[k]->calculate_matrix();
+			}
+
+			// now we got 3 matrices
+			// (1) before dof: base_matrix * kinematic_vector[0...j-1]
+			// (2) dof: kinematic_vector[j]
+			// (3) after_dof: kinematic_vector[j+1...kc_size-1]
+
+			auto v_local = after_dof * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
+			auto v_target = inv(before_dof) * target_position;
+
+			kinematic_vector[j]->optimize_value(into_vec3(v_local), into_vec3(v_target));
 		}
 
 		current_endeffector_matrix.identity();
diff --git a/CGII/src/IKViewer.h b/CGII/src/IKViewer.h
index 1684139..3b2b699 100644
--- a/CGII/src/IKViewer.h
+++ b/CGII/src/IKViewer.h
@@ -48,6 +48,7 @@ class IKViewer : public node, public drawable, public provider, public event_han
 	unsigned int max_iterations;
 
 	std::list<std::shared_ptr<Transform>> kinematic_chain;
+	std::vector<std::shared_ptr<Transform>> kinematic_vector;
 
 	void optimize();
 
diff --git a/CGII/src/SkeletonViewer.cpp b/CGII/src/SkeletonViewer.cpp
index 057c92e..28130df 100644
--- a/CGII/src/SkeletonViewer.cpp
+++ b/CGII/src/SkeletonViewer.cpp
@@ -12,7 +12,6 @@
 #include <cgv/base/find_action.h>
 
 #include "math_helper.h"
-#include "debughelpers.h"
 
 using namespace cgv::utils;
 
diff --git a/CGII/src/debughelpers.cpp b/CGII/src/debughelpers.cpp
deleted file mode 100644
index 9662c00..0000000
--- a/CGII/src/debughelpers.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "debughelpers.h"
-
-#include <iostream>
-
-void print_vec3(const Vec3 &v)
-{
-    std::cout << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")" << std::endl;
-}
-
-void print_vec3(const Vec4 &v)
-{
-    print_vec3(into_vec3(v));
-}
-
-Vec3 into_vec3(const Vec4 &v)
-{
-    return Vec3(v.x(), v.y(), v.z());
-}
-
-Vec4 into_vec4(const Vec3 &v)
-{
-    return Vec4(v.x(), v.y(), v.z(), 1.0f);
-}
\ No newline at end of file
diff --git a/CGII/src/debughelpers.h b/CGII/src/debughelpers.h
deleted file mode 100644
index 836285e..0000000
--- a/CGII/src/debughelpers.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#pragma once
-
-#include "math_helper.h"
-
-void print_vec3(const Vec3 &v);
-
-void print_vec3(const Vec4 &v);
-
-Vec3 into_vec3(const Vec4 &v);
-
-Vec4 into_vec4(const Vec3 &v);
\ No newline at end of file
diff --git a/framework/include/cgv/math/transformations.h b/framework/include/cgv/math/transformations.h
index 0407f82..82b853d 100644
--- a/framework/include/cgv/math/transformations.h
+++ b/framework/include/cgv/math/transformations.h
@@ -2,265 +2,309 @@
 #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)
+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; 
+	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)
+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; 
+	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)
+///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;
-}
+	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);
 
-///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)
+///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 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;
-    
+	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);
 
-    m(1,0) = sinr*cosy + cosr*sinp*siny;
-    m(1,1) = cosr*cosp;
-    m(1,2) = sinr*siny - cosr*sinp*cosy;
-    
+	return m;
+}
 
-    m(2,0) = -cosp*siny;
-    m(2,1) = sinp;
-    m(2,2) = cosp*cosy;
-   
+///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;
+}
 
-   
-    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> star(const vec<T>& v)
+const mat<T> rotate_33(const T &dirx, const T &diry, const T &dirz,
+					   const T &angle)
 {
-	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);
-    
+	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(2,0) = -v(1);
-    m(2,1) = v(0);
-    m(2,2) = 0;
-   
+	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)
+template <typename T>
+const mat<T> rotate_rodrigues_33(const vec<T> &r)
 {
 	T theta = length(r);
-	if(theta == 0)
+	if (theta == 0)
 		return identity<T>(3);
 
-	vec<T> rn   = r/theta;
+	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);
+
+	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)
+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>
+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; 
-	
+	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)
+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; 
+	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)
+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;
-	
+	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>
+///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)
+					  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; 
-	
+	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)
+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;
+	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;
 }
 
@@ -268,442 +312,619 @@ const mat<T> translate_44(const T&x, const T &y, const T&z)
 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;
+	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)
+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;
+	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>
+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));
+	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)
+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;
+	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)
+///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;
+	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)
+///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;
+	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)
+///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;
+	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)
+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 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(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(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;
+	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)
+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;
+	cgv::math::vec<T> vdir = dir;
 	vdir.normalize();
-	return rotate_33<T>(vdir(0),vdir(1),vdir(2), angle);
+	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)
+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;
+	cgv::math::vec<T> vdir = dir;
 	vdir.normalize();
-	return rotate_44<T>(vdir(0),vdir(1),vdir(2), 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)
+///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;
+	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(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(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(2,0) = -cosp*siny;
-    m(2,1) = sinp;
-    m(2,2) = cosp*cosy;
-    m(2,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(3,0) = m(3,1)= m(3,2) =0;
-    m(3,3) = 1;
-    return m;
+	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>
+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;
+	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)
+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;
+	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)
+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;
+	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>
+///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)
+					  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;
+	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)
+const mat<T> perspective_44(const T &fovy, const T &aspect, const T &znear,
+							const T &zfar)
 {
-	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;
+	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>
+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)
+						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> 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));
+	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);
+	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>
+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));
+	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);
+	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)
+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;
+	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)
+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);
+	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;
 
-///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;
-} 
+	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>
+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) 
+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;
+	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