00001 #include <math.h>
00002 #include <Math.h>
00003 #include "ArsToolbox.h"
00004
00025 int ArsToolbox::lineLineCrossPoint(double x1, double y1, double x2, double y2, double x3, double y3,
00026 double x4, double y4, double *cpx, double *cpy)
00027 {
00028 double ua, ub;
00029 double men;
00030
00031 ua = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3));
00032 ub = ((x2-x1)*(y1-y3) - (y2-y1)*(x1-x3));
00033 men = ((y4-y3)*(x2-x1) - (x4-x3)*(y2-y1));
00034
00035
00036 if(men == 0)
00037 {
00038 return 1;
00039 }
00040
00041 else if(ua == 0 && ub == 0 && men == 0)
00042 {
00043 return 2;
00044 }
00045
00046 else
00047 {
00048 ua /= men;
00049 ub /= men;
00050 *cpx = (x1 + ua*(x2-x1));
00051 *cpy = (y1 + ub*(y2-y1));
00052 }
00053
00054 return 0;
00055 }
00056
00077 int ArsToolbox::circleCircleIntersection(double x0, double y0, double r0,
00078 double x1, double y1, double r1,
00079 double *xi, double *yi,
00080 double *xi_prime, double *yi_prime)
00081 {
00082 double a, dx, dy, d, h, rx, ry;
00083 double x2, y2;
00084
00085
00086
00087
00088 dx = x1 - x0;
00089 dy = y1 - y0;
00090
00091
00092 d = sqrt((dy*dy) + (dx*dx));
00093
00094
00095 if (d > (r0 + r1))
00096 {
00097
00098 return 1;
00099 }
00100 if (d < fabs(r0 - r1))
00101 {
00102
00103 return 2;
00104 }
00105
00106
00107
00108
00109
00110
00111
00112 a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
00113
00114
00115 x2 = x0 + (dx * a/d);
00116 y2 = y0 + (dy * a/d);
00117
00118
00119
00120
00121 h = sqrt((r0*r0) - (a*a));
00122
00123
00124
00125
00126 rx = -dy * (h/d);
00127 ry = dx * (h/d);
00128
00129
00130 *xi = x2 + rx;
00131 *xi_prime = x2 - rx;
00132 *yi = y2 + ry;
00133 *yi_prime = y2 - ry;
00134
00135 return 0;
00136 }
00137
00151 double ArsToolbox::vectorsAngle(double v1ex, double v1ey, double v2ex, double v2ey)
00152 {
00153 return acos((v1ex*v2ex + v1ey*v2ey) / (sqrt((v1ex*v1ex) + (v1ey*v1ey))*sqrt((v2ex*v2ex) + (v2ey*v2ey))));
00154 }
00155
00166 double ArsToolbox::vectorNorm(double v1x, double v1y)
00167 {
00168 return sqrt((v1x*v1x) + (v1y*v1y));
00169 }
00170
00186 int ArsToolbox::circleLineIntersection(double p1x, double p1y, double p2x, double p2y, double cx, double cy, double r, double *v1x, double *v1y, double *v2x, double *v2y)
00187 {
00188 ArsToolbox::circleLineIntersection(p1x, p1y, p2x, p2y, cx, cy, r, v1x, v1y, v2x, v2y, false);
00189 }
00190
00206 int ArsToolbox::circleLineIntersection(double p1x, double p1y, double p2x, double p2y, double cx, double cy, double r, double *v1x, double *v1y, double *v2x, double *v2y, bool line)
00207 {
00208 if (p1x < p2x)
00209 {
00210
00211 double tmp = p1x;
00212 p1x = p2x;
00213 p2x = tmp;
00214
00215 tmp = p1y;
00216 p1y = p2y;
00217 p2y = tmp;
00218 }
00219
00220
00221 double dx = p2x - p1x;
00222 double dy = p2y - p1y;
00223
00224
00225 double A = dx * dx + dy * dy;
00226 double B = 2 * (dx * (p1x - cx) + dy * (p1y - cy));
00227 double C = (p1x - cx) * (p1x - cx) + (p1y - cy) * (p1y - cy) - r * r;
00228
00229
00230 double det = B * B - 4 * A * C;
00231
00232
00233 if ((A <= 0.0000001) || (det < 0))
00234 return 0;
00235
00236 else if (det == 0)
00237 {
00238 double t = -B / (2 * A);
00239 if ((t >= 0.0 && t <= 1.0 ) ||
00240
00241 (line))
00242 {
00243 *v1x = p1x + t * dx;
00244 *v1y = p1y + t * dy;
00245 *v2x = (*v1x);
00246 *v2y = (*v1y);
00247 return 1;
00248 }
00249 else
00250 return 0;
00251 }
00252
00253 else
00254 {
00255 int count = 0;
00256 double t;
00257 t = (-B + sqrt(det)) / (2 * A);
00258 if ((t >= 0.0 && t <= 1.0 ) ||
00259
00260 (line))
00261 {
00262 *v1x = p1x + t * dx;
00263 *v1y = p1y + t * dy;
00264 count++;
00265 }
00266 t = (-B - sqrt(det)) / (2 * A);
00267 if ((t >= 0.0 && t <= 1.0 ) ||
00268
00269 (line))
00270 {
00271 if (count == 1)
00272 {
00273 *v2x = p1x + t * dx;
00274 *v2y = p1y + t * dy;
00275 }
00276 else
00277 {
00278 *v1x = p1x + t * dx;
00279 *v1y = p1y + t * dy;
00280 }
00281 count++;
00282 }
00283 return count;
00284 }
00285 }
00286
00300 int ArsToolbox::createLineTo(double sx, double sy, double angle, double length, double *ex, double *ey)
00301 {
00302 *ex = cos(angle) * length + sx;
00303 *ey = sin(angle) * length + sy;
00304 }
00305
00306 double ArsToolbox::linePointDistance(double dX1, double dY1, double dX2, double dY2,
00307 double dPX, double dPY)
00308 {
00309
00310 if (dX1 == dX2)
00311 return fabs(dPX - dX1);
00312 if (dY1 == dY2)
00313 return fabs(dPY - dY1);
00314
00315
00316 double A = (dY2 - dY1) / (dX2 - dX1);
00317 double C = dY1 - dX1 * A;
00318
00319 return fabs(A*dPX - dPY + C) / sqrt(A*A + 1);
00320 }
00321
00322 double ArsToolbox::trim(double value, double min, double max)
00323 {
00324 if (value < min)
00325 return min;
00326 else if (value > max)
00327 return max;
00328 else
00329 return value;
00330 }
00331
00342 int ArsToolbox::distancePointLine(VecPosition point, VecPosition lineStart, VecPosition lineEnd, double * distance)
00343 {
00344 double lineMag;
00345 double U;
00346 VecPosition intersection;
00347
00348 lineMag =(lineEnd - lineStart).getMagnitude();
00349
00350 U = ( ( ( point.getX() - lineStart.getX() ) * ( lineEnd.getX() - lineStart.getX() ) ) +
00351 ( ( point.getY() - lineStart.getY() ) * ( lineEnd.getY() - lineStart.getY() ) ) ) /
00352 ( lineMag * lineMag );
00353
00354 if( U < 0.0 || U > 1.0 )
00355 return 1;
00356
00357 intersection.setX(lineStart.getX() + U * ( lineEnd.getX() - lineStart.getX() ));
00358 intersection.setY(lineStart.getY() + U * ( lineEnd.getY() - lineStart.getY() ));
00359
00360 *distance = (point-intersection).getMagnitude();
00361
00362 return 0;
00363 }
00364
00376 double ArsToolbox::computeLeastInterceptTime(VecPosition b0, VecPosition v0, VecPosition r0, double vR, double tau)
00377 {
00378 VecPosition pTold;
00379 VecPosition U;
00380
00381 double tOld, tNew;
00382 double sNew;
00383 double d;
00384 double gDerivT;
00385 double gTold;
00386
00387 int check = 0;
00388
00389 if(v0.getMagnitude() == 0) {
00390 d = (r0 - b0).getMagnitude();
00391 }
00392 else
00393 {
00394 VecPosition v0Norm = v0;
00395 v0Norm.normalize();
00396
00397 check = ArsToolbox::distancePointLine(r0, b0 + (v0Norm * (-300)), (b0 + (v0Norm * 300)), &d);
00398 }
00399
00400
00401 if(check == 1) {
00402 return -1.0;
00403 }
00404
00405
00406 tOld = d / vR;
00407
00408 do {
00409
00410 pTold = (b0 + ((v0 * tau) * (1 - exp((-tOld)/tau))));
00411 gTold = ((pTold - r0).getMagnitude() - (vR * tOld));
00412
00413 U = (pTold - r0);
00414 U.normalize();
00415
00416
00417
00418 VecPosition ss = ((v0 * exp(-tOld / tau)) * U);
00419 double skalarnySucin = (ss.getX() + ss.getY());
00420
00421 gDerivT = (skalarnySucin - vR);
00422 sNew = tOld + (gDerivT / (vR - skalarnySucin));
00423
00424
00425 ss = (v0 * U);
00426 double skalarnySucinPodmienka = (ss.getX() + ss.getY());
00427
00428 if(skalarnySucinPodmienka < 0) {
00429 tNew = tOld + (gTold / (vR - skalarnySucin));
00430 }
00431 else if(skalarnySucinPodmienka > 0 && gDerivT < 0) {
00432 VecPosition pSNew = (b0 + ((v0 * tau) * (1 - exp((-sNew)/tau))));
00433
00434 VecPosition UNew = pSNew - r0;
00435 UNew.normalize();
00436
00437 ss = ((v0 * exp(-tOld / tau)) * UNew);
00438 double skalarnySucinSNew = (ss.getX() + ss.getY());
00439
00440 tNew = tOld + (gTold / (vR - skalarnySucinSNew));
00441 }
00442 else {
00443 tNew = tOld + gTold / vR;
00444 }
00445 tOld = tNew;
00446
00447 } while(gTold > 0.1);
00448
00449 return tOld;
00450 }