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-2022 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)
42bool IsPointOnSegment( const VECTOR2I& aSegStart, const VECTOR2I& aSegEnd,
43 const VECTOR2I& aTestPoint )
44{
45 VECTOR2I vectSeg = aSegEnd - aSegStart; // Vector from S1 to S2
46 VECTOR2I 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.
61bool SegmentIntersectsSegment( const VECTOR2I& a_p1_l1, const VECTOR2I& a_p2_l1,
62 const VECTOR2I& a_p1_l2, const VECTOR2I& a_p2_l2,
63 VECTOR2I* 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 int64_t dX_a, dY_a, dX_b, dY_b, dX_ab, dY_ab;
70 int64_t 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 = int64_t{ a_p2_l1.x } - a_p1_l1.x;
79 dY_a = int64_t{ a_p2_l1.y } - a_p1_l1.y;
80 dX_b = int64_t{ a_p2_l2.x } - a_p1_l2.x;
81 dY_b = int64_t{ a_p2_l2.y } - a_p1_l2.y;
82 dX_ab = int64_t{ a_p1_l2.x } - a_p1_l1.x;
83 dY_ab = int64_t{ 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
129bool TestSegmentHit( const VECTOR2I& aRefPoint, const VECTOR2I& aStart, const VECTOR2I& aEnd,
130 int aDist )
131{
132 int xmin = aStart.x;
133 int xmax = aEnd.x;
134 int ymin = aStart.y;
135 int ymax = aEnd.y;
136 VECTOR2I delta = aStart - aRefPoint;
137
138 if( xmax < xmin )
139 std::swap( xmax, xmin );
140
141 if( ymax < ymin )
142 std::swap( ymax, ymin );
143
144 // First, check if we are outside of the bounding box
145 if( ( ymin - aRefPoint.y > aDist ) || ( aRefPoint.y - ymax > aDist ) )
146 return false;
147
148 if( ( xmin - aRefPoint.x > aDist ) || ( aRefPoint.x - xmax > aDist ) )
149 return false;
150
151 // Next, eliminate easy cases
152 if( aStart.x == aEnd.x && aRefPoint.y > ymin && aRefPoint.y < ymax )
153 return std::abs( delta.x ) <= aDist;
154
155 if( aStart.y == aEnd.y && aRefPoint.x > xmin && aRefPoint.x < xmax )
156 return std::abs( delta.y ) <= aDist;
157
158 SEG segment( aStart, aEnd );
159 return segment.SquaredDistance( aRefPoint ) < SEG::Square( aDist + 1 );
160}
161
162
163const VECTOR2I CalcArcMid( const VECTOR2I& aStart, const VECTOR2I& aEnd, const VECTOR2I& aCenter,
164 bool aMinArcAngle )
165{
166 VECTOR2I startVector = aStart - aCenter;
167 VECTOR2I endVector = aEnd - aCenter;
168
169 EDA_ANGLE startAngle( startVector );
170 EDA_ANGLE endAngle( endVector );
171 EDA_ANGLE midPointRotAngle = ( startAngle - endAngle ).Normalize180() / 2;
172
173 if( !aMinArcAngle )
174 midPointRotAngle += ANGLE_180;
175
176 VECTOR2I newMid = aStart;
177 RotatePoint( newMid, aCenter, midPointRotAngle );
178
179 return newMid;
180}
181
182
183void RotatePoint( int* pX, int* pY, const EDA_ANGLE& aAngle )
184{
185 VECTOR2I pt;
186 EDA_ANGLE angle = aAngle;
187
188 angle.Normalize();
189
190 // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
191 if( angle == ANGLE_0 )
192 {
193 pt = VECTOR2I( *pX, *pY );
194 }
195 else if( angle == ANGLE_90 ) /* sin = 1, cos = 0 */
196 {
197 pt = VECTOR2I( *pY, -*pX );
198 }
199 else if( angle == ANGLE_180 ) /* sin = 0, cos = -1 */
200 {
201 pt = VECTOR2I( -*pX, -*pY );
202 }
203 else if( angle == ANGLE_270 ) /* sin = -1, cos = 0 */
204 {
205 pt = VECTOR2I( -*pY, *pX );
206 }
207 else
208 {
209 double sinus = angle.Sin();
210 double cosinus = angle.Cos();
211
212 pt.x = KiROUND( ( *pY * sinus ) + ( *pX * cosinus ) );
213 pt.y = KiROUND( ( *pY * cosinus ) - ( *pX * sinus ) );
214 }
215
216 *pX = pt.x;
217 *pY = pt.y;
218}
219
220
221void RotatePoint( int* pX, int* pY, int cx, int cy, const EDA_ANGLE& angle )
222{
223 int ox, oy;
224
225 ox = *pX - cx;
226 oy = *pY - cy;
227
228 RotatePoint( &ox, &oy, angle );
229
230 *pX = ox + cx;
231 *pY = oy + cy;
232}
233
234
235void RotatePoint( wxPoint* point, const wxPoint& centre, const EDA_ANGLE& angle )
236{
237 int ox, oy;
238
239 ox = point->x - centre.x;
240 oy = point->y - centre.y;
241
242 RotatePoint( &ox, &oy, angle );
243
244 point->x = ox + centre.x;
245 point->y = oy + centre.y;
246}
247
248
249void RotatePoint( double* pX, double* pY, double cx, double cy, const EDA_ANGLE& angle )
250{
251 double ox, oy;
252
253 ox = *pX - cx;
254 oy = *pY - cy;
255
256 RotatePoint( &ox, &oy, angle );
257
258 *pX = ox + cx;
259 *pY = oy + cy;
260}
261
262
263void RotatePoint( double* pX, double* pY, const EDA_ANGLE& aAngle )
264{
265 EDA_ANGLE angle = aAngle;
266 VECTOR2D pt;
267
268 angle.Normalize();
269
270 // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees.
271 if( angle == ANGLE_0 )
272 {
273 pt = VECTOR2D( *pX, *pY );
274 }
275 else if( angle == ANGLE_90 ) /* sin = 1, cos = 0 */
276 {
277 pt = VECTOR2D( *pY, -*pX );
278 }
279 else if( angle == ANGLE_180 ) /* sin = 0, cos = -1 */
280 {
281 pt = VECTOR2D( -*pX, -*pY );
282 }
283 else if( angle == ANGLE_270 ) /* sin = -1, cos = 0 */
284 {
285 pt = VECTOR2D( -*pY, *pX );
286 }
287 else
288 {
289 double sinus = angle.Sin();
290 double cosinus = angle.Cos();
291
292 pt.x = ( *pY * sinus ) + ( *pX * cosinus );
293 pt.y = ( *pY * cosinus ) - ( *pX * sinus );
294 }
295
296 *pX = pt.x;
297 *pY = pt.y;
298}
299
300
301const VECTOR2D CalcArcCenter( const VECTOR2D& aStart, const VECTOR2D& aEnd,
302 const EDA_ANGLE& aAngle )
303{
304 EDA_ANGLE angle( aAngle );
305 VECTOR2I start = aStart;
306 VECTOR2I end = aEnd;
307
308 if( angle < ANGLE_0 )
309 {
310 std::swap( start, end );
311 angle = -angle;
312 }
313
314 if( angle > ANGLE_180 )
315 {
316 std::swap( start, end );
318 }
319
320 double chord = ( start - end ).EuclideanNorm();
321 double r = ( chord / 2.0 ) / ( angle / 2.0 ).Sin();
322 double d_squared = r * r - chord* chord / 4.0;
323 double d = 0.0;
324
325 if( d_squared > 0.0 )
326 d = sqrt( d_squared );
327
328 VECTOR2D vec2 = (end - start).Resize( d );
329 VECTOR2D vc = (end - start).Resize( chord / 2 );
330
331 RotatePoint( vec2, -ANGLE_90 );
332
333 return VECTOR2D( start + vc + vec2 );
334}
335
336
337const VECTOR2D CalcArcCenter( const VECTOR2D& aStart, const VECTOR2D& aMid, const VECTOR2D& aEnd )
338{
339 VECTOR2D center;
340 double yDelta_21 = aMid.y - aStart.y;
341 double xDelta_21 = aMid.x - aStart.x;
342 double yDelta_32 = aEnd.y - aMid.y;
343 double xDelta_32 = aEnd.x - aMid.x;
344
345 // This is a special case for aMid as the half-way point when aSlope = 0 and bSlope = inf
346 // or the other way around. In that case, the center lies in a straight line between
347 // aStart and aEnd
348 if( ( ( xDelta_21 == 0.0 ) && ( yDelta_32 == 0.0 ) ) ||
349 ( ( yDelta_21 == 0.0 ) && ( xDelta_32 == 0.0 ) ) )
350 {
351 center.x = ( aStart.x + aEnd.x ) / 2.0;
352 center.y = ( aStart.y + aEnd.y ) / 2.0 ;
353 return center;
354 }
355
356 // Prevent div=0 errors
357 if( xDelta_21 == 0.0 )
358 xDelta_21 = std::numeric_limits<double>::epsilon();
359
360 if( xDelta_32 == 0.0 )
361 xDelta_32 = -std::numeric_limits<double>::epsilon();
362
363 double aSlope = yDelta_21 / xDelta_21;
364 double bSlope = yDelta_32 / xDelta_32;
365
366 double daSlope = aSlope * VECTOR2D( 0.5 / yDelta_21, 0.5 / xDelta_21 ).EuclideanNorm();
367 double dbSlope = bSlope * VECTOR2D( 0.5 / yDelta_32, 0.5 / xDelta_32 ).EuclideanNorm();
368
369 if( aSlope == bSlope )
370 {
371 if( aStart == aEnd )
372 {
373 // This is a special case for a 360 degrees arc. In this case, the center is halfway between
374 // the midpoint and either end point
375 center.x = ( aStart.x + aMid.x ) / 2.0;
376 center.y = ( aStart.y + aMid.y ) / 2.0 ;
377 return center;
378 }
379 else
380 {
381 // If the points are colinear, the center is at infinity, so offset
382 // the slope by a minimal amount
383 // Warning: This will induce a small error in the center location
384 aSlope += std::numeric_limits<double>::epsilon();
385 bSlope -= std::numeric_limits<double>::epsilon();
386 }
387 }
388
389 // Prevent divide by zero error
390 if( aSlope == 0.0 )
391 aSlope = std::numeric_limits<double>::epsilon();
392
393 // What follows is the calculation of the center using the slope of the two lines as well as
394 // the propagated error that occurs when rounding to the nearest nanometer. The error can be
395 // ±0.5 units but can add up to multiple nanometers after the full calculation is performed.
396 // All variables starting with `d` are the delta of that variable. This is approximately equal
397 // to the standard deviation.
398 // We ignore the possible covariance between variables. We also truncate our series expansion
399 // at the first term. These are reasonable assumptions as the worst-case scenario is that we
400 // underestimate the potential uncertainty, which would potentially put us back at the status quo
401 double abSlopeStartEndY = aSlope * bSlope * ( aStart.y - aEnd.y );
402 double dabSlopeStartEndY = abSlopeStartEndY * std::sqrt( ( daSlope / aSlope * daSlope / aSlope )
403 + ( dbSlope / bSlope * dbSlope / bSlope )
404 + ( M_SQRT1_2 / ( aStart.y - aEnd.y )
405 * M_SQRT1_2 / ( aStart.y - aEnd.y ) ) );
406
407 double bSlopeStartMidX = bSlope * ( aStart.x + aMid.x );
408 double dbSlopeStartMidX = bSlopeStartMidX * std::sqrt( ( dbSlope / bSlope * dbSlope / bSlope )
409 + ( M_SQRT1_2 / ( aStart.x + aMid.x )
410 * M_SQRT1_2 / ( aStart.x + aMid.x ) ) );
411
412 double aSlopeMidEndX = aSlope * ( aMid.x + aEnd.x );
413 double daSlopeMidEndX = aSlopeMidEndX * std::sqrt( ( daSlope / aSlope * daSlope / aSlope )
414 + ( M_SQRT1_2 / ( aMid.x + aEnd.x )
415 * M_SQRT1_2 / ( aMid.x + aEnd.x ) ) );
416
417 double twiceBASlopeDiff = 2 * ( bSlope - aSlope );
418 double dtwiceBASlopeDiff = 2 * std::sqrt( dbSlope * dbSlope + daSlope * daSlope );
419
420 double centerNumeratorX = abSlopeStartEndY + bSlopeStartMidX - aSlopeMidEndX;
421 double dCenterNumeratorX = std::sqrt( dabSlopeStartEndY * dabSlopeStartEndY
422 + dbSlopeStartMidX * dbSlopeStartMidX
423 + daSlopeMidEndX * daSlopeMidEndX );
424
425 double centerX = ( abSlopeStartEndY + bSlopeStartMidX - aSlopeMidEndX ) / twiceBASlopeDiff;
426
427 double dCenterX = centerX * std::sqrt( ( dCenterNumeratorX / centerNumeratorX * dCenterNumeratorX / centerNumeratorX )
428 + ( dtwiceBASlopeDiff / twiceBASlopeDiff * dtwiceBASlopeDiff / twiceBASlopeDiff ) );
429
430
431 double centerNumeratorY = ( ( aStart.x + aMid.x ) / 2.0 - centerX );
432 double dCenterNumeratorY = std::sqrt( 1.0 / 8.0 + dCenterX * dCenterX );
433
434 double centerFirstTerm = centerNumeratorY / aSlope;
435 double dcenterFirstTermY = centerFirstTerm * std::sqrt(
436 ( dCenterNumeratorY/ centerNumeratorY * dCenterNumeratorY / centerNumeratorY )
437 + ( daSlope / aSlope * daSlope / aSlope ) );
438
439 double centerY = centerFirstTerm + ( aStart.y + aMid.y ) / 2.0;
440 double dCenterY = std::sqrt( dcenterFirstTermY * dcenterFirstTermY + 1.0 / 8.0 );
441
442 double rounded100CenterX = std::floor( ( centerX + 50.0 ) / 100.0 ) * 100.0;
443 double rounded100CenterY = std::floor( ( centerY + 50.0 ) / 100.0 ) * 100.0;
444 double rounded10CenterX = std::floor( ( centerX + 5.0 ) / 10.0 ) * 10.0;
445 double rounded10CenterY = std::floor( ( centerY + 5.0 ) / 10.0 ) * 10.0;
446
447 // The last step is to find the nice, round numbers near our baseline estimate and see if they are within our uncertainty
448 // range. If they are, then we use this round value as the true value. This is justified because ALL values within the
449 // uncertainty range are equally true. Using a round number will make sure that we are on a multiple of 1mil or 100nm
450 // when calculating centers.
451 if( std::abs( rounded100CenterX - centerX ) < dCenterX && std::abs( rounded100CenterY - centerY ) < dCenterY )
452 {
453 center.x = rounded100CenterX;
454 center.y = rounded100CenterY;
455 }
456 else if( std::abs( rounded10CenterX - centerX ) < dCenterX && std::abs( rounded10CenterY - centerY ) < dCenterY )
457 {
458 center.x = rounded10CenterX;
459 center.y = rounded10CenterY;
460 }
461 else
462 {
463 center.x = centerX;
464 center.y = centerY;
465 }
466
467
468 return center;
469}
470
471
472const VECTOR2I CalcArcCenter( const VECTOR2I& aStart, const VECTOR2I& aMid, const VECTOR2I& aEnd )
473{
474 VECTOR2D dStart( static_cast<double>( aStart.x ), static_cast<double>( aStart.y ) );
475 VECTOR2D dMid( static_cast<double>( aMid.x ), static_cast<double>( aMid.y ) );
476 VECTOR2D dEnd( static_cast<double>( aEnd.x ), static_cast<double>( aEnd.y ) );
477 VECTOR2D dCenter = CalcArcCenter( dStart, dMid, dEnd );
478
479 VECTOR2I iCenter;
480
481 iCenter.x = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
482 dCenter.x,
483 double( std::numeric_limits<int>::max() / 2.0 ) ) );
484
485 iCenter.y = KiROUND( Clamp<double>( double( std::numeric_limits<int>::min() / 2.0 ),
486 dCenter.y,
487 double( std::numeric_limits<int>::max() / 2.0 ) ) );
488
489 return iCenter;
490}
491
492
Definition: seg.h:42
ecoord SquaredDistance(const SEG &aSeg) const
Definition: seg.cpp:75
static SEG::ecoord Square(int a)
Definition: seg.h:123
T EuclideanNorm() const
Compute the Euclidean norm of the vector, which is defined as sqrt(x ** 2 + y ** 2).
Definition: vector2d.h:293
static constexpr EDA_ANGLE & ANGLE_180
Definition: eda_angle.h:416
static constexpr EDA_ANGLE & ANGLE_360
Definition: eda_angle.h:418
static constexpr EDA_ANGLE & ANGLE_90
Definition: eda_angle.h:414
static constexpr EDA_ANGLE & ANGLE_0
Definition: eda_angle.h:412
static constexpr EDA_ANGLE & ANGLE_270
Definition: eda_angle.h:417
E_SERIE r
Definition: eserie.cpp:41
static DIRECTION_45::AngleType angle(const VECTOR2I &a, const VECTOR2I &b)
EDA_ANGLE abs(const EDA_ANGLE &aAngle)
Definition: eda_angle.h:401
constexpr int delta
const VECTOR2I CalcArcMid(const VECTOR2I &aStart, const VECTOR2I &aEnd, const VECTOR2I &aCenter, bool aMinArcAngle)
Return the middle point of an arc, half-way between aStart and aEnd.
Definition: trigo.cpp:163
bool TestSegmentHit(const VECTOR2I &aRefPoint, const VECTOR2I &aStart, const VECTOR2I &aEnd, int aDist)
Test if aRefPoint is with aDistance on the line defined by aStart and aEnd.
Definition: trigo.cpp:129
void RotatePoint(int *pX, int *pY, const EDA_ANGLE &aAngle)
Definition: trigo.cpp:183
bool SegmentIntersectsSegment(const VECTOR2I &a_p1_l1, const VECTOR2I &a_p2_l1, const VECTOR2I &a_p1_l2, const VECTOR2I &a_p2_l2, VECTOR2I *aIntersectionPoint)
Test if two lines intersect.
Definition: trigo.cpp:61
bool IsPointOnSegment(const VECTOR2I &aSegStart, const VECTOR2I &aSegEnd, const VECTOR2I &aTestPoint)
Test if aTestPoint is on line defined by aSegStart and aSegEnd.
Definition: trigo.cpp:42
const VECTOR2D CalcArcCenter(const VECTOR2D &aStart, const VECTOR2D &aEnd, const EDA_ANGLE &aAngle)
Definition: trigo.cpp:301
double EuclideanNorm(const VECTOR2I &vector)
Definition: trigo.h:129
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:85
VECTOR2< double > VECTOR2D
Definition: vector2d.h:617
VECTOR2< int > VECTOR2I
Definition: vector2d.h:618