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
|
/*
* 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>
template <typename T>
int sgn( T aVal )
{
return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
}
bool SEG::PointCloserThan( const VECTOR2I& aP, int aDist ) const
{
VECTOR2I d = B - A;
ecoord dist_sq = (ecoord) aDist * aDist;
SEG::ecoord l_squared = d.Dot( d );
SEG::ecoord t = d.Dot( aP - A );
if( t <= 0 || !l_squared )
return ( aP - A ).SquaredEuclideanNorm() < dist_sq;
else if( t >= l_squared )
return ( aP - B ).SquaredEuclideanNorm() < dist_sq;
int dxdy = abs( d.x ) - abs( d.y );
if( ( dxdy >= -1 && dxdy <= 1 ) || abs( d.x ) <= 1 || abs( d.y ) <= 1 )
{
int ca = -sgn( d.y );
int cb = sgn( d.x );
int cc = -ca * A.x - cb * A.y;
ecoord num = (ecoord) ca * aP.x + (ecoord) 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;
}
VECTOR2I nearest;
nearest.x = A.x + rescale( t, (ecoord) d.x, l_squared );
nearest.y = A.y + rescale( t, (ecoord) d.y, l_squared );
return ( nearest - aP ).SquaredEuclideanNorm() <= dist_sq;
}
SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const
{
// fixme: rather inefficient....
if( Intersect( aSeg ) )
return 0;
const VECTOR2I pts[4] =
{
aSeg.NearestPoint( A ) - A,
aSeg.NearestPoint( B ) - B,
NearestPoint( aSeg.A ) - aSeg.A,
NearestPoint( aSeg.B ) - aSeg.B
};
ecoord m = VECTOR2I::ECOORD_MAX;
for( int i = 0; i < 4; i++ )
m = std::min( m, pts[i].SquaredEuclideanNorm() );
return m;
}
OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
{
const VECTOR2I e( B - A );
const VECTOR2I f( aSeg.B - aSeg.A );
const VECTOR2I ac( aSeg.A - A );
ecoord d = f.Cross( e );
ecoord p = f.Cross( ac );
ecoord q = e.Cross( ac );
if( d == 0 )
return OPT_VECTOR2I();
if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
return OPT_VECTOR2I();
if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
return OPT_VECTOR2I();
if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
return OPT_VECTOR2I();
VECTOR2I ip( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
return ip;
}
bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
{
return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
}
bool SEG::Collide( const SEG& aSeg, int aClearance ) const
{
// check for intersection
// fixme: move to a method
if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
return true;
#define CHK( _seg, _pt ) \
if( (_seg).PointCloserThan( _pt, aClearance ) ) return true;
CHK( *this, aSeg.A );
CHK( *this, aSeg.B );
CHK( aSeg, A );
CHK( aSeg, B );
#undef CHK
return false;
}
bool SEG::Contains( const VECTOR2I& aP ) const
{
return PointCloserThan( aP, 1 );
}
|
