Skeleton tests

CGII/src/Skeleton.cpp

@@ -86,7 +86,8 @@ bool Skeleton::fromASFFile(const std::string& filename)
 					state = BoneData;
 				else if (str.find(":hierarchy") == 0)
 					state = Hierarchy;
-				else state = Ignore;
+				else
+					state = Ignore;
 				continue;
 			}
 			switch (state)
@@ -206,7 +207,6 @@ bool Skeleton::fromASFFile(const std::string& filename)
 						ss >> lower >> upper;
 						current_node->get_dof(n_dofs - i - 1)->set_limits(lower, upper);
 					}
-
 				}
 				break;
 			case Hierarchy:
@@ -256,6 +256,76 @@ void Skeleton::postprocess(Bone* node, const Vec3& global_position)
 	}
 }
 
+Vec3 into_vec3(const Vec4 &v)
+{
+	return Vec3(v.x(), v.y(), v.z());
+}
+
+void print_vec3(const Vec3 &v)
+{
+	std::cout << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")" << std::endl;
+}
+
+void Skeleton::get_bones(Bone *bone, std::vector<Bone *> &bone_list)
+{
+	float len = length(bone->get_bone_local_root_position() - bone->get_bone_local_tip_position());
+
+	if (len > 0.0f)
+		bone_list.emplace_back(bone);
+
+	for (int i = 0; i < bone->childCount(); i++)
+	{
+		get_bones(bone->child_at(i), bone_list);
+	}
+}
+
+int Skeleton::find_bone(Bone *bone, std::vector<Bone *> &bone_list)
+{
+	for (int i = 0; i < bone_list.size(); i++)
+	{
+		if (bone == bone_list[i])
+		{
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+void calc_points(Bone *bone, std::vector<Vec3> &points, Vec3 parent_pos)
+{
+	Vec3 new_pos = parent_pos + into_vec3(bone->get_bone_local_tip_position());
+
+	points.emplace_back(new_pos);
+
+	for (int i = 0; i < bone->childCount(); i++)
+	{
+		calc_points(bone->child_at(i), points, new_pos);
+	}
+}
+
+void calc_points(Bone *bone, std::vector<Vec3> &points, Vec3 parent_pos, float correction)
+{
+	Vec3 new_pos = parent_pos + (into_vec3(bone->get_bone_local_tip_position()) / correction);
+
+	points.emplace_back(new_pos);
+
+	for (int i = 0; i < bone->childCount(); i++)
+	{
+		calc_points(bone->child_at(i), points, new_pos, correction);
+	}
+}
+
+float abs(float &v)
+{
+	if (v < 0.0f)
+	{
+		return -v;
+	}
+
+	return v;
+}
+
 void Skeleton::write_pinocchio_file(const std::string &filename)
 {
 	std::ofstream o;
@@ -269,12 +339,95 @@ void Skeleton::write_pinocchio_file(const std::string& filename)
 	if (o)
 	{
 		/*Task 4.1: Write Pinocchio file into o */
+
+		std::vector<Bone *> bone_list;
+		get_bones(root, bone_list);
+
+		// gather all points
+		std::vector<Vec3> points;
+		calc_points(root, points, Vec3(0.0f, 0.0f, 0.0f));
+
+		Vec3 min(0.0f, 0.0f, 0.0f);
+		Vec3 max(0.0f, 0.0f, 0.0f);
+
+		for (auto &point : points)
+		{
+			//print_vec3(point);
+
+			if (point.x() < min.x())
+			{
+				min.x() = point.x();
+			}
+
+			if (point.y() < min.y())
+			{
+				min.y() = point.y();
+			}
+
+			if (point.z() < min.z())
+			{
+				min.z() = point.z();
+			}
+
+			if (point.x() > max.x())
+			{
+				max.x() = point.x();
+			}
+
+			if (point.y() > max.y())
+			{
+				max.y() = point.y();
+			}
+
+			if (point.z() > max.z())
+			{
+				max.z() = point.z();
+			}
+		}
+
+		//std::cout << "min: ";
+		//print_vec3(min);
+
+		//std::cout << "max: ";
+		//print_vec3(max);
+
+		float x_diff = abs(min.x()) + abs(max.x());
+		float y_diff = abs(min.y()) + abs(max.y());
+		float z_diff = abs(min.z()) + abs(max.z());
+
+		float max_diff = std::max(x_diff, std::max(y_diff, z_diff));
+
+		std::vector<Vec3> points2;
+		calc_points(root, points2, Vec3(0.0f, 0.0f, 0.0f), max_diff);
+
+		for (auto p : points2)
+		{
+			print_vec3(p);
+		}
+
+		//std::cout << "max_diff: " << max_diff << std::endl;
+
+		// ---------------------------- info ----------------------------
+		int i = 0;
+		for (Bone *bone : bone_list)
+		{
+			//float len = length(bone->get_bone_local_tip_position());
+
+			int parent_index = find_bone(bone->get_parent(), bone_list);
+
+			//std::cout << i++ << ": " << bone->get_name() << ": " << len << " parent: " << parent_index << std::endl;
+
+			Vec3 c = into_vec3(bone->get_bone_local_tip_position()) / max_diff;
+
+			o << i++ << " " << c.x() << " " << c.y() << " " << c.z() << " " << parent_index << std::endl;
+		}
+		//std::cout << std::endl
+		//		  << "---------------------------" << std::endl
+		//		  << std::endl;
 	}
 	o.close();
 }
 
-
-
 void Skeleton::read_pinocchio_file(std::string filename)
 {
 	std::ifstream o;
@@ -294,15 +447,11 @@ void Skeleton::read_pinocchio_file(std::string filename)
 	postprocess(root, get_origin_vec());
 }
 
-
-
 void Skeleton::get_skinning_matrices(std::vector<Mat4> &matrices)
 {
 	/*Task 4.6: Calculate skinning matrices */
 }
 
-
-
 Bone *Skeleton::find_bone(const std::string &name) const
 {
 	auto it = bones.find(name);

CGII/src/Skeleton.h

@@ -10,7 +10,6 @@
 
 #include <vector>
 
-
 class Skeleton : public IHasBoundingBox
 {
   public:
@@ -46,9 +45,8 @@ private:
 	Bone *root;
 	void postprocess(Bone *node, const Vec3 &global_position);
 
-	
+	void get_bones(Bone *bone, std::vector<Bone *> &bone_list);
-	
+	int find_bone(Bone *bone, std::vector<Bone *> &bone_list);
-	
 
 	std::map<const std::string, Bone *> bones;
 };

penis.txt

@@ -0,0 +1,30 @@
+0 0.0429472 -0.0565452 0.0264444 -1
+1 -7.5121e-09 -0.225528 0 0
+2 0 -0.236038 0 1
+3 8.92062e-09 -0.073179 -0.014522 2
+4 -5.48289e-17 -0.0374839 4.69168e-08 3
+5 -0.0411343 -0.0565452 0.0264444 -1
+6 0 -0.234226 0 5
+7 -7.5121e-09 -0.236596 0 6
+8 4.69506e-09 -0.0744582 -0.0151752 7
+9 2.00063e-16 -0.0381506 4.77513e-08 8
+10 0.000890649 0.0641375 -0.00609295 -1
+11 0.00178704 0.0645553 -0.00134698 10
+12 0.00128702 0.0650298 0.00152129 11
+13 -0.00170664 0.0550206 0.00542001 12
+14 0.00327919 0.0554969 -0.00390619 13
+15 0.00117224 0.0557831 -0.00196647 14
+16 0.105943 0.0378376 -0.00980558 12
+17 1.68454e-07 -0.157005 -2.90101e-07 16
+18 1.17764e-07 -0.10976 -2.02806e-07 17
+19 -3.12333e-18 -0.0548801 7.10897e-08 18
+20 -1.28279e-18 -0.0225364 2.91929e-08 19
+21 -1.0339e-18 -0.0181695 2.35361e-08 20
+22 1.78412e-08 -0.0260878 3.10974e-08 19
+23 -0.0988282 0.0432937 -0.0127496 12
+24 -1.77206e-07 -0.165162 -3.05173e-07 23
+25 -1.16433e-07 -0.108519 -2.00513e-07 24
+26 3.08802e-18 -0.0542597 6.70519e-08 25
+27 1.11643e-18 -0.0196147 2.4239e-08 26
+28 8.99718e-19 -0.0158139 1.95421e-08 27
+29 -1.50242e-08 -0.0227056 2.70658e-08 26