diff --git a/src/ifcgeom/mapping/IfcAxis2PlacementLinear.cpp b/src/ifcgeom/mapping/IfcAxis2PlacementLinear.cpp index 836a6391f8..212ca144b7 100644 --- a/src/ifcgeom/mapping/IfcAxis2PlacementLinear.cpp +++ b/src/ifcgeom/mapping/IfcAxis2PlacementLinear.cpp @@ -29,34 +29,26 @@ taxonomy::ptr mapping::map_impl(const IfcSchema::IfcAxis2PlacementLinear* inst) Logger::Error(std::runtime_error("Location must be IfcPointByDistanceExpression for IfcAxis2PlacementLinear")); } - Eigen::Vector3d o, axis(0, 0, 1), refDirection; - taxonomy::matrix4::ptr m = taxonomy::cast(map(inst->Location())); - o = m->components().col(3).head<3>(); + Eigen::Vector3d o = m->components().col(3).head<3>(); // From 8.9.3.4 IfcAxis2PlacementLinear there are 4 cases that need to be considered // 1) Axis is given but not RefDirection // 2) RefDirection is given but not Axis // 3) Neither Axis or RefDirection are provided // 4) Both Axis and RefDirection are provided + // When Axis or RefDirection are not provided explicitly in the IfcAxis2PlacementLinear, + // they are taken from the context of curve by evaluating the Location, which is an IfcPointByDistanceExpression on a curve. + // The curve tangent is used as RefDirection and the curve normal is used as Axis. const bool hasAxis = inst->Axis() != nullptr; const bool hasRef = inst->RefDirection() != nullptr; - /* - if (hasAxis != hasRef) { - Logger::Warning("Axis and RefDirection should be specified together", inst); - } - */ - + Eigen::Vector3d axis, refDirection; if (hasAxis && !hasRef) { taxonomy::direction3::ptr a = taxonomy::cast(map(inst->Axis())); axis = *a->components_; - refDirection = m->components().col(0).head<3>(); // RefDirection is the curve tangent when omitted - // refDirection is not necessarily orthogonal to axis. - // axis.cross(refDirection) gives y. y.cross(axis) gives x=refDirection - refDirection = axis.cross(refDirection).cross(axis); } else if (!hasAxis && hasRef) { taxonomy::direction3::ptr r = taxonomy::cast(map(inst->RefDirection())); refDirection = *r->components_; @@ -67,13 +59,10 @@ taxonomy::ptr mapping::map_impl(const IfcSchema::IfcAxis2PlacementLinear* inst) } else { taxonomy::direction3::ptr a = taxonomy::cast(map(inst->Axis())); axis = *a->components_; - taxonomy::direction3::ptr r = taxonomy::cast(map(inst->RefDirection())); refDirection = *r->components_; - refDirection = axis.cross(refDirection).cross(axis); // refDirection needs to be orthogonal to axis } - // axis and refDirection need to be orthogonal return taxonomy::make(o, axis, refDirection); } diff --git a/src/ifcgeom/taxonomy.h b/src/ifcgeom/taxonomy.h index c1162eebb0..7a9a4325ca 100644 --- a/src/ifcgeom/taxonomy.h +++ b/src/ifcgeom/taxonomy.h @@ -277,20 +277,40 @@ typedef item const* ptr; struct IFC_GEOM_API matrix4 : public item, public eigen_base { private: void init(const Eigen::Vector3d& o, const Eigen::Vector3d& z, const Eigen::Vector3d& x) { - auto Z = z.normalized(); - auto Y = Z.cross(x).normalized(); - auto X = Y.cross(Z); - components_ = new Eigen::Matrix4d; - (*components_) << - X(0), Y(0), Z(0), o(0), - X(1), Y(1), Z(1), o(1), - X(2), Y(2), Z(2), o(2), - 0, 0, 0, 1.; - if (is_identity()) { - // @todo detect this earlier to save us the heapalloc. - delete components_; - components_ = nullptr; - tag = IDENTITY; + auto valid_and_non_zero_extent = [](const Eigen::Vector3d&v,double eps = 1.e-9) { + return v.allFinite() && v.squaredNorm() > eps*eps; + }; + + // check z and x are valid vectors + if (!valid_and_non_zero_extent(z) || !valid_and_non_zero_extent(x)) { + throw std::runtime_error("Invalid during for matrix4 construction"); + } + + auto Z = z.normalized(); // ensure Z is a unit vector + auto Y = Z.cross(x); // Y is orthogonal to x and Z + + // make sure Y is a valid vector + if (!valid_and_non_zero_extent(Y)) { + throw std::runtime_error("Parallel or degenerate matrix4 construction"); + } + + Y.normalize(); // ensure Y is a unit vector + auto X = Y.cross(Z); // X is orthogonal to Y and Z, and a unit vector + + // check if this is going to be an identity matrix + double eps = 1.e-9; + if (X.isApprox(Eigen::Vector3d(1., 0., 0.), eps) && + Y.isApprox(Eigen::Vector3d(0., 1., 0.), eps) && + Z.isApprox(Eigen::Vector3d(0., 0., 1.), eps) && + o.isApprox(Eigen::Vector3d(0., 0., 0.), eps)) // in the full 4x4 matrix, this will be (0,0,0,1) + { + tag = IDENTITY; + } else { + components_ = new Eigen::Matrix4d; + (*components_) << X(0), Y(0), Z(0), o(0), + X(1), Y(1), Z(1), o(1), + X(2), Y(2), Z(2), o(2), + 0, 0, 0, 1.; } } public: