1 /*
2 * This program source code file is part of KiCad, a free EDA CAD application.
3 *
4 * Copyright (C) 2013 CERN
5 * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
6 *
7 * This program is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU General Public License
9 * as published by the Free Software Foundation; either version 2
10 * of the License, or (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, you may find one here:
19 * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
20 * or you may search the http://www.gnu.org website for the version 2 license,
21 * or you may write to the Free Software Foundation, Inc.,
22 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
23 */
24
25 #include <geometry/seg.h>
26
27 template <typename T>
sgn(T aVal)28 int sgn( T aVal )
29 {
30 return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
31 }
32
33
PointCloserThan(const VECTOR2I & aP,int aDist) const34 bool SEG::PointCloserThan( const VECTOR2I& aP, int aDist ) const
35 {
36 // See http://geomalgorithms.com/a02-_lines.html for some explanations and ideas.
37 VECTOR2I d = B - A;
38 ecoord dist_sq = (ecoord) aDist * aDist;
39
40 SEG::ecoord l_squared = d.Dot( d );
41 SEG::ecoord t = d.Dot( aP - A );
42
43 if( t <= 0 || !l_squared )
44 return ( aP - A ).SquaredEuclideanNorm() < dist_sq;
45 else if( t >= l_squared )
46 return ( aP - B ).SquaredEuclideanNorm() < dist_sq;
47
48 // JPC: This code is not trivial and is not commented
49 // and does not work for d.x or d.y = -1...1
50 // I am guessing it is here for calculation time optimization.
51 // if someone can understand it, please fix it.
52 // It can be tested with a segment having d.x or d.y value
53 // is -1 or +1 ("this" is a quasi vertical or horizontal segment)
54 int dxdy = std::abs( d.x ) - std::abs( d.y );
55
56 if( ( dxdy >= -1 && dxdy <= 1 ) // quasi 45 deg segment
57 /*|| std::abs( d.x ) <= 1 // quasi horizontal segment
58 || std::abs( d.y ) <= 1 // quasi vertical segment */ )
59 {
60 int ca = -sgn( d.y );
61 int cb = sgn( d.x );
62 int cc = -ca * A.x - cb * A.y;
63
64 ecoord num = (ecoord) ca * aP.x + (ecoord) cb * aP.y + cc;
65 num *= num;
66
67 if( ca && cb )
68 num >>= 1;
69
70 if( num > ( dist_sq + 100 ) )
71 return false;
72
73 else if( num < ( dist_sq - 100 ) )
74 return true;
75 }
76
77 VECTOR2I nearest;
78 nearest.x = A.x + rescale( t, (ecoord) d.x, l_squared );
79 nearest.y = A.y + rescale( t, (ecoord) d.y, l_squared );
80
81 return ( nearest - aP ).SquaredEuclideanNorm() <= dist_sq;
82 }
83
84
SquaredDistance(const SEG & aSeg) const85 SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const
86 {
87 // fixme: rather inefficient....
88 if( Intersect( aSeg ) )
89 return 0;
90
91 const VECTOR2I pts[4] =
92 {
93 aSeg.NearestPoint( A ) - A,
94 aSeg.NearestPoint( B ) - B,
95 NearestPoint( aSeg.A ) - aSeg.A,
96 NearestPoint( aSeg.B ) - aSeg.B
97 };
98
99 ecoord m = VECTOR2I::ECOORD_MAX;
100
101 for( int i = 0; i < 4; i++ )
102 m = std::min( m, pts[i].SquaredEuclideanNorm() );
103
104 return m;
105 }
106
107
NearestPoint(const SEG & aSeg) const108 const VECTOR2I SEG::NearestPoint( const SEG& aSeg ) const
109 {
110 if( auto p = Intersect( aSeg ) )
111 return *p;
112
113 const VECTOR2I pts_origin[4] =
114 {
115 aSeg.NearestPoint( A ),
116 aSeg.NearestPoint( B ),
117 NearestPoint( aSeg.A ),
118 NearestPoint( aSeg.B )
119 };
120
121 const ecoord pts_dist[4] =
122 {
123 ( pts_origin[0] - A ).SquaredEuclideanNorm(),
124 ( pts_origin[1] - B ).SquaredEuclideanNorm(),
125 ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
126 ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
127 };
128
129 int min_i = 0;
130
131 for( int i = 0; i < 4; i++ )
132 {
133 if( pts_dist[i] < pts_dist[min_i] )
134 min_i = i;
135 }
136
137 return pts_origin[min_i];
138 }
139
140
Intersect(const SEG & aSeg,bool aIgnoreEndpoints,bool aLines) const141 OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
142 {
143 const VECTOR2I e( B - A );
144 const VECTOR2I f( aSeg.B - aSeg.A );
145 const VECTOR2I ac( aSeg.A - A );
146
147 ecoord d = f.Cross( e );
148 ecoord p = f.Cross( ac );
149 ecoord q = e.Cross( ac );
150
151 if( d == 0 )
152 return OPT_VECTOR2I();
153
154 if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
155 return OPT_VECTOR2I();
156
157 if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
158 return OPT_VECTOR2I();
159
160 if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
161 return OPT_VECTOR2I();
162
163 VECTOR2I ip( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
164 aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
165
166 return ip;
167 }
168
169
ccw(const VECTOR2I & aA,const VECTOR2I & aB,const VECTOR2I & aC) const170 bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
171 {
172 return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
173 }
174
175
Collide(const SEG & aSeg,int aClearance) const176 bool SEG::Collide( const SEG& aSeg, int aClearance ) const
177 {
178 // check for intersection
179 // fixme: move to a method
180 if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
181 ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
182 return true;
183
184 #define CHK( _seg, _pt ) \
185 if( (_seg).PointCloserThan( _pt, aClearance ) ) return true;
186
187 CHK( *this, aSeg.A );
188 CHK( *this, aSeg.B );
189 CHK( aSeg, A );
190 CHK( aSeg, B );
191 #undef CHK
192
193 return false;
194 }
195
196
Contains(const VECTOR2I & aP) const197 bool SEG::Contains( const VECTOR2I& aP ) const
198 {
199 return PointCloserThan( aP, 1 );
200 }
201