kicad/common/trigo.cpp

381 lines
9.4 KiB
C++
Raw Normal View History

/**
* @file trigo.cpp
* @brief Trigonometric functions.
*/
#include <fctsys.h>
#include <macros.h>
#include <trigo.h>
// Dick Hollenbeck's KiROUND R&D // This provides better project control over rounding to int from double // than wxRound() did. This scheme provides better logging in Debug builds // and it provides for compile time calculation of constants. #include <stdio.h> #include <assert.h> #include <limits.h> //-----<KiROUND KIT>------------------------------------------------------------ /** * KiROUND * rounds a floating point number to an int using * "round halfway cases away from zero". * In Debug build an assert fires if will not fit into an int. */ #if defined( DEBUG ) // DEBUG: a macro to capture line and file, then calls this inline static inline int KiRound( double v, int line, const char* filename ) { v = v < 0 ? v - 0.5 : v + 0.5; if( v > INT_MAX + 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' > 0 ' for int\n", __FUNCTION__, filename, line, v ); } else if( v < INT_MIN - 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' < 0 ' for int\n", __FUNCTION__, filename, line, v ); } return int( v ); } #define KiROUND( v ) KiRound( v, __LINE__, __FILE__ ) #else // RELEASE: a macro so compile can pre-compute constants. #define KiROUND( v ) int( (v) < 0 ? (v) - 0.5 : (v) + 0.5 ) #endif //-----</KiROUND KIT>----------------------------------------------------------- // Only a macro is compile time calculated, an inline function causes a static constructor // in a situation like this. // Therefore the Release build is best done with a MACRO not an inline function. int Computed = KiROUND( 14.3 * 8 ); int main( int argc, char** argv ) { for( double d = double(INT_MAX)-1; d < double(INT_MAX)+8; d += 2.0 ) { int i = KiROUND( d ); printf( "t: %d %.16g\n", i, d ); } return 0; }
2012-04-19 06:55:45 +00:00
#include <common.h>
static bool DistanceTest( int seuil, int dx, int dy, int spot_cX, int spot_cY );
2009-06-13 17:06:07 +00:00
bool TestSegmentHit( wxPoint aRefPoint, wxPoint aStart, wxPoint aEnd, int aDist )
{
return DistanceTest( aDist, aEnd.x - aStart.x, aEnd.y - aStart.y,
aRefPoint.x - aStart.x, aRefPoint.y - aStart.y );
2009-06-13 17:06:07 +00:00
}
bool DistanceTest( int seuil, int dx, int dy, int spot_cX, int spot_cY )
2007-08-08 20:51:08 +00:00
{
/* We can have 4 cases::
* horizontal segment
* vertical segment
* 45 degrees segment
* other slopes
*/
int cXrot, cYrot, segX, segY;
int pointX, pointY;
2007-08-08 20:51:08 +00:00
segX = dx;
segY = dy;
pointX = spot_cX;
pointY = spot_cY;
2007-08-08 20:51:08 +00:00
/* Recalculating coord for the segment is in 1st quadrant (coord >= 0) */
if( segX < 0 ) /* set > 0 by symmetry about the Y axis */
2007-08-08 20:51:08 +00:00
{
segX = -segX;
pointX = -pointX;
2007-08-08 20:51:08 +00:00
}
if( segY < 0 ) /* set > 0 by symmetry about the X axis */
2007-08-08 20:51:08 +00:00
{
segY = -segY;
pointY = -pointY;
2007-08-08 20:51:08 +00:00
}
if( segY == 0 ) /* horizontal */
2007-08-08 20:51:08 +00:00
{
if( abs( pointY ) <= seuil )
{
if( ( pointX >= 0 ) && ( pointX <= segX ) )
2007-08-08 20:51:08 +00:00
return 1;
if( ( pointX < 0 ) && ( pointX >= -seuil ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( pointX * pointX ) + ( pointY * pointY ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
if( ( pointX > segX ) && ( pointX <= ( segX + seuil ) ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( ( pointX - segX ) * ( pointX - segX ) )
+ ( pointY * pointY ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
}
}
else if( segX == 0 ) /* vertical */
2007-08-08 20:51:08 +00:00
{
if( abs( pointX ) <= seuil )
{
if( ( pointY >= 0 ) && ( pointY <= segY ) )
2007-08-08 20:51:08 +00:00
return true;
if( ( pointY < 0 ) && ( pointY >= -seuil ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( pointY * pointY ) + ( pointX * pointX ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
if( ( pointY > segY ) && ( pointY <= ( segY + seuil ) ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( ( pointY - segY ) * ( pointY - segY ) )
+ ( pointX * pointX ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
}
}
else if( segX == segY ) /* 45 degrees */
2007-08-08 20:51:08 +00:00
{
/* Rotate axes of 45 degrees. mouse was then
* Coord: x1 = x * y * cos45 + sin45
* y1 = y * cos45 - sin45 x *
* And the segment of track is horizontal.
* Coord recalculation of the mouse (sin45 = cos45 = .707 = 7 / 10
* Note: sin or cos45 = .707, and when recalculating coord
* dx45 and dy45, lect coeff .707 is neglected, dx and dy are
* actually 0707 times
* Too big. (security hole too small)
* Spot_cX, Y * must be by .707 * .707 = 0.5
*/
2007-08-08 20:51:08 +00:00
cXrot = (pointX + pointY) >> 1;
cYrot = (pointY - pointX) >> 1;
/* Recalculating coord of segment extremity, which will be vertical
* following the orientation of axes on the screen: dx45 = pointx
* (or pointy) and 1.414 is actually greater, and dy45 = 0
*/
2007-08-08 20:51:08 +00:00
// * Threshold should be .707 to reflect the change in coeff dx, dy
seuil *= 7;
seuil /= 10;
if( abs( cYrot ) <= seuil ) /* ok on vertical axis */
2007-08-08 20:51:08 +00:00
{
if( ( cXrot >= 0 ) && ( cXrot <= segX ) )
2007-08-08 20:51:08 +00:00
return true;
/* Check extremes using the radius of a circle. */
if( ( cXrot < 0 ) && ( cXrot >= -seuil ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( cXrot * cXrot ) + ( cYrot * cYrot ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
if( ( cXrot > segX ) && ( cXrot <= ( segX + seuil ) ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( ( cXrot - segX ) * ( cXrot - segX ) )
+ ( cYrot * cYrot ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
}
}
else /* any orientation */
2007-08-08 20:51:08 +00:00
{
/* There is a change of axis (rotation), so that the segment
* track is horizontal in the new reference
*/
2007-08-08 20:51:08 +00:00
int angle;
// Dick Hollenbeck's KiROUND R&D // This provides better project control over rounding to int from double // than wxRound() did. This scheme provides better logging in Debug builds // and it provides for compile time calculation of constants. #include <stdio.h> #include <assert.h> #include <limits.h> //-----<KiROUND KIT>------------------------------------------------------------ /** * KiROUND * rounds a floating point number to an int using * "round halfway cases away from zero". * In Debug build an assert fires if will not fit into an int. */ #if defined( DEBUG ) // DEBUG: a macro to capture line and file, then calls this inline static inline int KiRound( double v, int line, const char* filename ) { v = v < 0 ? v - 0.5 : v + 0.5; if( v > INT_MAX + 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' > 0 ' for int\n", __FUNCTION__, filename, line, v ); } else if( v < INT_MIN - 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' < 0 ' for int\n", __FUNCTION__, filename, line, v ); } return int( v ); } #define KiROUND( v ) KiRound( v, __LINE__, __FILE__ ) #else // RELEASE: a macro so compile can pre-compute constants. #define KiROUND( v ) int( (v) < 0 ? (v) - 0.5 : (v) + 0.5 ) #endif //-----</KiROUND KIT>----------------------------------------------------------- // Only a macro is compile time calculated, an inline function causes a static constructor // in a situation like this. // Therefore the Release build is best done with a MACRO not an inline function. int Computed = KiROUND( 14.3 * 8 ); int main( int argc, char** argv ) { for( double d = double(INT_MAX)-1; d < double(INT_MAX)+8; d += 2.0 ) { int i = KiROUND( d ); printf( "t: %d %.16g\n", i, d ); } return 0; }
2012-04-19 06:55:45 +00:00
angle = KiROUND( ( atan2( (double) segY, (double) segX ) * 1800.0 / M_PI ) );
cXrot = pointX;
cYrot = pointY;
2007-08-08 20:51:08 +00:00
RotatePoint( &cXrot, &cYrot, angle ); /* Rotate the point to be tested */
RotatePoint( &segX, &segY, angle ); /* Rotate the segment */
2007-08-08 20:51:08 +00:00
/* The track is horizontal, following the amendments to coordinate
* axis and, therefore segX = length of segment
*/
if( abs( cYrot ) <= seuil ) /* vertical axis */
2007-08-08 20:51:08 +00:00
{
if( ( cXrot >= 0 ) && ( cXrot <= segX ) )
2007-08-08 20:51:08 +00:00
return true;
if( ( cXrot < 0 ) && ( cXrot >= -seuil ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( cXrot * cXrot ) + ( cYrot * cYrot ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
if( ( cXrot > segX ) && ( cXrot <= ( segX + seuil ) ) )
2007-08-08 20:51:08 +00:00
{
if( ( ( ( cXrot - segX ) * ( cXrot - segX ) )
+ ( cYrot * cYrot ) ) <= ( seuil * seuil ) )
2007-08-08 20:51:08 +00:00
return true;
}
}
}
2007-08-08 20:51:08 +00:00
return false;
}
int ArcTangente( int dy, int dx )
{
double fangle;
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;
}
fangle = atan2( (double) dy, (double) dx ) / M_PI * 1800;
// Dick Hollenbeck's KiROUND R&D // This provides better project control over rounding to int from double // than wxRound() did. This scheme provides better logging in Debug builds // and it provides for compile time calculation of constants. #include <stdio.h> #include <assert.h> #include <limits.h> //-----<KiROUND KIT>------------------------------------------------------------ /** * KiROUND * rounds a floating point number to an int using * "round halfway cases away from zero". * In Debug build an assert fires if will not fit into an int. */ #if defined( DEBUG ) // DEBUG: a macro to capture line and file, then calls this inline static inline int KiRound( double v, int line, const char* filename ) { v = v < 0 ? v - 0.5 : v + 0.5; if( v > INT_MAX + 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' > 0 ' for int\n", __FUNCTION__, filename, line, v ); } else if( v < INT_MIN - 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' < 0 ' for int\n", __FUNCTION__, filename, line, v ); } return int( v ); } #define KiROUND( v ) KiRound( v, __LINE__, __FILE__ ) #else // RELEASE: a macro so compile can pre-compute constants. #define KiROUND( v ) int( (v) < 0 ? (v) - 0.5 : (v) + 0.5 ) #endif //-----</KiROUND KIT>----------------------------------------------------------- // Only a macro is compile time calculated, an inline function causes a static constructor // in a situation like this. // Therefore the Release build is best done with a MACRO not an inline function. int Computed = KiROUND( 14.3 * 8 ); int main( int argc, char** argv ) { for( double d = double(INT_MAX)-1; d < double(INT_MAX)+8; d += 2.0 ) { int i = KiROUND( d ); printf( "t: %d %.16g\n", i, d ); } return 0; }
2012-04-19 06:55:45 +00:00
return KiROUND( fangle );
}
2011-12-14 04:29:25 +00:00
void RotatePoint( int* pX, int* pY, double angle )
{
int tmp;
while( angle < 0 )
angle += 3600;
while( angle >= 3600 )
angle -= 3600;
// 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
{
2011-12-14 04:29:25 +00:00
double fangle = DEG2RAD( angle / 10.0 );
double sinus = sin( fangle );
double cosinus = cos( fangle );
double fpx = (*pY * sinus ) + (*pX * cosinus );
double fpy = (*pY * cosinus ) - (*pX * sinus );
// Dick Hollenbeck's KiROUND R&D // This provides better project control over rounding to int from double // than wxRound() did. This scheme provides better logging in Debug builds // and it provides for compile time calculation of constants. #include <stdio.h> #include <assert.h> #include <limits.h> //-----<KiROUND KIT>------------------------------------------------------------ /** * KiROUND * rounds a floating point number to an int using * "round halfway cases away from zero". * In Debug build an assert fires if will not fit into an int. */ #if defined( DEBUG ) // DEBUG: a macro to capture line and file, then calls this inline static inline int KiRound( double v, int line, const char* filename ) { v = v < 0 ? v - 0.5 : v + 0.5; if( v > INT_MAX + 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' > 0 ' for int\n", __FUNCTION__, filename, line, v ); } else if( v < INT_MIN - 0.5 ) { printf( "%s: in file %s on line %d, val: %.16g too ' < 0 ' for int\n", __FUNCTION__, filename, line, v ); } return int( v ); } #define KiROUND( v ) KiRound( v, __LINE__, __FILE__ ) #else // RELEASE: a macro so compile can pre-compute constants. #define KiROUND( v ) int( (v) < 0 ? (v) - 0.5 : (v) + 0.5 ) #endif //-----</KiROUND KIT>----------------------------------------------------------- // Only a macro is compile time calculated, an inline function causes a static constructor // in a situation like this. // Therefore the Release build is best done with a MACRO not an inline function. int Computed = KiROUND( 14.3 * 8 ); int main( int argc, char** argv ) { for( double d = double(INT_MAX)-1; d < double(INT_MAX)+8; d += 2.0 ) { int i = KiROUND( d ); printf( "t: %d %.16g\n", i, d ); } return 0; }
2012-04-19 06:55:45 +00:00
*pX = KiROUND( fpx );
*pY = KiROUND( fpy );
}
}
2011-12-14 04:29:25 +00:00
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;
}
2011-12-14 04:29:25 +00:00
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;
}
2011-12-14 04:29:25 +00:00
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;
}
2011-12-14 04:29:25 +00:00
void RotatePoint( double* pX, double* pY, double angle )
{
double tmp;
while( angle < 0 )
angle += 3600;
while( angle >= 3600 )
angle -= 3600;
// 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
{
2011-12-14 04:29:25 +00:00
double fangle = DEG2RAD( angle / 10.0 );
double sinus = sin( fangle );
double cosinus = cos( fangle );
double fpx = (*pY * sinus ) + (*pX * cosinus );
double fpy = (*pY * cosinus ) - (*pX * sinus );
2011-09-21 12:51:46 +00:00
*pX = fpx;
*pY = fpy;
}
}
2007-08-08 20:51:08 +00:00
double EuclideanNorm( wxPoint vector )
{
return hypot( (double) vector.x, (double) vector.y );
}
double DistanceLinePoint( wxPoint linePointA, wxPoint linePointB, wxPoint referencePoint )
{
return fabs( (double) ( (linePointB.x - linePointA.x) * (linePointA.y - referencePoint.y) -
(linePointA.x - referencePoint.x ) * (linePointB.y - linePointA.y) )
/ EuclideanNorm( linePointB - linePointA ) );
}
bool HitTestPoints( wxPoint pointA, wxPoint pointB, double threshold )
{
wxPoint vectorAB = pointB - pointA;
double distance = EuclideanNorm( vectorAB );
return distance < threshold;
}
double CrossProduct( wxPoint vectorA, wxPoint vectorB )
{
return (double)vectorA.x * vectorB.y - (double)vectorA.y * vectorB.x;
}
double GetLineLength( const wxPoint& aPointA, const wxPoint& aPointB )
{
return hypot( (double) aPointA.x - (double) aPointB.x,
(double) aPointA.y - (double) aPointB.y );
}