347 lines
9.3 KiB
C++
347 lines
9.3 KiB
C++
/*
|
|
* 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 <fctsys.h>
|
|
#include <macros.h>
|
|
#include <trigo.h>
|
|
#include <common.h>
|
|
#include <math_for_graphics.h>
|
|
|
|
// 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;
|
|
}
|
|
}
|