Add ccd frame

CGII/src/AtomicTransform.cpp

@@ -92,6 +92,12 @@ void AtomicRotationTransform::optimize_value(const Vec3& local_vector, const Vec
 	// use only degrees defined by dofs
 	// if inverse == true use -angle with set_value()
 
+	double result = 0.0;
+
+	if (inverse)
+		result = -result;
+
+	set_value(result);
 }
 
 AtomicTranslationTransform::AtomicTranslationTransform(int dim)

CGII/src/IKViewer.cpp

@@ -64,6 +64,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 	/*Task 3.1: Calculate kinematic chain*/
 
+	this->base = base;
+	this->endeffector = endeffector;
+
 	// get list of bones in order of endeffector to base
 	std::vector<Bone *> bones;
 	Bone *current_bone = endeffector;
@@ -84,8 +87,12 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 	for (Bone *bone : bones)
 	{
-		// TODO: calculate and add transforms to queue
+		for (int i = bone->dof_count() - 1; i >= 0; i--)
-		Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_with_dofs());
+		{
+			kinematic_chain.emplace_front(bone->get_dof(i));
+		}
+
+		Transform *tmp = new StaticTransform(bone->calculate_transform_prev_to_current_without_dofs());
 		std::shared_ptr<Transform> transform = std::shared_ptr<Transform>(tmp);
 		kinematic_chain.emplace_front(transform);
 	}
@@ -114,6 +121,25 @@ void IKViewer::optimize()
 	//  before_dof -> dof -> after_dof
 
 	/*Task 3.3: Implement CCD	*/
+	for (int i = 0; i < max_iterations; i++)
+	{
+		for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); i++)
+		{
+			//it->optimize_value(TODO: local_vector, target, inverse);
+		}
+
+		current_endeffector_matrix.identity();
+
+		for (auto &transform : kinematic_chain)
+		{
+			current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
+		}
+
+		Vec4 current_endeffector_position = current_endeffector_matrix * endeffector->get_bone_local_tip_position();
+
+		if (distance_threshold >= length(current_endeffector_position - target_position))
+			break;
+	}
 
 	//used for correct GUI behavior
 	data->dof_changed_by_ik = false;

CGII/src/IKViewer.h

@@ -41,6 +41,9 @@ class IKViewer : public node, public drawable, public provider, public event_han
 	Mat4 current_endeffector_matrix; //transform from base to endeffector
 	Mat4 current_base_matrix;		 //transform from global origin to base
 
+	Bone *base;
+	Bone *endeffector;
+
 	unsigned int max_iterations;
 
 	std::list<std::shared_ptr<Transform>> kinematic_chain;

CGII/src/SkeletonViewer.cpp

@@ -34,10 +34,6 @@ void SkeletonViewer::draw_skeleton_subtree(Bone *node, const Mat4 &global_to_par
 	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] =