Finish first task

CGII/build/cmake/CMakeLists.txt

@@ -33,7 +33,8 @@ cgv_add_module(CGII
 	../../src/Skeleton.cpp
 	../../src/SkeletonViewer.cpp
 	../../src/SkinnedMeshViewer.cpp
-	../../src/main.cpp)
+	../../src/main.cpp
+	../../src/debughelpers.cpp)
 
 # Set include directories
 include_directories(

CGII/src/IKViewer.cpp

@@ -5,6 +5,7 @@
 #include "IKViewer.h"
 
 #include "math_helper.h"
+#include "debughelpers.h"
 
 #include <unordered_set>
 
@@ -79,12 +80,26 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 		current_bone = current_bone->get_parent();
 	}
 
+	kinematic_chain.clear();
+
 	for (Bone *bone : bones)
 	{
 		// TODO: calculate and add transforms to queue
-		std::shared_ptr<Transform> transform;
+		Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_with_dofs());
+		std::shared_ptr<Transform> transform = std::shared_ptr<Transform>(tmp);
 		kinematic_chain.emplace_front(transform);
 	}
+
+	current_endeffector_matrix.identity();
+
+	for (auto &transform : kinematic_chain)
+	{
+		current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
+	}
+
+	target_position = current_endeffector_matrix * endeffector->get_bone_local_tip_position();
+
+	print_vec3(target_position);
 }
 
 void IKViewer::optimize()

CGII/src/SkeletonViewer.cpp

@@ -12,15 +12,15 @@
 #include <cgv/base/find_action.h>
 
 #include "math_helper.h"
+#include "debughelpers.h"
 
 using namespace cgv::utils;
 
 cgv::render::shader_program Mesh::prog;
 
 // The constructor of this class
-SkeletonViewer::SkeletonViewer(DataStore* data)
+SkeletonViewer::SkeletonViewer(DataStore *data)
-	: node("Skeleton Viewer"), data(data)
+	: node("Skeleton Viewer"), data(data), animation(nullptr), animationTime(0)
-	, animation(nullptr), animationTime(0)
 {
 	connect(data->skeleton_changed, this, &SkeletonViewer::skeleton_changed);
 
@@ -28,22 +28,27 @@ SkeletonViewer::SkeletonViewer(DataStore* data)
 }
 
 //draws a part of a skeleton, represented by the given root node
-void SkeletonViewer::draw_skeleton_subtree(Bone* node, const Mat4& global_to_parent_local, context& c, int level, bool arrows, bool indicators)
+void SkeletonViewer::draw_skeleton_subtree(Bone *node, const Mat4 &global_to_parent_local, context &c, int level, bool arrows, bool indicators)
 {
 	auto global_to_current_local = global_to_parent_local * node->calculate_transform_prev_to_current_with_dofs();
 	auto my_root_position = global_to_current_local * node->get_bone_local_root_position();
 	auto my_tip_position = global_to_current_local * node->get_bone_local_tip_position();
+
+	std::cout << "bone: " << node->get_name() << std::endl;
+	print_vec3(my_root_position);
+	print_vec3(my_tip_position);
+
 	if (arrows)
 	{
 		const GLubyte colors[][3] =
 			{
-			{ 27,158,119 },
+				{27, 158, 119},
-			{ 217,95,2 },
+				{217, 95, 2},
-			{ 117,112,179 },
+				{117, 112, 179},
-			{ 231,41,138 },
+				{231, 41, 138},
-			{ 102,166,30 },
+				{102, 166, 30},
-			{ 230,171,2 },
+				{230, 171, 2},
-			{ 166,118,29 },
+				{166, 118, 29},
 			};
 		glColor3ubv(colors[level % 7]);
 		c.tesselate_arrow(cgv::math::fvec<double, 3u>(my_root_position.x(), my_root_position.y(), my_root_position.z()),
@@ -100,9 +105,10 @@ void SkeletonViewer::skeleton_changed(std::shared_ptr<Skeleton> s)
 	generate_tree_view_nodes();
 
 	//Fit view to skeleton
-	std::vector<cgv::render::view*> view_ptrs;
+	std::vector<cgv::render::view *> view_ptrs;
 	cgv::base::find_interface<cgv::render::view>(get_node(), view_ptrs);
-	if (view_ptrs.empty()) {
+	if (view_ptrs.empty())
+	{
 		// If there is no view, we cannot update it
 		cgv::gui::message("could not find a view to adjust!!");
 	}
@@ -120,7 +126,7 @@ void SkeletonViewer::skeleton_changed(std::shared_ptr<Skeleton> s)
 	post_redraw();
 }
 
-void SkeletonViewer::recursive_connect_signals(Bone* b)
+void SkeletonViewer::recursive_connect_signals(Bone *b)
 {
 	for (int i = 0; i < b->dof_count(); ++i)
 		connect(b->get_dof(i)->changed_signal, this, &SkeletonViewer::dof_changed);
@@ -146,7 +152,7 @@ void SkeletonViewer::generate_tree_view_nodes()
 	generate_tree_view_nodes(tree_view, data->get_skeleton()->get_root());
 }
 
-void SkeletonViewer::generate_tree_view_nodes(gui_group_ptr parent, Bone* bone)
+void SkeletonViewer::generate_tree_view_nodes(gui_group_ptr parent, Bone *bone)
 {
 	if (bone->childCount() == 0)
 	{
@@ -166,7 +172,7 @@ void SkeletonViewer::generate_tree_view_nodes(gui_group_ptr parent, Bone* bone)
 
 void SkeletonViewer::start_choose_base()
 {
-	Bone* b = data->get_endeffector();
+	Bone *b = data->get_endeffector();
 	data->set_endeffector(nullptr);
 	data->set_base(b);
 }
@@ -177,7 +183,7 @@ void SkeletonViewer::tree_selection_changed(base_ptr p, bool select)
 
 	if (select)
 	{
-		Bone* bone = gui_to_bone.at(p);
+		Bone *bone = gui_to_bone.at(p);
 		generate_bone_gui(bone);
 		data->set_endeffector(bone);
 	}
@@ -313,7 +319,7 @@ void SkeletonViewer::create_gui()
 				 rebind(this, &SkeletonViewer::load_pinocchio));
 }
 
-void SkeletonViewer::generate_bone_gui(Bone* bone)
+void SkeletonViewer::generate_bone_gui(Bone *bone)
 {
 	// Add bone-specific gui elements to bone_group.
 	// Use the layout "\n" to specify vertical alignment
@@ -329,7 +335,7 @@ void SkeletonViewer::generate_bone_gui(Bone* bone)
 	}
 }
 
-void SkeletonViewer::draw(context& c) 
+void SkeletonViewer::draw(context &c)
 {
 	glDisable(GL_LIGHTING);
 	glDisable(GL_CULL_FACE);

CGII/src/debughelpers.cpp

@@ -0,0 +1,23 @@
+#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);
+}

CGII/src/debughelpers.h

@@ -0,0 +1,11 @@
+#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);

CGII/src/math_helper.h

@@ -21,7 +21,7 @@ const cgv::math::fmat<T, 4, 4> rotate(const cgv::math::fvec<T, 3> axis, T angle_
 
 ///creates a 4x4 translation matrix
 template <typename T>
-const cgv::math::fmat<T, 4, 4> translate(const T&x, const T &y, const T&z)
+const cgv::math::fmat<T, 4, 4> translate(const T &x, const T &y, const T &z)
 {
 	cgv::math::fmat<T, 4, 4> m;
 	m.identity();