/* * nearest point routines for D2<SBasis> and Piecewise<D2<SBasis>> * * Authors: * * Marco Cecchetti <mrcekets at gmail.com> * * Copyright 2007-2008 authors * * This library is free software; you can redistribute it and/or * modify it either under the terms of the GNU Lesser General Public * License version 2.1 as published by the Free Software Foundation * (the "LGPL") or, at your option, under the terms of the Mozilla * Public License Version 1.1 (the "MPL"). If you do not alter this * notice, a recipient may use your version of this file under either * the MPL or the LGPL. * * You should have received a copy of the LGPL along with this library * in the file COPYING-LGPL-2.1; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * You should have received a copy of the MPL along with this library * in the file COPYING-MPL-1.1 * * The contents of this file are subject to the Mozilla Public License * Version 1.1 (the "License"); you may not use this file except in * compliance with the License. You may obtain a copy of the License at * http://www.mozilla.org/MPL/ * * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY * OF ANY KIND, either express or implied. See the LGPL or the MPL for * the specific language governing rights and limitations. */ #include <2geom/nearest-point.h> #include <algorithm> namespace Geom { //////////////////////////////////////////////////////////////////////////////// // D2<SBasis> versions /* * Return the parameter t of a nearest point on the portion of the curve "c", * related to the interval [from, to], to the point "p". * The needed curve derivative "dc" is passed as parameter. * The function return the first nearest point to "p" that is found. */ double nearest_point( Point const& p, D2<SBasis> const& c, D2<SBasis> const& dc, double from, double to ) { if ( from > to ) std::swap(from, to); if ( from < 0 || to > 1 ) { THROW_RANGEERROR("[from,to] interval out of bounds"); } if (c.isConstant()) return from; SBasis dd = dot(c - p, dc); std::vector<double> zeros = Geom::roots(dd); double closest = from; double min_dist_sq = L2sq(c(from) - p); double distsq; for ( unsigned int i = 0; i < zeros.size(); ++i ) { distsq = L2sq(c(zeros[i]) - p); if ( min_dist_sq > L2sq(c(zeros[i]) - p) ) { closest = zeros[i]; min_dist_sq = distsq; } } if ( min_dist_sq > L2sq( c(to) - p ) ) closest = to; return closest; } /* * Return the parameters t of all the nearest points on the portion of * the curve "c", related to the interval [from, to], to the point "p". * The needed curve derivative "dc" is passed as parameter. */ std::vector<double> all_nearest_points( Point const& p, D2<SBasis> const& c, D2<SBasis> const& dc, double from, double to ) { std::swap(from, to); if ( from > to ) std::swap(from, to); if ( from < 0 || to > 1 ) { THROW_RANGEERROR("[from,to] interval out of bounds"); } std::vector<double> result; if (c.isConstant()) { result.push_back(from); return result; } SBasis dd = dot(c - p, dc); std::vector<double> zeros = Geom::roots(dd); std::vector<double> candidates; candidates.push_back(from); candidates.insert(candidates.end(), zeros.begin(), zeros.end()); candidates.push_back(to); std::vector<double> distsq; distsq.reserve(candidates.size()); for ( unsigned int i = 0; i < candidates.size(); ++i ) { distsq.push_back( L2sq(c(candidates[i]) - p) ); } unsigned int closest = 0; double dsq = distsq[0]; for ( unsigned int i = 1; i < candidates.size(); ++i ) { if ( dsq > distsq[i] ) { closest = i; dsq = distsq[i]; } } for ( unsigned int i = 0; i < candidates.size(); ++i ) { if( distsq[closest] == distsq[i] ) { result.push_back(candidates[i]); } } return result; } //////////////////////////////////////////////////////////////////////////////// // Piecewise< D2<SBasis> > versions double nearest_point( Point const& p, Piecewise< D2<SBasis> > const& c, double from, double to ) { if ( from > to ) std::swap(from, to); if ( from < c.cuts[0] || to > c.cuts[c.size()] ) { THROW_RANGEERROR("[from,to] interval out of bounds"); } unsigned int si = c.segN(from); unsigned int ei = c.segN(to); if ( si == ei ) { double nearest= nearest_point(p, c[si], c.segT(from, si), c.segT(to, si)); return c.mapToDomain(nearest, si); } double t; double nearest = nearest_point(p, c[si], c.segT(from, si)); unsigned int ni = si; double dsq; double mindistsq = distanceSq(p, c[si](nearest)); Rect bb(Geom::Point(0,0),Geom::Point(0,0)); for ( unsigned int i = si + 1; i < ei; ++i ) { bb = *bounds_fast(c[i]); dsq = distanceSq(p, bb); if ( mindistsq <= dsq ) continue; t = nearest_point(p, c[i]); dsq = distanceSq(p, c[i](t)); if ( mindistsq > dsq ) { nearest = t; ni = i; mindistsq = dsq; } } bb = *bounds_fast(c[ei]); dsq = distanceSq(p, bb); if ( mindistsq > dsq ) { t = nearest_point(p, c[ei], 0, c.segT(to, ei)); dsq = distanceSq(p, c[ei](t)); if ( mindistsq > dsq ) { nearest = t; ni = ei; } } return c.mapToDomain(nearest, ni); } std::vector<double> all_nearest_points( Point const& p, Piecewise< D2<SBasis> > const& c, double from, double to ) { if ( from > to ) std::swap(from, to); if ( from < c.cuts[0] || to > c.cuts[c.size()] ) { THROW_RANGEERROR("[from,to] interval out of bounds"); } unsigned int si = c.segN(from); unsigned int ei = c.segN(to); if ( si == ei ) { std::vector<double> all_nearest = all_nearest_points(p, c[si], c.segT(from, si), c.segT(to, si)); for ( unsigned int i = 0; i < all_nearest.size(); ++i ) { all_nearest[i] = c.mapToDomain(all_nearest[i], si); } return all_nearest; } std::vector<double> all_t; std::vector< std::vector<double> > all_np; all_np.push_back( all_nearest_points(p, c[si], c.segT(from, si)) ); std::vector<unsigned int> ni; ni.push_back(si); double dsq; double mindistsq = distanceSq( p, c[si](all_np.front().front()) ); Rect bb(Geom::Point(0,0),Geom::Point(0,0)); for ( unsigned int i = si + 1; i < ei; ++i ) { bb = *bounds_fast(c[i]); dsq = distanceSq(p, bb); if ( mindistsq < dsq ) continue; all_t = all_nearest_points(p, c[i]); dsq = distanceSq( p, c[i](all_t.front()) ); if ( mindistsq > dsq ) { all_np.clear(); all_np.push_back(all_t); ni.clear(); ni.push_back(i); mindistsq = dsq; } else if ( mindistsq == dsq ) { all_np.push_back(all_t); ni.push_back(i); } } bb = *bounds_fast(c[ei]); dsq = distanceSq(p, bb); if ( mindistsq >= dsq ) { all_t = all_nearest_points(p, c[ei], 0, c.segT(to, ei)); dsq = distanceSq( p, c[ei](all_t.front()) ); if ( mindistsq > dsq ) { for ( unsigned int i = 0; i < all_t.size(); ++i ) { all_t[i] = c.mapToDomain(all_t[i], ei); } return all_t; } else if ( mindistsq == dsq ) { all_np.push_back(all_t); ni.push_back(ei); } } std::vector<double> all_nearest; for ( unsigned int i = 0; i < all_np.size(); ++i ) { for ( unsigned int j = 0; j < all_np[i].size(); ++j ) { all_nearest.push_back( c.mapToDomain(all_np[i][j], ni[i]) ); } } all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()), all_nearest.end()); return all_nearest; } } // end namespace Geom