00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #ifndef _INTERSECTIONPOOLUTILS_H
00034 #define _INTERSECTIONPOOLUTILS_H
00035
00036
00037 #include "IntersectionPoint.h"
00038 #include "IntersectionLink.h"
00039 #include "ParamObjectInt.h"
00040 #include "ParamPointInt.h"
00041 #include "ParamCurveInt.h"
00042 #include "ParamSurfaceInt.h"
00043 #include "Param0FunctionInt.h"
00044 #include "Param1FunctionInt.h"
00045 #include "Param2FunctionInt.h"
00046 #include "SplineCurve.h"
00047 #include "CurveOnSurface.h"
00048 #include "generic_graph_algorithms.h"
00049 #include "Point.h"
00050 #include "LineCloud.h"
00051 #include "PointCloud.h"
00052 #include <boost/shared_ptr.hpp>
00053 #include <vector>
00054 #include <set>
00055 #include <iostream>
00056 #include <fstream>
00057
00058
00059 namespace Go {
00062
00063
00064
00065
00066 void debug_write_line(const Point& p1, const Point& p2,
00067 const char* fname)
00068
00069 {
00070 std::ofstream os(fname);
00071 double array[6];
00072 std::copy(p1.begin(), p1.end(), array);
00073 std::copy(p2.begin(), p2.end(), array+3);
00074 LineCloud cloud(array, 1);
00075 cloud.writeStandardHeader(os);
00076 cloud.write(os);
00077 os.close();
00078 }
00079
00080
00081
00082 void debug_write_point(const Point& p1, const char* fname)
00083
00084 {
00085 std::ofstream os(fname);
00086 PointCloud<3> pcloud(p1.begin(), 1);
00087 pcloud.writeStandardHeader(os);
00088 pcloud.write(os);
00089 os.close();
00090 }
00091
00092
00093
00094 bool no_parent(const boost::shared_ptr<IntersectionPoint>& p)
00095
00096 {
00097 return p->parentPoint().get() == 0;
00098 }
00099
00100
00101
00102 IntersectionPoint* flip_intersecting_objects(IntersectionPoint* p)
00103
00104 {
00105 p->flipObjects();
00106 return p;
00107 }
00108
00109
00110
00111 template<class T>
00112 struct raw_pointer_comp
00113
00114 {
00115 bool operator()(boost::shared_ptr<T> A, boost::shared_ptr<T> B)
00116 {
00117 return A.get() < B.get();
00118 }
00119 };
00120
00121
00122
00123 class CrossesValue
00124
00125 {
00126 public:
00127 CrossesValue(int par_dir, double value, double eps)
00128 : par_dir_(par_dir), value_(value), eps_(eps) {}
00129 bool operator()(const boost::shared_ptr<IntersectionLink>& l) const
00130 {
00131 IntersectionPoint *p1, *p2;
00132 l->getIntersectionPoints(p1, p2);
00133 double t1 = p1->getPar()[par_dir_];
00134 double t2 = p2->getPar()[par_dir_];
00135 bool crosses;
00136 bool identity = true;
00137 if (t1 > t2) {
00138 std::swap(t1, t2);
00139 }
00140 crosses = (((value_ - t1) > eps_ && (t2 - value_) > eps_));
00141
00142 int numpar = p1->numParams1() + p1->numParams2();
00143 for (int ki=0; ki<numpar; ki++)
00144 {
00145 if (ki == par_dir_)
00146 continue;
00147 if (fabs(p1->getPar()[ki] - p2->getPar()[ki]) > eps_)
00148 identity = false;
00149 }
00150 return (crosses && !identity);
00151 }
00152 typedef const boost::shared_ptr<IntersectionLink> argument_type;
00153 typedef bool result_type;
00154 private:
00155 int par_dir_;
00156 double value_;
00157 double eps_;
00158 };
00159
00160
00161
00162 class TestInDomain
00163
00164 {
00165 public:
00166 TestInDomain(const ParamObjectInt* obj1,
00167 const ParamObjectInt* obj2,
00168 boost::shared_ptr<IntersectionPoint> ref_point)
00169 {
00170 lower_limit_.reserve(4);
00171 upper_limit_.reserve(4);
00172 epsilon_.reserve(4);
00173 int obj1_params = obj1->numParams();
00174 int obj2_params = obj2->numParams();
00175 int i;
00176 for (i = 0; i < obj1_params; ++i) {
00177 lower_limit_.push_back(obj1->startParam(i));
00178 upper_limit_.push_back(obj1->endParam(i));
00179 epsilon_.push_back(ref_point->parameterTolerance(i));
00180 }
00181 for (i = 0; i < obj2_params; ++i) {
00182 lower_limit_.push_back(obj2->startParam(i));
00183 upper_limit_.push_back(obj2->endParam(i));
00184 epsilon_.push_back(ref_point->parameterTolerance(obj1_params + i));
00185 }
00186 }
00187
00188 bool operator()(const boost::shared_ptr<IntersectionLink>& l) const
00189 {
00190 IntersectionPoint *p1, *p2;
00191 l->getIntersectionPoints(p1, p2);
00192
00193 int num_param = lower_limit_.size();
00194 for (int i = 0; i < num_param; ++i) {
00195 if (p1->getPar(i) < lower_limit_[i] - epsilon_[i] ||
00196 p1->getPar(i) > upper_limit_[i] + epsilon_[i] ||
00197 p2->getPar(i) < lower_limit_[i] - epsilon_[i] ||
00198 p2->getPar(i) > upper_limit_[i] + epsilon_[i]) {
00199
00200
00201 return false;
00202 }
00203 }
00204
00205 return true;
00206 }
00207 typedef const boost::shared_ptr<IntersectionLink> argument_type;
00208 typedef bool result_type;
00209 private:
00210 std::vector<double> lower_limit_;
00211 std::vector<double> upper_limit_;
00212 std::vector<double> epsilon_;
00213 };
00214
00215
00216
00217 bool link_is_iso_in(boost::shared_ptr<IntersectionLink> link, int dir)
00218
00219 {
00220 return link->isIsoparametricIn(dir);
00221 }
00222
00223
00224
00225 bool link_is_iso(boost::shared_ptr<IntersectionLink> link)
00226
00227 {
00228 return link->isIsoparametric();
00229 }
00230
00231
00232
00233 bool link_is_iso_in_other_than(boost::shared_ptr<IntersectionLink> link, int dir)
00234
00235 {
00236 int num_param = link->numParams();
00237 for (int i = 0; i < num_param; ++i) {
00238 if (i != dir && link->isIsoparametricIn(i)) {
00239 return true;
00240 }
00241 }
00242 return false;
00243 }
00244
00245
00246
00247 template<class T1, class T2>
00248 bool compare_first(const std::pair<T1, T2>& x, const std::pair<T1, T2>& y)
00249
00250 {
00251 return x.first < y.first;
00252 }
00253
00254
00255
00256 class ClosestPointCalculator
00257
00258 {
00259 public:
00260 ClosestPointCalculator(boost::shared_ptr<ParamObjectInt> obj)
00261 : point_(boost::dynamic_pointer_cast<ParamPointInt>(obj)),
00262 curve_(boost::dynamic_pointer_cast<ParamCurveInt>(obj)),
00263 surf_(boost::dynamic_pointer_cast<ParamSurfaceInt>(obj)),
00264 func0_(boost::dynamic_pointer_cast<Param0FunctionInt>(obj)),
00265 func1_(boost::dynamic_pointer_cast<Param1FunctionInt>(obj)),
00266 func2_(boost::dynamic_pointer_cast<Param2FunctionInt>(obj))
00267 {
00268
00269
00270 }
00271
00272
00273
00274
00275
00276
00277 double compute(const Point& pt, double* unknown_par, double eps)
00278 {
00279
00280
00281
00282
00283 ASSERT((point_ != 0) + (curve_ != 0) + (surf_ != 0)
00284 + (func0_ != 0) + (func1_ != 0) + (func2_ != 0));
00285 Point cl_pt;
00286 double dist;
00287 if (point_) {
00288
00289 point_->point(cl_pt, 0);
00290 dist = pt.dist(cl_pt);
00291 }
00292 if (func0_) {
00293
00294 func0_->point(cl_pt, 0);
00295 dist = pt.dist(cl_pt);
00296 }
00297 else if (curve_) {
00298 boost::shared_ptr<ParamCurve> parcurve = curve_->getParamCurve();
00299 parcurve->closestPoint(pt,
00300 parcurve->startparam(),
00301 parcurve->endparam(),
00302 *unknown_par,
00303 cl_pt,
00304 dist,
00305 unknown_par);
00306 } else if (func1_) {
00307 boost::shared_ptr<ParamCurve> parcurve = func1_->getParamCurve();
00308 parcurve->closestPoint(pt,
00309 parcurve->startparam(),
00310 parcurve->endparam(),
00311 *unknown_par,
00312 cl_pt,
00313 dist,
00314 unknown_par);
00315 } else if (surf_) {
00316 surf_->getParamSurface()->closestPoint(pt,
00317 unknown_par[0],
00318 unknown_par[1],
00319 cl_pt,
00320 dist,
00321 eps,
00322 0,
00323 unknown_par);
00324 } else {
00325 func2_->getParamSurface()->closestPoint(pt,
00326 unknown_par[0],
00327 unknown_par[1],
00328 cl_pt,
00329 dist,
00330 eps,
00331 0,
00332 unknown_par);
00333 }
00334 return dist;
00335 }
00336
00337
00338 private:
00339 boost::shared_ptr<ParamPointInt> point_;
00340 boost::shared_ptr<ParamCurveInt> curve_;
00341 boost::shared_ptr<ParamSurfaceInt> surf_;
00342 boost::shared_ptr<Param0FunctionInt> func0_;
00343 boost::shared_ptr<Param1FunctionInt> func1_;
00344 boost::shared_ptr<Param2FunctionInt> func2_;
00345 };
00346
00347
00348
00349 class LockedDirDistFunc
00350
00351 {
00352 public:
00353 LockedDirDistFunc(const ParamObjectInt* const o1,
00354 const ParamObjectInt* const o2,
00355 int fixed_dir,
00356 double fixed_value) :
00357 o1_(o1),
00358 o2_(o2),
00359 par2ix_(o1_->numParams()),
00360 fixed_(fixed_dir),
00361 num_par_(o1->numParams() + o2->numParams() - 1),
00362 pvec1_(3), pvec2_(3)
00363 {
00364 ASSERT(num_par_ <= 3);
00365 ASSERT(fixed_dir >= 0 && fixed_dir < 4);
00366 int o1_pars = o1->numParams();
00367 for (int i = 0; i < num_par_; ++i) {
00368 int ix = (i < fixed_) ? i : i+1;
00369 min_par_[i] = (ix < o1_pars)
00370 ? o1->startParam(ix) : o2->startParam(ix - o1_pars);
00371 max_par_[i] = (ix < o1_pars)
00372 ? o1->endParam(ix) : o2->endParam(ix - o1_pars);
00373 }
00374 tmp_[fixed_dir] = fixed_value;
00375 }
00376
00377 double operator()(const double* arg) const;
00378 void grad(const double* arg, double* grad) const;
00379 double minPar(int n) const { return min_par_[n];}
00380 double maxPar(int n) const { return max_par_[n];}
00381 private:
00382 void fillParams(const double* arg) const;
00383
00384 const ParamObjectInt* o1_;
00385 const ParamObjectInt* o2_;
00386 const int par2ix_;
00387 const int fixed_;
00388 const int num_par_;
00389 double min_par_[3];
00390 double max_par_[3];
00391
00392 mutable double tmp_[4];
00393
00394 mutable Point p1_, p2_, diff_;
00395 mutable std::vector<Point> pvec1_, pvec2_;
00396 };
00397
00398
00399
00400 class ConnectionFunctor
00401
00402 {
00403 public:
00404 ConnectionFunctor(const std::vector<boost::
00405 shared_ptr<IntersectionPoint> >& ipoints)
00406 : vec_(ipoints) {}
00407 bool operator()(int a, int b)
00408 { return vec_[a]->isConnectedTo(vec_[b]);}
00409
00410 private:
00411 const std::vector<boost::shared_ptr<IntersectionPoint> >& vec_;
00412 };
00413
00414
00415
00416
00417
00418
00419 void add_reachables_from(const IntersectionPoint* pt,
00420 const IntersectionPoint* last_pt,
00421 std::set<IntersectionPoint*>& result)
00422
00423 {
00424 std::vector<IntersectionPoint*> n;
00425 pt->getNeighbours(n);
00426 for (int i = 0; i < int(n.size()); ++i) {
00427 if (n[i] != last_pt && result.find(n[i]) == result.end()) {
00428
00429 result.insert(n[i]);
00430 add_reachables_from(n[i], pt, result);
00431 }
00432 }
00433 }
00434
00435
00436
00437 void estimate_seed_by_interpolation(const IntersectionPoint* p1,
00438 const IntersectionPoint* p2,
00439 int fixed_dir,
00440 double fixed_value,
00441 double* result)
00442
00443 {
00444 result[fixed_dir] = fixed_value;
00445 const double p1_fixed = p1->getPar(fixed_dir);
00446 const double p2_fixed = p2->getPar(fixed_dir);
00447 const double span = p2_fixed - p1_fixed;
00448
00449 const int num_param = p1->numParams1() + p1->numParams2();
00450
00451 double ratio = (span == 0) ? 0.5 : (fixed_value - p1_fixed) / span;
00452 for (int i = 0; i < num_param; ++i) {
00453 if (i != fixed_dir) {
00454 *result++ = (1 - ratio) * p1->getPar(i) + ratio * p2->getPar(i);
00455 }
00456 }
00457 }
00458
00459
00460
00461 void determine_seed(double* par,
00462 int par_start,
00463 int par_end,
00464 const Point& pt,
00465 const IntersectionPoint * const ip1,
00466 const IntersectionPoint * const ip2)
00467
00468 {
00469 const Point& ip1_point = ip1->getPoint();
00470 const Point& ip2_point = ip2->getPoint();
00471 double dist1 = pt.dist(ip1_point);
00472 double dist2 = pt.dist(ip2_point);
00473 double total_dist = dist1 + dist2;
00474 double ratio = (total_dist == 0.0) ? 0 : dist1 / total_dist;
00475
00476 int count = 0;
00477 for (int i = par_start; i < par_end; ++i) {
00478 par[count++] = (1 - ratio) * ip1->getPar(i) + ratio * ip2->getPar(i);
00479 }
00480 }
00481
00482
00483
00484 int find_point_in(IntersectionPoint* p,
00485 const std::vector<boost::
00486 shared_ptr<IntersectionPoint> >& vec)
00487
00488 {
00489 int result;
00490 for (result = 0; result < int(vec.size()); ++result) {
00491 if (vec[result].get() == p) {
00492 break;
00493 }
00494 }
00495 return result;
00496 }
00497
00498
00499
00500 boost::shared_ptr<const CurveOnSurface>
00501 make_curve_on_surface(boost::shared_ptr<const ParamSurface> surf,
00502 double u_start,
00503 double v_start,
00504 double u_end,
00505 double v_end,
00506 double p_start,
00507 double p_end)
00508
00509 {
00510 ASSERT(p_start < p_end);
00511 std::vector<double> cpoints(4);
00512 cpoints[0] = u_start;
00513 cpoints[1] = v_start;
00514 cpoints[2] = u_end;
00515 cpoints[3] = v_end;
00516 std::vector<double> kvec(4);
00517 kvec[0] = kvec[1] = p_start;
00518 kvec[2] = kvec[3] = p_end;
00519 boost::shared_ptr<ParamCurve> pcurve(new SplineCurve(2, 2, &kvec[0],
00520 &cpoints[0], 2));
00521 boost::shared_ptr<ParamSurface> temp_surf
00522 = boost::const_pointer_cast<ParamSurface>(surf);
00523 boost::shared_ptr<const CurveOnSurface>
00524 curve_on_surf(new CurveOnSurface(temp_surf, pcurve, true));
00525 return curve_on_surf;
00526 }
00527
00528
00529
00530 void extract_chains(const std::vector<boost::
00531 shared_ptr<IntersectionPoint> > pts,
00532 std::vector<std::vector<boost::
00533 shared_ptr<IntersectionPoint> > >& chains)
00534
00535 {
00536 chains.clear();
00537
00538 std::vector<std::vector<int> > paths, cycles;
00539 std::vector<int> singles;
00540 get_individual_paths(pts.size(), ConnectionFunctor(pts),
00541 paths, cycles, singles);
00542
00543
00544 for (int i = 0; i < int(singles.size()); ++i) {
00545 chains.push_back(std::vector<boost::shared_ptr<IntersectionPoint> >
00546 (1, pts[singles[i]]));
00547 }
00548
00549
00550 for (int p = 0; p < int(paths.size()); ++p) {
00551 chains.push_back(std::vector<boost::
00552 shared_ptr<IntersectionPoint> >());
00553 std::vector<boost::shared_ptr<IntersectionPoint> >& cur_vec
00554 = chains.back();
00555 for (int pp = 0; pp < int(paths[p].size()); ++pp) {
00556 cur_vec.push_back(pts[paths[p][pp]]);
00557 }
00558 }
00559 }
00560
00561
00562
00563 void LockedDirDistFunc::fillParams(const double* arg) const
00564
00565 {
00566 for (int i = 0; i < num_par_; ++i) {
00567 int ix = (i < fixed_) ? i : i+1;
00568 tmp_[ix] = arg[i];
00569 }
00570 }
00571
00572
00573
00574 double LockedDirDistFunc::operator()(const double* arg) const
00575
00576 {
00577 fillParams(arg);
00578 o1_->point(p1_, tmp_);
00579 o2_->point(p2_, tmp_ + par2ix_);
00580 return p1_.dist2(p2_);
00581 }
00582
00583
00584
00585 void LockedDirDistFunc::grad(const double* arg, double* grad) const
00586
00587 {
00588 fillParams(arg);
00589 int o1_pars = o1_->numParams();
00590 int o2_pars = o2_->numParams();
00591
00592 if (o1_pars > 0) {
00593 o1_->point(pvec1_, tmp_, 1);
00594 } else {
00595 o1_->point(pvec1_[0], tmp_);
00596 }
00597 if (o2_pars > 0) {
00598 o2_->point(pvec2_, tmp_ + par2ix_, 1);
00599 } else {
00600 o2_->point(pvec2_[0], tmp_ + par2ix_);
00601 }
00602
00603 diff_ = pvec2_[0] - pvec1_[0];
00604 double* comp = grad;
00605 for (int i = 0; i < o1_pars; ++i) {
00606 if (i != fixed_) {
00607 *comp++ = -2 * diff_ * pvec1_[i+1];
00608 }
00609 }
00610 for (int i = 0; i < o2_pars; ++i) {
00611 if (i + o1_pars != fixed_) {
00612 *comp++ = 2 * diff_ * pvec2_[i+1];
00613 }
00614 }
00615 }
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00662 }
00663
00664
00665 #endif // _INTERSECTIONPOOLUTILS_H
00666
00667