Least common ancestor frame

CGII/src/AtomicTransform.cpp

@@ -80,6 +80,18 @@ void AtomicRotationTransform::drawActualIndicator(float size)
 	glEnd();
 }
 
+template <class T>
+T clamp(T &v, T &c1, T &c2)
+{
+	if (v < c1)
+		return c1;
+
+	if (v > c2)
+		return c2;
+
+	return v;
+}
+
 void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
 {
 	/*Task: Implement parameter optimization*/
@@ -97,7 +109,7 @@ void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec
 	if (inverse)
 		result = -result;
 
-	result = std::clamp(result, lower_limit, upper_limit);
+	result = clamp(result, lower_limit, upper_limit);
 
 	set_value(result);
 }

CGII/src/IKViewer.cpp

@@ -38,6 +38,19 @@ void IKViewer::base_changed(Bone *b)
 	post_redraw();
 }
 
+int IKViewer::find_element(std::vector<Bone *> &v, Bone *element)
+{
+	for (int i = 0; i < v.size(); i++)
+	{
+		if (v[i] == element)
+		{
+			return i;
+		}
+	}
+
+	return -1;
+}
+
 void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 {
 	std::cout << "calculate_kinematic_chain" << std::endl;
@@ -63,38 +76,108 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 	std::cout << std::endl;
 
 	/*Task 3.1: Calculate kinematic chain*/
+	kinematic_chain.clear();
 
 	this->base = base;
 	this->endeffector = endeffector;
 
 	// get list of bones in order of endeffector to base
-	std::vector<Bone *> bones;
+	std::vector<Bone *> endeffector_tree;
+	std::vector<Bone *> base_tree;
+
+	// gather all bones from endeffector to root
 	Bone *current_bone = endeffector;
 
 	while (1)
 	{
-		bones.emplace_back(current_bone);
+		endeffector_tree.emplace_back(current_bone);
 
 		// break if we reached base bone
-		if (current_bone == base)
+		if (current_bone->get_parent() == nullptr)
 			break;
 
 		// set next bone in hierarchy
 		current_bone = current_bone->get_parent();
 	}
 
-	kinematic_chain.clear();
+	// gather all bones from base to root
+	current_bone = base;
 
-	for (Bone *bone : bones)
+	while (1)
 	{
-		for (int i = bone->dof_count() - 1; i >= 0; i--)
-		{
-			kinematic_chain.emplace_front(bone->get_dof(i));
-		}
+		base_tree.emplace_back(current_bone);
 
-		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);
+		// break if we reached base bone
+		if (current_bone->get_parent() == nullptr)
+			break;
+
+		// set next bone in hierarchy
+		current_bone = current_bone->get_parent();
+	}
+
+	// add all transform from endeffector to common ancestor and to reverse base
+
+	// state == 0 means, iterating through endeffector_tree
+	// state == 1 means, reverse iterating through base_tree
+	int state = 0;
+
+	int endeffector_index = 0;
+	int base_index = 0;
+
+	//TODO: check if common ancestor is visited twice
+
+	while (1)
+	{
+		if (state == 0)
+		{
+			if (endeffector_index >= endeffector_tree.size())
+				break;
+
+			Bone *bone = endeffector_tree[endeffector_index];
+
+			for (int i = bone->dof_count() - 1; i >= 0; i--)
+			{
+				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);
+
+			int index = find_element(base_tree, bone);
+			if (index != -1)
+			{
+				base_index = index;
+				state = 1;
+				continue;
+			}
+
+			endeffector_index++;
+		}
+		else if (state == 1)
+		{
+			if (base_index < 0)
+				break;
+
+			Bone *bone = base_tree[base_index];
+
+			for (int i = bone->dof_count() - 1; i >= 0; i--)
+			{
+				Transform *tmp = new InverseTransform(bone->get_dof(i));
+				std::shared_ptr<Transform> inverse_dof = std::shared_ptr<Transform>(tmp);
+				kinematic_chain.emplace_front(inverse_dof);
+			}
+
+			std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
+			kinematic_chain.emplace_front(inverse_static);
+
+			base_index--;
+		}
+		else
+		{
+			// can't ever happen, but when it does, we will know
+			abort();
+		}
 	}
 
 	current_endeffector_matrix.identity();
@@ -107,6 +190,7 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 	target_position = current_endeffector_matrix * endeffector->get_bone_local_tip_position();
 
 	print_vec3(target_position);
+	std::cout << std::endl;
 }
 
 void IKViewer::optimize()
@@ -125,7 +209,7 @@ void IKViewer::optimize()
 	{
 		for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); i++)
 		{
-			it->optimize_value(/*local_vector*/, target_position);
+			//it->optimize_value(/*local_vector*/, target_position);
 		}
 
 		current_endeffector_matrix.identity();

CGII/src/IKViewer.h

@@ -33,6 +33,7 @@ class IKViewer : public node, public drawable, public provider, public event_han
 	void endeffector_changed(Bone *);
 	void base_changed(Bone *);
 
+	int find_element(std::vector<Bone *> &v, Bone *element);
 	void calculate_kinematic_chain(Bone *base, Bone *endeffector);
 
 	bool modifying; //Specifies if the user is currently modifying the IK target position