IntersectionPoolUtils.h

//===========================================================================
// GoTools - SINTEF Geometry Tools
//
// GoTools module: Intersections, version 1.0
//
// Copyright (C) 2000-2007 SINTEF ICT, Applied Mathematics, Norway.
//
// This program is free software; you can redistribute it and/or          
// modify it under the terms of the GNU General Public License            
// as published by the Free Software Foundation version 2 of the License. 
//
// This program is distributed in the hope that it will be useful,        
// but WITHOUT ANY WARRANTY; without even the implied warranty of         
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          
// GNU General Public License for more details.                           
//
// You should have received a copy of the GNU General Public License      
// along with this program; if not, write to the Free Software            
// Foundation, Inc.,                                                      
// 59 Temple Place - Suite 330,                                           
// Boston, MA  02111-1307, USA.                                           
//
// Contact information: E-mail: tor.dokken@sintef.no                      
// SINTEF ICT, Department of Applied Mathematics,                         
// P.O. Box 124 Blindern,                                                 
// 0314 Oslo, Norway.                                                     
//
// Other licenses are also available for this software, notably licenses
// for:
// - Building commercial software.                                        
// - Building software whose source code you wish to keep private.        
//===========================================================================
#ifndef _INTERSECTIONPOOLUTILS_H
#define _INTERSECTIONPOOLUTILS_H


#include "IntersectionPoint.h"
#include "IntersectionLink.h"
#include "ParamObjectInt.h"
#include "ParamPointInt.h"
#include "ParamCurveInt.h"
#include "ParamSurfaceInt.h"
#include "Param0FunctionInt.h"
#include "Param1FunctionInt.h"
#include "Param2FunctionInt.h"
#include "SplineCurve.h"
#include "CurveOnSurface.h"
#include "generic_graph_algorithms.h"
#include "Point.h"
#include "LineCloud.h"
#include "PointCloud.h"
#include <boost/shared_ptr.hpp>
#include <vector>
#include <set>
#include <iostream>
#include <fstream>


namespace Go {



//===========================================================================
void debug_write_line(const Point& p1, const Point& p2,
                      const char* fname)
//===========================================================================
{
    std::ofstream os(fname);
    double array[6];
    std::copy(p1.begin(), p1.end(), array);
    std::copy(p2.begin(), p2.end(), array+3);
    LineCloud cloud(array, 1);
    cloud.writeStandardHeader(os);
    cloud.write(os);
    os.close();
}


//===========================================================================
void debug_write_point(const Point& p1, const char* fname)
//===========================================================================
{
    std::ofstream os(fname);
    PointCloud<3> pcloud(p1.begin(), 1);
    pcloud.writeStandardHeader(os);
    pcloud.write(os);
    os.close();
}


//===========================================================================
bool no_parent(const boost::shared_ptr<IntersectionPoint>& p) 
//===========================================================================
{
    return p->parentPoint().get() == 0;
}


//===========================================================================
IntersectionPoint* flip_intersecting_objects(IntersectionPoint* p)
//===========================================================================
{ 
    p->flipObjects(); 
    return p;
}


//===========================================================================
template<class T>
struct raw_pointer_comp 
//===========================================================================
{
    bool operator()(boost::shared_ptr<T> A, boost::shared_ptr<T> B)
    {
        return A.get() < B.get();
    }
};


//===========================================================================
class CrossesValue  // predicate for STL function
//===========================================================================
{
public:
    CrossesValue(int par_dir, double value, double eps) 
        : par_dir_(par_dir), value_(value), eps_(eps) {}
    bool operator()(const boost::shared_ptr<IntersectionLink>& l) const 
    {
        IntersectionPoint *p1, *p2;
        l->getIntersectionPoints(p1, p2);
        double t1 = p1->getPar()[par_dir_];
        double t2 = p2->getPar()[par_dir_];
        bool crosses;
        bool identity = true;
        if (t1 > t2) {
            std::swap(t1, t2);
        }
        crosses = (((value_ - t1) > eps_ && (t2 - value_) > eps_));

        int numpar = p1->numParams1() + p1->numParams2();
        for (int ki=0; ki<numpar; ki++)
        {
            if (ki == par_dir_)
                continue;
            if (fabs(p1->getPar()[ki] - p2->getPar()[ki]) > eps_)
                identity = false;
        }
        return (crosses && !identity);
    }
    typedef const boost::shared_ptr<IntersectionLink> argument_type;
    typedef bool result_type;
private:
    int par_dir_;
    double value_;
    double eps_;
};


//===========================================================================
class TestInDomain // predicate for STL function
//===========================================================================
{
public:
    TestInDomain(const ParamObjectInt* obj1, 
                 const ParamObjectInt* obj2,
                 boost::shared_ptr<IntersectionPoint> ref_point)
    {
        lower_limit_.reserve(4);
        upper_limit_.reserve(4);
        epsilon_.reserve(4);
        int obj1_params = obj1->numParams();
        int obj2_params = obj2->numParams();
        int i;
        for (i = 0; i < obj1_params; ++i) {
            lower_limit_.push_back(obj1->startParam(i));
            upper_limit_.push_back(obj1->endParam(i));
            epsilon_.push_back(ref_point->parameterTolerance(i));
        }
        for (i = 0; i < obj2_params; ++i) {
            lower_limit_.push_back(obj2->startParam(i));
            upper_limit_.push_back(obj2->endParam(i));
            epsilon_.push_back(ref_point->parameterTolerance(obj1_params + i));
        }
    }
    
    bool operator()(const boost::shared_ptr<IntersectionLink>& l) const
    {
        IntersectionPoint *p1, *p2;
        l->getIntersectionPoints(p1, p2);
        
        int num_param = lower_limit_.size();
        for (int i = 0; i < num_param; ++i) {
            if (p1->getPar(i) < lower_limit_[i] - epsilon_[i] ||
                p1->getPar(i) > upper_limit_[i] + epsilon_[i] ||
                p2->getPar(i) < lower_limit_[i] - epsilon_[i] ||
                p2->getPar(i) > upper_limit_[i] + epsilon_[i]) {
                // p1 or p2 for this link was found to lie outside of
                // the domain
                return false;
            }
        }
        // everything was inside the domain
        return true;
    }
    typedef const boost::shared_ptr<IntersectionLink> argument_type;
    typedef bool result_type;
private:
    std::vector<double> lower_limit_;
    std::vector<double> upper_limit_;
    std::vector<double> epsilon_;
};


//===========================================================================
bool link_is_iso_in(boost::shared_ptr<IntersectionLink> link, int dir)
//===========================================================================
{
    return link->isIsoparametricIn(dir);
}


//===========================================================================
bool link_is_iso(boost::shared_ptr<IntersectionLink> link)
//===========================================================================
{
    return link->isIsoparametric();
}


//===========================================================================
bool link_is_iso_in_other_than(boost::shared_ptr<IntersectionLink> link, int dir)
//===========================================================================
{
    int num_param = link->numParams();
    for (int i = 0; i < num_param; ++i) {
        if (i != dir && link->isIsoparametricIn(i)) {
            return true;
        }
    }
    return false;
}


//===========================================================================
template<class T1, class T2>
bool compare_first(const std::pair<T1, T2>& x, const std::pair<T1, T2>& y)
//===========================================================================
{
    return x.first < y.first;
}


//===========================================================================
class ClosestPointCalculator
//===========================================================================
{
public:
    ClosestPointCalculator(boost::shared_ptr<ParamObjectInt> obj)
        : point_(boost::dynamic_pointer_cast<ParamPointInt>(obj)),
          curve_(boost::dynamic_pointer_cast<ParamCurveInt>(obj)),
          surf_(boost::dynamic_pointer_cast<ParamSurfaceInt>(obj)),
          func0_(boost::dynamic_pointer_cast<Param0FunctionInt>(obj)),
          func1_(boost::dynamic_pointer_cast<Param1FunctionInt>(obj)),
          func2_(boost::dynamic_pointer_cast<Param2FunctionInt>(obj))
    {
        // check that exactly one of free_curve_ and free_surface_ is
        // active
    }
    
    // calculate the closest point between the two objects at the
    // parameter value 'known_par' for the reference object.  The
    // distance at this point is returned.  The value(s) pointed to by
    // 'unknown_par' are also used as seed to the closest point
    // algorithm.
    double compute(const Point& pt, double* unknown_par, double eps)
    {
        // check that exactly one of free_curve_ and free_surface_ is
        // active
//      int nmb = (point_ != 0) + (curve_ != 0) + (surf_ != 0)
//          + (func0_ != 0) + (func1_ != 0) + (func2_ != 0);
        ASSERT((point_ != 0) + (curve_ != 0) + (surf_ != 0)
               + (func0_ != 0) + (func1_ != 0) + (func2_ != 0));
        Point cl_pt;
        double dist;
        if (point_) { // the other object is a point
            // very easy.  The closest point must be that point!
            point_->point(cl_pt, 0);
            dist = pt.dist(cl_pt);
        }
        if (func0_) { // the other object is a point
            // very easy.  The closest point must be that point!
            func0_->point(cl_pt, 0);
            dist = pt.dist(cl_pt);
        }
        else if (curve_) { // the other object is a curve
            boost::shared_ptr<ParamCurve> parcurve = curve_->getParamCurve();
            parcurve->closestPoint(pt, 
                                   parcurve->startparam(), 
                                   parcurve->endparam(),
                                   *unknown_par,
                                   cl_pt,
                                   dist,
                                   unknown_par);
        } else if (func1_) { // the other object is a one-valued function
            boost::shared_ptr<ParamCurve> parcurve = func1_->getParamCurve();
            parcurve->closestPoint(pt,
                                   parcurve->startparam(),
                                   parcurve->endparam(),
                                   *unknown_par,
                                   cl_pt,
                                   dist,
                                   unknown_par);
        } else if (surf_) { // the other object is a surface
            surf_->getParamSurface()->closestPoint(pt, 
                                                   unknown_par[0], 
                                                   unknown_par[1], 
                                                   cl_pt, 
                                                   dist, 
                                                   eps, 
                                                   0, 
                                                   unknown_par);
        } else { // the other object is a two-valued function
            func2_->getParamSurface()->closestPoint(pt, 
                                                    unknown_par[0], 
                                                    unknown_par[1], 
                                                    cl_pt, 
                                                    dist, 
                                                    eps,
                                                    0,
                                                    unknown_par);
        }
        return dist;
    }
    
    
private:
    boost::shared_ptr<ParamPointInt> point_;
    boost::shared_ptr<ParamCurveInt> curve_;
    boost::shared_ptr<ParamSurfaceInt> surf_;
    boost::shared_ptr<Param0FunctionInt> func0_;
    boost::shared_ptr<Param1FunctionInt> func1_;
    boost::shared_ptr<Param2FunctionInt> func2_;
};


//===========================================================================
class LockedDirDistFunc
//===========================================================================
{
public:
    LockedDirDistFunc(const ParamObjectInt* const o1, 
                      const ParamObjectInt* const o2, 
                      int fixed_dir,
                      double fixed_value) :
        o1_(o1), 
        o2_(o2), 
        par2ix_(o1_->numParams()),
        fixed_(fixed_dir), 
        num_par_(o1->numParams() + o2->numParams() - 1),
        pvec1_(3), pvec2_(3)
    {
        ASSERT(num_par_ <= 3);
        ASSERT(fixed_dir >= 0 && fixed_dir < 4);
        int o1_pars = o1->numParams();
        for (int i = 0; i < num_par_; ++i) {
            int ix = (i < fixed_) ? i : i+1;
            min_par_[i] = (ix < o1_pars) 
                ? o1->startParam(ix) : o2->startParam(ix - o1_pars);
            max_par_[i] = (ix < o1_pars)
                ? o1->endParam(ix) : o2->endParam(ix - o1_pars);
        }
        tmp_[fixed_dir] = fixed_value;
    }

    double operator()(const double* arg) const; 
    void grad(const double* arg, double* grad) const; 
    double minPar(int n) const { return min_par_[n];}
    double maxPar(int n) const { return max_par_[n];}
private:
    void fillParams(const double* arg) const;

    const ParamObjectInt* o1_;
    const ParamObjectInt* o2_;
    const int par2ix_;
    const int fixed_;
    const int num_par_;
    double min_par_[3];
    double max_par_[3];

    mutable double tmp_[4];

    mutable Point p1_, p2_, diff_;
    mutable std::vector<Point> pvec1_, pvec2_;
};


//===========================================================================
class ConnectionFunctor
//===========================================================================
{
public:
    ConnectionFunctor(const std::vector<boost::
                      shared_ptr<IntersectionPoint> >& ipoints)
        : vec_(ipoints) {}
    bool operator()(int a, int b)
    { return vec_[a]->isConnectedTo(vec_[b]);}

private:
    const std::vector<boost::shared_ptr<IntersectionPoint> >& vec_;
};


//===========================================================================


//===========================================================================
void add_reachables_from(const IntersectionPoint* pt,
                         const IntersectionPoint* last_pt,
                         std::set<IntersectionPoint*>& result)
//===========================================================================
{
    std::vector<IntersectionPoint*> n;
    pt->getNeighbours(n);
    for (int i = 0; i < int(n.size()); ++i) {
        if (n[i] != last_pt && result.find(n[i]) == result.end()) {
            // this point must be added
            result.insert(n[i]);
            add_reachables_from(n[i], pt, result);
        }
    }
}


//===========================================================================
void estimate_seed_by_interpolation(const IntersectionPoint* p1,
                                    const IntersectionPoint* p2,
                                    int fixed_dir,
                                    double fixed_value,
                                    double* result)
//===========================================================================
{
    result[fixed_dir] = fixed_value;
    const double p1_fixed = p1->getPar(fixed_dir);
    const double p2_fixed = p2->getPar(fixed_dir);
    const double span = p2_fixed - p1_fixed;
    //ASSERT(fabs(span) > 0);
    const int num_param = p1->numParams1() + p1->numParams2();
    
    double ratio = (span == 0) ? 0.5 : (fixed_value - p1_fixed) / span;
    for (int i = 0; i < num_param; ++i) {
        if (i != fixed_dir) {
            *result++ = (1 - ratio) * p1->getPar(i) + ratio * p2->getPar(i);
        }
    }
}


//===========================================================================
void determine_seed(double* par, 
                    int par_start,
                    int par_end,
                    const Point& pt,
                    const IntersectionPoint * const ip1,
                    const IntersectionPoint * const ip2)
//===========================================================================
{
    const Point& ip1_point = ip1->getPoint();
    const Point& ip2_point = ip2->getPoint();
    double dist1 = pt.dist(ip1_point);
    double dist2 = pt.dist(ip2_point);
    double total_dist = dist1 + dist2;
    double ratio = (total_dist == 0.0) ? 0 : dist1 / total_dist;

    int count = 0;
    for (int i = par_start; i < par_end; ++i) {
        par[count++] = (1 - ratio) * ip1->getPar(i) + ratio * ip2->getPar(i);
    }
}


//===========================================================================
int find_point_in(IntersectionPoint* p,
                  const std::vector<boost::
                  shared_ptr<IntersectionPoint> >& vec)
//===========================================================================
{
    int result;
    for (result = 0; result < int(vec.size()); ++result) {
        if (vec[result].get() == p) {
            break;
        }
    }
    return result;
}


//===========================================================================
boost::shared_ptr<const CurveOnSurface>
make_curve_on_surface(boost::shared_ptr<const ParamSurface> surf,
                      double u_start,
                      double v_start,
                      double u_end,
                      double v_end,
                      double p_start,
                      double p_end)
//===========================================================================
{
    ASSERT(p_start < p_end);
    std::vector<double> cpoints(4);
    cpoints[0] = u_start;
    cpoints[1] = v_start;
    cpoints[2] = u_end;
    cpoints[3] = v_end;
    std::vector<double> kvec(4);
    kvec[0] = kvec[1] = p_start;
    kvec[2] = kvec[3] = p_end;
    boost::shared_ptr<ParamCurve> pcurve(new SplineCurve(2, 2, &kvec[0],
                                                  &cpoints[0], 2));
    boost::shared_ptr<ParamSurface> temp_surf
        = boost::const_pointer_cast<ParamSurface>(surf);
    boost::shared_ptr<const CurveOnSurface>
        curve_on_surf(new CurveOnSurface(temp_surf, pcurve, true));
    return curve_on_surf;
}


//===========================================================================
void extract_chains(const std::vector<boost::
                    shared_ptr<IntersectionPoint> > pts,
                    std::vector<std::vector<boost::
                    shared_ptr<IntersectionPoint> > >& chains)
//===========================================================================
{
    chains.clear();
    // find out which IntersectionPoints are connected
    std::vector<std::vector<int> > paths, cycles;
    std::vector<int> singles;
    get_individual_paths(pts.size(), ConnectionFunctor(pts),
                         paths, cycles, singles);

    // generating chains of 0 length  (single points)
    for (int i = 0; i < int(singles.size()); ++i) {
        chains.push_back(std::vector<boost::shared_ptr<IntersectionPoint> >
                         (1, pts[singles[i]]));
    }

    // generating chains of nonzero length (linked points)
    for (int p = 0; p < int(paths.size()); ++p) {
        chains.push_back(std::vector<boost::
                         shared_ptr<IntersectionPoint> >());
        std::vector<boost::shared_ptr<IntersectionPoint> >& cur_vec
            = chains.back();
        for (int pp = 0; pp < int(paths[p].size()); ++pp) {
            cur_vec.push_back(pts[paths[p][pp]]);
        }
    }
}


//===========================================================================
void LockedDirDistFunc::fillParams(const double* arg) const 
//===========================================================================
{
    for (int i = 0; i < num_par_; ++i) {
        int ix = (i < fixed_) ? i : i+1;
        tmp_[ix] = arg[i];
    }
}


//===========================================================================
double LockedDirDistFunc::operator()(const double* arg) const
//===========================================================================
{
    fillParams(arg);
    o1_->point(p1_, tmp_);
    o2_->point(p2_, tmp_ + par2ix_);
    return p1_.dist2(p2_);
}


//===========================================================================
void LockedDirDistFunc::grad(const double* arg, double* grad) const
//===========================================================================
{
    fillParams(arg);
    int o1_pars = o1_->numParams();
    int o2_pars = o2_->numParams();

    if (o1_pars > 0) {
        o1_->point(pvec1_, tmp_, 1);
    } else {
        o1_->point(pvec1_[0], tmp_);
    }
    if (o2_pars > 0) {
        o2_->point(pvec2_, tmp_ + par2ix_, 1);
    } else {
        o2_->point(pvec2_[0], tmp_ + par2ix_);
    }

    diff_ = pvec2_[0] - pvec1_[0];
    double* comp = grad;
    for (int i = 0; i < o1_pars; ++i) {
        if (i != fixed_) {
            *comp++ = -2 * diff_ * pvec1_[i+1];
        }
    }
    for (int i = 0; i < o2_pars; ++i) {
        if (i + o1_pars != fixed_) {
            *comp++ = 2 * diff_ * pvec2_[i+1];
        }
    }
}


// //===========================================================================
// void find_most_distant(const std::vector<boost::shared_ptr<IntersectionPoint> > pts, 
//                     int pdir1, 
//                     int pdir2, 
//                     double& dist1, 
//                     double& dist2, 
//                     boost::shared_ptr<IntersectionPoint>& pt1_dir1,
//                     boost::shared_ptr<IntersectionPoint>& pt2_dir1,
//                     boost::shared_ptr<IntersectionPoint>& pt1_dir2,
//                     boost::shared_ptr<IntersectionPoint>& pt2_dir2)
// //===========================================================================
// {
//     // find out which IntersectionPoints are connected
//     std::vector<std::vector<int> > paths, cycles;
//     get_individual_paths(pts.size(), ConnectionFunctor(pts), paths, cycles);
//     // ASSERT(cycles.size() == 0); // this should always be true

//     // find 'longest parametric span' of a chain of IntersectionPoints in the two specified
//     // parametric directions
//     dist1 = dist2 = 0;
//     pt1_dir1 = pt2_dir1 = pt1_dir2 = pt2_dir2 = boost::shared_ptr<IntersectionPoint>(); 
//     boost::shared_ptr<IntersectionPoint> tmp1, tmp2;
//     for (int p = 0; p < int(paths.size()); ++p) {
//      tmp1 = pts[paths[p].front()];
//      tmp2 = pts[paths[p].back()];
//      double d1_tmp = fabs(tmp1->getPar(pdir1) - tmp2->getPar(pdir1));
//      double d2_tmp = fabs(tmp1->getPar(pdir2) - tmp2->getPar(pdir2));
//      if (d1_tmp >= dist1) {
//          dist1 = d1_tmp;
//          pt1_dir1 = tmp1;
//          pt2_dir1 = tmp2;
//      } 
//      if (d2_tmp >= dist2) {
//          dist2 = d2_tmp;
//          pt1_dir2 = tmp1;
//          pt2_dir2 = tmp2;
//      }
//     }
// }


// //===========================================================================

} // namespace Go


#endif // _INTERSECTIONPOOLUTILS_H

---