Merge branch 'master' of ssh://141.30.224.77:23/hodasemi/CGII

CGII/build/cmake/CMakeLists.txt

@@ -33,8 +33,7 @@ cgv_add_module(CGII
 	../../src/Skeleton.cpp
 	../../src/SkeletonViewer.cpp
 	../../src/SkinnedMeshViewer.cpp
-	../../src/main.cpp
-	../../src/debughelpers.cpp)
+	../../src/main.cpp)
 
 # Set include directories
 include_directories(

CGII/src/AtomicTransform.cpp

@@ -92,19 +92,92 @@ T clamp(T &v, T &c1, T &c2)
 	return v;
 }
 
+float angle(const Vec3 &v1, const Vec3 &v2)
+{
+	float d = dot(v1, v2);
+
+	float v1_length = length(v1);
+	float v2_length = length(v2);
+
+	return std::acos(d / (v1_length * v2_length));
+}
+
+float a(float rcos, float x)
+{
+	return rcos + x * x * (1 - rcos);
+}
+
+float b(float rcos, float rsin, float x, float y, float z)
+{
+	return -z * rsin + y * x * (1 - rcos);
+}
+
+float c(float rcos, float rsin, float x, float y, float z)
+{
+	return y * rsin + z * x * (1 - rcos);
+}
+
+float d(float rcos, float rsin, float x, float y, float z)
+{
+	return z * rsin + x * y * (1 - rcos);
+}
+
+float e(float rcos, float y)
+{
+	return rcos + y * y * (1 - rcos);
+}
+
+float f(float rcos, float rsin, float x, float y, float z)
+{
+	return -x * rsin + z * y * (1 - rcos);
+}
+
+float g(float rcos, float rsin, float x, float y, float z)
+{
+	return -y * rsin + x * z * (1 - rcos);
+}
+
+float h(float rcos, float rsin, float x, float y, float z)
+{
+	return x * rsin + y * z * (1 - rcos);
+}
+
+float j(float rcos, float z)
+{
+	rcos + z *z *(1 - rcos);
+}
+
+float AtomicRotationTransform::least_squares(const Vec3 &local_vector, const Vec3 &target, float r)
+{
+	float angler = r * PI / 180.0f;
+
+	float rcos = cos(angler);
+	float rsin = sin(angler);
+
+	float x = axis.x();
+	float y = axis.y();
+	float z = axis.z();
+
+	float xs = a(rcos, x) * local_vector.x() + b(rcos, rsin, x, y, z) * local_vector.y() + c(rcos, rsin, x, y, z) * local_vector.z();
+	float ys = d(rcos, rsin, x, y, z) * local_vector.x() + e(rcos, y) * local_vector.y() + f(rcos, rsin, x, y, z) * local_vector.z();
+	float zs = g(rcos, rsin, x, y, z) * local_vector.x() + h(rcos, rsin, x, y, z) * local_vector.y() + j(rcos, z);
+
+	float x_diff = xs - target.x();
+	float y_diff = ys - target.y();
+	float z_diff = zs - target.z();
+
+	float squares = x_diff * x_diff + y_diff * y_diff + z_diff * z_diff;
+}
+
 void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
 {
 	/*Task: Implement parameter optimization*/
 
-	// target into local_target
-	// get dofs
-	// project target into plane decided by which dofs exist
-	// scal mult local_target and local_vector
+	// optimize this that: target = this->calculate_matrix() * local_vector;
 
-	// use only degrees defined by dofs
-	// if inverse == true use -angle with set_value()
+	float first_guess = angle(local_vector, target);
 
-	double result = 0.0;
+	double result = least_squares(local_vector, target, first_guess);
 
 	if (inverse)
 		result = -result;

CGII/src/AtomicTransform.h

@@ -14,30 +14,30 @@
 
 class Transform
 {
-public:
+  public:
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) = 0;
 };
 
 class StaticTransform : public Transform
 {
-public:
-	StaticTransform(const Mat4& t) : t(t) {}
+  public:
+	StaticTransform(const Mat4 &t) : t(t) {}
 
 	Mat4 calculate_matrix() { return t; }
-	void optimize_value(const Vec3& local_vector, const Vec3& target) { }
+	void optimize_value(const Vec3 &local_vector, const Vec3 &target) {}
 
-private:
+  private:
 	Mat4 t;
 };
 
 //Represents an arbitrary affine transform with exactly one scalar parameter
 class AtomicTransform : public cgv::gui::control_provider<double>, public Transform
 {
-public:
+  public:
 	//Sets the limits of the scalar parameter
 	void set_limits(double lower, double upper);
 
@@ -45,17 +45,17 @@ public:
 	const double get_upper_limit() const;
 
 	//Sets the current scalar parameter. Ignore ud.
-	virtual void set_value(const double& value, void* ud = nullptr);
+	virtual void set_value(const double &value, void *ud = nullptr);
 
 	//Gets the current scalar parameter. Ignore ud.
-	const double get_value(void* ud = nullptr) const;
+	const double get_value(void *ud = nullptr) const;
 
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false) = 0;
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) { optimize_value(local_vector, target, false); }
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) { optimize_value(local_vector, target, false); }
 
 	//Draws an indicator that visualizes the transform, including its limits.
 	virtual void drawIndicator(float size) = 0;
@@ -73,7 +73,7 @@ public:
 	//Get the order in which the transform is specified in the animation file.
 	int get_index_in_amc() const { return index_in_amc; }
 
-protected:
+  protected:
 	double lower_limit, upper_limit;
 	double value;
 	std::string title;
@@ -82,79 +82,82 @@ protected:
 
 class AtomicRotationTransform : public AtomicTransform
 {
-public:
+  public:
 	AtomicRotationTransform(Vec3 axis);
 	virtual Mat4 calculate_matrix();
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
 	virtual void drawIndicator(float size);
 	virtual void drawActualIndicator(float size);
 
-protected:
+  private:
+	virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
+
+  protected:
 	Vec3 axis;
 };
 
 class AtomicXRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicXRotationTransform() : AtomicRotationTransform(Vec3(1, 0, 0)) { title = "X-Rotation"; }
 };
 
 class AtomicYRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicYRotationTransform() : AtomicRotationTransform(Vec3(0, 1, 0)) { title = "Y-Rotation"; }
 };
 
 class AtomicZRotationTransform : public AtomicRotationTransform
 {
-public:
+  public:
 	AtomicZRotationTransform() : AtomicRotationTransform(Vec3(0, 0, 1)) { title = "Z-Rotation"; }
 };
 
 class AtomicTranslationTransform : public AtomicTransform
 {
-public:
+  public:
 	AtomicTranslationTransform(int dim);
 	virtual Mat4 calculate_matrix();
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
-	virtual void drawIndicator(float size) { };
-	virtual void drawActualIndicator(float size) { };
+	virtual void drawIndicator(float size){};
+	virtual void drawActualIndicator(float size){};
 
-private:
+  private:
 	int dim;
 };
 
 class AtomicXTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicXTranslationTransform() : AtomicTranslationTransform(0) { title = "X-Translation"; }
 };
 
 class AtomicYTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicYTranslationTransform() : AtomicTranslationTransform(1) { title = "Y-Translation"; }
 };
 
 class AtomicZTranslationTransform : public AtomicTranslationTransform
 {
-public:
+  public:
 	AtomicZTranslationTransform() : AtomicTranslationTransform(2) { title = "Z-Translation"; }
 };
 
 class InverseTransform : public Transform
 {
-public:
-	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) { }
+  public:
+	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) {}
 
 	Mat4 calculate_matrix() { return cgv::math::inv(t->calculate_matrix()); }
-	void optimize_value(const Vec3& local_vector, const Vec3& target)
+	void optimize_value(const Vec3 &local_vector, const Vec3 &target)
 	{
 		t->optimize_value(local_vector, target, true);
 	}
 
-private:
+  private:
 	std::shared_ptr<AtomicTransform> t;
 };

CGII/src/IKViewer.cpp

@@ -5,7 +5,6 @@
 #include "IKViewer.h"
 
 #include "math_helper.h"
-#include "debughelpers.h"
 
 #include <unordered_set>
 
@@ -13,6 +12,11 @@
 #include <cgv/math/inv.h>
 #include <cgv/math/mat.h>
 
+Vec3 into_vec3(const Vec4 &v)
+{
+	return Vec3(v.x(), v.y(), v.z());
+}
+
 IKViewer::IKViewer(DataStore *data)
 	: node("IK Viewer"), data(data), modifying(false), target_position(0, 0, 0, 1), max_iterations(20)
 {
@@ -97,6 +101,8 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 		current_base_matrix = current_base_matrix * transform->calculate_matrix();
 	}
 
+	current_base_matrix = current_base_matrix * base->get_translation_transform_current_joint_to_next();
+
 	if (!endeffector)
 		return;
 
@@ -129,6 +135,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 	//TODO: check if common ancestor is visited twice
 
+	std::shared_ptr<Transform> static_trans = std::shared_ptr<Transform>(new StaticTransform(endeffector->get_translation_transform_current_joint_to_next()));
+	kinematic_chain.emplace_front(static_trans);
+
 	while (1)
 	{
 		if (state == 0)
@@ -164,16 +173,16 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 
 			Bone *bone = base_tree[base_index];
 
-			for (int i = bone->dof_count() - 1; i >= 0; i--)
+			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);
+
+			for (int i = 0; i < bone->dof_count(); 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
@@ -183,17 +192,19 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
 		}
 	}
 
+	std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(base->get_translation_transform_current_joint_to_next())));
+	kinematic_chain.emplace_front(inverse_static);
+
 	current_endeffector_matrix.identity();
+	kinematic_vector.clear();
 
 	for (auto &transform : kinematic_chain)
 	{
+		kinematic_vector.emplace_back(transform);
 		current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
 	}
 
-	target_position = current_base_matrix * current_endeffector_matrix * endeffector->get_bone_local_tip_position();
-
-	print_vec3(target_position);
-	std::cout << std::endl;
+	target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
 }
 
 void IKViewer::optimize()
@@ -204,15 +215,40 @@ void IKViewer::optimize()
 	auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
 	float distance_threshold = 0.0001f * std::max({skeleton_size.x(), skeleton_size.y(), skeleton_size.z()});
 
-	//split the current matrix in:
-	//  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++)
+		int kc_size = kinematic_vector.size();
+
+		// reverse iterate through kinematic chain
+		for (int j = kc_size - 1; j >= 0; j--)
 		{
-			//it->optimize_value(/*local_vector*/, target_position);
+			Mat4 before_dof = current_base_matrix;
+
+			for (int k = 0; k < j; k++)
+			{
+				before_dof = before_dof * kinematic_vector[k]->calculate_matrix();
+			}
+
+			Mat4 after_dof;
+			after_dof.identity();
+
+			int after_dof_index = j + 1;
+
+			for (int k = j + 1; k < kc_size; k++)
+			{
+				after_dof = after_dof * kinematic_vector[k]->calculate_matrix();
+			}
+
+			// now we got 3 matrices
+			// (1) before dof: base_matrix * kinematic_vector[0...j-1]
+			// (2) dof: kinematic_vector[j]
+			// (3) after_dof: kinematic_vector[j+1...kc_size-1]
+
+			auto v_local = after_dof * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
+			auto v_target = inv(before_dof) * target_position;
+
+			kinematic_vector[j]->optimize_value(into_vec3(v_local), into_vec3(v_target));
 		}
 
 		current_endeffector_matrix.identity();

CGII/src/IKViewer.h

@@ -48,6 +48,7 @@ class IKViewer : public node, public drawable, public provider, public event_han
 	unsigned int max_iterations;
 
 	std::list<std::shared_ptr<Transform>> kinematic_chain;
+	std::vector<std::shared_ptr<Transform>> kinematic_vector;
 
 	void optimize();
 

CGII/src/SkeletonViewer.cpp

@@ -12,7 +12,6 @@
 #include <cgv/base/find_action.h>
 
 #include "math_helper.h"
-#include "debughelpers.h"
 
 using namespace cgv::utils;
 

CGII/src/debughelpers.cpp

@@ -1,23 +0,0 @@
-#include "debughelpers.h"
-
-#include <iostream>
-
-void print_vec3(const Vec3 &v)
-{
-    std::cout << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")" << std::endl;
-}
-
-void print_vec3(const Vec4 &v)
-{
-    print_vec3(into_vec3(v));
-}
-
-Vec3 into_vec3(const Vec4 &v)
-{
-    return Vec3(v.x(), v.y(), v.z());
-}
-
-Vec4 into_vec4(const Vec3 &v)
-{
-    return Vec4(v.x(), v.y(), v.z(), 1.0f);
-}

CGII/src/debughelpers.h

@@ -1,11 +0,0 @@
-#pragma once
-
-#include "math_helper.h"
-
-void print_vec3(const Vec3 &v);
-
-void print_vec3(const Vec4 &v);
-
-Vec3 into_vec3(const Vec4 &v);
-
-Vec4 into_vec4(const Vec3 &v);