src/ArsToolbox.cpp

Go to the documentation of this file.
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         // usecky su paralelne
00036         if(men == 0)
00037         {
00038                 return 1;
00039         }
00040         // usecky su zhodne
00041         else if(ua == 0 && ub == 0 && men == 0)
00042         {
00043                 return 2;
00044         }
00045         // crosspoint existuje a jeho suradnice su (cpx, cpy)
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   /* dx and dy are the vertical and horizontal distances between
00086    * the circle centers.
00087    */
00088   dx = x1 - x0;
00089   dy = y1 - y0;
00090 
00091   /* Determine the straight-line distance between the centers. */
00092   d = sqrt((dy*dy) + (dx*dx));
00093 
00094   /* Check for solvability. */
00095   if (d > (r0 + r1))
00096   {
00097     /* no solution. circles do not intersect. */
00098     return 1;
00099   }
00100   if (d < fabs(r0 - r1))
00101   {
00102     /* no solution. one circle is contained in the other */
00103     return 2;
00104   }
00105 
00106   /* 'point 2' is the point where the line through the circle
00107    * intersection points crosses the line between the circle
00108    * centers.  
00109    */
00110 
00111   /* Determine the distance from point 0 to point 2. */
00112   a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
00113 
00114   /* Determine the coordinates of point 2. */
00115   x2 = x0 + (dx * a/d);
00116   y2 = y0 + (dy * a/d);
00117 
00118   /* Determine the distance from point 2 to either of the
00119    * intersection points.
00120    */
00121   h = sqrt((r0*r0) - (a*a));
00122 
00123   /* Now determine the offsets of the intersection points from
00124    * point 2.
00125    */
00126   rx = -dy * (h/d);
00127   ry = dx * (h/d);
00128 
00129   /* Determine the absolute intersection points. */
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         // switch x
00211         double tmp = p1x;
00212         p1x = p2x;
00213         p2x = tmp;
00214         // switch y
00215         tmp = p1y;
00216         p1y = p2y;
00217         p2y = tmp;
00218     }
00219     
00220     // x1:x2, y1:y2
00221     double dx = p2x - p1x;
00222     double dy = p2y - p1y;
00223 
00224     // circle
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     // determinant of matrix 2x2
00230     double det = B * B - 4 * A * C;
00231 
00232     // zero points    
00233     if ((A <= 0.0000001) || (det < 0))
00234         return 0;
00235     // exactly 1 point
00236     else if (det == 0)
00237     {
00238         double t = -B / (2 * A);
00239         if ((t >= 0.0 && t <= 1.0 /*&& p1x <= p2x*/) ||
00240 //            (t <= 0.0 && t >= -1.0 && p1x >= p2x) ||
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     // two points
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 /*&& p1x <= p2x*/) ||
00259 //            (t <= 0.0 && t >= -1.0 && p1x >= p2x) ||
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 /*&& p1x <= p2x*/) ||
00268 //            (t <= 0.0 && t >= -1.0 && p1x >= p2x) ||
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     // paralell with X,Y coordinates is simple solution
00310     if (dX1 == dX2)
00311         return fabs(dPX - dX1);
00312     if (dY1 == dY2)
00313         return fabs(dPY - dY1);
00314         
00315     // y = Ax + C; Ax + By + C = 0 => A = ?, B = -1, C = 0 
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;   // closest point does not fall within the line segment
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         // closest point does not fall within the line segment
00401         if(check == 1) {
00402                 return -1.0;
00403         }
00404         
00405         // t0
00406         tOld = d / vR;
00407         
00408         do {
00409                 // zaciatok cyklu
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                 // skalarny sucin
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                 // podmienka
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 }

Generated on Thu Apr 26 22:45:26 2007 for GangOfSix(GOS)-RoboCupTeamProject by  doxygen 1.5.1-p1