src/2geom/nearest-point.cpp

   1 /*
   2  * nearest point routines for D2<SBasis> and Piecewise<D2<SBasis>>
   3  *
   4  * Authors:
   5  *
   6  *              Marco Cecchetti <mrcekets at gmail.com>
   7  *
   8  * Copyright 2007-2008  authors
   9  *
  10  * This library is free software; you can redistribute it and/or
  11  * modify it either under the terms of the GNU Lesser General Public
  12  * License version 2.1 as published by the Free Software Foundation
  13  * (the "LGPL") or, at your option, under the terms of the Mozilla
  14  * Public License Version 1.1 (the "MPL"). If you do not alter this
  15  * notice, a recipient may use your version of this file under either
  16  * the MPL or the LGPL.
  17  *
  18  * You should have received a copy of the LGPL along with this library
  19  * in the file COPYING-LGPL-2.1; if not, write to the Free Software
  20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  21  * You should have received a copy of the MPL along with this library
  22  * in the file COPYING-MPL-1.1
  23  *
  24  * The contents of this file are subject to the Mozilla Public License
  25  * Version 1.1 (the "License"); you may not use this file except in
  26  * compliance with the License. You may obtain a copy of the License at
  27  * http://www.mozilla.org/MPL/
  28  *
  29  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
  30  * OF ANY KIND, either express or implied. See the LGPL or the MPL for
  31  * the specific language governing rights and limitations.
  32  */
  33
  34
  35 #include <2geom/nearest-point.h>
  36 #include <algorithm>
  37
  38
  39 namespace Geom
  40 {
  41
  42 ////////////////////////////////////////////////////////////////////////////////
  43 // D2<SBasis> versions
  44
  45 /*
  46  * Return the parameter t of a nearest point on the portion of the curve "c",
  47  * related to the interval [from, to], to the point "p".
  48  * The needed curve derivative "dc" is passed as parameter.
  49  * The function return the first nearest point to "p" that is found.
  50  */
  51
  52 double nearest_point( Point const& p,
  53                       D2<SBasis> const& c,
  54                       D2<SBasis> const& dc,
  55                       double from, double to )
  56 {
  57     if ( from > to ) std::swap(from, to);
  58     if ( from < 0 || to > 1 )
  59     {
  60         THROW_RANGEERROR("[from,to] interval out of bounds");
  61     }
  62     if (c.isConstant()) return from;
  63     SBasis dd = dot(c - p, dc);
  64     std::vector<double> zeros = Geom::roots(dd);
  65
  66     double closest = from;
  67     double min_dist_sq = L2sq(c(from) - p);
  68     double distsq;
  69     for ( unsigned int i = 0; i < zeros.size(); ++i )
  70     {
  71         distsq = L2sq(c(zeros[i]) - p);
  72         if ( min_dist_sq > L2sq(c(zeros[i]) - p) )
  73         {
  74             closest = zeros[i];
  75             min_dist_sq = distsq;
  76         }
  77     }
  78     if ( min_dist_sq > L2sq( c(to) - p ) )
  79         closest = to;
  80     return closest;
  81
  82 }
  83
  84 /*
  85  * Return the parameters t of all the nearest points on the portion of
  86  * the curve "c", related to the interval [from, to], to the point "p".
  87  * The needed curve derivative "dc" is passed as parameter.
  88  */
  89
  90 std::vector<double>
  91 all_nearest_points( Point const& p,
  92             D2<SBasis> const& c,
  93             D2<SBasis> const& dc,
  94             double from, double to )
  95 {
  96     std::swap(from, to);
  97     if ( from > to ) std::swap(from, to);
  98     if ( from < 0 || to > 1 )
  99     {
 100         THROW_RANGEERROR("[from,to] interval out of bounds");
 101     }
 102
 103     std::vector<double> result;
 104     if (c.isConstant())
 105     {
 106         result.push_back(from);
 107         return result;
 108     }
 109     SBasis dd = dot(c - p, dc);
 110
 111     std::vector<double> zeros = Geom::roots(dd);
 112     std::vector<double> candidates;
 113     candidates.push_back(from);
 114     candidates.insert(candidates.end(), zeros.begin(), zeros.end());
 115     candidates.push_back(to);
 116     std::vector<double> distsq;
 117     distsq.reserve(candidates.size());
 118     for ( unsigned int i = 0; i < candidates.size(); ++i )
 119     {
 120         distsq.push_back( L2sq(c(candidates[i]) - p) );
 121     }
 122     unsigned int closest = 0;
 123     double dsq = distsq[0];
 124     for ( unsigned int i = 1; i < candidates.size(); ++i )
 125     {
 126         if ( dsq > distsq[i] )
 127         {
 128             closest = i;
 129             dsq = distsq[i];
 130         }
 131     }
 132     for ( unsigned int i = 0; i < candidates.size(); ++i )
 133     {
 134         if( distsq[closest] == distsq[i] )
 135         {
 136             result.push_back(candidates[i]);
 137         }
 138     }
 139     return result;
 140 }
 141
 142
 143 ////////////////////////////////////////////////////////////////////////////////
 144 // Piecewise< D2<SBasis> > versions
 145
 146
 147 double nearest_point( Point const& p,
 148                       Piecewise< D2<SBasis> > const& c,
 149                       double from, double to )
 150 {
 151     if ( from > to ) std::swap(from, to);
 152     if ( from < c.cuts[0] || to > c.cuts[c.size()] )
 153     {
 154         THROW_RANGEERROR("[from,to] interval out of bounds");
 155     }
 156
 157     unsigned int si = c.segN(from);
 158     unsigned int ei = c.segN(to);
 159     if ( si == ei )
 160     {
 161         double nearest=
 162             nearest_point(p, c[si], c.segT(from, si), c.segT(to, si));
 163         return c.mapToDomain(nearest, si);
 164     }
 165     double t;
 166     double nearest = nearest_point(p, c[si], c.segT(from, si));
 167     unsigned int ni = si;
 168     double dsq;
 169     double mindistsq = distanceSq(p, c[si](nearest));
 170     Rect bb(Geom::Point(0,0),Geom::Point(0,0));
 171     for ( unsigned int i = si + 1; i < ei; ++i )
 172     {
 173         bb = *bounds_fast(c[i]);
 174         dsq = distanceSq(p, bb);
 175         if ( mindistsq <= dsq ) continue;
 176         t = nearest_point(p, c[i]);
 177         dsq = distanceSq(p, c[i](t));
 178         if ( mindistsq > dsq )
 179         {
 180             nearest = t;
 181             ni = i;
 182             mindistsq = dsq;
 183         }
 184     }
 185     bb = *bounds_fast(c[ei]);
 186     dsq = distanceSq(p, bb);
 187     if ( mindistsq > dsq )
 188     {
 189         t = nearest_point(p, c[ei], 0, c.segT(to, ei));
 190         dsq = distanceSq(p, c[ei](t));
 191         if ( mindistsq > dsq )
 192         {
 193             nearest = t;
 194             ni = ei;
 195         }
 196     }
 197     return c.mapToDomain(nearest, ni);
 198 }
 199
 200 std::vector<double>
 201 all_nearest_points( Point const& p,
 202                     Piecewise< D2<SBasis> > const& c,
 203                     double from, double to )
 204 {
 205     if ( from > to ) std::swap(from, to);
 206     if ( from < c.cuts[0] || to > c.cuts[c.size()] )
 207     {
 208         THROW_RANGEERROR("[from,to] interval out of bounds");
 209     }
 210
 211     unsigned int si = c.segN(from);
 212     unsigned int ei = c.segN(to);
 213     if ( si == ei )
 214     {
 215         std::vector<double>     all_nearest =
 216             all_nearest_points(p, c[si], c.segT(from, si), c.segT(to, si));
 217         for ( unsigned int i = 0; i < all_nearest.size(); ++i )
 218         {
 219             all_nearest[i] = c.mapToDomain(all_nearest[i], si);
 220         }
 221         return all_nearest;
 222     }
 223     std::vector<double> all_t;
 224     std::vector< std::vector<double> > all_np;
 225     all_np.push_back( all_nearest_points(p, c[si], c.segT(from, si)) );
 226     std::vector<unsigned int> ni;
 227     ni.push_back(si);
 228     double dsq;
 229     double mindistsq = distanceSq( p, c[si](all_np.front().front()) );
 230     Rect bb(Geom::Point(0,0),Geom::Point(0,0));
 231     for ( unsigned int i = si + 1; i < ei; ++i )
 232     {
 233         bb = *bounds_fast(c[i]);
 234         dsq = distanceSq(p, bb);
 235         if ( mindistsq < dsq ) continue;
 236         all_t = all_nearest_points(p, c[i]);
 237         dsq = distanceSq( p, c[i](all_t.front()) );
 238         if ( mindistsq > dsq )
 239         {
 240             all_np.clear();
 241             all_np.push_back(all_t);
 242             ni.clear();
 243             ni.push_back(i);
 244             mindistsq = dsq;
 245         }
 246         else if ( mindistsq == dsq )
 247         {
 248             all_np.push_back(all_t);
 249             ni.push_back(i);
 250         }
 251     }
 252     bb = *bounds_fast(c[ei]);
 253     dsq = distanceSq(p, bb);
 254     if ( mindistsq >= dsq )
 255     {
 256         all_t = all_nearest_points(p, c[ei], 0, c.segT(to, ei));
 257         dsq = distanceSq( p, c[ei](all_t.front()) );
 258         if ( mindistsq > dsq )
 259         {
 260             for ( unsigned int i = 0; i < all_t.size(); ++i )
 261             {
 262                 all_t[i] = c.mapToDomain(all_t[i], ei);
 263             }
 264             return all_t;
 265         }
 266         else if ( mindistsq == dsq )
 267         {
 268             all_np.push_back(all_t);
 269             ni.push_back(ei);
 270         }
 271     }
 272     std::vector<double> all_nearest;
 273     for ( unsigned int i = 0; i < all_np.size(); ++i )
 274     {
 275         for ( unsigned int j = 0; j < all_np[i].size(); ++j )
 276         {
 277             all_nearest.push_back( c.mapToDomain(all_np[i][j], ni[i]) );
 278         }
 279     }
 280     all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()),
 281                       all_nearest.end());
 282     return all_nearest;
 283 }
 284
 285 } // end namespace Geom
 286
 287