KiCad PCB EDA Suite
seg.cpp
Go to the documentation of this file.
1 /*
2  * This program source code file is part of KiCad, a free EDA CAD application.
3  *
4  * Copyright (C) 2013 CERN
5  * @author Tomasz Wlostowski <[email protected]>
6  * Copyright (C) 2021 KiCad Developers, see AUTHORS.txt for contributors.
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * as published by the Free Software Foundation; either version 2
11  * of the License, or (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, you may find one here:
20  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
21  * or you may search the http://www.gnu.org website for the version 2 license,
22  * or you may write to the Free Software Foundation, Inc.,
23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
24  */
25 
26 #include <algorithm> // for min
27 #include <geometry/seg.h>
28 #include <math/util.h> // for rescale
29 #include <math/vector2d.h> // for VECTOR2I, VECTOR2
30 #include <trigo.h> // for RAD2DEG
31 
32 template <typename T>
33 int sgn( T aVal )
34 {
35  return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
36 }
37 
38 template <typename T>
39 constexpr T sqrt_helper(T x, T lo, T hi)
40 {
41  if (lo == hi)
42  return lo;
43 
44  const T mid = (lo + hi + 1) / 2;
45  if (x / mid < mid)
46  return sqrt_helper<T>(x, lo, mid - 1);
47  else
48  return sqrt_helper(x, mid, hi);
49 }
50 
51 template <typename T>
52 constexpr T ct_sqrt(T x)
53 {
54  return sqrt_helper<T>(x, 0, x / 2 + 1);
55 }
56 
57 template <typename T>
58 T isqrt(T x)
59 {
60  T r = (T) std::sqrt((double) x);
61  T sqrt_max = ct_sqrt(std::numeric_limits<T>::max());
62 
63  while (r < sqrt_max && r * r < x)
64  r++;
65  while (r > sqrt_max || r * r > x)
66  r--;
67 
68  return r;
69 }
70 
71 
72 SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const
73 {
74  if( Intersects( aSeg ) )
75  return 0;
76 
77  const VECTOR2I pts[4] =
78  {
79  aSeg.NearestPoint( A ) - A,
80  aSeg.NearestPoint( B ) - B,
81  NearestPoint( aSeg.A ) - aSeg.A,
82  NearestPoint( aSeg.B ) - aSeg.B
83  };
84 
86 
87  for( int i = 0; i < 4; i++ )
88  m = std::min( m, pts[i].SquaredEuclideanNorm() );
89 
90  return m;
91 }
92 
93 
94 double SEG::AngleDegrees( const SEG& aOther ) const
95 {
96  VECTOR2I thisVec = A - B;
97  VECTOR2I otherVec = aOther.A - aOther.B;
98 
99  double thisVecAngle = NormalizeAngle180( RAD2DECIDEG( thisVec.Angle() ) );
100  double otherVecAngle = NormalizeAngle180( RAD2DECIDEG( otherVec.Angle() ) );
101  double angleDegrees = std::abs( NormalizeAngle180( thisVecAngle - otherVecAngle ) ) / 10.0;
102 
103  return std::min( 180.0 - angleDegrees, angleDegrees );
104 }
105 
106 
107 const VECTOR2I SEG::NearestPoint( const SEG& aSeg ) const
108 {
109  if( OPT_VECTOR2I p = Intersect( aSeg ) )
110  return *p;
111 
112  const VECTOR2I pts_origin[4] =
113  {
114  aSeg.NearestPoint( A ),
115  aSeg.NearestPoint( B ),
116  NearestPoint( aSeg.A ),
117  NearestPoint( aSeg.B )
118  };
119 
120 
121  const VECTOR2I* pts_out[4] =
122  {
123  &A,
124  &B,
125  &pts_origin[2],
126  &pts_origin[3]
127  };
128 
129  const ecoord pts_dist[4] =
130  {
131  ( pts_origin[0] - A ).SquaredEuclideanNorm(),
132  ( pts_origin[1] - B ).SquaredEuclideanNorm(),
133  ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
134  ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
135  };
136 
137  int min_i = 0;
138 
139  for( int i = 0; i < 4; i++ )
140  {
141  if( pts_dist[i] < pts_dist[min_i] )
142  min_i = i;
143  }
144 
145  return *pts_out[min_i];
146 }
147 
148 
149 bool SEG::intersects( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines, VECTOR2I* aPt ) const
150 {
151  const VECTOR2I e( B - A );
152  const VECTOR2I f( aSeg.B - aSeg.A );
153  const VECTOR2I ac( aSeg.A - A );
154 
155  ecoord d = f.Cross( e );
156  ecoord p = f.Cross( ac );
157  ecoord q = e.Cross( ac );
158 
159  if( d == 0 )
160  return false;
161 
162  if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
163  return false;
164 
165  if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
166  return false;
167 
168  if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
169  return false;
170 
171  if( aPt )
172  {
173  *aPt = VECTOR2I( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
174  aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
175  }
176 
177  return true;
178 }
179 
180 
181 bool SEG::Intersects( const SEG& aSeg ) const
182 {
183  return intersects( aSeg );
184 }
185 
186 
187 OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
188 {
189  VECTOR2I ip;
190 
191  if( intersects( aSeg, aIgnoreEndpoints, aLines, &ip ) )
192  return ip;
193  else
194  return OPT_VECTOR2I();
195 }
196 
197 
199 {
200  VECTOR2I slope( B - A );
201  VECTOR2I endPoint = slope.Perpendicular() + aP;
202 
203  return SEG( aP, endPoint );
204 }
205 
206 
207 SEG SEG::ParallelSeg( const VECTOR2I& aP ) const
208 {
209  VECTOR2I slope( B - A );
210  VECTOR2I endPoint = slope + aP;
211 
212  return SEG( aP, endPoint );
213 }
214 
215 
216 bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
217 {
218  return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
219 }
220 
221 
222 bool SEG::Collide( const SEG& aSeg, int aClearance, int* aActual ) const
223 {
224  // check for intersection
225  // fixme: move to a method
226  if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
227  ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
228  {
229  if( aActual )
230  *aActual = 0;
231 
232  return true;
233  }
234 
235  ecoord dist_sq = VECTOR2I::ECOORD_MAX;
236 
237  dist_sq = std::min( dist_sq, SquaredDistance( aSeg.A ) );
238  dist_sq = std::min( dist_sq, SquaredDistance( aSeg.B ) );
239  dist_sq = std::min( dist_sq, aSeg.SquaredDistance( A ) );
240  dist_sq = std::min( dist_sq, aSeg.SquaredDistance( B ) );
241 
242  if( dist_sq == 0 || dist_sq < (ecoord) aClearance * aClearance )
243  {
244  if( aActual )
245  *aActual = isqrt( dist_sq );
246 
247  return true;
248  }
249 
250  return false;
251 }
252 
253 
254 bool SEG::Contains( const VECTOR2I& aP ) const
255 {
256  return Distance( aP ) <= 1;
257 }
258 
259 
260 const VECTOR2I SEG::NearestPoint( const VECTOR2I& aP ) const
261 {
262  VECTOR2I d = B - A;
263  ecoord l_squared = d.Dot( d );
264 
265  if( l_squared == 0 )
266  return A;
267 
268  ecoord t = d.Dot( aP - A );
269 
270  if( t < 0 )
271  return A;
272  else if( t > l_squared )
273  return B;
274 
275  int xp = rescale( t, (ecoord) d.x, l_squared );
276  int yp = rescale( t, (ecoord) d.y, l_squared );
277 
278  return A + VECTOR2I( xp, yp );
279 }
280 
281 
282 const VECTOR2I SEG::ReflectPoint( const VECTOR2I& aP ) const
283 {
284  VECTOR2I d = B - A;
285  VECTOR2I::extended_type l_squared = d.Dot( d );
286  VECTOR2I::extended_type t = d.Dot( aP - A );
287  VECTOR2I c;
288 
289  if( !l_squared )
290  c = aP;
291  else
292  {
293  c.x = A.x + rescale( t, static_cast<VECTOR2I::extended_type>( d.x ), l_squared );
294  c.y = A.y + rescale( t, static_cast<VECTOR2I::extended_type>( d.y ), l_squared );
295  }
296 
297  return 2 * c - aP;
298 }
299 
300 
302 {
303  VECTOR2I d = B - A;
304  ecoord l_squared = d.Dot( d );
305 
306  if( l_squared == 0 )
307  return A;
308 
309  ecoord t = d.Dot( aP - A );
310 
311  int xp = rescale( t, ecoord{ d.x }, l_squared );
312  int yp = rescale( t, ecoord{ d.y }, l_squared );
313 
314  return A + VECTOR2I( xp, yp );
315 }
316 
317 
318 int SEG::Distance( const SEG& aSeg ) const
319 {
320  return isqrt( SquaredDistance( aSeg ) );
321 }
322 
323 
324 int SEG::Distance( const VECTOR2I& aP ) const
325 {
326  return isqrt( SquaredDistance( aP ) );
327 }
328 
329 
330 int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
331 {
332  ecoord p = ecoord{ A.y } - B.y;
333  ecoord q = ecoord{ B.x } - A.x;
334  ecoord r = -p * A.x - q * A.y;
335  ecoord l = p * p + q * q;
336  ecoord det = p * aP.x + q * aP.y + r;
337  ecoord dist_sq = 0;
338 
339  if( l > 0 )
340  {
341  dist_sq = rescale( det, det, l );
342  }
343 
344  ecoord dist = isqrt( dist_sq );
345 
346  return aDetermineSide ? dist : std::abs( dist );
347 }
348 
VECTOR2_TRAITS< T >::extended_type extended_type
Definition: vector2d.h:76
extended_type Cross(const VECTOR2< T > &aVector) const
Compute cross product of self with aVector.
Definition: vector2d.h:513
bool Intersects(const SEG &aSeg) const
Definition: seg.cpp:181
T isqrt(T x)
Definition: seg.cpp:58
int Distance(const SEG &aSeg) const
Compute minimum Euclidean distance to segment aSeg.
Definition: seg.cpp:318
SEG PerpendicularSeg(const VECTOR2I &aP) const
Compute a segment perpendicular to this one, passing through point aP.
Definition: seg.cpp:198
bool ccw(const VECTOR2I &aA, const VECTOR2I &aB, const VECTOR2I &aC) const
Definition: seg.cpp:216
VECTOR2< T > Perpendicular() const
Compute the perpendicular vector.
Definition: vector2d.h:314
SEG()
Create an empty (0, 0) segment.
Definition: seg.h:54
OPT_VECTOR2I Intersect(const SEG &aSeg, bool aIgnoreEndpoints=false, bool aLines=false) const
Compute intersection point of segment (this) with segment aSeg.
Definition: seg.cpp:187
bool Collide(const SEG &aSeg, int aClearance, int *aActual=nullptr) const
Definition: seg.cpp:222
VECTOR2I::extended_type ecoord
Definition: seg.h:43
const VECTOR2I ReflectPoint(const VECTOR2I &aP) const
Reflect a point using this segment as axis.
Definition: seg.cpp:282
double RAD2DECIDEG(double rad)
Definition: trigo.h:234
ecoord SquaredDistance(const SEG &aSeg) const
Definition: seg.cpp:72
int LineDistance(const VECTOR2I &aP, bool aDetermineSide=false) const
Return the closest Euclidean distance between point aP and the line defined by the ends of segment (t...
Definition: seg.cpp:330
VECTOR2< int > VECTOR2I
Definition: vector2d.h:622
SEG ParallelSeg(const VECTOR2I &aP) const
Compute a segment parallel to this one, passing through point aP.
Definition: seg.cpp:207
T NormalizeAngle180(T Angle)
Normalize angle to be in the -180.0 .. 180.0 range.
Definition: trigo.h:387
static constexpr extended_type ECOORD_MAX
Definition: vector2d.h:79
constexpr T sqrt_helper(T x, T lo, T hi)
Definition: seg.cpp:39
VECTOR2I LineProject(const VECTOR2I &aP) const
Compute the perpendicular projection point of aP on a line passing through ends of the segment.
Definition: seg.cpp:301
OPT< VECTOR2I > OPT_VECTOR2I
Definition: seg.h:38
constexpr T ct_sqrt(T x)
Definition: seg.cpp:52
const VECTOR2I NearestPoint(const VECTOR2I &aP) const
Compute a point on the segment (this) that is closest to point aP.
Definition: seg.cpp:260
int sgn(T aVal)
Definition: seg.cpp:33
double Angle() const
Compute the angle of the vector.
Definition: vector2d.h:307
E_SERIE r
Definition: eserie.cpp:41
Definition: seg.h:40
extended_type Dot(const VECTOR2< T > &aVector) const
Compute dot product of self with aVector.
Definition: vector2d.h:521
bool intersects(const SEG &aSeg, bool aIgnoreEndpoints=false, bool aLines=false, VECTOR2I *aPt=nullptr) const
Definition: seg.cpp:149
VECTOR2I A
Definition: seg.h:48
T rescale(T aNumerator, T aValue, T aDenominator)
Scale a number (value) by rational (numerator/denominator).
Definition: util.h:98
double AngleDegrees(const SEG &aOther) const
Determine the smallest angle between two segments (result in degrees)
Definition: seg.cpp:94
bool Contains(const SEG &aSeg) const
Definition: seg.h:331
VECTOR2I B
Definition: seg.h:49