From 4dec31933357033ada2dd6b135c7be286cae5ab4 Mon Sep 17 00:00:00 2001 From: Richard Brice <37087370+RickBrice@users.noreply.github.com> Date: Sat, 4 May 2024 16:03:36 -0700 Subject: [PATCH] Re-write of IfcCurveSegment to eliminate recursion problem and provides a more standardized approach for all parent curve types --- src/ifcgeom/mapping/IfcCurveSegment.cpp | 1743 ++++++++--------------- 1 file changed, 632 insertions(+), 1111 deletions(-) diff --git a/src/ifcgeom/mapping/IfcCurveSegment.cpp b/src/ifcgeom/mapping/IfcCurveSegment.cpp index a469b60a4a..ec16e5e852 100644 --- a/src/ifcgeom/mapping/IfcCurveSegment.cpp +++ b/src/ifcgeom/mapping/IfcCurveSegment.cpp @@ -25,21 +25,15 @@ using namespace ifcopenshell::geometry; #include "../profile_helper.h" -#include - -#include -#include #include #include +#include +#include +#include namespace { // @todo: rb is there a common math library these functions can be moved to? -auto sign = [](double v) -> double { return v ? v/fabs(v) : 1.0; }; - -// @todo change the calculation at end of this to std::lerp when upgrading to C++ 20 -template -auto compute_adjustment = [](double u, const T& a, const T& b, double l) -> double { return l == 0.0 ? 0.0 : u * (b - a) / l; }; - +auto sign = [](double v) -> double { return v ? v / fabs(v) : 1.0; }; enum segment_type_t { ST_HORIZONTAL, @@ -50,316 +44,48 @@ enum segment_type_t { // @todo use std::numbers::pi when upgrading to C++ 20 static const double PI = boost::math::constants::pi(); - -namespace { - double translate_to_length_measure(const IfcSchema::IfcCurve* crv, double param_value) { - if (std::abs(param_value) < 1.e-7) { - return param_value; - } else if (crv->as()) { - // @todo we should actually check magnitude of the vector - return param_value; - } else if (crv->as()) { - // @todo this is wrong. - return param_value; - } else if (auto circ = crv->as()) { - return circ->Radius() * param_value; - } else { - throw std::runtime_error("Unsupported curve measure type"); - } - } - - double translate_if_param_value(const IfcSchema::IfcCurve* crv, IfcSchema::IfcCurveMeasureSelect* val) { - if (auto param = val->as()) { - // We don't care whether length- or positive length measure. - return translate_to_length_measure(crv, *param); - } else { - return *val->data().getArgument(0); - } +double translate_to_length_measure(const IfcSchema::IfcCurve* crv, double param_value) { + if (std::abs(param_value) < 1.e-7) { + return param_value; + } else if (crv->as()) { + // @todo we should actually check magnitude of the vector + return param_value; + } else if (crv->as()) { + // @todo this is wrong. + return param_value; + } else if (auto circ = crv->as()) { + return circ->Radius() * param_value; + } else { + throw std::runtime_error("Unsupported curve measure type"); } } - -// Current implementation uses the same segment_geometry_adjuster for all ParentCurve types. -// Comment/Uncomment to change the type of segment geometry adjuster -// Future implementations could use specialized adjusters based on ParentCurve type -#define GEOMETRY_ADJUSTER segment_geometry_adjuster -//#define GEOMETRY_ADJUSTER linear_segment_geometry_adjuster - -// Curve segments are evaluated using a parametric function over the curve length, u -// IfcCurveSegment.TransitionCode defines how the end of a segment connects to the next segment. -// When segments are continuously joined, the placement at u = length should be equal to the placement at u = 0 -// of the next segment. However, numerical errors can cause these two points to be slightly offset -// from one another (the tangents could be slightly different as well). -// -// The sources of these numerical errors include geometric approximations (series expansion versus integration -// for spiral curves), the IfcCurveSegment.SegmentStart or .SegmentLength parameters contain roundoff or -// truncation error, minor errors in placement at the start of a segment can magnify error at the end -// of the segment. There are probably others as well. -// -// The evaluation of the relative location of the end and start points of adjacent segments occurs -// after the IfcCurveSegment.Placement is applied to the ParentCurve. The ParentCurve can be defined in -// a convenient coordinate system, such as the center of a circle or the origin of a line at (0,0). The Placement -// them moves the computed geometry to its relative position. It is the geometry after applying the Placement -// that needs to be evaluated and any difference forms the bases for the adjustments made by segment_geometry_adjuster -// or one of its subclasses. -// -// This class applies the IfcCurveSegment.Placement to inst_. The placement at the start of next_inst_ can then be -// obtained from mapping->map and compared to the end placement of inst_ and the placement at u can be adjusted -// as needed. This default implementation doesn't make any adjustments. Subclass and override the transform_and_adjust -// function to specialize the refinement of the placement at u. -class segment_geometry_adjuster { - public: - segment_geometry_adjuster(mapping* mapping, const IfcSchema::IfcCurveSegment* inst, const IfcSchema::IfcCurveSegment* next_inst) : - mapping_(mapping), - inst_(inst), - next_inst_(next_inst), - end_of_inst_(Eigen::Matrix4d::Identity()), - start_of_next_inst_(Eigen::Matrix4d::Identity()), - transition_code_(inst->Transition()) - { - transformation_matrix_ = taxonomy::cast(mapping_->map(inst_->Placement()))->ccomponents(); - - length_ = fabs(translate_if_param_value(inst_->ParentCurve(), inst_->SegmentLength()) * mapping_->get_length_unit()); - } - - void init() { - if (initialized_) { - return; - } - - if (next_inst_) { - // if there is a next segment, get the coordinates at the start. - // Note that mapping->map(next_inst) causes mapping to occur recursively - // through all of the curve segments until the end of curve is reached. - // Mapping of IfcCompositeCurve, IfcGradientCurve, and IfcSegmentedReferenceCurve may - // need to traverse the IfcCurveSegment objects in reverse order to avoid recursion. - auto next = taxonomy::cast(mapping_->map(next_inst_)); - if (next == nullptr) { - valid_ = false; - } else { - start_of_next_inst_ = next->evaluate(0.0); - } - } else { - // there is not a next segment, however IfcGradientCurve and IfcSegmentedReferenceCurve - // have an optional EndPoint attribute that serves the same purpose as the zero-length - // "next segment" at the end of the curve. The Ifc specification is a little redundant - // in that the "zero length" segment is required thereby negating the need for EndPoint - // but some implementations use the EndPoint instead of the "zero length" segment - // - // Get the parent of this segment. If it is a IfcGradientCurve or IfcSegmentedReferenceCurve - // look for the optional EndPoint attribute - auto curves = inst_->UsingCurves(); - if (curves && curves->size()) { - auto curve = *curves->begin(); - const IfcSchema::IfcPlacement* placement = nullptr; - if (curve->as()) { - auto s = curve->as(); - placement = s->EndPoint(); - } else if (curve->as()) { - auto s = curve->as(); - placement = s->EndPoint(); - } - if (placement) { - start_of_next_inst_ = taxonomy::cast(mapping_->map(placement))->ccomponents(); - } - } - } +double translate_if_param_value(const IfcSchema::IfcCurve* crv, IfcSchema::IfcCurveMeasureSelect* val) { + if (auto param = val->as()) { + // We don't care whether length- or positive length measure. + return translate_to_length_measure(crv, *param); + } else { + return *val->data().getArgument(0); } - - // To determine the geometry adjustments the curve segment needs to be evaluated - // without adjustments. This function toggles the application of geometry adjustments - void enable_adjustments(bool adjustments) { adjustments_ = adjustments; } - bool enable_adjustments() const { return adjustments_; } - - // This object doesn't have access to the eval_ property of the curve_segment_evaluator. - // The end point of the segment being adjusted, without adjustments, is computed externally - // and provided to the curve_segment_adjustor through this method - void set_segment_end_point(const Eigen::Matrix4d& end_of_inst) { - end_of_inst_ = end_of_inst; - // @todo figure out when to call this now that we defer - // calling init() to circumvent stackoverflow on high - // recursion amount on long alignments - // Currently empty function. - // init_adjustments(); - } - - // Transforms the ParentCurve geometry with the IfcCurveSegment.Placement and - // applies geometric adjustments to the geometry, if enabled - virtual Eigen::Matrix4d transform_and_adjust(double u, const Eigen::Matrix4d& parent_curve_point) { - // transform the parent curve's value into the segment curve's coordinate system - Eigen::Matrix4d segment_curve_point = transformation_matrix_ * parent_curve_point; - if (adjustments_ && valid_) { - init(); - apply_adjustments(u, segment_curve_point); - } - return segment_curve_point; - } - - const Eigen::Matrix4d& get_placement() const { return transformation_matrix_; } - - protected: - // precompute any values that are constant when applying geometry adjustments - // (subclasses to override as needed). - virtual void init_adjustments() { /*do nothing*/ } - - // Applies geometric adjustment to the segment curve point evaluated at u - // This default implementation does nothing - virtual void apply_adjustments(double /*u*/, Eigen::Matrix4d& /*p*/) const { /* do nothing - override in subclass if needed */ } - - const Eigen::Matrix4d& get_end_of_segment() const { return end_of_inst_; } - const Eigen::Matrix4d& get_start_of_next_segment() const { return start_of_next_inst_; } - IfcSchema::IfcTransitionCode::Value get_transition_code() const { return transition_code_; } - double get_length() const { return length_; } - - mapping* mapping_; - const IfcSchema::IfcCurveSegment* inst_; - const IfcSchema::IfcCurveSegment* next_inst_; - - bool adjustments_ = true; - Eigen::Matrix4d transformation_matrix_; - Eigen::Matrix4d end_of_inst_; - Eigen::Matrix4d start_of_next_inst_; - double length_; - IfcSchema::IfcTransitionCode::Value transition_code_; - bool valid_ = true; - bool initialized_ = false; -}; - -// This class refines the geometric adjustment along the segment by dividing the -// difference between the segment end point and the start point of the next segment -// into equal adjustments and applying the incremental adjustment to each position at u -class linear_segment_geometry_adjuster : public segment_geometry_adjuster { - public: - using segment_geometry_adjuster::segment_geometry_adjuster; - - protected: - void init_adjustments() override { - // @todo: rb - implement to improve efficiency - // cache delta = (start_next - end_this)/length - // adjustment is then adj = u*delta - } - - void apply_adjustments(double u, Eigen::Matrix4d& p) const override { - // make the adjustments based on the transition code - // all segments must connect end to end except for last segment IfcTransitionCode_DISCONTINUOUS for open curve - auto transition_code = get_transition_code(); - if (transition_code == IfcSchema::IfcTransitionCode::IfcTransitionCode_DISCONTINUOUS) - return; - - const auto& end_this = get_end_of_segment(); - const auto& start_next = get_start_of_next_segment(); - auto xe = end_this.col(3)(0); - auto ye = end_this.col(3)(1); - auto xs = start_next.col(3)(0); - auto ys = start_next.col(3)(1); - auto length = get_length(); - auto x = compute_adjustment(u, xe, xs, length); - auto y = compute_adjustment(u, ye, ys, length); - - p.col(3)(0) += x; - p.col(3)(1) += y; - - if (transition_code == IfcSchema::IfcTransitionCode::IfcTransitionCode_CONTSAMEGRADIENT or - transition_code == IfcSchema::IfcTransitionCode::IfcTransitionCode_CONTSAMEGRADIENTSAMECURVATURE) { - - for (int i = 0; i < 2; i++) { - auto dxe = end_this.col(i)(0); - auto dye = end_this.col(i)(1); - auto dxs = start_next.col(i)(0); - auto dys = start_next.col(i)(1); - auto dx = compute_adjustment(u,dxe,dxs,length); - auto dy = compute_adjustment(u,dye,dys,length); - p.col(i)(0) += dx; - p.col(i)(1) += dy; - p.col(i).normalize(); - } - } - } -}; - -// specializes segment_geometry_adjuster for cant segments. -class cant_adjuster : public GEOMETRY_ADJUSTER { - public: - using GEOMETRY_ADJUSTER::GEOMETRY_ADJUSTER; - - Eigen::Matrix4d transform_and_adjust(double u, const Eigen::Matrix4d& parent_curve_point) override { - // Consider a line connection two rails. The upwards vector normal to that line is used to define - // the cant tilt. For no tilt, the vector is upwards so the tilt angle is PI/2. - // If the left rail is higher than the right angle, the tilt is clockwise and the tilt angle is less than PI/2 - - // The cant (D) and half the railhead distance is needed to compute the tilt angle. - // tan(tilt_angle) = 2*D/rail_head_distance - // - // However, the rail head distance is not known from the geometric definition. It is only known in the - // business logic definition. - // - // From the geometric definition, the cant and tilt angle are known at both ends of the segment. - // From this, the rail head distance can be computed as follows: - // - // Get the placement at the start of this segment and the start of the next segment - auto& start_this = get_start_of_segment(); - auto& start_next = get_start_of_next_segment(); - - // Get the cant at the start of this and the next segment - auto start_cant = start_this.col(3)(1); - auto next_cant = start_next.col(3)(1); - - // Compute the tilt angle at start of this and start of next segment - // This is the angle of the normal vector to the line connecting the rail heads - auto tilt_start_this = atan2(start_this.col(2)(2), start_this.col(2)(1)); - auto tilt_start_next = atan2(start_next.col(2)(2), start_next.col(2)(1)); - - // Compute half the rail head distance - // Cant is measured half way between rails, so it is easier to work with half the rail head distance - // Need to do this calculation with a non-zero cant value. The tilt angle is PI/2 for zero cant - // and the tangent of PI/2 is infinity - not helpful - double h; - if (start_cant) { - h = start_cant * tan(tilt_start_this); - } else { - h = next_cant * tan(tilt_start_next); - } - - // Get the cant from the parent curve point - // Using the cant and half the rail head distance, compute the tilt angle - // For cant tilt toward the left (CCW rotation), the tilt angle used to - // compute h is greater than PI/2 and the tangent of that angle is negative. - // For this reason, use fabs(h) so tilt is between 0 and PI - double cant = parent_curve_point.col(3)(1); - auto tilt = -atan2(fabs(h), cant); - - // Create a transformation matrix for twist about the RefDirection (x-axis) - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - m.col(2)(2) = cos(tilt); - m.col(2)(3) = sin(tilt); - - // apply cant tilt to the parent curve point - Eigen::Matrix4d p = m * parent_curve_point; - - return p; - - // apply the base class transformation, which is just applying the IfcCurveSegment placement - //return GEOMETRY_ADJUSTER::transform_and_adjust(u, p); - } - - protected: - const Eigen::Matrix4d& get_start_of_segment() const { return transformation_matrix_; } -}; +} // vector of parent curve types that are supported for IfcCurveSegment.ParentCurve typedef boost::mpl::vector< - IfcSchema::IfcLine + IfcSchema::IfcLine + , IfcSchema::IfcPolyline + , IfcSchema::IfcCircle + , IfcSchema::IfcPolynomialCurve #ifdef SCHEMA_HAS_IfcClothoid - , IfcSchema::IfcClothoid + , IfcSchema::IfcClothoid #endif #if defined SCHEMA_HAS_IfcCosineSpiral - , IfcSchema::IfcCosineSpiral + , IfcSchema::IfcCosineSpiral #endif #if defined SCHEMA_HAS_IfcSineSpiral , IfcSchema::IfcSineSpiral #endif #if defined SCHEMA_HAS_IfcSecondOrderPolynomialSpiral - , IfcSchema::IfcSecondOrderPolynomialSpiral + , IfcSchema::IfcSecondOrderPolynomialSpiral #endif #if defined SCHEMA_HAS_IfcThirdOrderPolynomialSpiral , IfcSchema::IfcThirdOrderPolynomialSpiral @@ -367,17 +93,12 @@ typedef boost::mpl::vector< #if defined SCHEMA_HAS_IfcSeventhOrderPolynomialSpiral , IfcSchema::IfcSeventhOrderPolynomialSpiral #endif - - , IfcSchema::IfcPolyline - , IfcSchema::IfcCircle - , IfcSchema::IfcPolynomialCurve > curve_seg_types; class curve_segment_evaluator { private: mapping* mapping_ = nullptr; - const IfcSchema::IfcCurveSegment* inst_ = nullptr; // this curve segment instance - const IfcSchema::IfcCurveSegment* next_inst_ = nullptr; // next curve segment instance, if it exists + const IfcSchema::IfcCurveSegment* inst_ = nullptr; // this curve segment instance double length_unit_; double start_; double length_; // length along the curve, as provided from the IfcCurveSegment @@ -387,108 +108,88 @@ class curve_segment_evaluator { double projected_length_; // for vertical segments, this is the length of curve projected onto the "Distance Along" axis - std::shared_ptr geometry_adjuster_; // object that positions the segment using the IfcCurveSegment.Placement and makes geometry adjustments - - std::optional> eval_; // function for the curve. Function takes distances along, u, and returns the 4x4 position matrix + std::optional> parent_curve_fn_; // function for the parent curve. Function takes distances along, u, and returns the 4x4 position matrix + std::optional parent_curve_placement_; // placement matrix for the parent curve public: - curve_segment_evaluator(mapping* mapping, const IfcSchema::IfcCurveSegment* inst, const IfcSchema::IfcCurveSegment* next_inst, double length_unit, segment_type_t segment_type, size_t current_segment_count) + curve_segment_evaluator(mapping* mapping, const IfcSchema::IfcCurveSegment* inst, double length_unit, segment_type_t segment_type, size_t current_segment_count) : mapping_(mapping), - inst_(inst), - next_inst_(next_inst), - length_unit_(length_unit), - segment_type_(segment_type), - parent_curve_(inst->ParentCurve()), - current_segment_count_(current_segment_count) - { - /* - if (!inst->SegmentStart()->as() || !inst->SegmentLength()->as()) { - // @nb Parameter values are forbidden in the specification until parametrization is provided for all spirals - throw std::runtime_error("Unsupported curve measure type"); - } - */ - + inst_(inst), + length_unit_(length_unit), + segment_type_(segment_type), + parent_curve_(inst->ParentCurve()), + current_segment_count_(current_segment_count) { start_ = translate_if_param_value(inst->ParentCurve(), inst->SegmentStart()) * length_unit; length_ = translate_if_param_value(inst->ParentCurve(), inst->SegmentLength()) * length_unit; } - void compute_segment_end_point() - { - // The segment_geometry_adjuster needs to have both the end point of this segment - // and the start point of the next segment. The start point of the next - // segment is easy to get and is handled by the segment_geometry_adjuster. - // The end point of this segment must be computed by calling the eval_ callback - // at u = length_. But things are a little more complicated than that. eval_ will - // use segment_geometry_adjuster to correct deviations between this segment's end point and - // the next segments start point. In order to compute those adjustments, the - // end point of this segment, without correction, must be known. The end point not known - // at this time because segment_geometry_adjuster doesn't have access to the eval_ callback. - // Additionally, the eval_ callback needs to know if it is evaluating the segment geometry - // with our without geometric adjustments. - // - // Solving that conundrum is the purpose of this function. The geometric adjustments - // of geometry_adjuster are disabled, eval_ is called to get the unadjusted end point - // of this segment, the geometry_adjuster is updated with the end point so it can - // compute and apply geometry adjustments. - if (eval_ && geometry_adjuster_) { - geometry_adjuster_->enable_adjustments(false); // disable adjustments - auto end_point = (*eval_)(fabs(length_)); // compute the end point without correction - geometry_adjuster_->set_segment_end_point(end_point); // save the unadjusted end point it can be used to compute adjustments - geometry_adjuster_->enable_adjustments(true); // enable adjustments + // Take the boost::type value from mpl::for_each and test it against our curve instance + template + void operator()(boost::type) { + if (parent_curve_->as()) { + (*this)(parent_curve_->as()); } } - void set_spiral_function(mapping* mapping_, double s, std::function fnX, std::function fnY) { - if (segment_type_ == ST_HORIZONTAL) { - auto start = start_; + double length() const { + return (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_CANT) ? length_ : projected_length_; + } + + const std::optional>& parent_curve_function() const { + return parent_curve_fn_; + } + + const std::optional& parent_curve_placement() const { + return parent_curve_placement_; + } + + void set_spiral_function(double s, std::function fnX, std::function fnY) { + if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { projected_length_ = length_; - auto spiral = inst_->ParentCurve()->as(); - auto position = spiral->Position()->as(); - auto location = position->Location()->as(); - - // parent curve placement information - auto pcX = location->Coordinates()[0]; - auto pcY = location->Coordinates()[1]; - - // orientation of the coordinate system at the center point - // normalize the direction ratios - auto ref_direction = position->RefDirection(); - double pcDx = 1.0, pcDy = 0.0; - if (ref_direction) { - auto dr = ref_direction->DirectionRatios(); - double m_squared = std::inner_product(dr.begin(), dr.end(), dr.begin(), 0.0); - double m = sqrt(m_squared); - std::for_each(dr.begin(), dr.end(), [m](auto& d) { return d / m; }); - // dx,dy of the parent curve X-axis - pcDx = dr[0]; - pcDy = dr[1]; - } // start of trimmed curve double pcStartX = 0.0, pcStartY = 0.0; double pcStartDx = 1.0, pcStartDy = 0.0; - if (start) - { - // the spiral doesn't start at the inflection point - // compute the point where it starts - auto x = boost::math::quadrature::trapezoidal(fnX, 0.0, start / s); - auto y = boost::math::quadrature::trapezoidal(fnY, 0.0, start / s); + if (start_) { + // the spiral doesn't start at the inflection point + // compute the point where it starts + pcStartX = boost::math::quadrature::trapezoidal(fnX, 0.0, start_ / s); + pcStartY = boost::math::quadrature::trapezoidal(fnY, 0.0, start_ / s); // compute the slope of the spiral at the start point - auto dx = s ? fnX(start/s) / s : 1.0; - auto dy = s ? fnY(start/s) / s : 0.0; - - pcStartX = x * pcDx - y * pcDy + pcX; - pcStartY = x * pcDy + y * pcDx + pcY; - pcStartDx = dx * pcDx - dy * pcDy; - pcStartDy = dx * pcDy + dy * pcDx; + pcStartDx = s ? fnX(start_ / s) / s : 1.0; + pcStartDy = s ? fnY(start_ / s) / s : 0.0; } - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - eval_ = [start, s, pcX, pcY, pcDx, pcDy, pcStartX, pcStartY, pcStartDx, pcStartDy, fnX, fnY, geometry_adjuster = geometry_adjuster_](double u) { + Eigen::Matrix4d p = Eigen::Matrix4d::Identity(); + p.col(0) = Eigen::Vector4d(pcStartDx, pcStartDy, 0, 0); + p.col(1) = Eigen::Vector4d(-pcStartDy, pcStartDx, 0, 0); + p.col(3) = Eigen::Vector4d(pcStartX, pcStartY, 0, 1); + parent_curve_placement_ = p; - u += start; + std::function convert_u; + if (segment_type_ == ST_HORIZONTAL) + { + convert_u = [](double u) -> double { return u; }; + } else { + // This functor is f'(x) = dy/dx + auto df = [fnX,fnY](double t) -> double { + return fnY(t) / fnX(t); + }; + + // This functor computes the curve length + // Integral (sqrt (f'(x) ^ 2 + 1)dx + convert_u = [df](double x) -> double { + auto fs = [df](double x) -> double { + return sqrt(pow(df(x), 2) + 1); + }; + auto s = boost::math::quadrature::trapezoidal(fs, 0.0, x); + return s; + }; + } + + parent_curve_fn_ = [start=start_, s, convert_u, fnX, fnY](double u) { + u = convert_u(u+start); // integration limits, integrate from a to b auto b = s ? u / s : 0.0; @@ -499,258 +200,139 @@ class curve_segment_evaluator { auto dx = s ? fnX(b) / s : 1.0; auto dy = s ? fnY(b) / s : 0.0; - auto x1 = x * pcDx - y * pcDy + pcX; - auto y1 = x * pcDy + y * pcDx + pcY; - auto dx1 = dx * pcDx - dy * pcDy; - auto dy1 = dx * pcDy + dy * pcDx; - - auto x2 = (x1 - pcStartX) * pcStartDx - (y1 - pcStartY) * (-pcStartDy); - auto y2 = (x1 - pcStartX) * (-pcStartDy) + (y1 - pcStartY) * pcStartDx; - auto dx2 = dx1 * pcStartDx + dy1 * (-pcStartDy); - auto dy2 = dx1 * (-pcStartDy) + dy1 * pcStartDx; - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - m.col(0) = Eigen::Vector4d(dx2, dy2, 0, 0); - m.col(1) = Eigen::Vector4d(-dy2, dx2, 0, 0); - m.col(3) = Eigen::Vector4d(x2, y2, 0.0, 1.0); - return geometry_adjuster->transform_and_adjust(u, m); - - //auto pcX = boost::math::quadrature::trapezoidal(fnX, 0.0, b) + pcCenterX - pcStartX; - //auto pcY = boost::math::quadrature::trapezoidal(fnY, 0.0, b) + pcCenterY - pcStartY; - - //auto angle1 = atan2(pcStartDy, pcStartDx); - //auto angle2 = atan2(pcDy, pcDx); - //auto angle = angle2 - angle1; - //auto csX = pcX * cos(angle) - pcY * sin(angle); - //auto csY = pcX * sin(angle) + pcY * cos(angle); - - //// 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 = s ? fnX(b) / s : 1.0; - //auto dy = s ? fnY(b) / s : 0.0; - - - //Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - //m.col(0) = Eigen::Vector4d(dx, dy, 0, 0); - //m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); - //m.col(3) = Eigen::Vector4d(csX, csY, 0.0, 1.0); - //return geometry_adjuster->transform_and_adjust(u, m); - }; - } else if (segment_type_ == ST_VERTICAL) { - - // This functor is f'(x) = dy/dx - auto df = [fnX,fnY](double t) -> double { - return fnY(t) / fnX(t); - }; - - // This functor computes the curve length - // Integral (sqrt (f'(x) ^ 2 + 1)dx - auto fc = [df](double x) -> double { - auto fs = [df](double x) -> double { - return sqrt(pow(df(x), 2) + 1); - }; - auto s = boost::math::quadrature::trapezoidal(fs, 0.0, x); - return s; - }; - - eval_ = [s,fnX,fnY,fc](double u) -> Eigen::Matrix4d { - // find x when u - s = 0 - std::uintmax_t max_iter = 5000; - //auto max_iter_ = max_iter; - auto tol = [](double a, double b) { return fabs(b - a) < 1.0E-09; }; - auto ux = u; - try { - auto f = [fc, u](double x) -> double { return fc(x) - u; }; - auto result = boost::math::tools::bracket_and_solve_root(f, u, 2.0, true, tol, max_iter); - ux = result.first; - } catch (...) { - Logger::Warning("root solver failed"); - } - - // integration limits, integrate from a to b - auto a = 0.0; - auto b = s ? u / s : 0.0; - auto y = boost::math::quadrature::trapezoidal(fnY, a, b); // - start_y; - - auto dx = s ? fnX(b)/s : 1.0; - auto dy = s ? fnY(b)/s : 0.0; - - Eigen::Matrix4d m; m.col(0) = Eigen::Vector4d(dx, dy, 0, 0); m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); - m.col(2) = Eigen::Vector4d(0, 0, 1.0, 0); - m.col(3) = Eigen::Vector4d(0.0, y, 0.0, 1.0); - + m.col(3) = Eigen::Vector4d(x, y, 0, 1); return m; }; } else if (segment_type_ == ST_CANT) { Logger::Error(std::runtime_error("Unexpected segment type encountered - cant is handled in set_cant_spiral_function - should never get here")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } - else { + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { Logger::Error(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; } } - // defines the eval_ functor for cant segments. + // defines the parent_curve_fn_ functor for cant segments. // Cant returns D at a distance along the curve, u. // CantSlope returns the slope of the Cant function at u. CantSlope(u) is the derivative of Cant(u) - void set_cant_spiral_function(mapping* mapping_, std::function Cant, std::function CantSlope) { - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - eval_ = [geometry_adjuster = geometry_adjuster_, Cant, CantSlope](double u) -> Eigen::Matrix4d { + void set_cant_spiral_function(std::function Cant, std::function CantSlope) { + parent_curve_fn_ = [Cant, CantSlope](double u) -> Eigen::Matrix4d { auto cant = Cant(u); auto slope = CantSlope(u); auto angle = atan(slope); auto dx = cos(angle); - auto dy = sin(angle); + auto dy = sin(angle); - Eigen::Matrix4d m; + Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); m.col(0) = Eigen::Vector4d(dx, dy, 0, 0); m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); - m.col(2) = Eigen::Vector4d(0, 0, 1.0, 0); m.col(3) = Eigen::Vector4d(0.0, cant, 0.0, 1.0); - - return geometry_adjuster->transform_and_adjust(u, m); + return m; }; - } - // defines the eval_ functor for constant cant segments. - // the parent curve is IfcClothoid - // For all the other cant types with spiral parent curves, just applying the cant_adjuster works - // when compared to the results published at https://github.com/bSI-RailwayRoom/IFC-Rail-Unit-Test-Reference-Code/ - // However, for IfcClothoid, the cant needs to be adjusted by the IfcCurveSegment.Placement.Y value to make the results - // match those from the bSI Railway Room unit tests - void set_clothoid_cant_spiral_function(mapping* mapping_, std::function Cant, std::function CantSlope) { - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - auto cant_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - cant_adjuster_->enable_adjustments(current_segment_count_ <= 64); - eval_ = [geometry_adjuster = geometry_adjuster_,cant_adjuster=cant_adjuster_, Cant, CantSlope](double u) -> Eigen::Matrix4d { - auto cant = Cant(u); - auto slope = CantSlope(u); - - // this is the hack that makes this function different from set_cant_spiral_function - cant += geometry_adjuster->get_placement().col(3)(1); - - auto angle = atan(slope); - auto dx = cos(angle); - auto dy = sin(angle); - - Eigen::Matrix4d m; - m.col(0) = Eigen::Vector4d(dx, dy, 0, 0); - m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); - m.col(2) = Eigen::Vector4d(0, 0, 1.0, 0); - m.col(3) = Eigen::Vector4d(0.0, cant, 0.0, 1.0); - - return cant_adjuster->transform_and_adjust(u, m); - }; + parent_curve_placement_ = (*parent_curve_fn_)(0); } #ifdef SCHEMA_HAS_IfcClothoid - void operator()(const IfcSchema::IfcClothoid* c) { - // see https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcClothoid.htm - // also see, https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/concepts/Partial_Templates/Geometry/Curve_Segment_Geometry/Clothoid_Transition_Segment/content.html, - auto A = c->ClothoidConstant(); + void operator()(const IfcSchema::IfcClothoid* c) { + auto A = c->ClothoidConstant(); - if (segment_type_ == ST_CANT) { - auto Cant = [A,L=length_*length_unit_](double t) -> double - { return A ? L*A * t / fabs(pow(A, 3)) : 0.0; }; - auto CantSlope = [A, L = length_ * length_unit_](double /*t*/) -> double - { return A ? L*A / fabs(pow(A, 3)) : 0.0; }; - set_clothoid_cant_spiral_function(mapping_, Cant, CantSlope); - } else { - auto s = fabs(A * sqrt(PI)); // curve length when u = 1.0 - 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 A ? s * sin(PI * A * t * t / (2 * fabs(A))) : 0.0; }; - set_spiral_function(mapping_, s, fn_x, fn_y); - } - } + if (segment_type_ == ST_CANT) { + auto Cant = [A, L = length_ * length_unit_](double t) -> double { return A ? L * A * t / fabs(pow(A, 3)) : 0.0; }; + auto CantSlope = [A, L = length_ * length_unit_](double /*t*/) -> double { return A ? L * A / fabs(pow(A, 3)) : 0.0; }; + set_cant_spiral_function(Cant, CantSlope); + } else { + auto s = fabs(A * sqrt(PI)); // curve length when u = 1.0 + 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 A ? s * sin(PI * A * t * t / (2 * fabs(A))) : 0.0; }; + set_spiral_function(s, fn_x, fn_y); + } + } #endif #if defined SCHEMA_HAS_IfcCosineSpiral - void operator()(const IfcSchema::IfcCosineSpiral* c) { - auto constant_term = c->ConstantTerm(); - auto cosine_term = c->CosineTerm(); - auto L = length()*length_unit_; - if (segment_type_ == ST_HORIZONTAL) { + void operator()(const IfcSchema::IfcCosineSpiral* c) { + auto constant_term = c->ConstantTerm(); + auto cosine_term = c->CosineTerm(); + auto L = length() * length_unit_; + if (segment_type_ == ST_HORIZONTAL) { - auto theta = [constant_term, cosine_term, L, lu = length_unit_](double t) -> double { - auto a0 = constant_term.has_value() ? t / (constant_term.value() * lu) : 0.0; - auto a1 = (L / PI) * (1.0 / (cosine_term * lu)) * sin((PI / L) * t); - return a0 + a1; - }; - 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_, s, fn_x, fn_y); - } else if (segment_type_ == ST_CANT) { - auto Cant = [constant_term, cosine_term, L, lu = length_unit_](double t) -> double - { - auto a0 = constant_term.has_value() ? L / (constant_term.value() * lu) : 0.0; - auto a1 = (L / (cosine_term * lu)) * cos(PI * t*lu/L); - return a0 + a1; - }; - auto CantSlope = [cosine_term, L, lu = length_unit_](double t) -> double { - auto a1 = -(PI/L)*(L / (cosine_term * lu)) * sin(PI * t * lu / L); - return a1; - }; - set_cant_spiral_function(mapping_, Cant, CantSlope); - } else if (segment_type_ == ST_VERTICAL) { - Logger::Error(std::runtime_error("IfcCosineSpiral cannot be used for vertical alignment")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } else { - Logger::Error(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } - } + auto theta = [constant_term, cosine_term, L, lu = length_unit_](double t) -> double { + auto a0 = constant_term.has_value() ? t / (constant_term.value() * lu) : 0.0; + auto a1 = (L / PI) * (1.0 / (cosine_term * lu)) * sin((PI / L) * t); + return a0 + a1; + }; + 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(s, fn_x, fn_y); + } else if (segment_type_ == ST_CANT) { + auto Cant = [constant_term, cosine_term, L, lu = length_unit_](double t) -> double { + auto a0 = constant_term.has_value() ? L / (constant_term.value() * lu) : 0.0; + auto a1 = (L / (cosine_term * lu)) * cos(PI * t * lu / L); + return a0 + a1; + }; + auto CantSlope = [cosine_term, L, lu = length_unit_](double t) -> double { + auto a1 = -(PI / L) * (L / (cosine_term * lu)) * sin(PI * t * lu / L); + return a1; + }; + set_cant_spiral_function(Cant, CantSlope); + } else if (segment_type_ == ST_VERTICAL) { + Logger::Error(std::runtime_error("IfcCosineSpiral cannot be used for vertical alignment")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { + Logger::Error(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } + } #endif #if defined SCHEMA_HAS_IfcSineSpiral - void operator()(const IfcSchema::IfcSineSpiral* c) { - auto constant_term = c->ConstantTerm(); - auto linear_term = c->LinearTerm(); - auto sine_term = c->SineTerm(); - auto L = length() * length_unit_; - if (segment_type_ == ST_HORIZONTAL) { - auto theta = [constant_term, linear_term, sine_term, L, lu = length_unit_](double t) -> double { - auto a0 = constant_term.has_value() ? t / (constant_term.value() * lu) : 0.0; - auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(t / (linear_term.value() * lu), 2.0) / 2.0 : 0.0; - auto a2 = -1.0 * (L / (2 * PI * sine_term * lu)) * (cos(2 * PI * t / L) - 1.0); - return a0 + a1 + a2; - }; - 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_, s, fn_x, fn_y); - } else if (segment_type_ == ST_CANT) { - auto Cant = [constant_term,linear_term,sine_term, L, lu = length_unit_](double t) -> double { - auto a0 = constant_term.has_value() ? L / (constant_term.value() * lu) : 0.0; - auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(L / (linear_term.value() * lu), 2.0) * (t/L) : 0.0; - auto a2 = (L / (sine_term * lu)) * sin(2 * PI * t / L); - return a0 + a1 + a2; - }; - auto CantSlope = [linear_term, sine_term, L, lu = length_unit_](double t) -> double { - auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(L / (linear_term.value() * lu), 2.0) * (1.0 / L) : 0.0; - auto a2 = (2*PI/L)*(L / (sine_term * lu)) * cos(2 * PI * t / L); - return a1 + a2; - }; - set_cant_spiral_function(mapping_, Cant, CantSlope); - } else if (segment_type_ == ST_VERTICAL) { - Logger::Error(std::runtime_error("IfcSineSpiral cannot be used for vertical alignment")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } else { - Logger::Error(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } - } + void operator()(const IfcSchema::IfcSineSpiral* c) { + auto constant_term = c->ConstantTerm(); + auto linear_term = c->LinearTerm(); + auto sine_term = c->SineTerm(); + auto L = length() * length_unit_; + if (segment_type_ == ST_HORIZONTAL) { + auto theta = [constant_term, linear_term, sine_term, L, lu = length_unit_](double t) -> double { + auto a0 = constant_term.has_value() ? t / (constant_term.value() * lu) : 0.0; + auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(t / (linear_term.value() * lu), 2.0) / 2.0 : 0.0; + auto a2 = -1.0 * (L / (2 * PI * sine_term * lu)) * (cos(2 * PI * t / L) - 1.0); + return a0 + a1 + a2; + }; + 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(s, fn_x, fn_y); + } else if (segment_type_ == ST_CANT) { + auto Cant = [constant_term, linear_term, sine_term, L, lu = length_unit_](double t) -> double { + auto a0 = constant_term.has_value() ? L / (constant_term.value() * lu) : 0.0; + auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(L / (linear_term.value() * lu), 2.0) * (t / L) : 0.0; + auto a2 = (L / (sine_term * lu)) * sin(2 * PI * t / L); + return a0 + a1 + a2; + }; + auto CantSlope = [linear_term, sine_term, L, lu = length_unit_](double t) -> double { + auto a1 = linear_term.has_value() ? sign(linear_term.value()) * pow(L / (linear_term.value() * lu), 2.0) * (1.0 / L) : 0.0; + auto a2 = (2 * PI / L) * (L / (sine_term * lu)) * cos(2 * PI * t / L); + return a1 + a2; + }; + set_cant_spiral_function(Cant, CantSlope); + } else if (segment_type_ == ST_VERTICAL) { + Logger::Error(std::runtime_error("IfcSineSpiral cannot be used for vertical alignment")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { + Logger::Error(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } + } #endif void polynomial_spiral(boost::optional A0, boost::optional A1, boost::optional A2, boost::optional A3, boost::optional A4, boost::optional A5, boost::optional A6, boost::optional A7) { - auto theta = [A0, A1, A2, A3, A4, A5, A6, A7, start=start_*length_unit_,lu=length_unit_](double t) { - //t += start; + auto theta = [A0, A1, A2, A3, A4, A5, A6, A7, start = start_ * length_unit_, lu = length_unit_](double t) { auto a0 = A0.has_value() ? t / (A0.value() * lu) : 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() * lu, 3)) : 0.0; @@ -760,17 +342,17 @@ class curve_segment_evaluator { 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() * lu * std::pow(t, 8) / (8 * fabs(std::pow(A7.value() * lu, 9))) : 0.0; return a0 + a1 + a2 + a3 + a4 + a5 + a6 + a7; - }; + }; - auto fn_x = [theta](double t) -> double { return cos(theta(t)); }; - auto fn_y = [theta](double t) -> double { return sin(theta(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_, s, fn_x, fn_y); + double s = 1.0; + set_spiral_function(s, fn_x, fn_y); } void polynomial_cant_spiral(boost::optional A0, boost::optional A1, boost::optional A2, boost::optional A3, boost::optional A4, boost::optional A5, boost::optional A6, boost::optional A7) { - auto Cant = [A0, A1, A2, A3, A4, A5, A6, A7, start=start_*length_unit_,L=length_*length_unit_, lu=length_unit_,length=length_](double t) { + auto Cant = [A0, A1, A2, A3, A4, A5, A6, A7, start = start_ * length_unit_, L = length_ * length_unit_, lu = length_unit_, length = length_](double t) { t += start; auto a0 = A0.has_value() ? 1 / (A0.value() * lu) : 0.0; auto a1 = A1.has_value() ? A1.value() * lu * t / fabs(std::pow(A1.value() * lu, 3)) : 0.0; @@ -780,7 +362,7 @@ class curve_segment_evaluator { auto a5 = A5.has_value() ? A5.value() * lu * std::pow(t, 5) / fabs(std::pow(A5.value() * lu, 7)) : 0.0; auto a6 = A6.has_value() ? std::pow(t, 6) / std::pow(A6.value() * lu, 7) : 0.0; auto a7 = A7.has_value() ? A7.value() * lu * std::pow(t, 7) / fabs(std::pow(A7.value() * lu, 9)) : 0.0; - return L*(a0 + a1 + a2 + a3 + a4 + a5 + a6 + a7); + return L * (a0 + a1 + a2 + a3 + a4 + a5 + a6 + a7); }; auto CantSlope = [A1, A2, A3, A4, A5, A6, A7, start = start_ * length_unit_, L = length_ * length_unit_, lu = length_unit_, length = length_](double t) { @@ -795,23 +377,22 @@ class curve_segment_evaluator { return L * (a1 + a2 + a3 + a4 + a5 + a6 + a7); }; - set_cant_spiral_function(mapping_, Cant, CantSlope); + set_cant_spiral_function(Cant, CantSlope); } #ifdef SCHEMA_HAS_IfcSecondOrderPolynomialSpiral - void operator()(const IfcSchema::IfcSecondOrderPolynomialSpiral* c) - { - auto A0 = c->ConstantTerm(); - auto A1 = c->LinearTerm(); - auto A2 = c->QuadraticTerm(); - boost::optional A3, A4, A5, A6, A7; + void operator()(const IfcSchema::IfcSecondOrderPolynomialSpiral* c) { + auto A0 = c->ConstantTerm(); + auto A1 = c->LinearTerm(); + auto A2 = c->QuadraticTerm(); + boost::optional A3, A4, A5, A6, A7; - if (segment_type_ == ST_CANT) { - polynomial_cant_spiral(A0, A1, A2, A3, A4, A5, A6, A7); - } else { - polynomial_spiral(A0, A1, A2, A3, A4, A5, A6, A7); - } - } + if (segment_type_ == ST_CANT) { + polynomial_cant_spiral(A0, A1, A2, A3, A4, A5, A6, A7); + } else { + polynomial_spiral(A0, A1, A2, A3, A4, A5, A6, A7); + } + } #endif #ifdef SCHEMA_HAS_IfcThirdOrderPolynomialSpiral @@ -822,7 +403,6 @@ class curve_segment_evaluator { auto A3 = c->CubicTerm(); boost::optional A4, A5, A6, A7; - if (segment_type_ == ST_CANT) { polynomial_cant_spiral(A0, A1, A2, A3, A4, A5, A6, A7); } else { @@ -850,552 +430,493 @@ class curve_segment_evaluator { } #endif - void operator()(const IfcSchema::IfcCircle* c) - { - if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { - auto R = c->Radius() * length_unit_; - auto position = c->Position()->as(); - auto location = position->Location()->as(); + void operator()(const IfcSchema::IfcCircle* c) { + if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { + auto R = c->Radius() * length_unit_; + auto parent_curve_position = taxonomy::cast(mapping_->map(c->Position()))->ccomponents(); - // center point of the parent curve - auto pcCenterX = location->Coordinates()[0]; - auto pcCenterY = location->Coordinates()[1]; - - // normalize the direction ratios - auto ref_direction = position->RefDirection(); - auto pcDx = 1.0, pcDy = 0.0; - if (ref_direction) { - auto dr = ref_direction->DirectionRatios(); - double m_squared = std::inner_product(dr.begin(), dr.end(), dr.begin(), 0.0); - double m = sqrt(m_squared); - std::for_each(dr.begin(), dr.end(), [m](auto& d) { return d / m; }); - // dx,dy of the parent curve X-axis - pcDx = dr[0]; - pcDy = dr[1]; - } + // center point of the parent curve + auto pcCenterX = parent_curve_position(0, 3); + auto pcCenterY = parent_curve_position(1, 3); + auto pcDx = parent_curve_position(0, 0); + auto pcDy = parent_curve_position(1, 0); - // angle from X = 0 to the parent curve X-axis - auto pc_axis_angle = atan2(pcDy, pcDx); - // sweep angle from the parent curve X-axis to the first point on the trimmed curve - auto sweep_start_angle = start_ / R; - // angle from X = 0 to the first point on the trimmed curve - auto start_angle = pc_axis_angle + sweep_start_angle; + // angle from X = 0 to the parent curve X-axis + auto pc_axis_angle = atan2(pcDy, pcDx); + // sweep angle from the parent curve X-axis to the first point on the trimmed curve + auto sweep_start_angle = start_ / R; + // angle from X = 0 to the first point on the trimmed curve + auto start_angle = pc_axis_angle + sweep_start_angle; - // first point on the trimmed curve - auto pcStartX = pcCenterX + R * cos(start_angle); - auto pcStartY = pcCenterY + R * sin(start_angle); + auto sign_l = sign(length_); - auto sign_l = sign(length_); + projected_length_ = length_; - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); + std::function convert_u; + if (segment_type_ == ST_HORIZONTAL) { + convert_u = [](double u) { return u; }; + } else { + auto curve_segment_placement = taxonomy::cast(mapping_->map(inst_->Placement()))->ccomponents(); + auto csStartX = curve_segment_placement(0, 3); + auto csStartY = curve_segment_placement(1, 3); + auto csStartDx = curve_segment_placement(0, 0); + auto csStartDy = curve_segment_placement(1, 0); + auto csCenterX = csStartX - sign_l * csStartDy * R; + auto csCenterY = csStartY + sign_l * csStartDx * R; - projected_length_ = length_; - eval_ = [R, pcCenterX, pcCenterY, pcStartX, pcStartY, pc_axis_angle, start_angle, sign_l, segment_type=segment_type_, geometry_adjuster = geometry_adjuster_](double u) - { - // If segment_type == ST_VERTICAL and adjustments are enabled the input u is measured along the horizontal. - // u needs to be the arc length along the circle. If adjustments are disabled u is one of the end points so its arc length - if (segment_type == ST_VERTICAL && geometry_adjuster->enable_adjustments()) { - // x and y are distance from center of circle as if circle was centered at (0,0) - auto x = pcStartX + u - pcCenterX; - auto y = -sign_l*sqrt(R * R - x * x); - // move x and y so they are relative to the center of the circle - x += pcCenterX; - y += pcCenterY; - // compute the distance between the start point and (x,y) - auto c = sqrt(pow(x - pcStartX,2.0) + pow(y - pcStartY,2.0)); - // compute the subtended angle - // c = 2R*sin(delta/2) - auto delta = 2 * asin(c / (2 * R)); - // compute the arc length (this will always be a positive value) - u = R * fabs(delta); - } + convert_u = [csStartX, csStartY, csCenterX, csCenterY, R, sign_l](double u) { + // for vertical, u is measured along the horizonal but we need it to be an arc length - // u is measured along the circle - // angle from the X=0 axis to the current point - auto delta = sign_l * u / R; - auto angle = start_angle + delta; + // x and y are coordinates on the curve segment for horizontal distance u from the start point + // u is a horizontal distance so x = csStartX + u + // Recognizing the triangle + // R^2 = (csStartX - csCenterX + u)^2 + (y - csCenterY)^2 + // solve for y + // (y - csCenterY) = sqrt( R^2 - (csStartX - csCenterX + u)^2 ) + // y = csCenterY + sqrt( R^2 - (csStartX - csCenterX + u)^2 ) + auto x = csStartX + u; + auto y = csCenterY - sign_l * sqrt(pow(R, 2) - pow(csStartX - csCenterX + u, 2)); - // point on the parent curve - auto pcX = R * cos(angle) + pcCenterX; - auto pcY = R * sin(angle) + pcCenterY; + // compute the chord distance between the start point and (x,y) + auto c = sqrt(pow(x - csStartX, 2.0) + pow(y - csStartY, 2.0)); - // translate parent curve point so it is relative to the parent curve start point - pcX -= pcStartX; - pcY -= pcStartY; + // compute the subtended angle + // c = 2R*sin(delta/2) + auto delta = 2 * asin(c / (2 * R)); - // rotate the parent curve point about its start point - // to eliminate the orientation of the parent curve axes - auto rotate = -(sign_l*PI / 2 + start_angle); - auto csX = pcX * cos(rotate) - pcY * sin(rotate); - auto csY = pcX * sin(rotate) + pcY * cos(rotate); + // compute the arc length (this will always be a positive value) + u = R * fabs(delta); + return u; + }; + } - // direction of vector tangent to the curve segment - // at this point the curve has been rotated so the start is - // tangent to [1,0] so dx, dy in that coordinate system - // is dependent only on delta - auto dx = cos(delta); - auto dy = sin(delta); + parent_curve_fn_ = [segment_type = segment_type_, R, pcCenterX, pcCenterY, start_angle, sign_l, convert_u](double u) { + u = convert_u(u); - // transform the point into the curve segment coordinate system - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - m.col(0) = Eigen::Vector4d(dx, dy, 0, 0); - m.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); - m.col(3) = Eigen::Vector4d(csX, csY, 0.0, 1.0); - return geometry_adjuster->transform_and_adjust(u, m); - }; - } - else if (segment_type_ == ST_CANT) { - Logger::Warning(std::runtime_error("Use of IfcCircle for cant is not supported")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } else { - Logger::Error(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } - } + // u is measured along the circle + // angle from the X=0 axis to the current point + auto delta = sign_l * u / R; + auto sweep_angle = start_angle + delta; + auto cos_sweep_angle = cos(sweep_angle); + auto sin_sweep_angle = sin(sweep_angle); + // point on the parent curve + auto pcX = R * cos_sweep_angle + pcCenterX; + auto pcY = R * sin_sweep_angle + pcCenterY; + + auto pcDx = -sign_l * sin_sweep_angle; + auto pcDy = sign_l * cos_sweep_angle; + + Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); + m.col(0) = Eigen::Vector4d(pcDx, pcDy, 0, 0); + m.col(1) = Eigen::Vector4d(-pcDy, pcDx, 0, 0); + m.col(3) = Eigen::Vector4d(pcX, pcY, 0.0, 1.0); + return m; + }; + + parent_curve_placement_ = (*parent_curve_fn_)(0.0); + } else if (segment_type_ == ST_CANT) { + Logger::Warning(std::runtime_error("Use of IfcCircle for cant is not supported")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { + Logger::Error(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } + } + void operator()(const IfcSchema::IfcPolyline* pl) { - if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { - struct Range { + if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { + struct Range { double u_start; double u_end; std::function compare; bool operator<(const Range& r) const { return u_start < r.u_start; } - }; - - using Function = std::function; - std::map fns; - - auto p = pl->Points(); - if (p->size() < 2) { + }; + + using Function = std::function; + std::map fns; + + auto p = pl->Points(); + if (p->size() < 2) { Logger::Error(std::runtime_error("invalid polyline - must have at least 2 points")); // this should never happen, but just in case it does - } - - auto std_compare = [](double u_start, double u, double u_end) { return u_start <= u && u < u_end; }; - auto end_compare = [](double u_start, double u, double u_end) { return u_start <= u && u <= (u_end + 0.001); }; - - auto begin = p->begin(); - auto iter = begin; - auto end = p->end(); - auto last = std::prev(end); - auto p1 = *(iter++); - - if (p1->Coordinates().size() != 2) { - Logger::Warning("Expected IfcPolyline.Points to be 2D", pl); - } - - auto u = 0.0; - for (; iter != end; iter++) { + } + + auto std_compare = [](double u_start, double u, double u_end) { return u_start <= u && u < u_end; }; + auto end_compare = [](double u_start, double u, double u_end) { return u_start <= u && u <= (u_end + 0.001); }; + + auto begin = p->begin(); + auto iter = begin; + auto end = p->end(); + auto last = std::prev(end); + auto p1 = *(iter++); + + if (segment_type_ == ST_HORIZONTAL) { + if (p1->Coordinates().size() != 2) { + Logger::Warning("Expected IfcPolyline.Points to be 2D", pl); + } + } else { + if (p1->Coordinates().size() != 3) { + Logger::Warning("Expected IfcPolyline.Points to be 3D", pl); + } + } + + auto u = 0.0; + for (; iter != end; iter++) { auto p2 = *iter; - + auto p1x = p1->Coordinates()[0]; auto p1y = p1->Coordinates()[1]; - + auto p1z = segment_type_ == ST_HORIZONTAL ? 0.0 : p1->Coordinates()[2]; + auto p2x = p2->Coordinates()[0]; auto p2y = p2->Coordinates()[1]; - + auto p2z = segment_type_ == ST_HORIZONTAL ? 0.0 : p2->Coordinates()[2]; + auto dx = p2x - p1x; auto dy = p2y - p1y; - auto l = sqrt(dx * dx + dy * dy); + auto dz = p2z - p1z; + auto l = sqrt(dx * dx + dy * dy + dz*dz); + if (l < mapping_->settings().get().get()) { - std::ostringstream os; - os << "Coincident IfcPolyline.Points are not expected. Skipping point " << std::distance(iter, begin) << std::endl; - Logger::Warning(os.str(), pl); - continue; // go to next point + std::ostringstream os; + os << "Coincident IfcPolyline.Points are not expected. Skipping point " << std::distance(iter, begin) << std::endl; + Logger::Warning(os.str(), pl); + continue; // go to next point } + + auto lh = sqrt(dx * dx + dy * dy); // horizontal length + auto ds = dz / lh; dx /= l; dy /= l; + dz /= l; - auto segment_type = segment_type_; - - auto fn = [p1x, p1y, dx, dy, segment_type](double u) { - auto x = segment_type == ST_HORIZONTAL ? p1x + u * dx : u; - auto y = p1y + u * dy; - - 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 - 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); - } else if (segment_type == ST_VERTICAL) { - // rotate about the Y-axis (slope along u is dx, slope vertically is dy, vertical position is y) - m.col(0) = Eigen::Vector4d(dx, 0, dy, 0); - m.col(1) = Eigen::Vector4d(0, 1, 0, 0); - m.col(2) = Eigen::Vector4d(-dy, 0, dx, 0); - 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; - }; - - fns.insert(std::make_pair(Range{u, u + l, iter == last ? end_compare : std_compare}, fn)); - - p1 = p2; - u = u + l; - } - - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - - projected_length_ = length_; - - eval_ = [fns, geometry_adjuster = geometry_adjuster_](double u) { - auto iter = std::find_if(fns.cbegin(), fns.cend(), [=](const auto& fn) { - auto [u_start, u_end, compare] = fn.first; - return compare(u_start, u, u_end); - }); - - if (iter == fns.end()) { - throw std::runtime_error("invalid distance from start"); // this should never happen, but just in case it does, throw an exception so the problem gets automatically detected + std::function convert_u; + if (segment_type_ == ST_HORIZONTAL) { + convert_u = [](double u) { return u; }; + } else { + convert_u = [ds](double u) { return u * ds; }; } + auto fn = [p1x, p1y, p1z, dx, dy, dz, convert_u](double u) { + u = convert_u(u); + + auto x = p1x + u * dx; + auto y = p1y + u * dy; + auto z = p1z + u * dz; + // rotation around z + Eigen::Matrix4d yaw = Eigen::Matrix4d::Identity(); + yaw.col(0) = Eigen::Vector4d(dx, dy, 0, 0); + yaw.col(1) = Eigen::Vector4d(-dy, dx, 0, 0); + + // rotation around y + Eigen::Matrix4d pitch = Eigen::Matrix4d::Identity(); + pitch.col(0) = Eigen::Vector4d(dx, 0, dz, 0); + pitch.col(2) = Eigen::Vector4d(-dz, 0, dx, 0); + + // rotaton around x + Eigen::Matrix4d roll = Eigen::Matrix4d::Identity(); + roll.col(1) = Eigen::Vector4d(0, dy, -dz, 0); + roll.col(2) = Eigen::Vector4d(0, dz, dy, 0); + + // translation + Eigen::Matrix4d position = Eigen::Matrix4d::Identity(); + position.col(3) = Eigen::Vector4d(x, y, z, 1); + + return yaw * pitch * roll * position; + }; + + fns.insert(std::make_pair(Range{u, u + l, iter == last ? end_compare : std_compare}, fn)); + + p1 = p2; + u = u + l; + } + + projected_length_ = length_; + + parent_curve_fn_ = [fns](double u) { + auto iter = std::find_if(fns.cbegin(), fns.cend(), [=](const auto& fn) { + auto [u_start, u_end, compare] = fn.first; + return compare(u_start, u, u_end); + }); + + if (iter == fns.end()) { + throw std::runtime_error("invalid distance from start"); // this should never happen, but just in case it does, throw an exception so the problem gets automatically detected + } + const auto& [u_start, u_end, compare] = iter->first; const auto& fn = iter->second; Eigen::Matrix4d m = fn(u - u_start); // (u - u_start) is distance from start of this segment of the polyline - return geometry_adjuster->transform_and_adjust(u, m); - }; - } else if (segment_type_ == ST_CANT) { - Logger::Warning(std::runtime_error("Use of IfcPolyline for cant is not supported")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } else { - Logger::Warning(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } - } - - void operator()(const IfcSchema::IfcLine* l) { - projected_length_ = length_; - - if (segment_type_ == ST_HORIZONTAL) { - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - - auto s = l->Pnt(); - auto c = s->Coordinates(); - auto v = l->Dir(); - - // 8.9.3.75 IfcVector https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcVector.htm - // 8.9.3.30 IfcDirection https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcDirection.htm - // "The IfcDirection does not imply a vector length, and the direction ratios does not have to be normalized." - // - // Therefore, the direction ratios need to be normalized to compute points on the line. Magnitude is not used - // because it relates to the parameterization of the line, which isn't currently done for IfcCurveSegment - auto dr = v->Orientation()->DirectionRatios(); - - // normalize the direction ratios - double m_squared = std::inner_product(dr.begin(), dr.end(), dr.begin(), 0.0); - double m = sqrt(m_squared); - std::for_each(dr.begin(), dr.end(), [m](auto& d) { return d / m; }); - auto pcDx = dr[0]; - auto pcDy = dr[1]; - - auto pcStartX = c[0] * length_unit_; - auto pcStartY = c[1] * length_unit_; - - eval_ = [pcStartX, pcStartY, pcDx, pcDy, geometry_adjuster = geometry_adjuster_](double u) { - auto pcX = pcStartX + pcDx*u; - auto pcY = pcStartY + pcDy*u; - - // translate parent curve point to the origin - pcX -= pcStartX; - pcY -= pcStartY; - - auto rotate = -atan2(pcDy, pcDx); - auto csX = pcX * cos(rotate) - pcY * sin(rotate); - auto csY = pcX * sin(rotate) + pcY * cos(rotate); - - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - m.col(0) = Eigen::Vector4d(1, 0, 0, 0); - m.col(1) = Eigen::Vector4d(0, 1, 0, 0); - m.col(3) = Eigen::Vector4d(csX, csY, 0.0, 1.0); - return geometry_adjuster->transform_and_adjust(u, m); - }; - } - else if (segment_type_ == ST_VERTICAL) { - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - - auto s = l->Pnt(); - auto c = s->Coordinates(); - auto v = l->Dir(); - - // 8.9.3.75 IfcVector https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcVector.htm - // 8.9.3.30 IfcDirection https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcDirection.htm - // "The IfcDirection does not imply a vector length, and the direction ratios does not have to be normalized." - // - // Therefore, the direction ratios need to be normalized to compute points on the line. Magnitude is not used - // because it relates to the parameterization of the line, which isn't currently done for IfcCurveSegment - auto dr = v->Orientation()->DirectionRatios(); - - // normalize the direction ratios - double m_squared = std::inner_product(dr.begin(), dr.end(), dr.begin(), 0.0); - double m = sqrt(m_squared); - std::for_each(dr.begin(), dr.end(), [m](auto& d) { return d / m; }); - auto dx = dr[0]; - auto dy = dr[1]; - - auto px = c[0] * length_unit_; - auto py = c[1] * length_unit_; - - eval_ = [px, py, dx, dy, geometry_adjuster=geometry_adjuster_](double u) { - auto x = px + u/dx; - auto y = py; - - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - m.col(3) = Eigen::Vector4d(x, y, 0.0, 1.0); - return geometry_adjuster->transform_and_adjust(u, m); + return m; }; + + parent_curve_placement_ = (*parent_curve_fn_)(0.0); + } else if (segment_type_ == ST_CANT) { + Logger::Warning(std::runtime_error("Use of IfcPolyline for cant is not supported")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { + Logger::Warning(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; } - else if (segment_type_ == ST_CANT) { - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); - auto cant_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - cant_adjuster_->enable_adjustments(current_segment_count_ <= 64); - eval_ = [geometry_adjuster = geometry_adjuster_,cant_adjuster=cant_adjuster_](double u) { + } + + void operator()(const IfcSchema::IfcLine* l) { + projected_length_ = length_; + + auto c = l->Pnt()->Coordinates(); + auto pcX = c[0] * length_unit_; + auto pcY = c[1] * length_unit_; + + // 8.9.3.75 IfcVector https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcVector.htm + // 8.9.3.30 IfcDirection https://standards.buildingsmart.org/IFC/RELEASE/IFC4_3/HTML/lexical/IfcDirection.htm + // "The IfcDirection does not imply a vector length, and the direction ratios does not have to be normalized." + // + // Therefore, the direction ratios need to be normalized to compute points on the line. Magnitude is not used + // because it relates to the parameterization of the line, which isn't currently done for IfcCurveSegment + auto dr = l->Dir()->Orientation()->DirectionRatios(); + + // normalize the direction ratios + double m_squared = std::inner_product(dr.begin(), dr.end(), dr.begin(), 0.0); + double m = sqrt(m_squared); + std::for_each(dr.begin(), dr.end(), [m](auto& d) { return d / m; }); + auto pcDx = dr[0]; + auto pcDy = dr[1]; + + Eigen::Matrix4d p = Eigen::Matrix4d::Identity(); + p.col(0) = Eigen::Vector4d(pcDx, pcDy, 0, 0); + p.col(1) = Eigen::Vector4d(-pcDy, pcDx, 0, 0); + p.col(3) = Eigen::Vector4d(pcX, pcY, 0, 1); + parent_curve_placement_ = p; + + if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { + std::function convert_u; + if (segment_type_ == ST_HORIZONTAL) { + convert_u = [](double u) { return u; }; // u is along curve + } else { + // u is along horizontal, convert to along curve + convert_u = [pcDx](double u) { return u/pcDx; }; + } + + parent_curve_fn_ = [pcX, pcY, pcDx, pcDy, convert_u](double u) { + u = convert_u(u); + + auto x = pcX + pcDx * u; + auto y = pcY + pcDy * u; + Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - return geometry_adjuster->transform_and_adjust(u, cant_adjuster->transform_and_adjust(u,m)); + m.col(0) = Eigen::Vector4d(pcDx, pcDy, 0, 0); + m.col(1) = Eigen::Vector4d(-pcDy, pcDx, 0, 0); + m.col(3) = Eigen::Vector4d(x, y, 0.0, 1.0); + return m; }; - } - else { - Logger::Warning(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; - } + } else if (segment_type_ == ST_CANT) { + parent_curve_fn_ = [](double /*u*/) { Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); return m; }; + } else { + Logger::Warning(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } } - void operator()(const IfcSchema::IfcPolynomialCurve* pc) { - // see https://forums.buildingsmart.org/t/ifcpolynomialcurve-clarification/4716 for discussion on IfcPolynomialCurve - auto coeffX = pc->CoefficientsX().get_value_or(std::vector()); - auto coeffY = pc->CoefficientsY().get_value_or(std::vector()); - auto coeffZ = pc->CoefficientsZ().get_value_or(std::vector()); - if (!coeffZ.empty()) - Logger::Warning("Expected IfcPolynomialCurve.CoefficientsZ to be undefined for alignment geometry. Coefficients ignored.", pc); + void operator()(const IfcSchema::IfcPolynomialCurve* pc) { + // see https://forums.buildingsmart.org/t/ifcpolynomialcurve-clarification/4716 for discussion on IfcPolynomialCurve + auto coeffX = pc->CoefficientsX().get_value_or(std::vector()); + auto coeffY = pc->CoefficientsY().get_value_or(std::vector()); + auto coeffZ = pc->CoefficientsZ().get_value_or(std::vector()); + if (!coeffZ.empty()) { + Logger::Warning("Expected IfcPolynomialCurve.CoefficientsZ to be undefined for alignment geometry. Coefficients ignored.", pc); + } - auto length_unit = length_unit_; + auto length_unit = length_unit_; - geometry_adjuster_ = std::make_shared(mapping_, inst_, next_inst_); - geometry_adjuster_->enable_adjustments(current_segment_count_ <= 64); + if (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_VERTICAL) { + projected_length_ = length_; - if (segment_type_ == ST_HORIZONTAL) { - // @rb need to work on this - u is distance along curve, this differs from vertical where u = x - projected_length_ = length_; + // There is one significant difference between IfcPolynomalCurve used for horizontal and vertical alignments. + // For horizontal alignment, u is the distance along the curve. For vertical alignment, u is the horizontal distance. + // From 4.2.2.2.8 the polynomial curve equation is in the form of y = Ax^3 for horizontal parabolic transition segments. + // To evaluate the horizontal function, the value of x that corresponds to the distance along the curve u is needed. + // This is what the convert_u functor does. For vertical curves, the convert_u functor simply returns x = u. + std::function convert_u; - // This functor evaluates the derivative of the Y polynomial - auto df = [coeffY, length_unit](double x) -> double { - auto begin = std::next(coeffY.begin()); - auto iter = begin; - auto end = coeffY.end(); - auto length_conversion = length_unit; - double value = 0; - for (; iter != end; iter++) { - auto exp = std::distance(begin, iter); - auto coeff = (*iter) * length_conversion; - value += (double)exp * coeff * pow(x, exp); - length_conversion /= length_unit; - } - return value; + if (segment_type_ == ST_HORIZONTAL) { + // Distance along the curve is Integral[0,x] (sqrt(f'(x)^2 + 1) dx + + // This functor is the derivative of y(x) => dy/dx = f'(x) + auto df = [coeffY, length_unit](double x) -> double { + auto begin = std::next(coeffY.begin()); + auto iter = begin; + auto end = coeffY.end(); + auto length_conversion = length_unit; + double value = 0; + for (; iter != end; iter++) { + auto exp = std::distance(begin, iter); + auto coeff = (*iter) * length_conversion; + value += (double)exp * coeff * pow(x, exp); + length_conversion /= length_unit; + } + return value; + }; + + // This functor computes the curve length + // Integral[0,x] (sqrt(f'(x)^2 + 1) dx + auto curve_length_fn = [df](double x) -> double { + auto fs = [df](double x) -> double { + return sqrt(pow(df(x), 2) + 1); + }; + auto s = boost::math::quadrature::trapezoidal(fs, 0.0, x); + return s; + }; + + // There isn't a closed form solution to get x that corresponds to a distance along the curve, u + // A numerical solution is required. + // This functor finds the value of x such that s(x) - u = 0, where u is the input value and s is the + // computed curve length. + convert_u = [curve_length_fn](double u) -> double { + std::uintmax_t max_iter = 5000; + auto tol = [](double a, double b) { return fabs(b - a) < 1.0E-09; }; + auto x = u; // start by assuming u = x (it's not, but it will be close) + try { + // set up the root finding function that evaluates s(x) - u + auto f = [curve_length_fn, u](double x) -> double { return curve_length_fn(x) - u; }; + // use a root finder to get x + auto result = boost::math::tools::bracket_and_solve_root(f, x, 2.0, true, tol, max_iter); + x = result.first; + } catch (...) { + Logger::Warning("root solver failed"); + } + return x; + }; + } else { + // for vertical, u = x + convert_u = [](double u) -> double { return u; }; + } + + // This functor evaluates the polynomial at a distance u along the curve + parent_curve_fn_ = [start = start_, coeffX, coeffY, length_unit, convert_u](double u) -> Eigen::Matrix4d { + auto x = convert_u(u + start); // find x for u + // evaluate the polynomial at x + std::array*, 2> coefficients{&coeffX, &coeffY}; + std::array position{0.0, 0.0}; // = SUM(coeff*u^pos) + std::array slope{0.0, 0.0}; // slope is derivative of the curve = SUM( coeff*pos*u^(pos-1) ) + for (int i = 0; i < 2; i++) { // loop over X and Y + auto length_conversion = length_unit; + auto begin = coefficients[i]->cbegin(); + auto end = coefficients[i]->cend(); + for (auto iter = begin; iter != end; iter++) { + auto exp = std::distance(begin, iter); + auto coeff = (*iter) * length_conversion; + position[i] += coeff * pow(x, exp); + + if (iter != begin) { + slope[i] += coeff * exp * pow(x, exp - 1); + } + + length_conversion /= length_unit; + } + } + + auto X = position[0]; + auto Y = position[1]; + + auto Dx = slope[0]; + auto Dy = slope[1]; + + auto angle = atan2(Dy, Dx); + Dx = cos(angle); + Dy = sin(angle); + + Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); + m.col(0) = Eigen::Vector4d(Dx, Dy, 0, 0); + m.col(1) = Eigen::Vector4d(-Dy, Dx, 0, 0); + m.col(3) = Eigen::Vector4d(X, Y, 0.0, 1.0); + return m; }; - // This functor computes the curve length - // Integral (sqrt (f'(x) ^ 2 + 1)dx - auto fc = [df](double x) -> double { - auto fs = [df](double x) -> double { - return sqrt(pow(df(x), 2) + 1); - }; - auto s = boost::math::quadrature::trapezoidal(fs, 0.0, x); - return s; - }; - - eval_ = [start=start_,coeffX,coeffY,length_unit,geometry_adjuster = geometry_adjuster_, fc](double u) -> Eigen::Matrix4d { - // find x when u - s = 0 - std::uintmax_t max_iter = 5000; - //auto max_iter_ = max_iter; - auto tol = [](double a, double b) { return fabs(b - a) < 1.0E-09; }; - auto ux = u; - try { - auto f = [fc, u](double x) -> double { return fc(x) - u; }; - auto result = boost::math::tools::bracket_and_solve_root(f, u, 2.0, true, tol, max_iter); - ux = result.first; - } catch (...) { - Logger::Warning("root solver failed"); - } - - std::array*, 2> coefficients{&coeffX, &coeffY}; - std::array position{0.0, 0.0}; // = SUM(coeff*u^pos) - std::array slope{0.0, 0.0}; // slope is derivative of the curve = SUM( coeff*pos*u^(pos-1) ) - for (int i = 0; i < 2; i++) { // loop over X and Y - auto length_conversion = length_unit; - auto begin = coefficients[i]->cbegin(); - auto end = coefficients[i]->cend(); - for (auto iter = begin; iter != end; iter++) { - auto exp = std::distance(begin, iter); - auto coeff = (*iter) * length_conversion; - position[i] += coeff * (pow(ux /*+ start*/, exp)/* - pow(start, exp)*/); - - if (iter != begin) { - slope[i] += coeff * exp * pow(ux/* + start*/, exp - 1); - } - - length_conversion /= length_unit; - } - } - - auto x = position[0]; - auto y = position[1]; - - auto dx = slope[0]; - auto dy = slope[1]; - - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - 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) - m.col(3) = Eigen::Vector4d(x, y, 0.0, 1.0); - return geometry_adjuster->transform_and_adjust(u + start, m); - }; - } - else if (segment_type_ == ST_VERTICAL) { - projected_length_ = length_; - - auto p = inst_->Placement()->Location()->as(); - double sx = p->Coordinates()[0] * length_unit_; - double sy = p->Coordinates()[1] * length_unit_; - eval_ = [start = start_, sx, sy, coeffX, coeffY, length_unit](double u) -> Eigen::Matrix4d { - std::array*, 2> coefficients{&coeffX, &coeffY}; - std::array position{0.0, 0.0}; // = SUM(coeff*u^pos) - std::array slope{0.0, 0.0}; // slope is derivative of the curve = SUM( coeff*pos*u^(pos-1) ) - for (int i = 0; i < 2; i++) { // loop over X and Y - auto length_conversion = length_unit; - auto begin = coefficients[i]->cbegin(); - auto end = coefficients[i]->cend(); - for (auto iter = begin; iter != end; iter++) { - auto exp = std::distance(begin, iter); - auto coeff = (*iter) * length_conversion; - position[i] += coeff * pow(u + start, exp); - - if (iter != begin) { - slope[i] += coeff * exp * pow(u, exp - 1); - } - length_conversion /= length_unit; - } - } - - auto x = position[0] - coeffX[0] + sx; - auto y = position[1] - coeffY[0] + sy; - auto dx = slope[0]; - auto dy = slope[1]; - - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); - 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) - m.col(3) = Eigen::Vector4d(x, y, 0.0, 1.0); - return m; - }; - } else if (segment_type_ == ST_CANT) { - Logger::Warning(std::runtime_error("Use of IfcPolynomialCurve for cant is not supported")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { - return Eigen::Matrix4d::Identity(); - }; - } else { - Logger::Error(std::runtime_error("Unexpected segment type encountered")); - eval_ = [](double /*u*/) -> Eigen::Matrix4d { - return Eigen::Matrix4d::Identity(); - }; - } - } - - // Take the boost::type value from mpl::for_each and test it against our curve instance - template - void operator()(boost::type) { - if (parent_curve_->as()) { - (*this)(parent_curve_->as()); - } - } - - double length() const { - return (segment_type_ == ST_HORIZONTAL || segment_type_ == ST_CANT) ? length_ : projected_length_; - } - - const std::optional>& evaluation_function() const { - return eval_; - } + parent_curve_placement_ = (*parent_curve_fn_)(0.0); + } else if (segment_type_ == ST_CANT) { + Logger::Warning(std::runtime_error("Use of IfcPolynomialCurve for cant is not supported")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } else { + Logger::Error(std::runtime_error("Unexpected segment type encountered")); + parent_curve_fn_ = [](double /*u*/) -> Eigen::Matrix4d { return Eigen::Matrix4d::Identity(); }; + } + } }; } // namespace taxonomy::ptr mapping::map_impl(const IfcSchema::IfcCurveSegment* inst) { - // Find the next segment after inst - const IfcSchema::IfcCurveSegment* next_inst = nullptr; - auto composite_curves = inst->UsingCurves(); + auto composite_curves = inst->UsingCurves(); + + bool is_horizontal = false; + bool is_vertical = false; + bool is_cant = false; + if (composite_curves) { - if (composite_curves->size() == 1) { - auto segments = (*composite_curves->begin())->as()->Segments(); - bool emit_next = false; - for (auto& s : *segments) { - if (emit_next) { - next_inst = s->as(); - break; - } - if (s == inst) { - emit_next = true; - } + for (auto& cc : *composite_curves) { + if (cc->as()) { + is_cant = true; + } else if (cc->as()) { + is_vertical = true; + } else { + is_horizontal = true; } - } - else { - Logger::Warning("IfcCurveSegment belongs to multiple IfcCompositeCurve instances. Cannot determine the next segment. Geometry adjustments will not be made."); - } + } } - bool is_horizontal = false; - bool is_vertical = false; - bool is_cant = false; + if ((is_horizontal + is_vertical + is_cant) != 1) { + // We have to choose the correct functor based on usage. We can't + // support multiple, because we don't know the caller at this point. + return nullptr; + } - if (composite_curves) { - for (auto& cc : *composite_curves) { - if (cc->as()) { - is_cant = true; - } - else if (cc->as()) { - is_vertical = true; - } - else { - is_horizontal = true; - } - } - } + auto segment_type = is_horizontal ? ST_HORIZONTAL : is_vertical ? ST_VERTICAL : ST_CANT; - if ((is_horizontal + is_vertical + is_cant) != 1) { - // We have to choose the correct functor based on usage. We can't - // support multiple, because we don't know the caller at this point. - return nullptr; - } + curve_segment_evaluator cse(this, inst, length_unit_, segment_type, current_segment_count_); + boost::mpl::for_each>(std::ref(cse)); + const auto& parent_curve_fn = cse.parent_curve_function(); + const auto& parent_curve_placement = cse.parent_curve_placement(); - auto segment_type = is_horizontal ? ST_HORIZONTAL : is_vertical ? ST_VERTICAL : ST_CANT; + if (!parent_curve_fn || !parent_curve_placement) { + Logger::Error(std::runtime_error(inst->ParentCurve()->declaration().name() + " not implemented"), inst); + } - curve_segment_evaluator cse(this, inst, next_inst, length_unit_, segment_type, current_segment_count_); - boost::mpl::for_each>(std::ref(cse)); - cse.compute_segment_end_point(); - - auto& eval_fn = cse.evaluation_function(); - if(!eval_fn) throw std::runtime_error(inst->ParentCurve()->declaration().name() + " not implemented"); - auto fn = *eval_fn; - auto length = fabs(cse.length()); + // Do a negative translation of the parent curve point relative to the start of the parent curve. + // This moves parent_curve_fn(u=0.0) to coordinate (0,0). + // This is done so the curve_segment_placement is applied relative to (0,0) + Eigen::Matrix4d translation = Eigen::Matrix4d::Identity(); + translation.col(3) = -1.0 * (*parent_curve_placement).col(3); + translation(3, 3) = 1.0; - // @todo it might be suboptimal that we no longer have the spans now - auto pwf = taxonomy::make(&settings_); - pwf->spans.push_back({ length, fn }); - pwf->instance = inst; - return pwf; + // does a rotation so that the tangent of the parent curve is in the direction (1,0) + // example: if the parent curve IfcLine is at a 30 degree clockwise angle, this does + // a 30 degree counter-clockwise rotation + // Clockwise rotation matrix = [cos(angle) -sin(angle)] + // [sin(angle) cos(angle)] + // + // Counter-clockwise rotation = [ cos(angle) sin(angle)] + // [-sin(angle) cos(angle)] + // + // That's just a sign flip in positions (0,1) and (1,0) + Eigen::Matrix4d rotation = *parent_curve_placement; + rotation(0, 1) *= -1.0; + rotation(1, 0) *= -1.0; + rotation.col(3) = Eigen::Vector4d(0, 0, 0, 1); + + const Eigen::Matrix4d& curve_segment_placement = taxonomy::cast(map(inst->Placement()))->ccomponents(); + + auto fn = [curve_segment_placement, rotation, translation, parent_curve_fn](double u) -> Eigen::Matrix4d { + const Eigen::Matrix4d& p = (*parent_curve_fn)(u); + return curve_segment_placement * rotation * translation * p; + }; + + // @todo it might be suboptimal that we no longer have the spans now + auto pwf = taxonomy::make(&settings_); + auto length = fabs(cse.length()); + pwf->spans.push_back({length, fn}); + pwf->instance = inst; + return pwf; } -#endif \ No newline at end of file +#endif