/* * This program source code file is part of KiCad, a free EDA CAD application. * * Copyright (C) 2014 Jean-Pierre Charras, jp.charras at wanadoo.fr * Copyright (C) 2014 KiCad Developers, see CHANGELOG.TXT for contributors. * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, you may find one here: * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html * or you may search the http://www.gnu.org website for the version 2 license, * or you may write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA */ /** * @file trigo.cpp * @brief Trigonometric and geometric basic functions. */ #include #include #include #include #include // Returns true if the point P is on the segment S. // faster than TestSegmentHit() because P should be exactly on S // therefore works fine only for H, V and 45 deg segm (suitable for wires in eeschema) bool IsPointOnSegment( const wxPoint& aSegStart, const wxPoint& aSegEnd, const wxPoint& aTestPoint ) { wxPoint vectSeg = aSegEnd - aSegStart; // Vector from S1 to S2 wxPoint vectPoint = aTestPoint - aSegStart; // Vector from S1 to P // Use long long here to avoid overflow in calculations if( (long long) vectSeg.x * vectPoint.y - (long long) vectSeg.y * vectPoint.x ) return false; /* Cross product non-zero, vectors not parallel */ if( ( (long long) vectSeg.x * vectPoint.x + (long long) vectSeg.y * vectPoint.y ) < ( (long long) vectPoint.x * vectPoint.x + (long long) vectPoint.y * vectPoint.y ) ) return false; /* Point not on segment */ return true; } // Returns true if the segment 1 intersectd the segment 2. bool SegmentIntersectsSegment( const wxPoint &a_p1_l1, const wxPoint &a_p2_l1, const wxPoint &a_p1_l2, const wxPoint &a_p2_l2 ) { //We are forced to use 64bit ints because the internal units can oveflow 32bit ints when // multiplied with each other, the alternative would be to scale the units down (i.e. divide // by a fixed number). long long dX_a, dY_a, dX_b, dY_b, dX_ab, dY_ab; long long num_a, num_b, den; //Test for intersection within the bounds of both line segments using line equations of the // form: // x_k(u_k) = u_k * dX_k + x_k(0) // y_k(u_k) = u_k * dY_k + y_k(0) // with 0 <= u_k <= 1 and k = [ a, b ] dX_a = a_p2_l1.x - a_p1_l1.x; dY_a = a_p2_l1.y - a_p1_l1.y; dX_b = a_p2_l2.x - a_p1_l2.x; dY_b = a_p2_l2.y - a_p1_l2.y; dX_ab = a_p1_l2.x - a_p1_l1.x; dY_ab = a_p1_l2.y - a_p1_l1.y; den = dY_a * dX_b - dY_b * dX_a ; //Check if lines are parallel if( den == 0 ) return false; num_a = dY_ab * dX_b - dY_b * dX_ab; num_b = dY_ab * dX_a - dY_a * dX_ab; //We wont calculate directly the u_k of the intersection point to avoid floating point // division but they could be calculated with: // u_a = (float) num_a / (float) den; // u_b = (float) num_b / (float) den; if( den < 0 ) { den = -den; num_a = -num_a; num_b = -num_b; } //Test sign( u_a ) and return false if negative if( num_a < 0 ) return false; //Test sign( u_b ) and return false if negative if( num_b < 0 ) return false; //Test to ensure (u_a <= 1) if( num_a > den ) return false; //Test to ensure (u_b <= 1) if( num_b > den ) return false; return true; } bool TestSegmentHit( const wxPoint &aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist ) { int xmin = aStart.x; int xmax = aEnd.x; int ymin = aStart.y; int ymax = aEnd.y; wxPoint delta = aStart - aRefPoint; if( xmax < xmin ) std::swap( xmax, xmin ); if( ymax < ymin ) std::swap( ymax, ymin ); // First, check if we are outside of the bounding box if( ( ymin - aRefPoint.y > aDist ) || ( aRefPoint.y - ymax > aDist ) ) return false; if( ( xmin - aRefPoint.x > aDist ) || ( aRefPoint.x - xmax > aDist ) ) return false; // Next, eliminate easy cases if( aStart.x == aEnd.x && aRefPoint.y > ymin && aRefPoint.y < ymax ) return std::abs( delta.x ) <= aDist; if( aStart.y == aEnd.y && aRefPoint.x > xmin && aRefPoint.x < xmax ) return std::abs( delta.y ) <= aDist; wxPoint len = aEnd - aStart; // Precision note here: // These are 32-bit integers, so squaring requires 64 bits to represent // exactly. 64-bit Doubles have only 52 bits in the mantissa, so we start to lose // precision at 2^53, which corresponds to ~ ±1nm @ 9.5cm, 2nm at 90cm, etc... // Long doubles avoid this ambiguity as well as the more expensive denormal double calc // Long doubles usually (sometimes more if SIMD) have at least 64 bits in the mantissa long double length_square = (long double) len.x * len.x + (long double) len.y * len.y; long double cross = std::abs( (long double) len.x * delta.y - (long double) len.y * delta.x ); long double dist_square = (long double) aDist * aDist; // The perpendicular distance to a line is the vector magnitude of the line from // a test point to the test line. That is the 2d determinant. Because we handled // the zero length case above, so we are guaranteed a unique solution. return ( ( length_square >= cross && dist_square >= cross ) || ( length_square * dist_square >= cross * cross ) ); } double ArcTangente( int dy, int dx ) { /* gcc is surprisingly smart in optimizing these conditions in a tree! */ if( dx == 0 && dy == 0 ) return 0; if( dy == 0 ) { if( dx >= 0 ) return 0; else return -1800; } if( dx == 0 ) { if( dy >= 0 ) return 900; else return -900; } if( dx == dy ) { if( dx >= 0 ) return 450; else return -1800 + 450; } if( dx == -dy ) { if( dx >= 0 ) return -450; else return 1800 - 450; } // Of course dy and dx are treated as double return RAD2DECIDEG( atan2( (double) dy, (double) dx ) ); } void RotatePoint( int* pX, int* pY, double angle ) { int tmp; NORMALIZE_ANGLE_POS( angle ); // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees. if( angle == 0 ) return; if( angle == 900 ) /* sin = 1, cos = 0 */ { tmp = *pX; *pX = *pY; *pY = -tmp; } else if( angle == 1800 ) /* sin = 0, cos = -1 */ { *pX = -*pX; *pY = -*pY; } else if( angle == 2700 ) /* sin = -1, cos = 0 */ { tmp = *pX; *pX = -*pY; *pY = tmp; } else { double fangle = DECIDEG2RAD( angle ); double sinus = sin( fangle ); double cosinus = cos( fangle ); double fpx = (*pY * sinus ) + (*pX * cosinus ); double fpy = (*pY * cosinus ) - (*pX * sinus ); *pX = KiROUND( fpx ); *pY = KiROUND( fpy ); } } void RotatePoint( int* pX, int* pY, int cx, int cy, double angle ) { int ox, oy; ox = *pX - cx; oy = *pY - cy; RotatePoint( &ox, &oy, angle ); *pX = ox + cx; *pY = oy + cy; } void RotatePoint( wxPoint* point, const wxPoint& centre, double angle ) { int ox, oy; ox = point->x - centre.x; oy = point->y - centre.y; RotatePoint( &ox, &oy, angle ); point->x = ox + centre.x; point->y = oy + centre.y; } void RotatePoint( VECTOR2I& point, const VECTOR2I& centre, double angle ) { wxPoint c( centre.x, centre.y ); wxPoint p( point.x, point.y ); RotatePoint(&p, c, angle); point.x = p.x; point.y = p.y; } void RotatePoint( double* pX, double* pY, double cx, double cy, double angle ) { double ox, oy; ox = *pX - cx; oy = *pY - cy; RotatePoint( &ox, &oy, angle ); *pX = ox + cx; *pY = oy + cy; } void RotatePoint( double* pX, double* pY, double angle ) { double tmp; NORMALIZE_ANGLE_POS( angle ); // Cheap and dirty optimizations for 0, 90, 180, and 270 degrees. if( angle == 0 ) return; if( angle == 900 ) /* sin = 1, cos = 0 */ { tmp = *pX; *pX = *pY; *pY = -tmp; } else if( angle == 1800 ) /* sin = 0, cos = -1 */ { *pX = -*pX; *pY = -*pY; } else if( angle == 2700 ) /* sin = -1, cos = 0 */ { tmp = *pX; *pX = -*pY; *pY = tmp; } else { double fangle = DECIDEG2RAD( angle ); double sinus = sin( fangle ); double cosinus = cos( fangle ); double fpx = (*pY * sinus ) + (*pX * cosinus ); double fpy = (*pY * cosinus ) - (*pX * sinus ); *pX = fpx; *pY = fpy; } } const VECTOR2I GetArcCenter( const VECTOR2I& aStart, const VECTOR2I& aMid, const VECTOR2I& aEnd ) { VECTOR2I center; double yDelta_21 = aMid.y - aStart.y; double xDelta_21 = aMid.x - aStart.x; double yDelta_32 = aEnd.y - aMid.y; double xDelta_32 = aEnd.x - aMid.x; // This is a special case for aMid as the half-way point when aSlope = 0 and bSlope = inf // or the other way around. In that case, the center lies in a straight line between // aStart and aEnd if( ( ( xDelta_21 == 0.0 ) && ( yDelta_32 == 0.0 ) ) || ( ( yDelta_21 == 0.0 ) && ( xDelta_32 == 0.0 ) ) ) { center.x = KiROUND( ( aStart.x + aEnd.x ) / 2.0 ); center.y = KiROUND( ( aStart.y + aEnd.y ) / 2.0 ); return center; } // Prevent div=0 errors if( xDelta_21 == 0.0 ) xDelta_21 = std::numeric_limits::epsilon(); if( xDelta_32 == 0.0 ) xDelta_32 = -std::numeric_limits::epsilon(); double aSlope = yDelta_21 / xDelta_21; double bSlope = yDelta_32 / xDelta_32; // If the points are colinear, the center is at infinity, so offset // the slope by a minimal amount // Warning: This will induce a small error in the center location if( yDelta_32 * xDelta_21 == yDelta_21 * xDelta_32 ) { aSlope += std::numeric_limits::epsilon(); bSlope -= std::numeric_limits::epsilon(); } if( aSlope == 0.0 ) aSlope = std::numeric_limits::epsilon(); if( bSlope == 0.0 ) bSlope = -std::numeric_limits::epsilon(); double result = ( aSlope * bSlope * ( aStart.y - aEnd.y ) + bSlope * ( aStart.x + aMid.x ) - aSlope * ( aMid.x + aEnd.x ) ) / ( 2 * ( bSlope - aSlope ) ); center.x = KiROUND( Clamp( double( std::numeric_limits::min() / 2 ), result, double( std::numeric_limits::max() / 2 ) ) ); result = ( ( ( aStart.x + aMid.x ) / 2.0 - center.x ) / aSlope + ( aStart.y + aMid.y ) / 2.0 ); center.y = KiROUND( Clamp( double( std::numeric_limits::min() / 2 ), result, double( std::numeric_limits::max() / 2 ) ) ); return center; }