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;
 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;
 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