trigo.cpp 12.6 KB
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436
/*
 * 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;
}


/* Function TestSegmentHit
 * test for hit on line segment
 * i.e. a reference point is within a given distance from segment
 * aRefPoint = reference point to test
 * aStart, aEnd are coordinates of end points segment
 * aDist = maximum distance for hit
 * Note: for calculation time reasons, the distance between the ref point
 * and the segment is not always exactly calculated
 * (we only know if the actual dist is < aDist, not exactly know this dist.
 * Because many times we have horizontal or vertical segments,
 * a special calcultaion is made for them
 * Note: sometimes we need to calculate the distande between 2 points
 * A square root should be calculated.
 * However, because we just compare 2 distnaces, to avoid calculating square root,
 * the square of distances are compared.
*/
static inline double square( int x )    // helper function to calculate x*x
{
    return (double) x * x;
}
bool TestSegmentHit( const wxPoint &aRefPoint, wxPoint aStart,
                     wxPoint aEnd, int aDist )
{
    // test for vertical or horizontal segment
    if( aEnd.x == aStart.x )
    {
        // vertical segment
        int ll = abs( aRefPoint.x - aStart.x );

        if( ll > aDist )
            return false;

        // To have only one case to examine, ensure aEnd.y > aStart.y
        if( aEnd.y < aStart.y )
            EXCHG( aStart.y, aEnd.y );

        if( aRefPoint.y <= aEnd.y && aRefPoint.y >= aStart.y )
            return true;

        // there is a special case: x,y near an end point (distance < dist )
        // the distance should be carefully calculated
        if( (aStart.y - aRefPoint.y) < aDist )
        {
            double dd = square( aRefPoint.x - aStart.x) +
                 square( aRefPoint.y - aStart.y );
            if( dd <= square( aDist ) )
                return true;
        }

        if( (aRefPoint.y - aEnd.y) < aDist )
        {
            double dd = square( aRefPoint.x - aEnd.x ) +
                 square( aRefPoint.y - aEnd.y );
            if( dd <= square( aDist ) )
                return true;
        }
    }
    else if( aEnd.y == aStart.y )
    {
        // horizontal segment
        int ll = abs( aRefPoint.y - aStart.y );

        if( ll > aDist )
            return false;

        // To have only one case to examine, ensure xf > xi
        if( aEnd.x < aStart.x )
            EXCHG( aStart.x, aEnd.x );

        if( aRefPoint.x <= aEnd.x && aRefPoint.x >= aStart.x )
            return true;

        // there is a special case: x,y near an end point (distance < dist )
        // the distance should be carefully calculated
        if( (aStart.x - aRefPoint.x) <= aDist )
        {
            double dd = square( aRefPoint.x - aStart.x ) +
                        square( aRefPoint.y - aStart.y );
            if( dd <= square( aDist ) )
                return true;
        }

        if( (aRefPoint.x - aEnd.x) <= aDist )
        {
            double dd = square( aRefPoint.x - aEnd.x ) +
                        square( aRefPoint.y - aEnd.y );
            if( dd <= square( aDist ) )
                return true;
        }
    }
    else
    {
        // oblique segment:
        // First, we need to calculate the distance between the point
        // and the line defined by aStart and aEnd
        // this dist should be < dist
        //
        // find a,slope such that aStart and aEnd lie on y = a + slope*x
        double  slope   = (double) (aEnd.y - aStart.y) / (aEnd.x - aStart.x);
        double  a   = (double) aStart.y - slope * aStart.x;
        // find c,orthoslope such that (x,y) lies on y = c + orthoslope*x,
        // where orthoslope=(-1/slope)
        // to calculate xp, yp = near point from aRefPoint
        // which is on the line defined by aStart, aEnd
        double  orthoslope   = -1.0 / slope;
        double  c   = (double) aRefPoint.y - orthoslope * aRefPoint.x;
        // find nearest point to (x,y) on line defined by aStart, aEnd
        double  xp  = (a - c) / (orthoslope - slope);
        double  yp  = a + slope * xp;
        // find distance to line, in fact the square of dist,
        // because we just know if it is > or < aDist
        double dd = square( aRefPoint.x - xp ) + square( aRefPoint.y - yp );
        double dist = square( aDist );

        if( dd > dist )    // this reference point is not a good candiadte.
            return false;

        // dd is < dist, therefore we should make a fine test
        if( fabs( slope ) > 0.7 )
        {
            // line segment more vertical than horizontal
            if( (aEnd.y > aStart.y && yp <= aEnd.y && yp >= aStart.y) ||
                (aEnd.y < aStart.y && yp >= aEnd.y && yp <= aStart.y) )
                return true;
        }
        else
        {
            // line segment more horizontal than vertical
            if( (aEnd.x > aStart.x && xp <= aEnd.x && xp >= aStart.x) ||
                (aEnd.x < aStart.x && xp >= aEnd.x && xp <= aStart.x) )
                return true;
        }

        // Here, the test point is still a good candidate,
        // however it is not "between" the end points of the segment.
        // It is "outside" the segment, but it could be near a segment end point
        // Therefore, we test the dist from the test point to each segment end point
        dd = square( aRefPoint.x - aEnd.x ) + square( aRefPoint.y - aEnd.y );
        if( dd <= dist )
            return true;
        dd = square( aRefPoint.x - aStart.x ) + square( aRefPoint.y - aStart.y );
        if( dd <= dist )
            return true;
    }

    return false;    // no hit
}


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( dy, 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( 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;
    }
}