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,6 +12,7 @@
 #include <cgv/base/find_action.h>
 
 #include "math_helper.h"
+#include "debughelpers.h"
 
 using namespace cgv::utils;
 
@@ -19,8 +20,7 @@ cgv::render::shader_program Mesh::prog;
 
 // The constructor of this class
 SkeletonViewer::SkeletonViewer(DataStore *data)
-	: node("Skeleton Viewer"), data(data)
-	, animation(nullptr), animationTime(0)
+	: node("Skeleton Viewer"), data(data), animation(nullptr), animationTime(0)
 {
 	connect(data->skeleton_changed, this, &SkeletonViewer::skeleton_changed);
 
@@ -33,6 +33,11 @@ void SkeletonViewer::draw_skeleton_subtree(Bone* node, const Mat4& global_to_par
 	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] =
@@ -102,7 +107,8 @@ void SkeletonViewer::skeleton_changed(std::shared_ptr<Skeleton> s)
 	//Fit view to skeleton
 	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!!");
 	}

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);