boost::polygon self intersection problem

ここ２つ前からの投稿の問題は解決しました。この前に geos というライブラリでもテストコードを実行させてみたところ

TopologyException: side location conflict at -752.63088031450525 -148.60182192355231

という例外が発生しました。boost::geometry はロバストに出来ていて、自分が例外キャッチするのを忘れていただけで、ちゃんと

Boost.Geometry Overlay invalid input exception

という例外が送出されていました。 　で、どういう問題が発生してたか boost::geometryのコードを読んでみたところ、図形が Simple でない＝自己交差を起こしている場合に例外が送出されていました。 なので、自己交差を解消してやれば、問題をクリアできそうな感触を得ました。 残念ながら、自己交差を解消する関数は、見当たらなかったので自作。スタイルが古いですが、古いコンパイラを使っているので auto とか使えなかったんです。 list::insert してるあたりがダサいですが、古い環境なんで、このままです(>_<) 結局、自己交差を解消してやれば、boost::polygon のコードも通りました。

#include <boost/cstdint.hpp>
#include <boost/polygon/polygon.hpp>
#include <vector>
#include <deque>
#include <cmath>
#include <iomanip>

namespace bpl = boost::polygon;
typedef double point_value_type;
typedef bpl::segment_data<point_value_type> Segment;
typedef bpl::polygon_with_holes_data<point_value_type> Polygon;
typedef bpl::polygon_data<point_value_type> Ring;
typedef bpl::polygon_traits<Polygon>::point_type Point;
typedef std::vector<Point> PointSet;
typedef std::vector<Polygon> PolygonSet;
typedef std::list<Segment> SegmentSet;

#define ACCY 1e-6

Polygon make_polygon( const point_value_type* pts, size_t count ) {
  Polygon polygon;
  polygon.self_.coords_.resize( count );
  for( size_t i = 0; i < count; ++i ) {
    polygon.self_.coords_[i].x( *pts++ );
    polygon.self_.coords_[i].y( *pts++ );
  }
  return polygon;
}

void construct( PolygonSet& polygons, Polygon polygon ) {
  using namespace bpl::operators;
  polygons |= polygon; 
}

void construct( SegmentSet& segments, const point_value_type* pts, size_t count ) {
  point_value_type ax = pts[count*2-2];
  point_value_type ay = pts[count*2-1];
  for( size_t i = 0; i < count; ++i ) {
    point_value_type bx = *pts++;
    point_value_type by = *pts++;
    segments.push_back( Segment( Point(ax, ay), Point(bx, by) ) );
    ax = bx;  ay = by;
  }
}

Polygon make_polygon(const SegmentSet& segments ) {
  Polygon polygon;
  size_t count = segments.size();
  polygon.self_.coords_.resize( count );
  size_t i = 0;
  SegmentSet::const_iterator s = segments.begin();
  SegmentSet::const_iterator e = segments.end();
  while( s != e ) {
    polygon.self_.coords_[i++] = bpl::low(*s++);
  }
  return polygon;
}


std::ostream& operator << (std::ostream& os, const Point& p) {
  os << std::setprecision(10);
  os << "{ " << bpl::x(p) << ", " << bpl::y(p) << " },\n";
  return os;
}

std::ostream& operator << (std::ostream& os, const SegmentSet& segments) {
  SegmentSet::const_iterator s = segments.begin();
  SegmentSet::const_iterator e = segments.end();
  while( s != e ) {
    os << bpl::low( *s++ );
  }
  return os;
}

//! ４点交点（線分による完全交差版）を取得する
/*!
 @attention 交点が存在しない場合、x,y は未定義である
 @retval true 交点が存在
 @retval false 交点は存在しない
*/
bool k_4t_close( 
 double& x, //!< [out] 交点Ｘ座標値 
 double& y, //!< [out] 交点Ｙ座標値
 double xk, //!< [in] 線分ＫＬの始点Ｘ座標値
 double yk, //!< [in] 線分ＫＬの始点Ｙ座標値
 double xl, //!< [in] 線分ＫＬの終点Ｘ座標値
 double yl, //!< [in] 線分ＫＬの終点Ｙ座標値
 double xm, //!< [in] 線分ＭＮの始点Ｘ座標値
 double ym, //!< [in] 線分ＭＮの始点Ｙ座標値
 double xn, //!< [in] 線分ＭＮの終点Ｘ座標値
 double yn   //!< [in] 線分ＭＮの終点Ｙ座標値
) {
 long double xlk = xl - xk;
 long double ylk = yl - yk;
 long double xnm = xn - xm;
 long double ynm = yn - ym;
 long double xmk = xm - xk;
 long double ymk = ym - yk;
 long double det = xnm * ylk - ynm * xlk;
 if( std::fabs( det ) < ACCY ) return false;
 long double s = (xnm * ymk - ynm * xmk ) / det;
 long double t = (xlk * ymk - ylk * xmk ) / det;
 if( s < 0.0 || 1.0 < s || t < 0.0 || 1.0 < t ) return false;
 x = xk + xlk * s;
 y = yk + ylk * s;
 return true;
}

void resolve_segment( SegmentSet& segments ) {
  SegmentSet::iterator is = segments.begin();
  SegmentSet::iterator ie = segments.end();
  while( is != ie ) {
    SegmentSet::iterator cs = segments.begin();
    SegmentSet::iterator ce = segments.end();
    while( cs != ce ) {
      if( cs != is ) {
        if( bpl::intersects( *cs, *is, false ) ) {
          //std::cout << "X" << std::endl;
          double xx, yy;
          Point a = bpl::low( *cs );
          Point b = bpl::high( *cs );
          Point c = bpl::low( *is );
          Point d = bpl::high( *is );
          k_4t_close( xx, yy,
            bpl::x( a ), bpl::y( a ), bpl::x( b ), bpl::y( b ),
            bpl::x( c ), bpl::y( c ), bpl::x( d ), bpl::y( d )
          );
          Point n( xx, yy );
          //std::cout << a << n << b << std::endl;
          *cs = Segment( n, b );
          cs = segments.insert( cs, Segment( a, n ) );
          *is = Segment( n, d );
          is = segments.insert( is, Segment( c, n ) );
        } else {
          //std::cout << "o" << std::endl;
        }
      }
      ++cs;
    }
    ++is;
  }
}


int main() {
  point_value_type pts[] = {
   -162.277344, -684.941406,
   -154.300781, -684.707031,
   -147.291016, -768.253906,
   -148.601562, -752.630859,
   -156.675781, -753.292969,
   -155.425781, -768.546875,
  };
  /*
  point_value_type pts2[] = {
   -155.425781, -768.546875 ,
   -162.277344, -684.941406 ,
   -154.300781, -684.707031 , 
   -148.6018219, -752.6308803 ,
   -147.291016, -768.253906 ,
   -148.601562, -752.630859 ,
   -148.6018219, -752.6308803 ,
   -156.675781, -753.292969 ,
  };
  */
  
  SegmentSet segments;
  construct( segments, pts, 6 );

  std::cout << segments;
  
  resolve_segment( segments );
  
  std::cout << "===== RESOLVE ====\n";
  std::cout << segments;
  
  PolygonSet polygons;
  //construct( polygons, make_polygon( pts, 6 ) );
  construct( polygons, make_polygon( segments ) );
  
  return 0;
}

追記：あれ？ insert のあたり、よく考えたら、まずいですかね・・・。