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 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 <fctsys.h>
31 #include <macros.h>
32 #include <trigo.h>
33 #include <common.h>
34 #include <math_for_graphics.h>
35 
36 // Returns true if the point P is on the segment S.
37 // faster than TestSegmentHit() because P should be exactly on S
38 // therefore works fine only for H, V and 45 deg segm (suitable for wires in eeschema)
39 bool IsPointOnSegment( const wxPoint& aSegStart, const wxPoint& aSegEnd,
40  const wxPoint& aTestPoint )
41 {
42  wxPoint vectSeg = aSegEnd - aSegStart; // Vector from S1 to S2
43  wxPoint vectPoint = aTestPoint - aSegStart; // Vector from S1 to P
44 
45  // Use long long here to avoid overflow in calculations
46  if( (long long) vectSeg.x * vectPoint.y - (long long) vectSeg.y * vectPoint.x )
47  return false; /* Cross product non-zero, vectors not parallel */
48 
49  if( ( (long long) vectSeg.x * vectPoint.x + (long long) vectSeg.y * vectPoint.y ) <
50  ( (long long) vectPoint.x * vectPoint.x + (long long) vectPoint.y * vectPoint.y ) )
51  return false; /* Point not on segment */
52 
53  return true;
54 }
55 
56 
57 // Returns true if the segment 1 intersectd the segment 2.
58 bool SegmentIntersectsSegment( const wxPoint &a_p1_l1, const wxPoint &a_p2_l1,
59  const wxPoint &a_p1_l2, const wxPoint &a_p2_l2,
60  wxPoint* aIntersectionPoint )
61 {
62 
63  //We are forced to use 64bit ints because the internal units can oveflow 32bit ints when
64  // multiplied with each other, the alternative would be to scale the units down (i.e. divide
65  // by a fixed number).
66  long long dX_a, dY_a, dX_b, dY_b, dX_ab, dY_ab;
67  long long num_a, num_b, den;
68 
69  //Test for intersection within the bounds of both line segments using line equations of the
70  // form:
71  // x_k(u_k) = u_k * dX_k + x_k(0)
72  // y_k(u_k) = u_k * dY_k + y_k(0)
73  // with 0 <= u_k <= 1 and k = [ a, b ]
74 
75  dX_a = a_p2_l1.x - a_p1_l1.x;
76  dY_a = a_p2_l1.y - a_p1_l1.y;
77  dX_b = a_p2_l2.x - a_p1_l2.x;
78  dY_b = a_p2_l2.y - a_p1_l2.y;
79  dX_ab = a_p1_l2.x - a_p1_l1.x;
80  dY_ab = a_p1_l2.y - a_p1_l1.y;
81 
82  den = dY_a * dX_b - dY_b * dX_a ;
83 
84  //Check if lines are parallel
85  if( den == 0 )
86  return false;
87 
88  num_a = dY_ab * dX_b - dY_b * dX_ab;
89  num_b = dY_ab * dX_a - dY_a * dX_ab;
90 
91  // Only compute the intersection point if requested
92  if( aIntersectionPoint )
93  {
94  *aIntersectionPoint = a_p1_l1;
95  aIntersectionPoint->x += KiROUND( dX_a * ( double )num_a / ( double )den );
96  aIntersectionPoint->y += KiROUND( dY_a * ( double )num_b / ( double )den );
97  }
98 
99  if( den < 0 )
100  {
101  den = -den;
102  num_a = -num_a;
103  num_b = -num_b;
104  }
105 
106  //Test sign( u_a ) and return false if negative
107  if( num_a < 0 )
108  return false;
109 
110  //Test sign( u_b ) and return false if negative
111  if( num_b < 0 )
112  return false;
113 
114  //Test to ensure (u_a <= 1)
115  if( num_a > den )
116  return false;
117 
118  //Test to ensure (u_b <= 1)
119  if( num_b > den )
120  return false;
121 
122  return true;
123 }
124 
125 
126 bool TestSegmentHit( const wxPoint &aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist )
127 {
128  int xmin = aStart.x;
129  int xmax = aEnd.x;
130  int ymin = aStart.y;
131  int ymax = aEnd.y;
132  wxPoint delta = aStart - aRefPoint;
133 
134  if( xmax < xmin )
135  std::swap( xmax, xmin );
136 
137  if( ymax < ymin )
138  std::swap( ymax, ymin );
139 
140  // First, check if we are outside of the bounding box
141  if( ( ymin - aRefPoint.y > aDist ) || ( aRefPoint.y - ymax > aDist ) )
142  return false;
143 
144  if( ( xmin - aRefPoint.x > aDist ) || ( aRefPoint.x - xmax > aDist ) )
145  return false;
146 
147  // Next, eliminate easy cases
148  if( aStart.x == aEnd.x && aRefPoint.y > ymin && aRefPoint.y < ymax )
149  return std::abs( delta.x ) <= aDist;
150 
151  if( aStart.y == aEnd.y && aRefPoint.x > xmin && aRefPoint.x < xmax )
152  return std::abs( delta.y ) <= aDist;
153 
154  // Special case for a segment with start == end (equal to a circle)
155  if ( aStart == aEnd )
156  {
157  long double length_square = (long double) delta.x * delta.x + (long double) delta.y * delta.y;
158  long double dist_square = (long double) aDist * aDist;
159 
160  return ( length_square <= dist_square );
161  }
162 
163  wxPoint len = aEnd - aStart;
164  // Precision note here:
165  // These are 32-bit integers, so squaring requires 64 bits to represent
166  // exactly. 64-bit Doubles have only 52 bits in the mantissa, so we start to lose
167  // precision at 2^53, which corresponds to ~ ±1nm @ 9.5cm, 2nm at 90cm, etc...
168  // Long doubles avoid this ambiguity as well as the more expensive denormal double calc
169  // Long doubles usually (sometimes more if SIMD) have at least 64 bits in the mantissa
170  long double length_square = (long double) len.x * len.x + (long double) len.y * len.y;
171  long double cross = std::abs( (long double) len.x * delta.y - (long double) len.y * delta.x );
172  long double dist_square = (long double) aDist * aDist;
173 
174  // The perpendicular distance to a line is the vector magnitude of the line from
175  // a test point to the test line. That is the 2d determinant. Because we handled
176  // the zero length case above, so we are guaranteed a unique solution.
177 
178  return ( ( length_square >= cross && dist_square >= cross ) ||
179  ( length_square * dist_square >= cross * cross ) );
180 }
181 
182 
183 double ArcTangente( int dy, int dx )
184 {
185 
186  /* gcc is surprisingly smart in optimizing these conditions in
187  a tree! */
188 
189  if( dx == 0 && dy == 0 )
190  return 0;
191 
192  if( dy == 0 )
193  {
194  if( dx >= 0 )
195  return 0;
196  else
197  return -1800;
198  }
199 
200  if( dx == 0 )
201  {
202  if( dy >= 0 )
203  return 900;
204  else
205  return -900;
206  }
207 
208  if( dx == dy )
209  {
210  if( dx >= 0 )
211  return 450;
212  else
213  return -1800 + 450;
214  }
215 
216  if( dx == -dy )
217  {
218  if( dx >= 0 )
219  return -450;
220  else
221  return 1800 - 450;
222  }
223 
224  // Of course dy and dx are treated as double
225  return RAD2DECIDEG( atan2( (double) dy, (double) dx ) );
226 }
227 
228 
229 void RotatePoint( int* pX, int* pY, double angle )
230 {
231  int tmp;
232 
234 
235  // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
236  if( angle == 0 )
237  return;
238 
239  if( angle == 900 ) /* sin = 1, cos = 0 */
240  {
241  tmp = *pX;
242  *pX = *pY;
243  *pY = -tmp;
244  }
245  else if( angle == 1800 ) /* sin = 0, cos = -1 */
246  {
247  *pX = -*pX;
248  *pY = -*pY;
249  }
250  else if( angle == 2700 ) /* sin = -1, cos = 0 */
251  {
252  tmp = *pX;
253  *pX = -*pY;
254  *pY = tmp;
255  }
256  else
257  {
258  double fangle = DECIDEG2RAD( angle );
259  double sinus = sin( fangle );
260  double cosinus = cos( fangle );
261  double fpx = (*pY * sinus ) + (*pX * cosinus );
262  double fpy = (*pY * cosinus ) - (*pX * sinus );
263  *pX = KiROUND( fpx );
264  *pY = KiROUND( fpy );
265  }
266 }
267 
268 
269 void RotatePoint( int* pX, int* pY, int cx, int cy, double angle )
270 {
271  int ox, oy;
272 
273  ox = *pX - cx;
274  oy = *pY - cy;
275 
276  RotatePoint( &ox, &oy, angle );
277 
278  *pX = ox + cx;
279  *pY = oy + cy;
280 }
281 
282 
283 void RotatePoint( wxPoint* point, const wxPoint& centre, double angle )
284 {
285  int ox, oy;
286 
287  ox = point->x - centre.x;
288  oy = point->y - centre.y;
289 
290  RotatePoint( &ox, &oy, angle );
291  point->x = ox + centre.x;
292  point->y = oy + centre.y;
293 }
294 
295 void RotatePoint( VECTOR2I& point, const VECTOR2I& centre, double angle )
296 {
297  wxPoint c( centre.x, centre.y );
298  wxPoint p( point.x, point.y );
299 
300  RotatePoint(&p, c, angle);
301 
302  point.x = p.x;
303  point.y = p.y;
304 }
305 
306 
307 void RotatePoint( double* pX, double* pY, double cx, double cy, double angle )
308 {
309  double ox, oy;
310 
311  ox = *pX - cx;
312  oy = *pY - cy;
313 
314  RotatePoint( &ox, &oy, angle );
315 
316  *pX = ox + cx;
317  *pY = oy + cy;
318 }
319 
320 
321 void RotatePoint( double* pX, double* pY, double angle )
322 {
323  double tmp;
324 
326 
327  // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
328  if( angle == 0 )
329  return;
330 
331  if( angle == 900 ) /* sin = 1, cos = 0 */
332  {
333  tmp = *pX;
334  *pX = *pY;
335  *pY = -tmp;
336  }
337  else if( angle == 1800 ) /* sin = 0, cos = -1 */
338  {
339  *pX = -*pX;
340  *pY = -*pY;
341  }
342  else if( angle == 2700 ) /* sin = -1, cos = 0 */
343  {
344  tmp = *pX;
345  *pX = -*pY;
346  *pY = tmp;
347  }
348  else
349  {
350  double fangle = DECIDEG2RAD( angle );
351  double sinus = sin( fangle );
352  double cosinus = cos( fangle );
353 
354  double fpx = (*pY * sinus ) + (*pX * cosinus );
355  double fpy = (*pY * cosinus ) - (*pX * sinus );
356  *pX = fpx;
357  *pY = fpy;
358  }
359 }
360 
361 
362 const VECTOR2I GetArcCenter( const VECTOR2I& aStart, const VECTOR2I& aMid, const VECTOR2I& aEnd )
363 {
364  VECTOR2I center;
365  double yDelta_21 = aMid.y - aStart.y;
366  double xDelta_21 = aMid.x - aStart.x;
367  double yDelta_32 = aEnd.y - aMid.y;
368  double xDelta_32 = aEnd.x - aMid.x;
369 
370  // This is a special case for aMid as the half-way point when aSlope = 0 and bSlope = inf
371  // or the other way around. In that case, the center lies in a straight line between
372  // aStart and aEnd
373  if( ( ( xDelta_21 == 0.0 ) && ( yDelta_32 == 0.0 ) ) ||
374  ( ( yDelta_21 == 0.0 ) && ( xDelta_32 == 0.0 ) ) )
375  {
376  center.x = KiROUND( ( aStart.x + aEnd.x ) / 2.0 );
377  center.y = KiROUND( ( aStart.y + aEnd.y ) / 2.0 );
378  return center;
379  }
380 
381  // Prevent div=0 errors
382  if( xDelta_21 == 0.0 )
383  xDelta_21 = std::numeric_limits<double>::epsilon();
384 
385  if( xDelta_32 == 0.0 )
386  xDelta_32 = -std::numeric_limits<double>::epsilon();
387 
388  double aSlope = yDelta_21 / xDelta_21;
389  double bSlope = yDelta_32 / xDelta_32;
390 
391  // If the points are colinear, the center is at infinity, so offset
392  // the slope by a minimal amount
393  // Warning: This will induce a small error in the center location
394  if( yDelta_32 * xDelta_21 == yDelta_21 * xDelta_32 )
395  {
396  aSlope += std::numeric_limits<double>::epsilon();
397  bSlope -= std::numeric_limits<double>::epsilon();
398  }
399 
400  if( aSlope == 0.0 )
401  aSlope = std::numeric_limits<double>::epsilon();
402 
403  if( bSlope == 0.0 )
404  bSlope = -std::numeric_limits<double>::epsilon();
405 
406 
407  double result = ( aSlope * bSlope * ( aStart.y - aEnd.y ) +
408  bSlope * ( aStart.x + aMid.x ) -
409  aSlope * ( aMid.x + aEnd.x ) ) / ( 2 * ( bSlope - aSlope ) );
410 
411  center.x = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2 ),
412  result,
413  double( std::numeric_limits<int>::max() / 2 ) ) );
414 
415  result = ( ( ( aStart.x + aMid.x ) / 2.0 - center.x ) / aSlope +
416  ( aStart.y + aMid.y ) / 2.0 );
417 
418  center.y = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2 ),
419  result,
420  double( std::numeric_limits<int>::max() / 2 ) ) );
421 
422  return center;
423 }
bool IsPointOnSegment(const wxPoint &aSegStart, const wxPoint &aSegEnd, const wxPoint &aTestPoint)
Function IsPointOnSegment.
Definition: trigo.cpp:39
static int KiROUND(double v)
Round a floating point number to an integer using "round halfway cases away from zero".
Definition: common.h:115
double RAD2DECIDEG(double rad)
Definition: trigo.h:215
void RotatePoint(int *pX, int *pY, double angle)
Definition: trigo.cpp:229
void NORMALIZE_ANGLE_POS(T &Angle)
Definition: trigo.h:252
#define abs(a)
Definition: auxiliary.h:84
This file contains miscellaneous commonly used macros and functions.
double ArcTangente(int dy, int dx)
Definition: trigo.cpp:183
#define max(a, b)
Definition: auxiliary.h:86
static DIRECTION_45::AngleType angle(const VECTOR2I &a, const VECTOR2I &b)
double DECIDEG2RAD(double deg)
Definition: trigo.h:214
bool SegmentIntersectsSegment(const wxPoint &a_p1_l1, const wxPoint &a_p2_l1, const wxPoint &a_p1_l2, const wxPoint &a_p2_l2, wxPoint *aIntersectionPoint)
Function SegmentIntersectsSegment.
Definition: trigo.cpp:58
The common library.
bool TestSegmentHit(const wxPoint &aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist)
Function TestSegmentHit test for hit on line segment i.e.
Definition: trigo.cpp:126
const VECTOR2I GetArcCenter(const VECTOR2I &aStart, const VECTOR2I &aMid, const VECTOR2I &aEnd)
Determine the center of an arc/circle, given three points on its circumference.
Definition: trigo.cpp:362
#define min(a, b)
Definition: auxiliary.h:85