From 2e7c43f59017273d43923000eda98838c8911e1f Mon Sep 17 00:00:00 2001
From: Thomas Krijnen <t.krijnen@gmail.com>
Date: Sun, 1 Sep 2019 10:36:49 +0200
Subject: [PATCH] Work on placements

---
 src/ifcgeom/schema/mapping.cpp | 73 ++++++++++++++++++++++++++++++----
 src/ifcgeom/schema/mapping.h   |  3 +-
 src/ifcgeom/taxonomy.h         | 12 +++++-
 3 files changed, 78 insertions(+), 10 deletions(-)

diff --git a/src/ifcgeom/schema/mapping.cpp b/src/ifcgeom/schema/mapping.cpp
index d057166367..caedda2c6f 100644
--- a/src/ifcgeom/schema/mapping.cpp
+++ b/src/ifcgeom/schema/mapping.cpp
@@ -236,33 +236,90 @@ taxonomy::item* mapping::map_impl(const IfcSchema::IfcProduct* inst) {
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcAxis2Placement3D* inst) {
-	// @todo length unit
-	return new taxonomy::matrix4();
+	Eigen::Vector3d o, axis(0, 0, 1), refDirection, X(1, 0, 0);
+	{
+		taxonomy::point3 v = as<taxonomy::point3>(map(inst->Location()));
+		o = v.components;
+	}
+	const bool hasAxis = inst->hasAxis();
+	const bool hasRef = inst->hasRefDirection();
+
+	if (hasAxis != hasRef) {
+		Logger::Warning("Axis and RefDirection should be specified together", inst);
+	}
+
+	if (hasAxis) {
+		taxonomy::point3 v = as<taxonomy::point3>(map(inst->Axis()));
+		axis = v.components;
+	}
+
+	if (hasRef) {
+		taxonomy::point3 v = as<taxonomy::point3>(map(inst->RefDirection()));
+		refDirection = v.components;
+	} else {
+		if (acos(axis.dot(X)) > 1.e-5) {
+			refDirection = { 1., 0., 0. };
+		} else {
+			refDirection = { 0., 0., 1. };
+		}
+		auto Xvec = axis.dot(refDirection) * axis;
+		auto Xaxis = refDirection - Xvec;
+		refDirection = Xaxis;
+	}
+	return new taxonomy::matrix4(o, axis, refDirection);
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcCartesianTransformationOperator2DnonUniform* inst) {
-	// @todo length unit
+	// @todo
 	return new taxonomy::matrix4();
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcCartesianTransformationOperator3DnonUniform* inst) {
-	// @todo length unit
+	// @todo
 	return new taxonomy::matrix4();
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcCartesianTransformationOperator2D* inst) {
-	// @todo length unit
+	// @todo
 	return new taxonomy::matrix4();
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcCartesianTransformationOperator3D* inst) {
-	// @todo length unit
+	// @todo
 	return new taxonomy::matrix4();
 }
 
 taxonomy::item* mapping::map_impl(const IfcSchema::IfcLocalPlacement* inst) {
-	// @todo length unit
-	return new taxonomy::matrix4();
+	IfcSchema::IfcLocalPlacement* current = (IfcSchema::IfcLocalPlacement*)inst;
+	auto m4 = new taxonomy::matrix4;
+	for (;;) {
+		IfcSchema::IfcAxis2Placement* relplacement = current->RelativePlacement();
+		if (relplacement->declaration().is(IfcSchema::IfcAxis2Placement3D::Class())) {
+			taxonomy::matrix4 trsf2 = as<taxonomy::matrix4>(map(relplacement));
+			// @todo check
+			m4->components = trsf2.components * m4->components;
+		}
+		if (current->hasPlacementRelTo()) {
+			IfcSchema::IfcObjectPlacement* parent = current->PlacementRelTo();
+			IfcSchema::IfcProduct::list::ptr parentPlaces = parent->PlacesObject();
+			bool parentPlacesType = false;
+			for (IfcSchema::IfcProduct::list::it iter = parentPlaces->begin();
+				iter != parentPlaces->end(); ++iter) {
+				if ((*iter)->declaration().is(*placement_rel_to_)) {
+					parentPlacesType = true;
+				}
+			}
+			if (parentPlacesType) {
+				break;
+			} else if (parent->declaration().is(IfcSchema::IfcLocalPlacement::Class())) {
+				current = (IfcSchema::IfcLocalPlacement*)current->PlacementRelTo();
+			} else {
+				break;
+			}
+		} else {
+			break;
+		}
+	}
 }
 
 IfcSchema::IfcProduct::list::ptr mapping::products_represented_by(const IfcSchema::IfcRepresentation* representation) {
diff --git a/src/ifcgeom/schema/mapping.h b/src/ifcgeom/schema/mapping.h
index 150196122c..c3334a4e61 100644
--- a/src/ifcgeom/schema/mapping.h
+++ b/src/ifcgeom/schema/mapping.h
@@ -18,10 +18,11 @@ namespace geometry {
 		IfcParse::IfcFile* file_;
 		double length_unit_, angle_unit_;
 		std::string length_unit_name_;
+		const IfcParse::declaration* placement_rel_to_;
 
 		void initialize_units_();
 	public:
-		POSTFIX_SCHEMA(mapping)(IfcParse::IfcFile* file) : file_(file) {
+		POSTFIX_SCHEMA(mapping)(IfcParse::IfcFile* file) : file_(file), placement_rel_to_(0) {
 			initialize_units_();
 		}
 		virtual ifcopenshell::geometry::taxonomy::item* map(const IfcUtil::IfcBaseClass*);
diff --git a/src/ifcgeom/taxonomy.h b/src/ifcgeom/taxonomy.h
index b657d4ae32..8ed21d8dd2 100644
--- a/src/ifcgeom/taxonomy.h
+++ b/src/ifcgeom/taxonomy.h
@@ -45,8 +45,18 @@ struct matrix4 : public item {
 	
 	Eigen::Matrix4d components;
 
+	matrix4() : components(Eigen::Matrix4d::Identity()), tag(IDENTITY) {}
 	matrix4(const Eigen::Matrix4d& c) : components(c), tag(OTHER) {}
-    matrix4() : components(Eigen::Matrix4d::Identity()), tag(IDENTITY) {}
+	matrix4(const Eigen::Vector3d& o, const Eigen::Vector3d& z, const Eigen::Vector3d& x) : tag(AFFINE_WO_SCALE) {
+		auto X = x.normalized();
+		auto Y = z.cross(x).normalized();
+		auto Z = z.normalized();
+		components << 
+			X(0), Y(0), Z(0), o(0),
+			X(1), Y(1), Z(1), o(0),
+			X(2), Y(2), Z(2), o(0),
+			0, 0, 0, 1.;
+	}
 
 	virtual item* clone() const { return new matrix4(*this); }
 	virtual kinds kind() const { return MATRIX4; }