src/2geom/nearest-point.cpp

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