KiCad PCB EDA Suite
trigo.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) 2014 Jean-Pierre Charras, jp.charras at wanadoo.fr
5  * Copyright (C) 2014-2019 KiCad Developers, see AUTHORS.txt for contributors.
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, you may find one here:
19  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
20  * or you may search the http://www.gnu.org website for the version 2 license,
21  * or you may write to the Free Software Foundation, Inc.,
22  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
23  */
24 
30 #include <limits> // for numeric_limits
31 #include <stdlib.h> // for abs
32 #include <type_traits> // for swap
33 
34 #include <geometry/seg.h>
35 #include <math/util.h>
36 #include <math/vector2d.h> // for VECTOR2I
37 #include <trigo.h>
38 
39 // Returns true if the point P is on the segment S.
40 // faster than TestSegmentHit() because P should be exactly on S
41 // therefore works fine only for H, V and 45 deg segm (suitable for wires in eeschema)
42 bool IsPointOnSegment( const wxPoint& aSegStart, const wxPoint& aSegEnd,
43  const wxPoint& aTestPoint )
44 {
45  wxPoint vectSeg = aSegEnd - aSegStart; // Vector from S1 to S2
46  wxPoint vectPoint = aTestPoint - aSegStart; // Vector from S1 to P
47 
48  // Use long long here to avoid overflow in calculations
49  if( (long long) vectSeg.x * vectPoint.y - (long long) vectSeg.y * vectPoint.x )
50  return false; /* Cross product non-zero, vectors not parallel */
51 
52  if( ( (long long) vectSeg.x * vectPoint.x + (long long) vectSeg.y * vectPoint.y ) <
53  ( (long long) vectPoint.x * vectPoint.x + (long long) vectPoint.y * vectPoint.y ) )
54  return false; /* Point not on segment */
55 
56  return true;
57 }
58 
59 
60 // Returns true if the segment 1 intersected the segment 2.
61 bool SegmentIntersectsSegment( const wxPoint &a_p1_l1, const wxPoint &a_p2_l1,
62  const wxPoint &a_p1_l2, const wxPoint &a_p2_l2,
63  wxPoint* aIntersectionPoint )
64 {
65 
66  //We are forced to use 64bit ints because the internal units can overflow 32bit ints when
67  // multiplied with each other, the alternative would be to scale the units down (i.e. divide
68  // by a fixed number).
69  long long dX_a, dY_a, dX_b, dY_b, dX_ab, dY_ab;
70  long long num_a, num_b, den;
71 
72  //Test for intersection within the bounds of both line segments using line equations of the
73  // form:
74  // x_k(u_k) = u_k * dX_k + x_k(0)
75  // y_k(u_k) = u_k * dY_k + y_k(0)
76  // with 0 <= u_k <= 1 and k = [ a, b ]
77 
78  dX_a = a_p2_l1.x - a_p1_l1.x;
79  dY_a = a_p2_l1.y - a_p1_l1.y;
80  dX_b = a_p2_l2.x - a_p1_l2.x;
81  dY_b = a_p2_l2.y - a_p1_l2.y;
82  dX_ab = a_p1_l2.x - a_p1_l1.x;
83  dY_ab = a_p1_l2.y - a_p1_l1.y;
84 
85  den = dY_a * dX_b - dY_b * dX_a ;
86 
87  //Check if lines are parallel
88  if( den == 0 )
89  return false;
90 
91  num_a = dY_ab * dX_b - dY_b * dX_ab;
92  num_b = dY_ab * dX_a - dY_a * dX_ab;
93 
94  // Only compute the intersection point if requested
95  if( aIntersectionPoint )
96  {
97  *aIntersectionPoint = a_p1_l1;
98  aIntersectionPoint->x += KiROUND( dX_a * ( double )num_a / ( double )den );
99  aIntersectionPoint->y += KiROUND( dY_a * ( double )num_b / ( double )den );
100  }
101 
102  if( den < 0 )
103  {
104  den = -den;
105  num_a = -num_a;
106  num_b = -num_b;
107  }
108 
109  //Test sign( u_a ) and return false if negative
110  if( num_a < 0 )
111  return false;
112 
113  //Test sign( u_b ) and return false if negative
114  if( num_b < 0 )
115  return false;
116 
117  //Test to ensure (u_a <= 1)
118  if( num_a > den )
119  return false;
120 
121  //Test to ensure (u_b <= 1)
122  if( num_b > den )
123  return false;
124 
125  return true;
126 }
127 
128 
129 bool TestSegmentHit( const wxPoint &aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist )
130 {
131  int xmin = aStart.x;
132  int xmax = aEnd.x;
133  int ymin = aStart.y;
134  int ymax = aEnd.y;
135  wxPoint delta = aStart - aRefPoint;
136 
137  if( xmax < xmin )
138  std::swap( xmax, xmin );
139 
140  if( ymax < ymin )
141  std::swap( ymax, ymin );
142 
143  // First, check if we are outside of the bounding box
144  if( ( ymin - aRefPoint.y > aDist ) || ( aRefPoint.y - ymax > aDist ) )
145  return false;
146 
147  if( ( xmin - aRefPoint.x > aDist ) || ( aRefPoint.x - xmax > aDist ) )
148  return false;
149 
150  // Next, eliminate easy cases
151  if( aStart.x == aEnd.x && aRefPoint.y > ymin && aRefPoint.y < ymax )
152  return std::abs( delta.x ) <= aDist;
153 
154  if( aStart.y == aEnd.y && aRefPoint.x > xmin && aRefPoint.x < xmax )
155  return std::abs( delta.y ) <= aDist;
156 
157  SEG segment( aStart, aEnd );
158  return segment.SquaredDistance( aRefPoint ) < SEG::Square( aDist + 1 );
159 }
160 
161 
162 double ArcTangente( int dy, int dx )
163 {
164 
165  /* gcc is surprisingly smart in optimizing these conditions in
166  a tree! */
167 
168  if( dx == 0 && dy == 0 )
169  return 0;
170 
171  if( dy == 0 )
172  {
173  if( dx >= 0 )
174  return 0;
175  else
176  return -1800;
177  }
178 
179  if( dx == 0 )
180  {
181  if( dy >= 0 )
182  return 900;
183  else
184  return -900;
185  }
186 
187  if( dx == dy )
188  {
189  if( dx >= 0 )
190  return 450;
191  else
192  return -1800 + 450;
193  }
194 
195  if( dx == -dy )
196  {
197  if( dx >= 0 )
198  return -450;
199  else
200  return 1800 - 450;
201  }
202 
203  // Of course dy and dx are treated as double
204  return RAD2DECIDEG( atan2( (double) dy, (double) dx ) );
205 }
206 
207 
208 void RotatePoint( int* pX, int* pY, double angle )
209 {
210  int tmp;
211 
213 
214  // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
215  if( angle == 0 )
216  return;
217 
218  if( angle == 900 ) /* sin = 1, cos = 0 */
219  {
220  tmp = *pX;
221  *pX = *pY;
222  *pY = -tmp;
223  }
224  else if( angle == 1800 ) /* sin = 0, cos = -1 */
225  {
226  *pX = -*pX;
227  *pY = -*pY;
228  }
229  else if( angle == 2700 ) /* sin = -1, cos = 0 */
230  {
231  tmp = *pX;
232  *pX = -*pY;
233  *pY = tmp;
234  }
235  else
236  {
237  double fangle = DECIDEG2RAD( angle );
238  double sinus = sin( fangle );
239  double cosinus = cos( fangle );
240  double fpx = (*pY * sinus ) + (*pX * cosinus );
241  double fpy = (*pY * cosinus ) - (*pX * sinus );
242  *pX = KiROUND( fpx );
243  *pY = KiROUND( fpy );
244  }
245 }
246 
247 
248 void RotatePoint( int* pX, int* pY, int cx, int cy, double angle )
249 {
250  int ox, oy;
251 
252  ox = *pX - cx;
253  oy = *pY - cy;
254 
255  RotatePoint( &ox, &oy, angle );
256 
257  *pX = ox + cx;
258  *pY = oy + cy;
259 }
260 
261 
262 void RotatePoint( wxPoint* point, const wxPoint& centre, double angle )
263 {
264  int ox, oy;
265 
266  ox = point->x - centre.x;
267  oy = point->y - centre.y;
268 
269  RotatePoint( &ox, &oy, angle );
270  point->x = ox + centre.x;
271  point->y = oy + centre.y;
272 }
273 
274 void RotatePoint( VECTOR2I& point, const VECTOR2I& centre, double angle )
275 {
276  wxPoint c( centre.x, centre.y );
277  wxPoint p( point.x, point.y );
278 
279  RotatePoint(&p, c, angle);
280 
281  point.x = p.x;
282  point.y = p.y;
283 }
284 
285 
286 void RotatePoint( double* pX, double* pY, double cx, double cy, double angle )
287 {
288  double ox, oy;
289 
290  ox = *pX - cx;
291  oy = *pY - cy;
292 
293  RotatePoint( &ox, &oy, angle );
294 
295  *pX = ox + cx;
296  *pY = oy + cy;
297 }
298 
299 
300 void RotatePoint( double* pX, double* pY, double angle )
301 {
302  double tmp;
303 
305 
306  // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
307  if( angle == 0 )
308  return;
309 
310  if( angle == 900 ) /* sin = 1, cos = 0 */
311  {
312  tmp = *pX;
313  *pX = *pY;
314  *pY = -tmp;
315  }
316  else if( angle == 1800 ) /* sin = 0, cos = -1 */
317  {
318  *pX = -*pX;
319  *pY = -*pY;
320  }
321  else if( angle == 2700 ) /* sin = -1, cos = 0 */
322  {
323  tmp = *pX;
324  *pX = -*pY;
325  *pY = tmp;
326  }
327  else
328  {
329  double fangle = DECIDEG2RAD( angle );
330  double sinus = sin( fangle );
331  double cosinus = cos( fangle );
332 
333  double fpx = (*pY * sinus ) + (*pX * cosinus );
334  double fpy = (*pY * cosinus ) - (*pX * sinus );
335  *pX = fpx;
336  *pY = fpy;
337  }
338 }
339 
340 
341 const VECTOR2D GetArcCenter( const VECTOR2D& aStart, const VECTOR2D& aMid, const VECTOR2D& aEnd )
342 {
343  VECTOR2D center;
344  double yDelta_21 = aMid.y - aStart.y;
345  double xDelta_21 = aMid.x - aStart.x;
346  double yDelta_32 = aEnd.y - aMid.y;
347  double xDelta_32 = aEnd.x - aMid.x;
348 
349  // This is a special case for aMid as the half-way point when aSlope = 0 and bSlope = inf
350  // or the other way around. In that case, the center lies in a straight line between
351  // aStart and aEnd
352  if( ( ( xDelta_21 == 0.0 ) && ( yDelta_32 == 0.0 ) ) ||
353  ( ( yDelta_21 == 0.0 ) && ( xDelta_32 == 0.0 ) ) )
354  {
355  center.x = ( aStart.x + aEnd.x ) / 2.0;
356  center.y = ( aStart.y + aEnd.y ) / 2.0 ;
357  return center;
358  }
359 
360  // Prevent div=0 errors
361  if( xDelta_21 == 0.0 )
362  xDelta_21 = std::numeric_limits<double>::epsilon();
363 
364  if( xDelta_32 == 0.0 )
365  xDelta_32 = -std::numeric_limits<double>::epsilon();
366 
367  double aSlope = yDelta_21 / xDelta_21;
368  double bSlope = yDelta_32 / xDelta_32;
369 
370  // If the points are colinear, the center is at infinity, so offset
371  // the slope by a minimal amount
372  // Warning: This will induce a small error in the center location
373  if( yDelta_32 * xDelta_21 == yDelta_21 * xDelta_32 )
374  {
375  aSlope += std::numeric_limits<double>::epsilon();
376  bSlope -= std::numeric_limits<double>::epsilon();
377  }
378 
379  if( aSlope == 0.0 )
380  aSlope = std::numeric_limits<double>::epsilon();
381 
382  if( bSlope == 0.0 )
383  bSlope = -std::numeric_limits<double>::epsilon();
384 
385 
386  center.x = ( aSlope * bSlope * ( aStart.y - aEnd.y ) +
387  bSlope * ( aStart.x + aMid.x ) -
388  aSlope * ( aMid.x + aEnd.x ) ) / ( 2 * ( bSlope - aSlope ) );
389 
390  center.y = ( ( ( aStart.x + aMid.x ) / 2.0 - center.x ) / aSlope +
391  ( aStart.y + aMid.y ) / 2.0 );
392 
393  return center;
394 }
395 
396 
397 const VECTOR2I GetArcCenter( const VECTOR2I& aStart, const VECTOR2I& aMid, const VECTOR2I& aEnd )
398 {
399  VECTOR2D dStart( static_cast<double>( aStart.x ), static_cast<double>( aStart.y ) );
400  VECTOR2D dMid( static_cast<double>( aMid.x ), static_cast<double>( aMid.y ) );
401  VECTOR2D dEnd( static_cast<double>( aEnd.x ), static_cast<double>( aEnd.y ) );
402  VECTOR2D dCenter = GetArcCenter( dStart, dMid, dEnd );
403 
404  VECTOR2I iCenter;
405 
406  iCenter.x = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
407  dCenter.x,
408  double( std::numeric_limits<int>::max() / 2.0 ) ) );
409 
410  iCenter.y = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
411  dCenter.y,
412  double( std::numeric_limits<int>::max() / 2.0 ) ) );
413 
414  return iCenter;
415 }
416 
417 
418 const wxPoint GetArcCenter( const wxPoint& aStart, const wxPoint& aMid, const wxPoint& aEnd )
419 {
420  VECTOR2D dStart( static_cast<double>( aStart.x ), static_cast<double>( aStart.y ) );
421  VECTOR2D dMid( static_cast<double>( aMid.x ), static_cast<double>( aMid.y ) );
422  VECTOR2D dEnd( static_cast<double>( aEnd.x ), static_cast<double>( aEnd.y ) );
423  VECTOR2D dCenter = GetArcCenter( dStart, dMid, dEnd );
424 
425  wxPoint iCenter;
426 
427  iCenter.x = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
428  dCenter.x,
429  double( std::numeric_limits<int>::max() / 2.0 ) ) );
430 
431  iCenter.y = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
432  dCenter.y,
433  double( std::numeric_limits<int>::max() / 2.0 ) ) );
434 
435  return iCenter;
436 }
bool IsPointOnSegment(const wxPoint &aSegStart, const wxPoint &aSegEnd, const wxPoint &aTestPoint)
Test if aTestPoint is on line defined by aSegStart and aSegEnd.
Definition: trigo.cpp:42
VECTOR2 defines a general 2D-vector/point.
Definition: vector2d.h:61
double RAD2DECIDEG(double rad)
Definition: trigo.h:219
ecoord SquaredDistance(const SEG &aSeg) const
Definition: seg.cpp:37
void RotatePoint(int *pX, int *pY, double angle)
Definition: trigo.cpp:208
void NORMALIZE_ANGLE_POS(T &Angle)
Definition: trigo.h:257
const VECTOR2D GetArcCenter(const VECTOR2D &aStart, const VECTOR2D &aMid, const VECTOR2D &aEnd)
Definition: trigo.cpp:341
static SEG::ecoord Square(int a)
Definition: seg.h:116
double ArcTangente(int dy, int dx)
Definition: trigo.cpp:162
Definition: seg.h:39
static DIRECTION_45::AngleType angle(const VECTOR2I &a, const VECTOR2I &b)
double DECIDEG2RAD(double deg)
Definition: trigo.h:218
bool SegmentIntersectsSegment(const wxPoint &a_p1_l1, const wxPoint &a_p2_l1, const wxPoint &a_p1_l2, const wxPoint &a_p2_l2, wxPoint *aIntersectionPoint)
Test if two lines intersect.
Definition: trigo.cpp:61
constexpr ret_type KiROUND(fp_type v)
Round a floating point number to an integer using "round halfway cases away from zero".
Definition: util.h:68
bool TestSegmentHit(const wxPoint &aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist)
Test if aRefPoint is with aDistance on the line defined by aStart and aEnd.
Definition: trigo.cpp:129