feature (WIP) : retrieve full hierarchy

Bug : the element are not at the right location

src/ifcconvert/ColladaSerializer.cpp

@@ -29,12 +29,19 @@
 #include <COLLADASWBaseInputElement.h>
 #include <COLLADASWAsset.h>
 
-#include <boost/lexical_cast.hpp>
-
 #include <string>
 #include <cmath>
 
+#include <boost/lexical_cast.hpp>
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/triangular.hpp>
+#include <boost/numeric/ublas/lu.hpp>
+#include <boost/numeric/ublas/io.hpp>
+
 using namespace IfcSchema;
+using namespace boost::numeric::ublas;
 
 static void collada_id(std::string &s)
 {
@@ -181,7 +188,7 @@ void ColladaSerializer::ColladaExporter::ColladaGeometries::close() {
 
 void ColladaSerializer::ColladaExporter::ColladaScene::add(
     const std::string& node_id, const std::string& node_name, const std::string& geom_name,
-    const std::vector<std::string>& material_ids, const std::vector<real_t>& matrix)
+    const std::vector<std::string>& material_ids, const std::vector<real_t>& posmatrix)
 {
 	if (!scene_opened) {
 		openVisualScene(scene_id);
@@ -195,13 +202,42 @@ void ColladaSerializer::ColladaExporter::ColladaScene::add(
 
 	// The matrix attribute of an entity is basically a 4x3 representation of its ObjectPlacement.
 	// Note that this placement is absolute, ie it is multiplied with all parent placements.
+	
 	double matrix_array[4][4] = {
-        { (double)matrix[0], (double)matrix[3], (double)matrix[6], (double)matrix[ 9] },
-        { (double)matrix[1], (double)matrix[4], (double)matrix[7], (double)matrix[10] },
-        { (double)matrix[2], (double)matrix[5], (double)matrix[8], (double)matrix[11] },
+        { (double)posmatrix[0], (double)posmatrix[3], (double)posmatrix[6], (double)posmatrix[ 9] },
+        { (double)posmatrix[1], (double)posmatrix[4], (double)posmatrix[7], (double)posmatrix[10] },
+        { (double)posmatrix[2], (double)posmatrix[5], (double)posmatrix[8], (double)posmatrix[11] },
         { 0, 0, 0, 1 }
 	};
+	
+	// If this is not the first parent, get the relative placement
+	if (parentNodes.size() > 0)
+	{
+		double relative[4][4] = {
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 }
+		};
+		std::cout << "placement de " << node_name << " | utilisation de " << parentNodes.top()->getNodeName() << "\n";
+		// Multiplication
+		for (int i = 0; i < 4; ++i) {
+			for (int j = 0; j < 4; ++j) {
+				for (int k = 0; k < 4; ++k) {
+					relative[i][j] += matrix_array[i][k] * matrixStack.top()(k, j);
+				}
+			}
+		}
 
+		// Copy from relative to matrix_array
+		for (int i = 0; i < 4; i++) {
+			for (int j = 0; j < 4; j++) {
+				matrix_array[i][j] = relative[i][j];
+			}
+		}
+
+	}
+	
     matrix_array[0][3] += serializer->settings().offset[0];
     matrix_array[1][3] += serializer->settings().offset[1];
     matrix_array[2][3] += serializer->settings().offset[2];
@@ -228,33 +264,180 @@ void ColladaSerializer::ColladaExporter::ColladaScene::addParent(const IfcGeom::
 	}
 
 
-	const std::vector<real_t> matrix = parent.transformation().matrix().data();
+	const std::vector<real_t> parentMatrix = parent.transformation().matrix().data();
 	
 	double matrix_array[4][4] = {
-		{ (double)matrix[0], (double)matrix[3], (double)matrix[6], (double)matrix[9] },
-		{ (double)matrix[1], (double)matrix[4], (double)matrix[7], (double)matrix[10] },
-		{ (double)matrix[2], (double)matrix[5], (double)matrix[8], (double)matrix[11] },
+		{ (double)parentMatrix[0], (double)parentMatrix[3], (double)parentMatrix[6], (double)parentMatrix[9] },
+		{ (double)parentMatrix[1], (double)parentMatrix[4], (double)parentMatrix[7], (double)parentMatrix[10] },
+		{ (double)parentMatrix[2], (double)parentMatrix[5], (double)parentMatrix[8], (double)parentMatrix[11] },
 		{ 0, 0, 0, 1 }
 	};
+	
+	// =========
+	
+	// If this is not the first parent, get the relative placement
+	if (parentNodes.size() > 0)
+	{
+		double relative[4][4] = {
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 },
+			{ 0, 0, 0, 0 }
+		};
+		std::cout << "placement de " << parent.name() << "_" << parent.type() << " | utilisation de " << parentNodes.top()->getNodeName() << "\n";
+		// Multiplication
+		for (int i = 0; i < 4; ++i) {
+			for (int j = 0; j < 4; ++j) {
+				for (int k = 0; k < 4; ++k) {
+					relative[i][j] += matrix_array[i][k] * matrixStack.top()(k, j);
+				}
+			}
+		}
 
+		// TODO : Handle the case where there was no inverse
+
+		for (int i = 0; i < 4; i++) {
+			for (int j = 0; j < 4; j++) {
+				matrix_array[i][j] = relative[i][j];
+			}
+		}		
+	}
+	
 	//adding the offset to the matrix
-	matrix_array[0][3] += serializer->settings().offset[0];
-	matrix_array[1][3] += serializer->settings().offset[1];
-	matrix_array[2][3] += serializer->settings().offset[2];
+	//matrix_array[0][3] += serializer->settings().offset[0];
+	//matrix_array[1][3] += serializer->settings().offset[1];
+	//matrix_array[2][3] += serializer->settings().offset[2];
 	
 	
 	const std::string id = "representation-" + boost::lexical_cast<std::string>(parent.id());
 
+	COLLADASW::Node *current_node;
 	current_node = new COLLADASW::Node(mSW);
 	current_node->setNodeId(id);
-	current_node->setNodeName(parent.name());
+	current_node->setNodeName(parent.type() + " " + parent.name());
 	current_node->addMatrix(matrix_array);
 	current_node->setType(COLLADASW::Node::NODE);
 	current_node->start();
+
+	// ==== Inverse the matrix and save it ====
+
+	double m[4][4] = {
+		{ (double)parentMatrix[0], (double)parentMatrix[3], (double)parentMatrix[6], (double)parentMatrix[9] },
+		{ (double)parentMatrix[1], (double)parentMatrix[4], (double)parentMatrix[7], (double)parentMatrix[10] },
+		{ (double)parentMatrix[2], (double)parentMatrix[5], (double)parentMatrix[8], (double)parentMatrix[11] },
+		{ 0, 0, 0, 1 }
+	};
+
+	double det =
+		m[0][3] * m[1][2] * m[2][1] * m[3][0] - m[0][2] * m[1][3] * m[2][1] * m[3][0] - m[0][3] * m[1][1] * m[2][2] * m[3][0] + m[0][1] * m[1][3] * m[2][2] * m[3][0] +
+		m[0][2] * m[1][1] * m[2][3] * m[3][0] - m[0][1] * m[1][2] * m[2][3] * m[3][0] - m[0][3] * m[1][2] * m[2][0] * m[3][1] + m[0][2] * m[1][3] * m[2][0] * m[3][1] +
+		m[0][3] * m[1][0] * m[2][2] * m[3][1] - m[0][0] * m[1][3] * m[2][2] * m[3][1] - m[0][2] * m[1][0] * m[2][3] * m[3][1] + m[0][0] * m[1][2] * m[2][3] * m[3][1] +
+		m[0][3] * m[1][1] * m[2][0] * m[3][2] - m[0][1] * m[1][3] * m[2][0] * m[3][2] - m[0][3] * m[1][0] * m[2][1] * m[3][2] + m[0][0] * m[1][3] * m[2][1] * m[3][2] +
+		m[0][1] * m[1][0] * m[2][3] * m[3][2] - m[0][0] * m[1][1] * m[2][3] * m[3][2] - m[0][2] * m[1][1] * m[2][0] * m[3][3] + m[0][1] * m[1][2] * m[2][0] * m[3][3] +
+		m[0][2] * m[1][0] * m[2][1] * m[3][3] - m[0][0] * m[1][2] * m[2][1] * m[3][3] - m[0][1] * m[1][0] * m[2][2] * m[3][3] + m[0][0] * m[1][1] * m[2][2] * m[3][3];
+
+
+	if (det != 0)
+	{
+		double inverse[4][4];
+		inverse[0][0] = m[1][2] * m[2][3] * m[3][1] - m[1][3] * m[2][2] * m[3][1] + m[1][3] * m[2][1] * m[3][2] - m[1][1] * m[2][3] * m[3][2] - m[1][2] * m[2][1] * m[3][3] + m[1][1] * m[2][2] * m[3][3];
+		inverse[0][1] = m[0][3] * m[2][2] * m[3][1] - m[0][2] * m[2][3] * m[3][1] - m[0][3] * m[2][1] * m[3][2] + m[0][1] * m[2][3] * m[3][2] + m[0][2] * m[2][1] * m[3][3] - m[0][1] * m[2][2] * m[3][3];
+		inverse[0][2] = m[0][2] * m[1][3] * m[3][1] - m[0][3] * m[1][2] * m[3][1] + m[0][3] * m[1][1] * m[3][2] - m[0][1] * m[1][3] * m[3][2] - m[0][2] * m[1][1] * m[3][3] + m[0][1] * m[1][2] * m[3][3];
+		inverse[0][3] = m[0][3] * m[1][2] * m[2][1] - m[0][2] * m[1][3] * m[2][1] - m[0][3] * m[1][1] * m[2][2] + m[0][1] * m[1][3] * m[2][2] + m[0][2] * m[1][1] * m[2][3] - m[0][1] * m[1][2] * m[2][3];
+
+		inverse[1][0] = m[1][3] * m[2][2] * m[3][0] - m[1][2] * m[2][3] * m[3][0] - m[1][3] * m[2][0] * m[3][2] + m[1][0] * m[2][3] * m[3][2] + m[1][2] * m[2][0] * m[3][3] - m[1][0] * m[2][2] * m[3][3];
+		inverse[1][1] = m[0][2] * m[2][3] * m[3][0] - m[0][3] * m[2][2] * m[3][0] + m[0][3] * m[2][0] * m[3][2] - m[0][0] * m[2][3] * m[3][2] - m[0][2] * m[2][0] * m[3][3] + m[0][0] * m[2][2] * m[3][3];
+		inverse[1][2] = m[0][3] * m[1][2] * m[3][0] - m[0][2] * m[1][3] * m[3][0] - m[0][3] * m[1][0] * m[3][2] + m[0][0] * m[1][3] * m[3][2] + m[0][2] * m[1][0] * m[3][3] - m[0][0] * m[1][2] * m[3][3];
+		inverse[1][3] = m[0][2] * m[1][3] * m[2][0] - m[0][3] * m[1][2] * m[2][0] + m[0][3] * m[1][0] * m[2][2] - m[0][0] * m[1][3] * m[2][2] - m[0][2] * m[1][0] * m[2][3] + m[0][0] * m[1][2] * m[2][3];
+
+		inverse[2][0] = m[1][1] * m[2][3] * m[3][0] - m[1][3] * m[2][1] * m[3][0] + m[1][3] * m[2][0] * m[3][1] - m[1][0] * m[2][3] * m[3][1] - m[1][1] * m[2][0] * m[3][3] + m[1][0] * m[2][1] * m[3][3];
+		inverse[2][1] = m[0][3] * m[2][1] * m[3][0] - m[0][1] * m[2][3] * m[3][0] - m[0][3] * m[2][0] * m[3][1] + m[0][0] * m[2][3] * m[3][1] + m[0][1] * m[2][0] * m[3][3] - m[0][0] * m[2][1] * m[3][3];
+		inverse[2][2] = m[0][1] * m[1][3] * m[3][0] - m[0][3] * m[1][1] * m[3][0] + m[0][3] * m[1][0] * m[3][1] - m[0][0] * m[1][3] * m[3][1] - m[0][1] * m[1][0] * m[3][3] + m[0][0] * m[1][1] * m[3][3];
+		inverse[2][3] = m[0][3] * m[1][1] * m[2][0] - m[0][1] * m[1][3] * m[2][0] - m[0][3] * m[1][0] * m[2][1] + m[0][0] * m[1][3] * m[2][1] + m[0][1] * m[1][0] * m[2][3] - m[0][0] * m[1][1] * m[2][3];
+
+		inverse[3][0] = m[1][2] * m[2][1] * m[3][0] - m[1][1] * m[2][2] * m[3][0] - m[1][2] * m[2][0] * m[3][1] + m[1][0] * m[2][2] * m[3][1] + m[1][1] * m[2][0] * m[3][2] - m[1][0] * m[2][1] * m[3][2];
+		inverse[3][1] = m[0][1] * m[2][2] * m[3][0] - m[0][2] * m[2][1] * m[3][0] + m[0][2] * m[2][0] * m[3][1] - m[0][0] * m[2][2] * m[3][1] - m[0][1] * m[2][0] * m[3][2] + m[0][0] * m[2][1] * m[3][2];
+		inverse[3][2] = m[0][2] * m[1][1] * m[3][0] - m[0][1] * m[1][2] * m[3][0] - m[0][2] * m[1][0] * m[3][1] + m[0][0] * m[1][2] * m[3][1] + m[0][1] * m[1][0] * m[3][2] - m[0][0] * m[1][1] * m[3][2];
+		inverse[3][3] = m[0][1] * m[1][2] * m[2][0] - m[0][2] * m[1][1] * m[2][0] + m[0][2] * m[1][0] * m[2][1] - m[0][0] * m[1][2] * m[2][1] - m[0][1] * m[1][0] * m[2][2] + m[0][0] * m[1][1] * m[2][2];
+
+
+		for (int i = 0; i < 4; ++i) {
+			for (int j = 0; j < 4; ++j) {
+				inverse[i][j] /= det;
+			}
+		}
+
+		std::cout << " ===== TEST INVERSE " << parent.name() << "===== \n";
+		double test[4][4];
+
+		for (int i = 0; i < 4; ++i) {
+			for (int j = 0; j < 4; ++j) {
+				test[i][j] = 0;
+			}
+		}
+
+		for (int i = 0; i < 4; ++i) {
+			for (int j = 0; j < 4; ++j) {
+				for (int k = 0; k < 4; ++k) {
+					if (false)
+					{
+						std::cout << "test [" << i << "][" << j << "] += " << m[i][k] << " * " << inverse[k][j] << "\n";
+					}
+					test[i][j] += m[i][k] * inverse[k][j]; 
+				}
+			}
+		}
+
+		std::cout << "matrix array : \n";
+		std::cout << m[0][0] << " | " << m[0][1] << " | " << m[0][2] << " | " << m[0][3] << " \n ";
+		std::cout << m[1][0] << " | " << m[1][1] << " | " << m[1][2] << " | " << m[1][3] << " \n ";
+		std::cout << m[2][0] << " | " << m[2][1] << " | " << m[2][2] << " | " << m[2][3] << " \n ";
+		std::cout << m[3][0] << " | " << m[3][1] << " | " << m[3][2] << " | " << m[3][3] << " \n ";
+
+		std::cout << "det = " << det << "\n";
+
+		std::cout << "test array : \n";
+		std::cout << test[0][0] << " | " << test[0][1] << " | " << test[0][2] << " | " << test[0][3] << " \n ";
+		std::cout << test[1][0] << " | " << test[1][1] << " | " << test[1][2] << " | " << test[1][3] << " \n ";
+		std::cout << test[2][0] << " | " << test[2][1] << " | " << test[2][2] << " | " << test[2][3] << " \n ";
+		std::cout << test[3][0] << " | " << test[3][1] << " | " << test[3][2] << " | " << test[3][3] << " \n ";
+		
+		matrix<double> save(4, 4);
+		for (int i = 0; i < 4; i++) {
+			for (int j = 0; j < 4; j++) {
+				save(i, j) = inverse[i][j];
+			}
+		}
+		
+		matrixStack.push(save);
+	}
+	else
+	{
+		std::cout << "couldn't inverse the position matrix \n";
+	}
+
+	// Add the node to the parent stack
+	parentNodes.push(current_node);
+	serializer->parentStackId.push(parent.id());
+
 }
 
-void ColladaSerializer::ColladaExporter::ColladaScene::closeParent(){
-	if (current_node != NULL) { current_node->end(); }
+void ColladaSerializer::ColladaExporter::ColladaScene::closeParent()
+{
+	// Get the top element
+	COLLADASW::Node *current_node = parentNodes.top();
+
+	// Close the node
+	current_node->end();
+
+	// Remove it from the stack
+	parentNodes.pop();
+	matrixStack.pop();
+	serializer->parentStackId.pop();
+
+	// Free the memory
+	delete current_node;
+	current_node = NULL;
 }
 
 void ColladaSerializer::ColladaExporter::ColladaScene::write() {
@@ -350,7 +533,7 @@ void ColladaSerializer::ColladaExporter::write(const IfcGeom::TriangulationEleme
 	
 	const std::string name = serializer->settings().get(SerializerSettings::USE_ELEMENT_GUIDS) ?
 		o->guid() : (serializer->settings().get(SerializerSettings::USE_ELEMENT_NAMES) ?
-			o->name() : (serializer->settings().get(SerializerSettings::USE_ELEMENT_TYPES) ? o->type() + slabSuffix : o->unique_id()));
+			o->name() : (serializer->settings().get(SerializerSettings::USE_ELEMENT_TYPES) ? o->type() + std::to_string(o->id()) + slabSuffix : o->unique_id()));
 	const std::string representation_id = "representation-" + boost::lexical_cast<std::string>(o->geometry().id());
 	std::vector<std::string> material_references;
 	foreach(const IfcGeom::Material& material, mesh.materials()) {
@@ -363,9 +546,9 @@ void ColladaSerializer::ColladaExporter::write(const IfcGeom::TriangulationEleme
 		material_references.push_back(material_name);
 	}
 
-	DeferredObject defered = (serializer->settings().get(SerializerSettings::USE_ELEMENT_HIERARCHY) && o->storey() != NULL ?
+	DeferredObject defered = (serializer->settings().get(SerializerSettings::USE_ELEMENT_HIERARCHY) ?
 		DeferredObject(name, representation_id, o->type(), o->transformation().matrix().data(), mesh.verts(), mesh.normals(), 
-			mesh.faces(), mesh.edges(), mesh.material_ids(), mesh.materials(), material_references, mesh.uvs(), *(o->storey())) : 
+			mesh.faces(), mesh.edges(), mesh.material_ids(), mesh.materials(), material_references, mesh.uvs(), o->parents()) :
 		DeferredObject(name, representation_id, o->type(), o->transformation().matrix().data(), mesh.verts(), mesh.normals(),
 				mesh.faces(), mesh.edges(), mesh.material_ids(), mesh.materials(), material_references, mesh.uvs()));
 	deferreds.push_back(defered);
@@ -408,9 +591,11 @@ void ColladaSerializer::ColladaExporter::endDocument() {
 	std::set<std::string> geometries_written;
 
 	//if the setting USE_ELEMENT_HIERARCHY is in use, we sort the deferreds objects by their parents.
+	
 	if (use_hierarchy) {
 		std::sort(deferreds.begin(), deferreds.end());
 	}
+	
 	for (std::vector<DeferredObject>::const_iterator it = deferreds.begin(); it != deferreds.end(); ++it) {
 		if (geometries_written.find(it->representation_id) != geometries_written.end()) {
 			continue;
@@ -425,29 +610,62 @@ void ColladaSerializer::ColladaExporter::endDocument() {
 	for (std::vector<DeferredObject>::const_iterator it = deferreds.begin(); it != deferreds.end(); ++it){
 		const std::string object_name = it->unique_id;
 
-		// if the setting USE_ELEMENT_HIERARCHY is used
-		// And if "it" has not parent AND a parent node is opened, OR it has a parent AND another parent node is opened
-		if (use_hierarchy && ((it->parent == NULL && parent_id != -1) || (it->parent != NULL && parent_id != it->parent->id())))
+		/*
+		std::cout << "******************************\n";
+		std::cout << "== " << it->unique_id << " ==\n";
+		std::cout << " has " << it->parents.size() << " parents \n";
+		for (unsigned i = 0; i < it->parents.size(); i++)
 		{
-			//close the parent tag if one is already open.
-			if (is_parent_tag_opened){
-				scene.closeParent();
-			}
+			std::cout << "=== " << it->parents.at(i)->name() << " | " << it->parents.at(i)->id() << " ===\n";
+		}
+		*/
 
-			if (it->parent != NULL){
-				parent_id = it->parent->id();
-				scene.addParent(*it->parent);
-				is_parent_tag_opened = true;
+		// TODO : handle the case where a representation object has no parent (Can this really happen ?)
+		if (use_hierarchy)
+		{
+			unsigned parentsNumber = it->parents.size();
+			bool finished = false;
+			while (!finished)
+			{
+				// If we need to add a parent
+				if (serializer->parentStackId.size() <= parentsNumber)
+				{
+					if (serializer->parentStackId.empty()) { scene.addParent(*(it->parents.at(0))); }
+					else
+					{
+						unsigned diff = parentsNumber - serializer->parentStackId.size();
+
+						// If we have the wrong parent in the list
+						if (serializer->parentStackId.top() != it->parents.at(parentsNumber - diff - 1)->id())
+						{
+							scene.closeParent();
+						}
+						// So far we have the right parents, we just need to add the missing ones
+						else
+						{
+							for (unsigned i = parentsNumber - diff; i < parentsNumber; i++) { scene.addParent(*(it->parents.at(i))); }
+
+							// if diff == 0, we can leave the loop. In fact we have the right number of parents, and the last one is ok
+							if (diff == 0) { finished = true; }
+						}
+					}
+				}
+				// IF serializer->parentStackId.size() > parentsNumber
+				else
+				{
+					// Close the finished nodes. After this we get the first case (serializer->parentStackId.size() <= parentsNumber)
+					while (serializer->parentStackId.size() > parentsNumber) { scene.closeParent(); }
+				}
 			}
 		}
 		
         /// @todo redundant information using ID as both ID and Name, maybe omit Name or allow specifying what would be used as the name
 		scene.add(object_name, object_name, it->representation_id, it->material_references, it->matrix);
 	}
-	//close the last parent tag.
-	if (is_parent_tag_opened) { 
-		scene.closeParent();
-	};
+
+	//close the remaining parent tags.
+	while (serializer->parentStackId.size() > 0) { scene.closeParent(); }
+
 	scene.write();
 	stream.endDocument();
 }

src/ifcconvert/ColladaSerializer.h

@@ -45,10 +45,16 @@
 
 #include "../ifcconvert/GeometrySerializer.h"
 
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/io.hpp>
+
+
 class ColladaSerializer : public GeometrySerializer
 {
 	// TODO The vast amount of implement details of ColladaSerializer could be hidden to the cpp file.
 private:
+	std::stack<int> parentStackId;
+
 	class ColladaExporter
 	{
 	private:
@@ -79,7 +85,8 @@ private:
 
 			const std::string scene_id;
 			bool scene_opened;
-			COLLADASW::Node *current_node;
+			std::stack<COLLADASW::Node*> parentNodes;
+			std::stack<boost::numeric::ublas::matrix<double>> matrixStack;
 		public:
 			ColladaScene(const std::string& scene_id, COLLADASW::StreamWriter& stream, ColladaSerializer *_serializer)
 				: COLLADASW::LibraryVisualScenes(&stream)
@@ -91,6 +98,7 @@ private:
                 const std::vector<std::string>& material_ids, const std::vector<real_t>& matrix);
 			void addParent(const IfcGeom::Element<real_t>& parent);
 			void closeParent();
+			COLLADASW::Node* GetDirectParent();
 			void write();
             ColladaSerializer *serializer;
 		};
@@ -125,39 +133,37 @@ private:
             ColladaEffects effects;
 		};
 		class DeferredObject {
+		
 		friend bool operator < (const DeferredObject & def_obj1, const DeferredObject & def_obj2)
 		{
-			// Retrieve the parents of the objects to compare
-			const IfcGeom::Element<real_t>* parent1 = def_obj1.parent;
-			const IfcGeom::Element<real_t>* parent2 = def_obj2.parent;
-			
-			// If a parent is null
-			if (parent1 == NULL || parent2 == NULL)
+			/*
+			std::cout << "*************** COMPARE ***************\n";
+			std::cout << "range 1 : " << def_obj1.unique_id << "\n";
+			for (unsigned i = 0; i < def_obj1.parents.size(); i++)
 			{
-				bool res = (parent1 == NULL) ? true : false;
-				return res;
+				std::cout << "=== " << def_obj1.parents.at(i)->name() << " | " << def_obj1.parents.at(i)->id() << " ===\n";
 			}
-			// If both parent are not null
+			std::cout << "range 2 : " << def_obj2.unique_id << "\n";
+			for (unsigned i = 0; i < def_obj2.parents.size(); i++)
+			{
+				std::cout << "=== " << def_obj2.parents.at(i)->name() << " | " << def_obj2.parents.at(i)->id() << " ===\n";
+			}
+			*/
+			unsigned size = (def_obj1.parents.size() < def_obj2.parents.size() ? def_obj1.parents.size() : def_obj2.parents.size());
+			int cpt = 0;
+
+			// Skip the shared parents
+			while (cpt < size && *(def_obj1.parents.at(cpt)) == *(def_obj2.parents.at(cpt))) { cpt++; }
+
+			// If a parent list container the other one
+			if (cpt >= size) { return (def_obj1.parents.size() < def_obj2.parents.size() ? true : false); }
 			else
 			{
-				// Retrieve the IfcBuildingStorey
-				IfcSchema::IfcBuildingStorey* storey1 = (IfcSchema::IfcBuildingStorey*)parent1->product();
-				IfcSchema::IfcBuildingStorey* storey2 = (IfcSchema::IfcBuildingStorey*)parent2->product();
-
-				bool res = true;
-
-				// Check if the storeys both have an elevation value
-				if (storey1->hasElevation() && storey2->hasElevation())
-				{
-					// Use the elevation in order to sort
-					res = storey1->Elevation() > storey2->Elevation() ? true : false;
-				}
-				// If the evelations are not set, use the names to sort
-				else { res = parent1->name().compare(parent2->name()) > 0 ? true : false; }
-				
-				return res;
+				return *(def_obj1.parents.at(cpt)) < *(def_obj2.parents.at(cpt));
 			}
 		}
+
+
 		public:
 			std::string unique_id, representation_id, type;
 			std::vector<real_t> matrix;
@@ -169,11 +175,11 @@ private:
 			std::vector<IfcGeom::Material> materials;
 			std::vector<std::string> material_references;
             std::vector<real_t> uvs;
-			const IfcGeom::Element<real_t>* parent;
+			std::vector<const IfcGeom::Element<real_t>*> parents;
             DeferredObject(const std::string& unique_id, const std::string& representation_id, const std::string& type, const std::vector<real_t>& matrix,
                 const std::vector<real_t>& vertices, const std::vector<real_t>& normals, const std::vector<int>& faces,
                 const std::vector<int>& edges, const std::vector<int>& material_ids, const std::vector<IfcGeom::Material>& materials,
-                const std::vector<std::string>& material_references, const std::vector<real_t>& uvs, const IfcGeom::Element<real_t>& _parent)
+                const std::vector<std::string>& material_references, const std::vector<real_t>& uvs, const std::vector<const IfcGeom::Element<real_t>*>& parent)
 				: unique_id(unique_id)
 				, representation_id(representation_id)
 				, type(type)
@@ -186,7 +192,7 @@ private:
 				, materials(materials)
 				, material_references(material_references)
                 , uvs(uvs)
-				, parent(&_parent)
+				, parents(parent)
 			{
 				
 			}
@@ -207,9 +213,8 @@ private:
 				, materials(materials)
 				, material_references(material_references)
 				, uvs(uvs)
-				, parent(NULL)
 			{
-				
+				parents.clear();
 			}
 		};
 		COLLADABU::NativeString filename;

src/ifcconvert/XmlSerializer.cpp

@@ -110,6 +110,7 @@ boost::optional<std::string> format_attribute(const Argument* argument, IfcUtil:
 				IfcSchema::IfcLocalPlacement* placement = e->as<IfcSchema::IfcLocalPlacement>();
 				gp_Trsf trsf;
 				IfcGeom::Kernel kernel;
+				
 				if (kernel.convert(placement, trsf)) {
 					std::stringstream stream;
 					for (int i = 1; i < 5; ++i) {

src/ifcgeom/IfcGeomElement.h

@@ -81,8 +81,21 @@ namespace IfcGeom {
 		std::string _unique_id;
 		Transformation<P> _transformation;
         IfcSchema::IfcProduct* product_;
-		const Element<P>* _storey;
+		std::vector<const IfcGeom::Element<P>*> _parents;
 	public:
+
+		friend bool operator == (const Element<P> & element1, const Element<P> & element2)
+		{
+			//std::cout << " //// Compare == " << element1.name() << element1.id() << " | " << element1.type() << " and " << element2.name() << element2.id() << " | " << element2.type() << "\n";
+			return element1.id() == element2.id();
+		}
+
+		friend bool operator < (const Element<P> & element1, const Element<P> & element2)
+		{
+			//std::cout << " //// Compare < " << element1.name() << element1.id() << " | " << element1.type() << " and " << element2.name() << element2.id() << " | " << element2.type() << "\n";
+			return element1.id() < element2.id();
+		}
+
 		int id() const { return _id; }
 		int parent_id() const { return _parent_id; }
 		const std::string& name() const { return _name; }
@@ -92,16 +105,21 @@ namespace IfcGeom {
 		const std::string& unique_id() const { return _unique_id; }
 		const Transformation<P>& transformation() const { return _transformation; }
         IfcSchema::IfcProduct* product() const { return product_; }
-		const Element<P>* storey() const { return _storey; }
-		void SetFloor(const Element<P>* floor) { _storey = floor; }
+		const std::vector<const IfcGeom::Element<P>*> parents() const { return _parents; }
+		void SetParents(std::vector<const IfcGeom::Element<P>*> newparents) { _parents = newparents; }
 
 		Element(const ElementSettings& settings, int id, int parent_id, const std::string& name, const std::string& type,
             const std::string& guid, const std::string& context, const gp_Trsf& trsf, IfcSchema::IfcProduct *product)
 			: _id(id), _parent_id(parent_id), _name(name), _type(type), _guid(guid), _context(context), _transformation(settings, trsf)
             , product_(product)
-		{
+		{ 
 			std::ostringstream oss;
-			oss << "product-" << IfcParse::IfcGlobalId(guid).formatted();
+			try { oss << "product-" << IfcParse::IfcGlobalId(guid).formatted(); }
+			catch (std::exception e)
+			{
+				oss << "product-cannotfindId";
+			}
+
 			if (!_context.empty()) {
 				std::string ctx = _context;
 				std::transform(ctx.begin(), ctx.end(), ctx.begin(), ::tolower);
@@ -109,7 +127,6 @@ namespace IfcGeom {
 				oss << "-" << ctx;
 			}
 			_unique_id = oss.str();
-
 		}
 		virtual ~Element() {}
 	};

src/ifcgeom/IfcGeomIterator.h

@@ -574,14 +574,17 @@ namespace IfcGeom {
             else if (current_serialization) { ret = current_serialization; }
             else if (current_shape_model) { ret = current_shape_model; }
 
-			// If we want to organize the element by floors we need to get the floor of the element 
+			// If we want to organize the element considering their hierarchy
 			if (settings.get(IteratorSettings::SEARCH_FLOOR))
 			{
+				// We are going to build a vector with the element parents.
+				// First, create the parent vector
+				std::vector<const IfcGeom::Element<P>*> parents;
+				
 				// if the element has a parent
 				if (ret->parent_id() != -1)
 				{
 					const IfcGeom::Element<P>* parent_object = NULL;
-
 					bool hasParent = true;
 
 					// get the parent 
@@ -591,20 +594,28 @@ namespace IfcGeom {
 						hasParent = false;
 					}
 
-					// We need to find an IfcBuildingStorey in the parent
-					while (parent_object != NULL && parent_object->type() != "IfcBuildingStorey" && hasParent)
+					// Add the previously found parent to the vector
+					if (hasParent) parents.insert(parents.begin(), parent_object);
+					
+					// We need to find all the parents
+					while (parent_object != NULL && hasParent)
 					{
+						// Find the next parent
 						try { parent_object = getObject(parent_object->parent_id()); }
 						catch (std::exception e)
 						{
+							std::cout << e.what();
 							hasParent = false;
 						}
 
-						hasParent = hasParent && parent_object->parent_id() != 1;
+						// Add the previously found parent to the vector
+						if (hasParent) parents.insert(parents.begin(), parent_object);
+
+						hasParent = hasParent && parent_object->parent_id() != -1;
 					}
 
-					if (hasParent) { ret->SetFloor(parent_object); }
-					else { ret->SetFloor(NULL); } // Set it so null in order to know that we didn't found any IfcBuildingStorey type parent
+					// when done push the parent list in the Element object
+					ret->SetParents(parents);
 				}
 			}
 
@@ -619,38 +630,47 @@ namespace IfcGeom {
 		}
 
 		const Element<P>* getObject(int id) {
-
+			//std::cout << "1";
 			gp_Trsf trsf;
 			int parent_id = -1;
 			std::string instance_type, product_name, product_guid;
             IfcSchema::IfcProduct* ifc_product = 0;
-
+			//std::cout << "2";
 			try {
 				const IfcUtil::IfcBaseClass* ifc_entity = ifc_file->entityById(id);
 				instance_type = IfcSchema::Type::ToString(ifc_entity->type());
 				if ( ifc_entity->is(IfcSchema::Type::IfcProduct) ) {
 					ifc_product = (IfcSchema::IfcProduct*)ifc_entity;
-					
+					//std::cout << "3";
 					product_guid = ifc_product->GlobalId();
 					product_name = ifc_product->hasName() ? ifc_product->Name() : "";
 					
 					parent_id = -1;
 					try {
+						//std::cout << "4";
 						IfcSchema::IfcObjectDefinition* parent_object = kernel.get_decomposing_entity(ifc_product);
 						if (parent_object) {
 							parent_id = parent_object->entity->id();
 						}
 					} catch (...) {}
-					
+					//std::cout << "5";
 					try {
 						kernel.convert(ifc_product->ObjectPlacement(), trsf);
 					} catch (...) {}
 				}
+				//std::cout << "6";
 			} catch(...) {}
-
+			//std::cout << "7";
 			ElementSettings element_settings(settings, unit_magnitude, instance_type);
+
+			//std::cout << "8";
+			//std::cout << "id " << id << "\n";
+			//std::cout << "parent_id " << parent_id << "\n";
+			//std::cout << "product_name " << product_name << "\n";
+			//std::cout << "instance_type " << instance_type << "\n";
+			//std::cout << "product_guid " << product_guid << "\n";
 			Element<P>* ifc_object = new Element<P>(element_settings, id, parent_id, product_name, instance_type, product_guid, "", trsf, ifc_product);
-			
+			//std::cout << "9\n";
 			return ifc_object;
 		}