Try to inv multiply parent orientation

CGII/src/Bone.cpp

@@ -9,6 +9,15 @@
 
 #include "math_helper.h"
 
+std::string from_Vec3(Vec3 &v)
+{
+	std::stringstream is;
+
+	is << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")";
+
+	return is.str();
+}
+
 float calc_angle(Vec3 dir_vec, Vec3 axis)
 {
 	float dot_result = dot(dir_vec, axis);
@@ -48,62 +57,31 @@ void Bone::calculate_matrices()
 
 	orientationTransformLocalToGlobal = cgv::math::inv(orientationTransformGlobalToLocal);
 
-	std::for_each(dofs.begin(), dofs.end(), [&](std::shared_ptr<AtomicTransform> &t) {
+	std::cout << name << " direction " << from_Vec3(direction_in_world_space) << std::endl;
-		//std::cout << "dofs " << t->get_name() << ": " << t->get_value() << std::endl;
+	std::cout << name << " length " << length << std::endl;
-	});
 
 	/*Task 2.1: Implement matrix calculation */
-	Mat4 matrix;
-	matrix.identity();
 
 	Vec3 vector = direction_in_world_space * length;
 
+	/*
+	Mat4 matrix;
+	matrix.identity();
+
 	translationTransformCurrentJointToNext = translate(vector);
 	orientationTransformPrevJointToCurrent = matrix;
-
-	/*
-	translationTransformCurrentJointToNext = translate(0.0f, 0.0f, length);
-
-	if (parent == NULL)
-	{
-		if (direction_in_world_space.x() == 0.0f &&
-		direction_in_world_space.y() == 0.0f &&
-		direction_in_world_space.z() == 0.0f) {
-			orientationTransformPrevJointToCurrent = rotate(Vec3(0.0f, 0.0f, 1.0f), 0.0f);
-		} else {
-std::cout << direction_in_world_space.x() << ", "
-				<< direction_in_world_space.y() << ", "
-				<< direction_in_world_space.z() << std::endl;
-
-		float z_angle = calc_angle(direction_in_world_space, Vec3(0.0f, 0.0f, 1.0f));
-		float y_angle = calc_angle(direction_in_world_space, Vec3(0.0f, 1.0f, 0.0f));
-		float x_angle = calc_angle(direction_in_world_space, Vec3(1.0f, 0.0f, 0.0f));
-
-		std::cout << "z_angle " << z_angle << std::endl;
-		std::cout << "y_angle " << y_angle << std::endl;
-		std::cout << "x_angle " << x_angle << std::endl;
-
-			Mat4 z_mat = rotate(Vec3(0.0f, 0.0f, 1.0f), z_angle);
-			Mat4 y_mat = rotate(Vec3(0.0f, 1.0f, 0.0f), y_angle);
-			Mat4 x_mat = rotate(Vec3(1.0f, 0.0f, 0.0f), x_angle);
-
-			Mat4 result = z_mat * y_mat * x_mat;
-
-			orientationTransformPrevJointToCurrent = result;
-		}
-	}
-	else
-	{
-		Mat4 parent_orientation = parent->get_orientation_transform_prev_joint_to_current();
-
-		float angle = calc_angle(direction_in_world_space, parent->get_direction_in_world_space());
-
-		Mat4 rot = rotate(Vec3(1.0f, 1.0f, 1.0f), angle);
-
-		orientationTransformPrevJointToCurrent = rot;
-	}
 	*/
 
+	Mat4 parent_orientation;
+
+	if (parent != NULL)
+		parent_orientation = parent->get_total_orientation();
+	else
+		parent_orientation.identity();
+
+	translationTransformCurrentJointToNext = translate(vector);
+	orientationTransformPrevJointToCurrent = orientationTransformGlobalToLocal * cgv::math::inv(parent_orientation);
+
 	/*Task 4.6: Implement matrix calculation */
 }
 
@@ -124,10 +102,22 @@ Mat4 Bone::calculate_transform_prev_to_current_with_dofs()
 Mat4 Bone::calculate_transform_prev_to_current_without_dofs()
 {
 	//Task 2.1: Implement matrix calculation
-	Mat4 t = translationTransformCurrentJointToNext * orientationTransformPrevJointToCurrent * orientationTransformLocalToGlobal * orientationTransformGlobalToLocal;
+	Mat4 t = translationTransformCurrentJointToNext * orientationTransformPrevJointToCurrent;
 	return t;
 }
 
+Mat4 Bone::get_total_orientation()
+{
+	Mat4 parent_orientation;
+
+	if (parent != NULL)
+		parent_orientation = parent->get_total_orientation();
+	else
+		parent_orientation.identity();
+
+	return orientationTransformPrevJointToCurrent * parent_orientation;
+}
+
 void Bone::add_dof(AtomicTransform *dof)
 {
 	dof->set_index_in_amc(dofs.size());

CGII/src/Bone.h

@@ -9,6 +9,8 @@
 #include <deque>
 #include <memory>
 
+std::string from_Vec3(Vec3 &v);
+
 // Represents a bone in a hierarchical skeleton
 class Bone
 {
@@ -97,6 +99,8 @@ class Bone
 	//Available after implementing task 4.6.
 	const Mat4 &get_binding_pose_matrix() const;
 
+	Mat4 get_total_orientation();
+
   private:
 	//The following attributes are read from the ASF file
 	std::deque<std::shared_ptr<AtomicTransform>> dofs; //Degrees of freedom for the bone

CGII/src/SkeletonViewer.cpp

@@ -28,22 +28,31 @@ 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)
+void SkeletonViewer::draw_skeleton_subtree(Bone *node, const Mat4 &global_to_parent_local, context &c, int depth)
 {
+	if (depth > 4)
+		return;
+
 	//Task: Draw skeleton
-	Mat4 local_transform = node->calculate_transform_prev_to_current_with_dofs()
+	Mat4 local_transform = node->calculate_transform_prev_to_current_with_dofs() * global_to_parent_local;
-							* global_to_parent_local;
 
 	Vec4 zero = Vec4(0.0f, 0.0f, 0.0f, 1.0f);
 
 	Vec4 p1 = global_to_parent_local * zero;
 	Vec4 p2 = local_transform * zero;
 
-	c.tesselate_arrow(Vec3(p1.x(), p1.y(), p1.z()), Vec3(p2.x(), p2.y(), p2.z()));
+	Vec3 p1_v3 = Vec3(p1.x(), p1.y(), p1.z());
+	Vec3 p2_v3 = Vec3(p2.x(), p2.y(), p2.z());
+
+	std::cout << node->get_name() << " start: " << from_Vec3(p1_v3) << ", end: " << from_Vec3(p2_v3) << std::endl;
+
+	c.tesselate_arrow(p1_v3, p2_v3);
+
+	depth++;
 
 	for (int i = 0; i < node->childCount(); i++)
 	{
-		draw_skeleton_subtree(node->child_at(i), local_transform, c);
+		draw_skeleton_subtree(node->child_at(i), local_transform, c, depth);
 	}
 }
 
@@ -297,5 +306,5 @@ void SkeletonViewer::draw(context &c)
 	glBlendFunc(GL_SRC_ALPHA, GL_ONE);
 
 	if (data->get_skeleton() != nullptr)
-		draw_skeleton_subtree(data->get_skeleton()->get_root(), data->get_skeleton()->get_origin(), c);
+		draw_skeleton_subtree(data->get_skeleton()->get_root(), data->get_skeleton()->get_origin(), c, 0);
 }

CGII/src/SkeletonViewer.h

@@ -50,7 +50,7 @@ class SkeletonViewer : public node, public drawable, public provider
 	void load_animation();
 	void start_choose_base();
 
-	void draw_skeleton_subtree(Bone *node, const Mat4 &global_to_parent_local, context &c);
+	void draw_skeleton_subtree(Bone *node, const Mat4 &global_to_parent_local, context &c, int depth);
 
 	void dof_changed(double new_value);
 	void recursive_connect_signals(Bone *b);