Branch data Line data Source code
1 : : /***************************************************************************
2 : : MathUtils.cpp
3 : : -------------
4 : : copyright : (C) 2004 by Marco Hugentobler
5 : : email : mhugent@geo.unizh.ch
6 : : ***************************************************************************/
7 : :
8 : : /***************************************************************************
9 : : * *
10 : : * This program is free software; you can redistribute it and/or modify *
11 : : * it under the terms of the GNU General Public License as published by *
12 : : * the Free Software Foundation; either version 2 of the License, or *
13 : : * (at your option) any later version. *
14 : : * *
15 : : ***************************************************************************/
16 : :
17 : : #include "MathUtils.h"
18 : : #include "qgslogger.h"
19 : : #include "qgspoint.h"
20 : : #include "Vector3D.h"
21 : :
22 : 0 : bool MathUtils::calcBarycentricCoordinates( double x, double y, QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *result )
23 : : {
24 : 0 : if ( p1 && p2 && p3 && result )
25 : : {
26 : 0 : QgsPoint p( x, y, 0 );
27 : 0 : double area = triArea( p1, p2, p3 );
28 : 0 : if ( area == 0 )//p1, p2, p3 are in a line
29 : : {
30 : 0 : return false;
31 : : }
32 : 0 : double area1 = triArea( &p, p2, p3 );
33 : 0 : double area2 = triArea( p1, &p, p3 );
34 : 0 : double area3 = triArea( p1, p2, &p );
35 : 0 : double u = area1 / area;
36 : 0 : double v = area2 / area;
37 : 0 : double w = area3 / area;
38 : 0 : result->setX( u );
39 : 0 : result->setY( v );
40 : 0 : result->setZ( w );
41 : 0 : return true;
42 : 0 : }
43 : : else//null pointer
44 : : {
45 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
46 : 0 : return false;
47 : : }
48 : 0 : }
49 : :
50 : 0 : bool MathUtils::BarycentricToXY( double u, double v, double w, QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *result )//this is wrong at the moment. Furthermore, the case, where the denominators are 0 have to be treated (two ways of calculating px and py)
51 : : {
52 : : Q_UNUSED( w )
53 : :
54 : : double px, py;
55 : 0 : if ( p1 && p2 && p3 && result )
56 : : {
57 : 0 : double area = triArea( p1, p2, p3 );
58 : :
59 : 0 : if ( area == 0 )
60 : : {
61 : 0 : QgsDebugMsg( QStringLiteral( "warning, p1, p2 and p3 are in a line" ) );
62 : 0 : return false;
63 : : }
64 : :
65 : 0 : double denominator = ( ( p2->y() - p3->y() ) * ( p1->x() - p3->x() ) - ( p3->y() - p1->y() ) * ( p3->x() - p2->x() ) );
66 : 0 : if ( denominator != 0 )//drop out py in the two equations
67 : : {
68 : 0 : px = ( 2 * u * area * ( p1->x() - p3->x() ) - 2 * v * area * ( p3->x() - p2->x() ) - p2->x() * p3->y() * ( p1->x() - p3->x() ) + p3->x() * p1->y() * ( p3->x() - p2->x() ) + p3->x() * p2->y() * ( p1->x() - p3->x() ) - p1->x() * p3->y() * ( p3->x() - p2->x() ) ) / denominator;
69 : 0 : if ( ( p3->x() - p2->x() ) != 0 )
70 : : {
71 : 0 : py = ( 2 * u * area - px * ( p2->y() - p3->y() ) - p2->x() * p3->y() + p3->x() * p2->y() ) / ( p3->x() - p2->x() );
72 : 0 : }
73 : : else
74 : : {
75 : 0 : py = ( 2 * v * area - px * ( p3->y() - p1->y() ) - p3->x() * p1->y() + p1->x() * p3->y() ) / ( p1->x() - p3->x() );
76 : : }
77 : 0 : }
78 : : else//drop out px in the two equations(maybe this possibility occurs only, if p1, p2 and p3 are coplanar
79 : : {
80 : 0 : py = ( 2 * u * area * ( p3->y() - p1->y() ) - 2 * v * area * ( p2->y() - p3->y() ) - p2->x() * p3->y() * ( p3->y() - p1->y() ) + p3->x() * p1->y() * ( p2->y() - p3->y() ) + p3->x() * p2->y() * ( p3->y() - p1->y() ) - p1->x() * p3->y() * ( p2->y() - p3->y() ) ) / ( ( p3->x() - p2->x() ) * ( p3->y() - p1->y() ) - ( p1->x() - p3->x() ) * ( p2->y() - p3->y() ) );
81 : 0 : if ( ( p2->y() - p3->y() ) != 0 )
82 : : {
83 : 0 : px = ( 2 * u * area - py * ( p3->x() - p2->x() ) - p2->x() * p3->y() + p3->x() * p2->y() ) / ( p2->y() - p3->y() );
84 : 0 : }
85 : : else
86 : : {
87 : 0 : px = ( 2 * v * area - py * ( p1->x() - p3->x() ) - p3->x() * p1->y() + p1->x() * p3->y() ) / ( p3->y() - p1->y() );
88 : : }
89 : : }
90 : 0 : result->setX( px );
91 : 0 : result->setY( py );
92 : 0 : return true;
93 : : }
94 : : else//null pointer
95 : : {
96 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
97 : 0 : return false;
98 : : }
99 : 0 : }
100 : :
101 : 0 : double MathUtils::calcBernsteinPoly( int n, int i, double t )
102 : : {
103 : 0 : if ( i < 0 )
104 : : {
105 : 0 : return 0;
106 : : }
107 : :
108 : 0 : return lower( n, i ) * std::pow( t, i ) * std::pow( ( 1 - t ), ( n - i ) );
109 : 0 : }
110 : :
111 : 0 : double MathUtils::cFDerBernsteinPoly( int n, int i, double t )
112 : : {
113 : 0 : return n * ( calcBernsteinPoly( n - 1, i - 1, t ) - calcBernsteinPoly( n - 1, i, t ) );
114 : : }
115 : :
116 : 0 : bool MathUtils::circumcenter( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *result )//version using the property that the distances from p1, p2, p3 to the circumcenter have to be equal. Possibly there is a bug
117 : : {
118 : 0 : if ( p1 && p2 && p3 && result )
119 : : {
120 : 0 : double distp1p2 = std::sqrt( ( p1->x() - p2->x() ) * ( p1->x() - p2->x() ) + ( p1->y() - p2->y() ) * ( p1->y() - p2->y() ) );
121 : 0 : double distp2p3 = std::sqrt( ( p2->x() - p3->x() ) * ( p2->x() - p3->x() ) + ( p2->y() - p3->y() ) * ( p2->y() - p3->y() ) );
122 : 0 : if ( distp1p2 > distp2p3 )
123 : : {
124 : : //swap p1 and p3 to avoid round-off errors
125 : 0 : QgsPoint *temp = p1;
126 : 0 : p1 = p3;
127 : 0 : p3 = temp;
128 : 0 : }
129 : 0 : double denominator = -p3->x() * p2->y() + p3->x() * p1->y() + p1->x() * p2->y() + p2->x() * p3->y() - p2->x() * p1->y() - p1->x() * p3->y();
130 : : //if one of the denominator is zero we will have problems
131 : 0 : if ( denominator == 0 )
132 : : {
133 : 0 : QgsDebugMsg( QStringLiteral( "error: the three points are in a line" ) );
134 : 0 : return false;
135 : : }
136 : : else
137 : : {
138 : 0 : result->setX( 0.5 * ( p1->x()*p1->x()*p2->y() - p1->x()*p1->x()*p3->y() - p3->x()*p3->x()*p2->y() - p1->y()*p2->x()*p2->x() - p1->y()*p1->y()*p3->y() - p3->y()*p3->y()*p2->y() + p1->y()*p1->y()*p2->y() + p3->y()*p2->x()*p2->x() - p1->y()*p2->y()*p2->y() + p1->y()*p3->y()*p3->y() + p1->y()*p3->x()*p3->x() + p3->y()*p2->y()*p2->y() ) / denominator );
139 : 0 : result->setY( -0.5 * ( p3->x()*p2->x()*p2->x() + p2->x()*p1->y()*p1->y() + p3->x()*p2->y()*p2->y() - p3->x()*p1->x()*p1->x() + p1->x()*p3->y()*p3->y() - p3->x()*p1->y()*p1->y() - p1->x()*p2->x()*p2->x() - p2->x()*p3->y()*p3->y() - p1->x()*p2->y()*p2->y() - p2->x()*p3->x()*p3->x() + p1->x()*p3->x()*p3->x() + p2->x()*p1->x()*p1->x() ) / denominator );
140 : :
141 : : #if 0
142 : : //debugging: test, if the distances from p1, p2, p3 to result are equal
143 : : double dist1 = std::sqrt( ( p1->getX() - result->getX() ) * ( p1->getX() - result->getX() ) + ( p1->getY() - result->getY() ) * ( p1->getY() - result->getY() ) );
144 : : double dist2 = std::sqrt( ( p2->getX() - result->getX() ) * ( p2->getX() - result->getX() ) + ( p2->getY() - result->getY() ) * ( p2->getY() - result->getY() ) );
145 : : double dist3 = std::sqrt( ( p3->getX() - result->getX() ) * ( p3->getX() - result->getX() ) + ( p3->getY() - result->getY() ) * ( p3->getY() - result->getY() ) );
146 : :
147 : : if ( dist1 - dist2 > 1 || dist2 - dist1 > 1 || dist1 - dist3 > 1 || dist3 - dist1 > 1 )
148 : : {
149 : : bool debug = true;
150 : : }
151 : : #endif
152 : :
153 : 0 : return true;
154 : : }
155 : : }
156 : : else
157 : : {
158 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
159 : 0 : return false;
160 : : }
161 : 0 : }
162 : :
163 : : #if 0
164 : : bool MathUtils::circumcenter( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *result )//version imitating the geometric construction
165 : : {
166 : : if ( p1 && p2 && p3 && result )
167 : : {
168 : : QgsPoint midpoint12( ( p1->getX() + p2->getX() ) / 2, ( p1->getY() + p2->getY() ) / 2, 0 );
169 : : QgsPoint midpoint23( ( p2->getX() + p3->getX() ) / 2, ( p2->getY() + p3->getY() ) / 2, 0 );
170 : : Vector3D v12( p2->getX() - p1->getX(), p2->getY() - p1->getY(), 0 );
171 : : Vector3D v23( p3->getX() - p2->getX(), p3->getY() - p2->getY(), 0 );
172 : : Vector3D n12;
173 : : MathUtils::normalLeft( &v12, &n12, 10000 );
174 : : Vector3D n23;
175 : : MathUtils::normalLeft( &v23, &n23, 10000 );
176 : : QgsPoint helppoint1( midpoint12.getX() + n12.getX(), midpoint12.getY() + n12.getY(), 0 );
177 : : QgsPoint helppoint2( midpoint23.getX() + n23.getX(), midpoint23.getY() + n23.getY(), 0 );
178 : : MathUtils::lineIntersection( &midpoint12, &helppoint1, &midpoint23, &helppoint2, result );
179 : :
180 : : //debugging: test, if the distances from p1, p2, p3 to result are equal
181 : : double dist1 = std::sqrt( ( p1->getX() - result->getX() ) * ( p1->getX() - result->getX() ) + ( p1->getY() - result->getY() ) * ( p1->getY() - result->getY() ) );
182 : : double dist2 = std::sqrt( ( p2->getX() - result->getX() ) * ( p2->getX() - result->getX() ) + ( p2->getY() - result->getY() ) * ( p2->getY() - result->getY() ) );
183 : : double dist3 = std::sqrt( ( p3->getX() - result->getX() ) * ( p3->getX() - result->getX() ) + ( p3->getY() - result->getY() ) * ( p3->getY() - result->getY() ) );
184 : :
185 : : if ( dist1 - dist2 > 1 || dist2 - dist1 > 1 || dist1 - dist3 > 1 || dist3 - dist1 > 1 )
186 : : {
187 : : bool debug = true;
188 : : }
189 : :
190 : : return true;
191 : :
192 : : }
193 : : else
194 : : {
195 : : cout << "null pointer in method MathUtils::circumcenter" << endl << flush;
196 : : return false;
197 : : }
198 : : }
199 : : #endif // 0
200 : :
201 : 0 : double MathUtils::distPointFromLine( QgsPoint *thepoint, QgsPoint *p1, QgsPoint *p2 )
202 : : {
203 : 0 : if ( thepoint && p1 && p2 )
204 : : {
205 : 0 : Vector3D normal( 0, 0, 0 );
206 : 0 : Vector3D line( p2->x() - p1->x(), p2->y() - p1->y(), 0 );
207 : 0 : MathUtils::normalLeft( &line, &normal, 1 );
208 : 0 : double a = normal.getX();
209 : 0 : double b = normal.getY();
210 : 0 : double c = -( normal.getX() * p2->x() + normal.getY() * p2->y() );
211 : 0 : double distance = std::fabs( ( a * thepoint->x() + b * thepoint->y() + c ) / ( std::sqrt( a * a + b * b ) ) );
212 : 0 : return distance;
213 : : }
214 : : else
215 : : {
216 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
217 : 0 : return 0;
218 : : }
219 : 0 : }
220 : :
221 : 0 : int MathUtils::faculty( int n )
222 : : {
223 : 0 : return std::tgamma( n + 1 );
224 : : }
225 : :
226 : 0 : bool MathUtils::inCircle( QgsPoint *testp, QgsPoint *p1, QgsPoint *p2, QgsPoint *p3 )
227 : : {
228 : 0 : double tolerance = 0.0001;//if the amount of aValue is below this, testp is approximately on the circle and we have to define another criterion to tell, if it is inside or outside.
229 : :
230 : 0 : if ( testp && p1 && p2 && p3 )
231 : : {
232 : 0 : double ax = p1->x();
233 : 0 : double ay = p1->y();
234 : 0 : double bx = p2->x();
235 : 0 : double by = p2->y();
236 : 0 : double cx = p3->x();
237 : 0 : double cy = p3->y();
238 : 0 : double px = testp->x();
239 : 0 : double py = testp->y();
240 : :
241 : 0 : double xmin = std::min( std::min( ax, px ), std::min( bx, cx ) );
242 : 0 : double ymin = std::min( std::min( ay, py ), std::min( by, cy ) );
243 : 0 : ax -= xmin;
244 : 0 : bx -= xmin;
245 : 0 : cx -= xmin;
246 : 0 : px -= xmin;
247 : 0 : ay -= ymin;
248 : 0 : by -= ymin;
249 : 0 : cy -= ymin;
250 : 0 : py -= ymin;
251 : :
252 : 0 : double aValue = ( ax * ax + ay * ay ) * triArea( p2, p3, testp );
253 : 0 : aValue = aValue - ( ( bx * bx + by * by ) * triArea( p1, p3, testp ) );
254 : 0 : aValue = aValue + ( ( cx * cx + cy * cy ) * triArea( p1, p2, testp ) );
255 : 0 : aValue = aValue - ( ( px * px + py * py ) * triArea( p1, p2, p3 ) );
256 : :
257 : 0 : return aValue > tolerance;
258 : : }
259 : : else
260 : : {
261 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
262 : 0 : return false;
263 : : }
264 : 0 : }
265 : :
266 : 0 : bool MathUtils::inDiametral( QgsPoint *p1, QgsPoint *p2, QgsPoint *point )
267 : : {
268 : 0 : return angle( p1, point, p2, point ) > 90;
269 : : }
270 : :
271 : : #if 0
272 : : bool MathUtils::inDiametral( QgsPoint *p1, QgsPoint *p2, QgsPoint *point )
273 : : {
274 : : if ( p1 && p2 && point )
275 : : {
276 : : Vector3D p1p2( p2->getX() - p1->getX(), p2->getY() - p1->getY(), 0 );
277 : : Vector3D orthogonalvec;//vector orthogonal to p1p2 (length radius)
278 : : QgsPoint midpoint( ( p1->getX() + p2->getX() ) / 2, ( p1->getY() + p2->getY() ) / 2, 0 );
279 : : double radius = p1p2.getLength() / 2;
280 : : normalLeft( &p1p2, &orthogonalvec, radius );
281 : : QgsPoint p3( midpoint.getX() + orthogonalvec.getX(), midpoint.getY() + orthogonalvec.getY(), 0 );
282 : : return inCircle( point, p1, p2, &p3 );
283 : : }
284 : : else
285 : : {
286 : : cout << "null pointer in MathUtils::inDiametral" << endl << flush;
287 : : return false;
288 : : }
289 : : }
290 : : #endif // 0
291 : :
292 : 0 : double MathUtils::leftOf( const QgsPoint &thepoint, const QgsPoint *p1, const QgsPoint *p2 )
293 : : {
294 : 0 : if ( p1 && p2 )
295 : : {
296 : : //just for debugging
297 : 0 : double f1 = thepoint.x() - p1->x();
298 : 0 : double f2 = p2->y() - p1->y();
299 : 0 : double f3 = thepoint.y() - p1->y();
300 : 0 : double f4 = p2->x() - p1->x();
301 : 0 : return f1 * f2 - f3 * f4;
302 : : //return thepoint->getX()-p1->getX())*(p2->getY()-p1->getY())-(thepoint->getY()-p1->getY())*(p2->getX()-p1->getX();//calculating the vectorproduct
303 : : }
304 : : else
305 : : {
306 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
307 : 0 : return 0;
308 : : }
309 : 0 : }
310 : :
311 : 0 : bool MathUtils::lineIntersection( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *p4 )
312 : : {
313 : 0 : if ( p1 && p2 && p3 && p4 )
314 : : {
315 : : double t1, t2;
316 : 0 : Vector3D p1p2( p2->x() - p1->x(), p2->y() - p1->y(), 0 );
317 : 0 : Vector3D p3p4( p4->x() - p3->x(), p4->y() - p3->y(), 0 );
318 : 0 : if ( ( p3p4.getX()*p1p2.getY() - p3p4.getY()*p1p2.getX() ) != 0 && p1p2.getX() != 0 ) //avoid division through zero
319 : : {
320 : 0 : t2 = ( p1->x() * p1p2.getY() - p1->y() * p1p2.getX() + p3->y() * p1p2.getX() - p3->x() * p1p2.getY() ) / ( p3p4.getX() * p1p2.getY() - p3p4.getY() * p1p2.getX() );
321 : 0 : t1 = ( p3->x() - p1->x() + t2 * p3p4.getX() ) / p1p2.getX();
322 : 0 : }
323 : 0 : else if ( ( p1p2.getX()*p3p4.getY() - p1p2.getY()*p3p4.getX() ) != 0 && p3p4.getX() != 0 )
324 : : {
325 : 0 : t1 = ( p3->x() * p3p4.getY() - p3->y() * p3p4.getX() - p1->x() * p3p4.getY() + p1->y() * p3p4.getX() ) / ( p1p2.getX() * p3p4.getY() - p1p2.getY() * p3p4.getX() );
326 : 0 : t2 = ( p1->x() + t1 * p1p2.getX() - p3->x() ) / p3p4.getX();
327 : 0 : }
328 : : else//the lines are parallel
329 : : {
330 : 0 : return false;
331 : : }
332 : :
333 : 0 : if ( t1 > 0 && t1 < 1 && t2 > 0 && t2 < 1 )
334 : : {
335 : 0 : if ( ( *p1 ) == ( *p3 ) || ( *p1 ) == ( *p4 ) || ( *p2 ) == ( *p3 ) || ( *p2 ) == ( *p4 ) ) //the lines touch each other, so they do not cross
336 : : {
337 : 0 : return false;
338 : : }
339 : 0 : return true;
340 : : }
341 : : else
342 : : {
343 : 0 : return false;
344 : : }
345 : : }
346 : :
347 : : else
348 : : {
349 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
350 : 0 : return false;
351 : : }
352 : 0 : }
353 : :
354 : 0 : bool MathUtils::lineIntersection( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *p4, QgsPoint *intersection_point )
355 : : {
356 : 0 : if ( p1 && p2 && p3 && p4 )
357 : : {
358 : : double t1, t2;
359 : 0 : Vector3D p1p2( p2->x() - p1->x(), p2->y() - p1->y(), 0 );
360 : 0 : Vector3D p3p4( p4->x() - p3->x(), p4->y() - p3->y(), 0 );
361 : 0 : if ( ( p3p4.getX()*p1p2.getY() - p3p4.getY()*p1p2.getX() ) != 0 && p1p2.getX() != 0 ) //avoid division through zero
362 : : {
363 : 0 : t2 = ( p1->x() * p1p2.getY() - p1->y() * p1p2.getX() + p3->y() * p1p2.getX() - p3->x() * p1p2.getY() ) / ( p3p4.getX() * p1p2.getY() - p3p4.getY() * p1p2.getX() );
364 : 0 : t1 = ( p3->x() - p1->x() + t2 * p3p4.getX() ) / p1p2.getX();
365 : 0 : }
366 : 0 : else if ( ( p1p2.getX()*p3p4.getY() - p1p2.getY()*p3p4.getX() ) != 0 && p3p4.getX() != 0 )
367 : : {
368 : 0 : t1 = ( p3->x() * p3p4.getY() - p3->y() * p3p4.getX() - p1->x() * p3p4.getY() + p1->y() * p3p4.getX() ) / ( p1p2.getX() * p3p4.getY() - p1p2.getY() * p3p4.getX() );
369 : 0 : t2 = ( p1->x() + t1 * p1p2.getX() - p3->x() ) / p3p4.getX();
370 : 0 : }
371 : : else//the lines are parallel
372 : : {
373 : 0 : intersection_point->setX( 0 );
374 : 0 : intersection_point->setY( 0 );
375 : 0 : intersection_point->setZ( 0 );
376 : 0 : return false;
377 : : }
378 : :
379 : 0 : if ( t1 > 0 && t1 < 1 && t2 > 0 && t2 < 1 )
380 : : {
381 : 0 : if ( ( *p1 ) == ( *p3 ) || ( *p1 ) == ( *p4 ) || ( *p2 ) == ( *p3 ) || ( *p2 ) == ( *p4 ) ) //the lines touch each other, so they do not cross
382 : : {
383 : 0 : intersection_point->setX( 0 );
384 : 0 : intersection_point->setY( 0 );
385 : 0 : intersection_point->setZ( 0 );
386 : 0 : return false;
387 : : }
388 : : //calculate the intersection point
389 : 0 : intersection_point->setX( p1->x() * ( 1 - t1 ) + p2->x()*t1 );
390 : 0 : intersection_point->setY( p1->y() * ( 1 - t1 ) + p2->y()*t1 );
391 : 0 : intersection_point->setZ( 0 );
392 : 0 : return true;
393 : : }
394 : : else
395 : : {
396 : 0 : return false;
397 : : }
398 : : }
399 : :
400 : : else
401 : : {
402 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
403 : 0 : return false;
404 : : }
405 : 0 : }
406 : :
407 : 0 : int MathUtils::lower( int n, int i )
408 : : {
409 : 0 : if ( i >= 0 && i <= n )
410 : : {
411 : 0 : return faculty( n ) / ( faculty( i ) * faculty( n - i ) );
412 : : }
413 : : else
414 : : {
415 : 0 : return 0;
416 : : }
417 : 0 : }
418 : :
419 : 0 : double MathUtils::triArea( QgsPoint *pa, QgsPoint *pb, QgsPoint *pc )
420 : : {
421 : 0 : if ( pa && pb && pc )
422 : : {
423 : 0 : double deter = ( pa->x() * pb->y() + pb->x() * pc->y() + pc->x() * pa->y() - pa->x() * pc->y() - pb->x() * pa->y() - pc->x() * pb->y() );
424 : 0 : return 0.5 * deter;
425 : : }
426 : : else//null pointer
427 : : {
428 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
429 : 0 : return 0;
430 : : }
431 : 0 : }
432 : :
433 : 0 : double MathUtils::calcCubicHermitePoly( int n, int i, double t )
434 : : {
435 : 0 : if ( n != 3 || i > n )
436 : : {
437 : 0 : QgsDebugMsg( QStringLiteral( "error, can't calculate hermite polynom" ) );
438 : 0 : }
439 : :
440 : 0 : if ( n == 3 && i == 0 )
441 : : {
442 : 0 : return ( calcBernsteinPoly( 3, 0, t ) + calcBernsteinPoly( 3, 1, t ) );
443 : : }
444 : :
445 : 0 : if ( n == 3 && i == 1 )
446 : : {
447 : 0 : return ( calcBernsteinPoly( 3, 1, t ) * 0.33333333 );
448 : : }
449 : :
450 : 0 : if ( n == 3 && i == 2 )
451 : : {
452 : 0 : return ( calcBernsteinPoly( 3, 2, t ) * -0.33333333 );
453 : : }
454 : :
455 : 0 : if ( n == 3 && i == 3 )
456 : : {
457 : 0 : return ( calcBernsteinPoly( 3, 2, t ) + calcBernsteinPoly( 3, 3, t ) );
458 : : }
459 : : else//something went wrong
460 : : {
461 : 0 : QgsDebugMsg( QStringLiteral( "unexpected error" ) );
462 : 0 : return 0;
463 : : }
464 : 0 : }
465 : :
466 : 0 : double MathUtils::cFDerCubicHermitePoly( int n, int i, double t )
467 : : {
468 : 0 : if ( n != 3 || i > n )
469 : : {
470 : 0 : QgsDebugMsg( QStringLiteral( "error, can't calculate hermite polynom" ) );
471 : 0 : }
472 : :
473 : 0 : if ( n == 3 && i == 0 )
474 : : {
475 : : //cout << "cFDerBernsteinPoly(3,0,t): " << cFDerBernsteinPoly(3,0,t) << endl << flush;
476 : : //cout << "cFDerBernsteinPoly(3,1,t): " << cFDerBernsteinPoly(3,1,t) << endl << flush;
477 : 0 : return ( cFDerBernsteinPoly( 3, 0, t ) + cFDerBernsteinPoly( 3, 1, t ) );
478 : : }
479 : :
480 : 0 : if ( n == 3 && i == 1 )
481 : : {
482 : : //cout << "cFDerBernsteinPoly(3,1,t)/3: " << cFDerBernsteinPoly(3,1,t)/3 << endl << flush;
483 : 0 : return ( cFDerBernsteinPoly( 3, 1, t ) / 3 );
484 : : }
485 : :
486 : 0 : if ( n == 3 && i == 2 )
487 : : {
488 : : //cout << "cFDerBernsteinPoly(3,2,t)/3: " << cFDerBernsteinPoly(3,2,t)/3 << endl << flush;
489 : 0 : return ( -cFDerBernsteinPoly( 3, 2, t ) / 3 );
490 : : }
491 : :
492 : 0 : if ( n == 3 && i == 3 )
493 : : {
494 : : //cout << "cFDerBernsteinPoly(3,2,t): " << cFDerBernsteinPoly(3,2,t) << endl << flush;
495 : : //cout << "cFDerBernsteinPoly(3,3,t): " << cFDerBernsteinPoly(3,3,t) << endl << flush;
496 : 0 : return ( cFDerBernsteinPoly( 3, 2, t ) + cFDerBernsteinPoly( 3, 3, t ) );
497 : : }
498 : : else
499 : : {
500 : 0 : QgsDebugMsg( QStringLiteral( "unexpected error" ) );
501 : 0 : return 0;
502 : : }
503 : 0 : }
504 : :
505 : 0 : bool MathUtils::derVec( const Vector3D *v1, const Vector3D *v2, Vector3D *result, double x, double y )
506 : : {
507 : 0 : if ( v1 && v2 && result )//no null pointers
508 : : {
509 : 0 : double u = ( x * v2->getY() - y * v2->getX() ) / ( v1->getX() * v2->getY() - v1->getY() * v2->getX() );
510 : 0 : double v = ( x * v1->getY() - y * v1->getX() ) / ( v2->getX() * v1->getY() - v2->getY() * v1->getX() );
511 : 0 : result->setX( x );
512 : 0 : result->setY( y );
513 : 0 : result->setZ( u * v1->getZ() + v * v2->getZ() );
514 : 0 : return true;
515 : : }
516 : 0 : return false;
517 : 0 : }
518 : :
519 : :
520 : 0 : bool MathUtils::normalLeft( Vector3D *v1, Vector3D *result, double length )
521 : : {
522 : 0 : if ( v1 && result )//we don't like null pointers
523 : : {
524 : 0 : if ( v1->getY() == 0 )//this would cause a division with zero
525 : : {
526 : 0 : result->setX( 0 );
527 : :
528 : 0 : if ( v1->getX() < 0 )
529 : : {
530 : 0 : result->setY( -length );
531 : 0 : }
532 : :
533 : : else
534 : : {
535 : 0 : result->setY( length );
536 : : }
537 : 0 : return true;
538 : : }
539 : :
540 : : //coefficients for the quadratic equation
541 : 0 : double a = 1 + ( v1->getX() * v1->getX() ) / ( v1->getY() * v1->getY() );
542 : 0 : double b = 0;
543 : 0 : double c = -( length * length );
544 : 0 : double d = b * b - 4 * a * c;
545 : :
546 : 0 : if ( d < 0 )//no solution in R
547 : : {
548 : 0 : QgsDebugMsg( QStringLiteral( "Determinant Error" ) );
549 : 0 : return false;
550 : : }
551 : :
552 : 0 : result->setX( ( -b + std::sqrt( d ) ) / ( 2 * a ) ); //take one of the two solutions of the quadratic equation
553 : 0 : result->setY( ( -result->getX()*v1->getX() ) / v1->getY() );
554 : :
555 : 0 : QgsPoint point1( 0, 0, 0 );
556 : 0 : QgsPoint point2( v1->getX(), v1->getY(), 0 );
557 : 0 : QgsPoint point3( result->getX(), result->getY(), 0 );
558 : :
559 : 0 : if ( !( leftOf( point1, &point2, &point3 ) < 0 ) )//if we took the solution on the right side, change the sign of the components of the result
560 : : {
561 : 0 : result->setX( -result->getX() );
562 : 0 : result->setY( -result->getY() );
563 : 0 : }
564 : :
565 : 0 : return true;
566 : 0 : }
567 : :
568 : : else
569 : : {
570 : 0 : return false;
571 : : }
572 : 0 : }
573 : :
574 : :
575 : :
576 : 0 : bool MathUtils::normalRight( Vector3D *v1, Vector3D *result, double length )
577 : : {
578 : 0 : if ( v1 && result )//we don't like null pointers
579 : : {
580 : :
581 : 0 : if ( v1->getY() == 0 )//this would cause a division with zero
582 : : {
583 : 0 : result->setX( 0 );
584 : :
585 : 0 : if ( v1->getX() < 0 )
586 : : {
587 : 0 : result->setY( length );
588 : 0 : }
589 : :
590 : : else
591 : : {
592 : 0 : result->setY( -length );
593 : : }
594 : 0 : return true;
595 : : }
596 : :
597 : : //coefficients for the quadratic equation
598 : 0 : double a = 1 + ( v1->getX() * v1->getX() ) / ( v1->getY() * v1->getY() );
599 : 0 : double b = 0;
600 : 0 : double c = -( length * length );
601 : 0 : double d = b * b - 4 * a * c;
602 : :
603 : 0 : if ( d < 0 )//no solution in R
604 : : {
605 : 0 : QgsDebugMsg( QStringLiteral( "Determinant Error" ) );
606 : 0 : return false;
607 : : }
608 : :
609 : 0 : result->setX( ( -b + std::sqrt( d ) ) / ( 2 * a ) ); //take one of the two solutions of the quadratic equation
610 : 0 : result->setY( ( -result->getX()*v1->getX() ) / v1->getY() );
611 : :
612 : 0 : QgsPoint point1( 0, 0, 0 );
613 : 0 : QgsPoint point2( v1->getX(), v1->getY(), 0 );
614 : 0 : QgsPoint point3( result->getX(), result->getY(), 0 );
615 : :
616 : 0 : if ( ( leftOf( point1, &point2, &point3 ) < 0 ) ) //if we took the solution on the right side, change the sign of the components of the result
617 : : {
618 : 0 : result->setX( -result->getX() );
619 : 0 : result->setY( -result->getY() );
620 : 0 : }
621 : :
622 : 0 : return true;
623 : 0 : }
624 : :
625 : : else
626 : : {
627 : 0 : return false;
628 : : }
629 : 0 : }
630 : :
631 : :
632 : 0 : void MathUtils::normalFromPoints( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, Vector3D *vec )
633 : : {
634 : 0 : if ( p1 && p2 && p3 && vec )//no null pointers
635 : : {
636 : 0 : double ax = p2->x() - p1->x();
637 : 0 : double ay = p2->y() - p1->y();
638 : 0 : double az = p2->z() - p1->z();
639 : 0 : double bx = p3->x() - p1->x();
640 : 0 : double by = p3->y() - p1->y();
641 : 0 : double bz = p3->z() - p1->z();
642 : :
643 : 0 : vec->setX( ay * bz - az * by );
644 : 0 : vec->setY( az * bx - ax * bz );
645 : 0 : vec->setZ( ax * by - ay * bx );
646 : 0 : }
647 : :
648 : 0 : }
649 : :
650 : 0 : double MathUtils::crossVec( QgsPoint *first, Vector3D *vec1, QgsPoint *second, Vector3D *vec2 )
651 : : {
652 : 0 : if ( first && vec1 && second && vec2 )
653 : : {
654 : 0 : if ( ( vec2->getX()*vec1->getY() - vec2->getY()*vec1->getX() ) != 0 )
655 : : {
656 : : /*cout << "first: " << first->getX() << "//" << first->getY() << "//" << first->getZ() << endl << flush;
657 : : cout << "vec1: " << vec1->getX() << "//" << vec1->getY() << "//" << vec1->getZ() << endl << flush;
658 : : cout << "second: " << second->getX() << "//" << second->getY() << "//" << second->getZ() << endl << flush;
659 : : cout << "vec2: " << vec2->getX() << "//" << vec2->getY() << "//" << vec2->getZ() << endl << flush;
660 : : cout << "t2: " << ((first->getX()*vec1->getY()-first->getY()*vec1->getX()-second->getX()*vec1->getY()+second->getY()*vec1->getX())/(vec2->getX()*vec1->getY()-vec2->getY()*vec1->getX())) << endl << flush;*/
661 : :
662 : 0 : return ( ( first->x() * vec1->getY() - first->y() * vec1->getX() - second->x() * vec1->getY() + second->y() * vec1->getX() ) / ( vec2->getX() * vec1->getY() - vec2->getY() * vec1->getX() ) );
663 : :
664 : : }
665 : : else//if a division by zero would occur
666 : : {
667 : 0 : QgsDebugMsg( QStringLiteral( "warning: vectors are parallel" ) );
668 : 0 : return 0;
669 : : }
670 : : }
671 : :
672 : :
673 : : else//null pointer
674 : : {
675 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
676 : 0 : return 0;
677 : : }
678 : 0 : }
679 : :
680 : 0 : bool MathUtils::pointInsideTriangle( double x, double y, QgsPoint *p1, QgsPoint *p2, QgsPoint *p3 )
681 : : {
682 : 0 : QgsPoint thepoint( x, y, 0 );
683 : 0 : if ( MathUtils::leftOf( thepoint, p1, p2 ) > 0 )
684 : : {
685 : 0 : return false;
686 : : }
687 : 0 : if ( MathUtils::leftOf( thepoint, p2, p3 ) > 0 )
688 : : {
689 : 0 : return false;
690 : : }
691 : 0 : if ( MathUtils::leftOf( thepoint, p3, p1 ) > 0 )
692 : : {
693 : 0 : return false;
694 : : }
695 : 0 : return true;
696 : 0 : }
697 : :
698 : 0 : bool MathUtils::normalMinDistance( Vector3D *tangent, Vector3D *target, Vector3D *result )
699 : : {
700 : 0 : if ( tangent && target && result )
701 : : {
702 : 0 : double xt = tangent->getX();
703 : 0 : double yt = tangent->getY();
704 : 0 : double zt = tangent->getZ();
705 : :
706 : 0 : double xw = target->getX();
707 : 0 : double yw = target->getY();
708 : 0 : double zw = target->getZ();
709 : :
710 : : double xg1, yg1, zg1;//the coordinates of the first result
711 : : double xg2, yg2, zg2;//the coordinates of the second result
712 : :
713 : : //calculate xg
714 : 0 : double xgalpha1 = 1 / ( 2 * xt * xt * yw * yw * zt * zt - 2 * zt * zt * zt * xt * zw * xw + yt * yt * yt * yt * zw * zw + yt * yt * zw * zw * zt * zt + xt * xt * yt * yt * xw * xw + xt * xt * yw * yw * yt * yt - 2 * xt * xt * xt * zt * zw * xw + yt * yt * yt * yt * xw * xw + yt * yt * yw * yw * zt * zt + 2 * xt * xt * yt * yt * zw * zw - 2 * yt * yt * yt * yw * zt * zw + zt * zt * xt * xt * zw * zw + zt * zt * zt * zt * xw * xw + xt * xt * zt * zt * xw * xw + 2 * zt * zt * xw * xw * yt * yt - 2 * xt * xt * yw * zt * yt * zw - 2 * xt * yt * yt * yt * xw * yw - 2 * xt * xt * xt * yw * yt * xw - 2 * xt * zt * zt * xw * yt * yw - 2 * xt * zt * xw * yt * yt * zw - 2 * yw * zt * zt * zt * yt * zw + xt * xt * xt * xt * yw * yw + yw * yw * zt * zt * zt * zt + xt * xt * xt * xt * zw * zw );
715 : 0 : if ( xgalpha1 < 0 )
716 : : {
717 : 0 : QgsDebugMsg( QStringLiteral( "warning, only complex solution of xg" ) );
718 : 0 : return false;
719 : : }
720 : 0 : xg1 = std::sqrt( xgalpha1 ) * ( -yt * yw * xt + yt * yt * xw + xw * zt * zt - zt * xt * zw );
721 : 0 : xg2 = -sqrt( xgalpha1 ) * ( -yt * yw * xt + yt * yt * xw + xw * zt * zt - zt * xt * zw );
722 : :
723 : : //calculate yg
724 : 0 : double ygalpha1 = 1 / ( 2 * xt * xt * yw * yw * zt * zt - 2 * zt * zt * zt * xt * zw * xw + yt * yt * yt * yt * zw * zw + yt * yt * zw * zw * zt * zt + xt * xt * yt * yt * xw * xw + xt * xt * yw * yw * yt * yt - 2 * xt * xt * xt * zt * zw * xw + yt * yt * yt * yt * xw * xw + yt * yt * yw * yw * zt * zt + 2 * xt * xt * yt * yt * zw * zw - 2 * yt * yt * yt * yw * zt * zw + zt * zt * xt * xt * zw * zw + zt * zt * zt * zt * xw * xw + xt * xt * zt * zt * xw * xw + 2 * zt * zt * xw * xw * yt * yt - 2 * xt * xt * yw * zt * yt * zw - 2 * xt * yt * yt * yt * xw * yw - 2 * xt * xt * xt * yw * yt * xw - 2 * xt * zt * zt * xw * yt * yw - 2 * xt * zt * xw * yt * yt * zw - 2 * yw * zt * zt * zt * yt * zw + xt * xt * xt * xt * yw * yw + yw * yw * zt * zt * zt * zt + xt * xt * xt * xt * zw * zw );
725 : 0 : if ( ygalpha1 < 0 )
726 : : {
727 : 0 : QgsDebugMsg( QStringLiteral( "warning, only complex solution of yg" ) );
728 : 0 : return false;
729 : : }
730 : 0 : yg1 = -sqrt( ygalpha1 ) * ( -yw * xt * xt - zt * zt * yw + zt * yt * zw + yt * xw * xt );
731 : 0 : yg2 = std::sqrt( ygalpha1 ) * ( -yw * xt * xt - zt * zt * yw + zt * yt * zw + yt * xw * xt );
732 : :
733 : : //calculate zg
734 : 0 : double zgalpha1 = 1 / ( 2 * xt * xt * yw * yw * zt * zt - 2 * zt * zt * zt * xt * zw * xw + yt * yt * yt * yt * zw * zw + yt * yt * zw * zw * zt * zt + xt * xt * yt * yt * xw * xw + xt * xt * yw * yw * yt * yt - 2 * xt * xt * xt * zt * zw * xw + yt * yt * yt * yt * xw * xw + yt * yt * yw * yw * zt * zt + 2 * xt * xt * yt * yt * zw * zw - 2 * yt * yt * yt * yw * zt * zw + zt * zt * xt * xt * zw * zw + zt * zt * zt * zt * xw * xw + xt * xt * zt * zt * xw * xw + 2 * zt * zt * xw * xw * yt * yt - 2 * xt * xt * yw * zt * yt * zw - 2 * xt * yt * yt * yt * xw * yw - 2 * xt * xt * xt * yw * yt * xw - 2 * xt * zt * zt * xw * yt * yw - 2 * xt * zt * xw * yt * yt * zw - 2 * yw * zt * zt * zt * yt * zw + xt * xt * xt * xt * yw * yw + yw * yw * zt * zt * zt * zt + xt * xt * xt * xt * zw * zw );
735 : 0 : if ( zgalpha1 < 0 )
736 : : {
737 : 0 : QgsDebugMsg( QStringLiteral( "warning, only complex solution of zg" ) );
738 : 0 : return false;
739 : : }
740 : 0 : zg1 = -sqrt( zgalpha1 ) * ( yt * yw * zt - yt * yt * zw + xw * zt * xt - xt * xt * zw );
741 : 0 : zg2 = std::sqrt( zgalpha1 ) * ( yt * yw * zt - yt * yt * zw + xw * zt * xt - xt * xt * zw );
742 : :
743 : 0 : double distance1 = std::sqrt( ( xw - xg1 ) * ( xw - xg1 ) + ( yw - yg1 ) * ( yw - yg1 ) + ( zw - zg1 ) * ( zw - zg1 ) );
744 : 0 : double distance2 = std::sqrt( ( xw - xg2 ) * ( xw - xg2 ) + ( yw - yg2 ) * ( yw - yg2 ) + ( zw - zg2 ) * ( zw - zg2 ) );
745 : :
746 : 0 : if ( distance1 <= distance2 )//find out, which solution is the maximum and which the minimum
747 : : {
748 : 0 : result->setX( xg1 );
749 : 0 : result->setY( yg1 );
750 : 0 : result->setZ( zg1 );
751 : 0 : }
752 : : else
753 : : {
754 : 0 : result->setX( xg2 );
755 : 0 : result->setY( yg2 );
756 : 0 : result->setZ( zg2 );
757 : : }
758 : 0 : return true;
759 : : }
760 : :
761 : : else
762 : : {
763 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
764 : 0 : return false;
765 : : }
766 : 0 : }
767 : :
768 : :
769 : 0 : double MathUtils::planeTest( QgsPoint *test, QgsPoint *pt1, QgsPoint *pt2, QgsPoint *pt3 )
770 : : {
771 : 0 : if ( test && pt1 && pt2 && pt3 )
772 : : {
773 : 0 : double a = ( pt1->z() * ( pt2->y() - pt3->y() ) + pt2->z() * ( pt3->y() - pt1->y() ) + pt3->z() * ( pt1->y() - pt2->y() ) ) / ( ( pt1->x() - pt2->x() ) * ( pt2->y() - pt3->y() ) - ( pt2->x() - pt3->x() ) * ( pt1->y() - pt2->y() ) );
774 : 0 : double b = ( pt1->z() * ( pt2->x() - pt3->x() ) + pt2->z() * ( pt3->x() - pt1->x() ) + pt3->z() * ( pt1->x() - pt2->x() ) ) / ( ( pt1->y() - pt2->y() ) * ( pt2->x() - pt3->x() ) - ( pt2->y() - pt3->y() ) * ( pt1->x() - pt2->x() ) );
775 : 0 : double c = pt1->z() - a * pt1->x() - b * pt1->y();
776 : 0 : double zpredicted = test->x() * a + test->y() * b + c;
777 : 0 : return ( test->z() - zpredicted );
778 : : }
779 : : else
780 : : {
781 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
782 : 0 : return 0;
783 : : }
784 : 0 : }
785 : :
786 : 0 : double MathUtils::angle( QgsPoint *p1, QgsPoint *p2, QgsPoint *p3, QgsPoint *p4 )
787 : : {
788 : 0 : if ( p1 && p2 && p3 && p4 )
789 : : {
790 : 0 : Vector3D v1( p2->x() - p1->x(), p2->y() - p1->y(), 0 );
791 : 0 : Vector3D v2( p4->x() - p3->x(), p4->y() - p3->y(), 0 );
792 : 0 : double value = acos( ( v1.getX() * v2.getX() + v1.getY() * v2.getY() ) / ( v1.getLength() * v2.getLength() ) ) * 180 / M_PI;
793 : 0 : return value;
794 : : }
795 : : else
796 : : {
797 : 0 : QgsDebugMsg( QStringLiteral( "warning, null pointer" ) );
798 : 0 : return 0;
799 : : }
800 : 0 : }
|
