Adds support for IfcCosineSpiral, IfcSineSpiral, and updates implementation to match results from bSI Railway Room test cases.

src/ifcgeom/mapping/IfcCurveSegment.cpp

@@ -319,6 +319,12 @@ typedef boost::mpl::vector<
 #ifdef SCHEMA_HAS_IfcClothoid
    , IfcSchema::IfcClothoid
 #endif
+#if defined SCHEMA_HAS_IfcCosineSpiral
+   , IfcSchema::IfcCosineSpiral
+#endif
+#if defined SCHEMA_HAS_IfcSineSpiral
+    , IfcSchema::IfcSineSpiral
+#endif
 #if defined SCHEMA_HAS_IfcSecondOrderPolynomialSpiral
    , IfcSchema::IfcSecondOrderPolynomialSpiral
 #endif
@@ -328,6 +334,7 @@ typedef boost::mpl::vector<
 #if defined SCHEMA_HAS_IfcSeventhOrderPolynomialSpiral
     , IfcSchema::IfcSeventhOrderPolynomialSpiral
 #endif
+
    , IfcSchema::IfcPolyline
    , IfcSchema::IfcCircle
    , IfcSchema::IfcPolynomialCurve
@@ -397,20 +404,19 @@ class curve_segment_evaluator {
         if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) {
             auto start = start_;
             auto segment_type = segment_type_;
-            auto transformation_matrix = taxonomy::cast<taxonomy::matrix4>(mapping_->map(c->Position()))->ccomponents();
             geometry_adjuster = std::make_shared<GEOMETRY_ADJUSTER>(mapping_, segment_type_, inst_, next_inst_);
             using boost::math::quadrature::trapezoidal;
-            auto start_x = trapezoidal(fnX, 0.0, start / s);
+            auto start_x = s ? trapezoidal(fnX, 0.0, start / s) : 0.0;
-            auto start_y = trapezoidal(fnY, 0.0, start / s);
+            auto start_y = s ? trapezoidal(fnY, 0.0, start / s) : 0.0;
-            auto start_dx = fnX(start / s)/s;
+            auto start_dx = s ? fnX(start / s)/s : 0.0;
-            auto start_dy = fnY(start / s)/s;
+            auto start_dy = s ? fnY(start / s)/s : 0.0;
-            eval_ = [start, s, start_x, start_y, start_dx,start_dy,fnX, fnY, transformation_matrix, segment_type, geometry_adjuster = this->geometry_adjuster](double u) {
+            eval_ = [start, s, start_x, start_y, start_dx,start_dy,fnX, fnY, segment_type, geometry_adjuster = this->geometry_adjuster](double u) {
 
                 u += start;
 
                 // integration limits, integrate from a to b
                 auto a = 0.0;
-                auto b = u / s;
+                auto b = s ? u / s : 0.0;
 
                 auto x = trapezoidal(fnX, a, b) - start_x;
                 auto y = trapezoidal(fnY, a, b) - start_y;
@@ -422,8 +428,8 @@ class curve_segment_evaluator {
 
                 // From https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcSpiral.htm, x = Integral(fnX du), y = Integral(fnY du)
                 // The tangent slope of a curve is the derivate of the curve, so the derivitive of an integral, is just the function
-                auto dx = fnX(b)/s;
+                auto dx = s ? fnX(b)/s : 1.0;
-                auto dy = fnY(b)/s;
+                auto dy = s ? fnY(b)/s : 0.0;
 
                 // rotate about the Z-axis
                 Eigen::Matrix4d m;
@@ -431,8 +437,7 @@ class curve_segment_evaluator {
                 m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); // vector perpendicular to the curve, towards the left when looking from start to end along the curve (this is used for IfcAxis2PlacementLinear.RefDirection when it is not provided)
                 m.col(2) = Eigen::Vector4d(0, 0, 1.0, 0);  // cross product of x and y and will always be up (this is used for IfcAxis2PlacementLinear.Axis when it is not provided)
                 m.col(3) = Eigen::Vector4d(x, y, 0.0, 1.0);
-                Eigen::Matrix4d result = transformation_matrix * m;
+                return geometry_adjuster->transform_and_adjust(u,m);
-                return geometry_adjuster->transform_and_adjust(u,result);
             };
         }
         else if (segment_type_ == ST_CANT) {
@@ -461,91 +466,62 @@ class curve_segment_evaluator {
       auto A = c->ClothoidConstant();
       auto s = fabs(A * sqrt(PI)); // curve length when u = 1.0
 
-      auto fn_x = [A, s](double t) -> double { return s * cos(PI * A * t * t / (2 * fabs(A))); };
+      auto fn_x = [A, s](double t) -> double { return A ? s * cos(PI * A * t * t / (2 * fabs(A))) : 0.0; };
-      auto fn_y = [A, s](double t) -> double { return s * sin(PI * A * t * t / (2 * fabs(A))); };
+      auto fn_y = [A, s](double t) -> double { return A ? s * sin(PI * A * t * t / (2 * fabs(A))) : 0.0; };
 
       set_spiral_function(mapping_, c, s, fn_x, fn_y);
    }
 #endif
 
-    void polynomial_spiral(const IfcSchema::IfcSpiral* c, boost::optional<double> A0, boost::optional<double> A1, boost::optional<double> A2, boost::optional<double> A3, boost::optional<double> A4, boost::optional<double> A5, boost::optional<double> A6, boost::optional<double> A7) {
+#if defined SCHEMA_HAS_IfcCosineSpiral
-      auto theta = [A0, A1, A2, A3, A4, A5, A6, A7](double t) {
+   void operator()(const IfcSchema::IfcCosineSpiral* c) {
+      auto const_term = c->ConstantTerm();
+      auto cos_term = c->CosineTerm();
+
+      auto theta = [const_term, cos_term](double t) -> double {
+          auto ct = const_term.get_value_or(0);
+          return ct + cos_term * sin(t);
+      };
+      auto fn_x = [theta](double t) -> double { return cos(theta(t)); };
+      auto fn_y = [theta](double t) -> double { return sin(theta(t)); };
+      double s = 1.0;
+      set_spiral_function(mapping_, c, s, fn_x, fn_y);
+   }
+#endif
+
+#if defined SCHEMA_HAS_IfcSineSpiral
+   void operator()(const IfcSchema::IfcSineSpiral* c) {
+      auto const_term = c->ConstantTerm();
+      auto cos_term = c->SineTerm();
+
+      auto theta = [const_term, cos_term](double t) -> double {
+          auto ct = const_term.get_value_or(0);
+          return ct + cos_term * cos(t);
+      };
+      auto fn_x = [theta](double t) -> double { return cos(theta(t)); };
+      auto fn_y = [theta](double t) -> double { return sin(theta(t)); };
+      double s = 1.0;
+      set_spiral_function(mapping_, c, s, fn_x, fn_y);
+   }
+#endif
+
+    void polynomial_spiral(const IfcSchema::IfcSpiral* c, double lu, boost::optional<double> A0, boost::optional<double> A1, boost::optional<double> A2, boost::optional<double> A3, boost::optional<double> A4, boost::optional<double> A5, boost::optional<double> A6, boost::optional<double> A7) {
+      auto theta = [A0, A1, A2, A3, A4, A5, A6, A7, lu](double t) {
           auto a0 = A0.has_value() ? t / A0.value() : 0.0;
-          auto a1 = A1.has_value() ? A1.value() * std::pow(t, 2) / (2 * fabs(std::pow(A1.value(), 3))) : 0.0;
+          auto a1 = A1.has_value() ? A1.value() * lu * std::pow(t, 2) / (2 * fabs(std::pow(A1.value() * lu, 3))) : 0.0;
-          auto a2 = A2.has_value() ? std::pow(t, 3) / (3 * std::pow(A2.value(), 3)) : 0.0;
+          auto a2 = A2.has_value() ? std::pow(t, 3) / (3 * std::pow(A2.value() * lu, 3)) : 0.0;
-          auto a3 = A3.has_value() ? A3.value() * std::pow(t, 4) / (4 * fabs(std::pow(A3.value(), 5))) : 0.0;
+          auto a3 = A3.has_value() ? A3.value() * lu * std::pow(t, 4) / (4 * fabs(std::pow(A3.value() * lu, 5))) : 0.0;
-          auto a4 = A4.has_value() ? std::pow(t, 5) / (5 * std::pow(A4.value(), 5)) : 0.0;
+          auto a4 = A4.has_value() ? std::pow(t, 5) / (5 * std::pow(A4.value() * lu, 5)) : 0.0;
-          auto a5 = A5.has_value() ? A5.value() * std::pow(t, 6) / (6 * fabs(std::pow(A5.value(), 7))) : 0.0;
+          auto a5 = A5.has_value() ? A5.value() * lu * std::pow(t, 6) / (6 * fabs(std::pow(A5.value() * lu, 7))) : 0.0;
-          auto a6 = A6.has_value() ? std::pow(t, 7) / (7 * std::pow(A6.value(), 7)) : 0.0;
+          auto a6 = A6.has_value() ? std::pow(t, 7) / (7 * std::pow(A6.value() * lu, 7)) : 0.0;
-          auto a7 = A7.has_value() ? A7.value() * std::pow(t, 8) / (8 * fabs(std::pow(A7.value(), 9))) : 0.0;
+          auto a7 = A7.has_value() ? A7.value() * lu * std::pow(t, 8) / (8 * fabs(std::pow(A7.value() * lu, 9))) : 0.0;
           return a0 + a1 + a2 + a3 + a4 + a5 + a6 + a7;
       };
 
-      // find the curve length when u = 1.0 (there doesn't seem to be a closed form equation for this so do it numerically).
+      auto fn_x = [theta](double t) -> double { return cos(theta(t)); };
-      // u = 1.0 when theta = PI/2... do a root finding for theta-PI/2 = 0
+      auto fn_y = [theta](double t) -> double { return sin(theta(t)); };
-      boost::uintmax_t max_iter = 500;
-      auto iter = max_iter;
-      double eps = 0.000001;
-      auto tol = [eps](const auto& a, const auto& b) { return std::fabs(b - a) < eps; };
-      // guess the solution by using the highest order term in the theta equation.
-      // the term is in the form k*t^n
-      // solve k*t^n = PI/2
-      // t = nth root of (PI/(2*k)) = std::pow((PI/(2*fabs(k)), 1.0/n);
-      // use abs(k) because depending on the direction of the curve we seek t when theta = PI/2 or -PI/2
-      double k = fabs(length());
-      double n = 1.0;
-      if (A7.has_value()) {
-            auto a7 = A7.value();
-            k = a7 / (8 * std::abs(std::pow(a7, 9)));
-            n = 8;
-      } else if (A6.has_value()) {
-            auto a6 = A6.value();
-            k = 1 / (7 * std::pow(a6, 7));
-            n = 7;
-      } else if (A5.has_value()) {
-            auto a5 = A5.value();
-            k = a5 / (6 * std::fabs(std::pow(a5, 7)));
-            n = 6;
-      } else if (A4.has_value()) {
-            auto a4 = A4.value();
-            k = 1. / (5 * std::pow(a4, 5));
-            n = 5;
-      } else if (A3.has_value()) {
-            auto a3 = A3.value();
-            k = a3 / (4 * std::fabs(std::pow(a3, 5)));
-            n = 4;
-      } else if (A2.has_value()) {
-            auto a2 = A2.value();
-            k = 1. / (3 * std::pow(a2, 3));
-            n = 3;
-      } else if (A1.has_value()) {
-            auto a1 = A1.value();
-            k = a1 / (2 * std::fabs(std::pow(a1, 3)));
-            n = 2;
-      } else if (A0.has_value()) {
-            auto a0 = A0.value();
-            k = 1 / a0;
-            n = 1;
-      }
-      auto guess = std::pow(PI / (2 * fabs(k)), 1. / n);
-
-      std::pair<double, double> result;
-      try {
-            auto sign_of_k = sign(k);
-            result = boost::math::tools::bracket_and_solve_root([sign_of_k,theta](double x) { return (sign_of_k*theta(x) - PI / 2.0); }, guess, 2.0, true, tol, iter);
-      } catch (const std::exception& e) {
-            Logger::Warning(std::string(e.what()));
-      }
-
-      if (iter == max_iter) {
-            Logger::Warning(std::string("bracket_and_solve_root did not converge"));
-      }
-
-      double s = result.first;
-
-      auto fn_x = [s, theta](double t) -> double { return s*cos(theta(s*t)); };
-      auto fn_y = [s, theta](double t) -> double { return s*sin(theta(s*t)); };
 
+      double s = 1.0;
       set_spiral_function(mapping_, c, s, fn_x, fn_y);
     }
 
@@ -556,7 +532,7 @@ class curve_segment_evaluator {
       auto A1 = c->LinearTerm();
       auto A2 = c->QuadraticTerm();
       boost::optional<double> A3, A4, A5, A6, A7;
-      polynomial_spiral(c, A0, A1, A2, A3, A4, A5, A6, A7);
+      polynomial_spiral(c, length_unit_, A0, A1, A2, A3, A4, A5, A6, A7);
     }
 #endif
 
@@ -567,7 +543,7 @@ class curve_segment_evaluator {
         auto A2 = c->QuadraticTerm();
         auto A3 = c->CubicTerm();
         boost::optional<double> A4, A5, A6, A7;
-        polynomial_spiral(c, A0, A1, A2, A3, A4, A5, A6, A7);
+        polynomial_spiral(c, length_unit_, A0, A1, A2, A3, A4, A5, A6, A7);
     }
 #endif
 
@@ -582,7 +558,7 @@ class curve_segment_evaluator {
         auto A6 = c->SexticTerm();
         auto A7 = c->SepticTerm();
 
-        polynomial_spiral(c, A0, A1, A2, A3, A4, A5, A6, A7);
+        polynomial_spiral(c, length_unit_, A0, A1, A2, A3, A4, A5, A6, A7);
     }
 #endif
 
@@ -596,13 +572,11 @@ class curve_segment_evaluator {
       auto start_x = R * cos(start_angle);
       auto start_y = R * sin(start_angle);
 
-		auto transformation_matrix = taxonomy::cast<taxonomy::matrix4>(mapping_->map(c->Position()))->ccomponents();
-
       auto segment_type = segment_type_;
 
       geometry_adjuster = std::make_shared<GEOMETRY_ADJUSTER>(mapping_, segment_type_, inst_, next_inst_);
 
-		eval_ = [R, start_x, start_y, start_angle, sign_l, transformation_matrix, segment_type, geometry_adjuster = this->geometry_adjuster](double u)
+      eval_ = [R, start_x, start_y, start_angle, sign_l, segment_type, geometry_adjuster = this->geometry_adjuster](double u)
          {
             auto angle = start_angle + sign_l * u / R;
 
@@ -612,7 +586,7 @@ class curve_segment_evaluator {
             auto x = R * dx - start_x;
             auto y = R * dy - start_y;
 
-            Eigen::Matrix4d m = Eigen::Matrix4d::Identity();
+            Eigen::Matrix4d m;
             if (segment_type == ST_HORIZONTAL || segment_type == ST_VERTICAL) {
                 // rotate about the Z-axis
                 m.col(0) = Eigen::Vector4d(dx, dy, 0, 0);
@@ -622,13 +596,14 @@ class curve_segment_evaluator {
             }
             else if (segment_type == ST_CANT) {
                 Logger::Warning(std::runtime_error("Use of IfcCircle for cant is not supported"));
+                m = Eigen::Matrix4d::Identity();
             } else {
                 Logger::Error(std::runtime_error("Unexpected segment type encountered"));
+                m = Eigen::Matrix4d::Identity();
             }
 
 
-            Eigen::Matrix4d result = transformation_matrix * m;
+            return geometry_adjuster->transform_and_adjust(u, m);
-            return geometry_adjuster->transform_and_adjust(u, result);
          };
    }
 
@@ -694,7 +669,7 @@ class curve_segment_evaluator {
                 auto x = segment_type == ST_HORIZONTAL ? p1x + u * dx : u;
                 auto y = p1y + u * dy;
 
-                Eigen::Matrix4d m = Eigen::Matrix4d::Identity();
+                Eigen::Matrix4d m;
                 if (segment_type == ST_HORIZONTAL) {
                     // rotate about the Z-axis
                     m.col(0) = Eigen::Vector4d(dx, dy, 0, 0);  // vector tangent to the curve, in the direction of the curve
@@ -709,8 +684,10 @@ class curve_segment_evaluator {
                     m.col(3) = Eigen::Vector4d(0, 0, y, 1.0); // y is an elevation so store it as z
                 } else if (segment_type == ST_CANT) {
                     Logger::Warning(std::runtime_error("Use of IfcPolyline for cant is not supported"));
+                    m = Eigen::Matrix4d::Identity();
                 } else {
                     Logger::Error(std::runtime_error("Unexpected segment type encountered"));
+                    m = Eigen::Matrix4d::Identity();
                 }
 
                 return m;
@@ -722,7 +699,6 @@ class curve_segment_evaluator {
          u = u + l;
       }
      
-		
          geometry_adjuster = std::make_shared<GEOMETRY_ADJUSTER>(mapping_, segment_type_, inst_, next_inst_);
 
          eval_ = [fns, geometry_adjuster = this->geometry_adjuster](double u) {
@@ -771,7 +747,7 @@ class curve_segment_evaluator {
             auto x = px + u * dx;
             auto y = py + u * dy;
 
-            Eigen::Matrix4d m = Eigen::Matrix4d::Identity();
+            Eigen::Matrix4d m;
             m.col(0) = Eigen::Vector4d(dx, dy, 0, 0);  // vector tangent to the curve, in the direction of the curve
             m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); // vector perpendicular to the curve, towards the left when looking from start to end along the curve (this is used for IfcAxis2PlacementLinear.RefDirection when it is not provided)
             m.col(2) = Eigen::Vector4d(0, 0, 1.0, 0);  // cross product of x and y and will always be up (this is used for IfcAxis2PlacementLinear.Axis when it is not provided)
@@ -802,15 +778,13 @@ class curve_segment_evaluator {
 
 
 
-		auto transformation_matrix = taxonomy::cast<taxonomy::matrix4>(mapping_->map(p->Position()))->ccomponents();
-
       auto segment_type = segment_type_;
       auto length_unit = length_unit_;
 
       geometry_adjuster = std::make_shared<GEOMETRY_ADJUSTER>(mapping_, segment_type_, inst_, next_inst_);
 
 
-      eval_ = [coeffX, coeffY, transformation_matrix, segment_type, length_unit, geometry_adjuster = this->geometry_adjuster](double u) {
+      eval_ = [coeffX, coeffY, segment_type, length_unit, geometry_adjuster = this->geometry_adjuster](double u) {
          std::array<const std::vector<double>*, 2> coefficients{&coeffX, &coeffY};
          std::array<double, 2> position{0.0, 0.0}; // = SUM(coeff*u^pos)
          std::array<double, 2> slope{0.0, 0.0}; // slope is derivative of the curve = SUM( coeff*pos*u^(pos-1) )
@@ -847,8 +821,10 @@ class curve_segment_evaluator {
          }
          else if (segment_type == ST_CANT) {
             Logger::Warning(std::runtime_error("Use of IfcPolynomialCurve for cant is not supported"));
+            m = Eigen::Matrix4d::Identity();
          } else {
             Logger::Error(std::runtime_error("Unexpected segment type encountered"));
+            m = Eigen::Matrix4d::Identity();
          }
 
             return geometry_adjuster->transform_and_adjust(u, m);