Add angle calculation of new pinocchio file

CGII/src/Skeleton.cpp

@@ -544,6 +544,21 @@ Bone *get_bone(int i, std::vector<Bone *> &bone_list)
 	return bone_list[i];
 }
 
+float angle(const Vec3 &v1, const Vec3 &v2)
+{
+	return std::acos(dot(v1, v2) / (length(v1) * length(v2))) * 180.0f / PI;
+}
+
+Vec3 project(Vec3 &v, Vec3 &normal)
+{
+	return v - (dot(v, normal) * normal);
+}
+
+float calc_angle_on_plane(Vec3 normal, Vec3 &old_direction, Vec3 &new_direction)
+{
+	return angle(project(old_direction, normal), project(new_direction, normal));
+}
+
 void Skeleton::read_pinocchio_file(std::string filename)
 {
 	std::ifstream o;
@@ -569,23 +584,56 @@ void Skeleton::read_pinocchio_file(std::string filename)
 
 			iss >> bone_id >> x >> y >> z >> parent_id;
 
-			//std::cout << bone_id << " " << x << " " << y << " " << z << " " << parent_id << std::endl;
-
 			Vec3 position(x, y, z);
 			Bone *current_bone = get_bone(bone_id, bone_list);
 
 			if (current_bone == nullptr)
 				abort();
 
+			Vec3 old_direction = current_bone->get_direction_in_world_space();
+
 			current_bone->set_length(position.normalize());
 			current_bone->set_direction_in_world_space(position);
 
-			// TODO: add new orientation
+			Vec3 new_direction = current_bone->get_direction_in_world_space();
+
+			// x rotation
+			float x_angle = calc_angle_on_plane(Vec3(1, 0, 0), old_direction, new_direction);
+
+			if (x_angle != 0.0f)
+			{
+				AtomicXRotationTransform *x_rotation = new AtomicXRotationTransform;
+				x_rotation->set_value(x_angle);
+
+				current_bone->add_axis_rotation(x_rotation);
+			}
+
+			// y rotation
+			float y_angle = calc_angle_on_plane(Vec3(0, 1, 0), old_direction, new_direction);
+
+			if (y_angle != 0.0f)
+			{
+				AtomicYRotationTransform *y_rotation = new AtomicYRotationTransform;
+				y_rotation->set_value(y_angle);
+
+				current_bone->add_axis_rotation(y_rotation);
+			}
+
+			// z rotation
+			float z_angle = calc_angle_on_plane(Vec3(0, 0, 1), old_direction, new_direction);
+
+			if (z_angle != 0.0f)
+			{
+				AtomicZRotationTransform *z_rotation = new AtomicZRotationTransform;
+				z_rotation->set_value(z_angle);
+
+				current_bone->add_axis_rotation(z_rotation);
+			}
 
 			current_bone->calculate_matrices();
 		}
 
-		/*Task 4.1: Read Pinocchio file */
+		reset_bounding_box();
 	}
 
 	o.close();