seg.cpp 4.22 KB
Newer Older
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
/*
 * This program source code file is part of KiCad, a free EDA CAD application.
 *
 * Copyright (C) 2013 CERN
 * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
 *
 * 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
 */

#include <geometry/seg.h>

Maciej Suminski's avatar
Maciej Suminski committed
27 28
template <typename T>
int sgn( T aVal )
29
{
Maciej Suminski's avatar
Maciej Suminski committed
30
    return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
31 32
}

33

Maciej Suminski's avatar
Maciej Suminski committed
34
bool SEG::PointCloserThan( const VECTOR2I& aP, int aDist ) const
35
{
Maciej Suminski's avatar
Maciej Suminski committed
36 37
    VECTOR2I d = B - A;
    ecoord dist_sq = (ecoord) aDist * aDist;
38

39
    SEG::ecoord l_squared = d.Dot( d );
Maciej Suminski's avatar
Maciej Suminski committed
40
    SEG::ecoord t = d.Dot( aP - A );
41 42

    if( t <= 0 || !l_squared )
Maciej Suminski's avatar
Maciej Suminski committed
43
        return ( aP - A ).SquaredEuclideanNorm() < dist_sq;
44
    else if( t >= l_squared )
Maciej Suminski's avatar
Maciej Suminski committed
45
        return ( aP - B ).SquaredEuclideanNorm() < dist_sq;
46

47
    int dxdy = abs( d.x ) - abs( d.y );
48

49
    if( ( dxdy >= -1 && dxdy <= 1 ) || abs( d.x ) <= 1 || abs( d.y ) <= 1 )
50
    {
51 52
        int ca = -sgn( d.y );
        int cb = sgn( d.x );
Maciej Suminski's avatar
Maciej Suminski committed
53
        int cc = -ca * A.x - cb * A.y;
54 55 56 57 58 59 60 61 62 63 64

        ecoord num = ca * aP.x + cb * aP.y + cc;
        num *= num;

        if( ca && cb )
            num >>= 1;

        if( num > ( dist_sq + 100 ) )
            return false;
        else if( num < ( dist_sq - 100 ) )
            return true;
65 66
    }

67
    VECTOR2I nearest;
Maciej Suminski's avatar
Maciej Suminski committed
68 69
    nearest.x = A.x + rescale( t, (ecoord) d.x, l_squared );
    nearest.y = A.y + rescale( t, (ecoord) d.y, l_squared );
70

71
    return ( nearest - aP ).SquaredEuclideanNorm() <= dist_sq;
72 73
}

74

75
SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const
76
{
77 78 79 80 81 82
    // fixme: rather inefficient....
    if( Intersect( aSeg ) )
        return 0;

    const VECTOR2I pts[4] =
    {
Maciej Suminski's avatar
Maciej Suminski committed
83 84 85 86
        aSeg.NearestPoint( A ) - A,
        aSeg.NearestPoint( B ) - B,
        NearestPoint( aSeg.A ) - aSeg.A,
        NearestPoint( aSeg.B ) - aSeg.B
87 88 89
    };

    ecoord m = VECTOR2I::ECOORD_MAX;
90

91 92 93 94
    for( int i = 0; i < 4; i++ )
        m = std::min( m, pts[i].SquaredEuclideanNorm() );

    return m;
95
}
96

97

98 99
OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
{
Maciej Suminski's avatar
Maciej Suminski committed
100 101 102
    const VECTOR2I  e( B - A );
    const VECTOR2I  f( aSeg.B - aSeg.A );
    const VECTOR2I  ac( aSeg.A - A );
103 104 105 106 107 108 109

    ecoord d = f.Cross( e );
    ecoord p = f.Cross( ac );
    ecoord q = e.Cross( ac );

    if( d == 0 )
        return OPT_VECTOR2I();
110 111

    if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
112
        return OPT_VECTOR2I();
113 114

    if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
115
        return OPT_VECTOR2I();
116 117

    if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
118 119
        return OPT_VECTOR2I();

Maciej Suminski's avatar
Maciej Suminski committed
120 121
    VECTOR2I ip( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
                 aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
122 123

     return ip;
124 125 126
}


Maciej Suminski's avatar
Maciej Suminski committed
127
bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
128
{
Maciej Suminski's avatar
Maciej Suminski committed
129
    return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
130 131
}

132

133
bool SEG::Collide( const SEG& aSeg, int aClearance ) const
134
{
135 136
    // check for intersection
    // fixme: move to a method
Maciej Suminski's avatar
Maciej Suminski committed
137 138
    if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
            ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
139
        return true;
140

141 142
#define CHK( _seg, _pt ) \
    if( (_seg).PointCloserThan( _pt, aClearance ) ) return true;
143

Maciej Suminski's avatar
Maciej Suminski committed
144 145 146 147
    CHK( *this, aSeg.A );
    CHK( *this, aSeg.B );
    CHK( aSeg, A );
    CHK( aSeg, B );
148 149
#undef CHK

150
    return false;
151 152
}

153 154 155

bool SEG::Contains( const VECTOR2I& aP ) const
{
156
    return PointCloserThan( aP, 1 );
157
}