Implement task 1 matrices

.vscode/settings.json

@@ -7,6 +7,52 @@
         "array": "cpp",
         "initializer_list": "cpp",
         "string_view": "cpp",
-        "valarray": "cpp"
-    }
+        "valarray": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "*.tcc": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "future": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "mutex": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "ostream": "cpp",
+        "ratio": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "typeinfo": "cpp",
+        "utility": "cpp",
+        "variant": "cpp",
+        "algorithm": "cpp"
+    },
+    "editor.formatOnSave": true
 }

CGII/src/Bone.cpp

@@ -9,11 +9,19 @@
 
 #include "math_helper.h"
 
+float calc_angle(Vec3 dir_vec, Vec3 axis)
+{
+	float dot_result = dot(dir_vec, axis);
+
+	float length_v1 = length(dir_vec);
+	float length_v2 = length(axis);
+
+	return acos(dot_result / (length_v1 * length_v2));
+}
+
 Bone::Bone()
 	: parent(nullptr), length(0.0f), direction_in_world_space(0.0, 0.0, 0.0), translationTransforms(0)
 {
-	start_point = Vec3(0.0f, 0.0f, 0.0f);
-	end_point = Vec3(0.0f, 0.0f, 0.0f);
 }
 
 Bone::~Bone()
@@ -33,24 +41,63 @@ void Bone::calculate_matrices()
 {
 	orientationTransformGlobalToLocal.identity();
 	std::for_each(orientation.begin(), orientation.end(), [&](AtomicTransform *t) {
+		std::cout << "atomic transform " << t->get_name() << ": " << t->get_value() << std::endl;
+
 		orientationTransformGlobalToLocal = orientationTransformGlobalToLocal * t->calculate_matrix();
 	});
 	orientationTransformLocalToGlobal = cgv::math::inv(orientationTransformGlobalToLocal);
 
-	if (parent != nullptr)
-		start_point = parent->end_point;
-
-	end_point = start_point + length * direction_in_world_space;
-
-	std::cout << name << " start at ("
-			  << start_point.x() << ", "
-			  << start_point.y() << ", "
-			  << start_point.z() << "), end at ("
-			  << end_point.x() << ", "
-			  << end_point.y() << ", "
-			  << end_point.z() << ")" << std::endl;
-
 	/*Task 2.1: Implement matrix calculation */
+	Mat4 matrix;
+	matrix.identity();
+
+	Vec3 vector = direction_in_world_space * length;
+
+	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;
+	}
+	*/
 
 	/*Task 4.6: Implement matrix calculation */
 }
@@ -58,16 +105,21 @@ void Bone::calculate_matrices()
 Mat4 Bone::calculate_transform_prev_to_current_with_dofs()
 {
 	//Task 2.1: Implement matrix calculation
-	Mat4 t;
-	t.identity();
+	Mat4 dofs;
+	dofs.identity();
+
+	std::for_each(orientation.begin(), orientation.end(), [&](AtomicTransform *t) {
+		dofs = dofs * t->calculate_matrix();
+	});
+
+	Mat4 t = translationTransformCurrentJointToNext * orientationTransformPrevJointToCurrent * dofs;
 	return t;
 }
 
 Mat4 Bone::calculate_transform_prev_to_current_without_dofs()
 {
 	//Task 2.1: Implement matrix calculation
-	Mat4 t;
-	t.identity();
+	Mat4 t = translationTransformCurrentJointToNext * orientationTransformPrevJointToCurrent;
 	return t;
 }
 

CGII/src/Bone.h

@@ -97,10 +97,6 @@ class Bone
 	//Available after implementing task 4.6.
 	const Mat4 &get_binding_pose_matrix() const;
 
-	// Testing area
-	Vec3 start_point; // same as parent end point
-	Vec3 end_point;   // start_point + (length * direction_in_world_space)
-
   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

@@ -31,13 +31,19 @@ SkeletonViewer::SkeletonViewer(DataStore *data)
 void SkeletonViewer::draw_skeleton_subtree(Bone *node, const Mat4 &global_to_parent_local, context &c)
 {
 	//Task: Draw skeleton
-	c.tesselate_arrow(node->start_point, node->end_point);
+	Mat4 local_transform = node->calculate_transform_prev_to_current_with_dofs()
+							* global_to_parent_local;
 
-	Mat4 transform;
+	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()));
 
 	for (int i = 0; i < node->childCount(); i++)
 	{
-		draw_skeleton_subtree(node->child_at(i), transform, c);
+		draw_skeleton_subtree(node->child_at(i), local_transform, c);
 	}
 }
 
@@ -58,6 +64,8 @@ void SkeletonViewer::stop_animation()
 
 void SkeletonViewer::skeleton_changed(std::shared_ptr<Skeleton> s)
 {
+	std::cout << "skeleton changed" << std::endl;
+
 	// This function is called whenever the according signal of the
 	// data store has been called.
 
@@ -96,6 +104,8 @@ void SkeletonViewer::recursive_connect_signals(Bone *b)
 
 void SkeletonViewer::dof_changed(double)
 {
+	std::cout << "dof changed" << std::endl;
+
 	if (!data->dof_changed_by_ik)
 		data->set_endeffector(nullptr);