Util.cpp
Go to the documentation of this file.
```00001 /***
00002  Some useful functions
00003 ***/
00004 #include "tensor_field_nav_core/Util.h"
00005
00006 void transform_point3D(float p[3], float rot_mat[16])
00007 {
00008     double tmp[3] = {0.};
00009
00010     tmp[0] = rot_mat[0] * p[0] + rot_mat[4] * p[1] + rot_mat[8]  * p[2] + rot_mat[12];
00011     tmp[1] = rot_mat[1] * p[0] + rot_mat[5] * p[1] + rot_mat[9]  * p[2] + rot_mat[13];
00012     tmp[2] = rot_mat[2] * p[0] + rot_mat[6] * p[1] + rot_mat[10] * p[2] + rot_mat[14];
00013
00014     p[0] = tmp[0];
00015     p[1] = tmp[1];
00016     p[2] = tmp[2];
00017 }
00018
00019 double bilinear_interpolate(double a, double b,
00020                                      double f00, double f01, double f10, double f11)
00021 {
00022     return (f00*(1-a)*(1-b)+f10*a*(1-b)+f01*(1-a)*b+f11*a*b);
00023 }
00024 bool is_repeated_elem(int *a, int b, int num)
00025 {
00026     int i;
00027     for(i = 0; i < num; i++)
00028     {
00029         if(a[i] == b)
00030             return true;
00031     }
00032     return false;
00033 }
00034 /* meaning of return value
00035  0----Intersection dosn't exists
00036  1----Intersection exists.
00037  2----two line segments are parallel.
00038  3----two line segments are collinear, but not overlap.
00039  4----two line segments are collinear, and share one same end point.
00040  5----two line segments are collinear, and overlap.
00041 */
00042
00043 int GetIntersection2(double PointA[2], double PointB[2], double PointC[2], double PointD[2], double t[2])
00044 {
00045
00046     double delta;
00047     double t1,t2;
00048     double a,b,c,d;
00049     double xba,yba,xdc,ydc,xca,yca;
00050
00051     xba=PointB[0]-PointA[0];    yba=PointB[1]-PointA[1];
00052     xdc=PointD[0]-PointC[0];    ydc=PointD[1]-PointC[1];
00053     xca=PointC[0]-PointA[0];    yca=PointC[1]-PointA[1];
00054
00055     delta=xba*ydc-yba*xdc;
00056     t1=xca*ydc-yca*xdc;
00057     t2=xca*yba-yca*xba;
00058
00059     if(delta!=0)
00060     {
00061         t[0]=t1/delta;   t[1]=t2/delta;
00062         /*two segments intersect (including intersect at end points)*/
00063         //if ( t[0]<=1 && t[0]>=0 && t[1]<=1 && t[1]>=0 ) return 1;
00064         if ( t[0]<=1 && (t[0]>=0 || fabs (t[0])<=1.e-8)
00065             && t[1]<=1 && (t[1]>=0|| fabs (t[1])<=1.e-8)) //set threshold to allow some numerical errors
00066             return 1;
00067         else return 0;
00068     }
00069
00070     else
00071     {
00072         /* AB & CD are parallel. */
00073         if ( (t1!=0) && (t2!=0) ) return 2;
00074
00075         /* when AB & CD are collinear */
00076
00077         /*if AB isn't a vertical line segment, project to x-axis */
00078         if(PointA[0]!=PointB[0])
00079         {
00080             a=min(PointA[0],PointB[0]); b=max(PointA[0],PointB[0]);
00081             c=min(PointC[0],PointD[0]); d=max(PointC[0],PointD[0]);
00082
00083             if ( (d<a) || (c>b) ) return  3;
00084             else if( (d==a) || (c==b) ) return 4;
00085             else return 5;
00086         }
00087
00088         else         /* if AB is a vertical line segment, project to y-axis */
00089         {
00090
00091             a=min(PointA[1],PointB[1]); b=max(PointA[1],PointB[1]);
00092             c=min(PointC[1],PointD[1]); d=max(PointC[1],PointD[1]);
00093
00094             if( (d<a) || (c>b) ) return  3;
00095             else if( (d==a) || (c==b) ) return 4;
00096             else return 5;
00097         }
00098     }
00099 }
00100 double * MatrixInver(double A[],int m,int n) //zhuanzhi
00101 {
00102     int i,j;
00103     double *B=NULL;
00104     B=(double *)malloc(m*n*sizeof(double));
00105
00106     for(i=0;i<n;i++)
00107         for(j=0;j<m;j++)
00108             B[i*m+j]=A[j*n+i];
00109
00110     return B;
00111 }
00112
00113 double *MatrixOpp(double A[],int m,int n) //inverse
00114 {
00115     int i,j,x,y,k;
00116     double *SP=NULL,*AB=NULL,*B=NULL,XX,*C;
00117     SP=(double *)malloc(m*n*sizeof(double));
00118     AB=(double *)malloc(m*n*sizeof(double));
00119     B=(double *)malloc(m*n*sizeof(double));
00120
00121     XX=Surplus(A,m,n);
00122     XX=1/XX;
00123
00124     for(i=0;i<m;i++)
00125         for(j=0;j<n;j++)
00126         {
00127             for(k=0;k<m*n;k++)
00128                 B[k]=A[k];
00129             {
00130                 for(x=0;x<n;x++)
00131                     B[i*n+x]=0;
00132                 for(y=0;y<m;y++)
00133                     B[m*y+j]=0;
00134                 B[i*n+j]=1;
00135                 SP[i*n+j]=Surplus(B,m,n);
00136                 AB[i*n+j]=XX*SP[i*n+j];
00137             }
00138         }
00139
00140         C=MatrixInver(AB,m,n);
00141
00142         free(SP);
00143         free(AB);
00144         free(B);
00145
00146         return C;
00147 }
00148
00149 double Surplus(double A[],int m,int n) //hanglieshi
00150 {
00151
00152     int i,j,k,p,r;
00153     double XX,temp=1,temp1=1,s=0,s1=0;
00154
00155     if(n==2)
00156     {for(i=0;i<m;i++)
00157     for(j=0;j<n;j++)
00158         if((i+j)%2) temp1*=A[i*n+j];
00159         else temp*=A[i*n+j];
00160         XX=temp-temp1;}
00161     else{
00162         for(k=0;k<n;k++)
00163         {for(i=0,j=k;i<m,j<n;i++,j++)
00164         temp*=A[i*n+j];
00165         if(m-i)
00166         {for(p=m-i,r=m-1;p>0;p--,r--)
00167         temp*=A[r*n+p-1];}
00168         s+=temp;
00169         temp=1;
00170         }
00171
00172         for(k=n-1;k>=0;k--)
00173         {for(i=0,j=k;i<m,j>=0;i++,j--)
00174         temp1*=A[i*n+j];
00175         if(m-i)
00176         {for(p=m-1,r=i;r<m;p--,r++)
00177         temp1*=A[r*n+p];}
00178         s1+=temp1;
00179         temp1=1;
00180         }
00181
00182         XX=s-s1;}
00183     return XX;
00184 }
00185
00186 int solve_ten_cubic(double a, double b, double c, double d, double solutions[4])
00187 {
00188     if(fabs(a) < 1e-6)
00189         return solve_ten_quadratic(b, c, d, solutions);
00190     b /= a;
00191     c /= a;
00192     d /= a;
00193     double Q = (9.*b*c-27.*d-2.*b*b*b)/54.;
00194     double D = (3.*c-b*b)/9.;
00195     double R = D*D*D + Q*Q;
00196     double S, T;
00197
00198     if(R>=0)
00199     {
00200         double s_R = sqrt(R);
00201         S = get_sign(Q+s_R)*pow(fabs(Q+s_R), 1./3);
00202         T = get_sign(Q-s_R)*pow(fabs(Q-s_R), 1./3);
00203
00204         solutions[0] = -b/3.+(S+T);
00205         solutions[1] = -b/3.-(S+T)/2.;
00206         solutions[2] = -b/3.-(S+T)/2.;
00207         solutions[3] = fabs(sqrt(3.)/2.*(S-T));
00208         if(solutions[3] == 0) /*no complex roots*/
00209         {
00210             if(solutions[1] != solutions[0] )
00211                 return 2;  /*two real roots*/
00212             else
00213                 return 1;
00214         }
00215         else /*contains two complex roots*/
00216         {
00217             return 4;
00218         }
00219     }
00220
00221     else   /*we have distinct real roots*/
00222     {
00223         double th = acos(Q/sqrt(-D*D*D));
00224         solutions[0] = 2*sqrt(-D)*cos(th/3.)-b/3;
00225         solutions[1] = 2*sqrt(-D)*cos((th+2*M_PI)/3.)-b/3;
00226         solutions[2] = 2*sqrt(-D)*cos((th+4*M_PI)/3.)-b/3.;
00227         return 3;
00228     }
00229     return 0;
00230 }
00231
00232
00233 int solve_ten_cubic_3(double a, double b, double c, double d, double solutions[4])
00234 {
00235     if(fabs(a) < 1e-6)
00236         return solve_ten_quadratic(b, c, d, solutions);
00237     double Q = (9*a*b*c-27*a*a*d-2*b*b*b)/(54*a*a*a);
00238     double D = (3*a*c-b*b)/(9*a*a);
00239     double R = D*D*D + Q*Q;
00240     double S, T;
00241
00242     if(R>=0)
00243     {
00244         double s_R = sqrt(R);
00245         S = get_sign(Q+s_R)*pow(fabs(Q+s_R), 1./3);
00246         T = get_sign(Q-s_R)*pow(fabs(Q-s_R), 1./3);
00247
00248         solutions[0] = -b/(3*a)+(S+T);
00249         solutions[1] = -b/(3*a)-(S+T)/2.;
00250         solutions[2] = -b/(3*a)-(S+T)/2.;
00251         solutions[3] = fabs(sqrt(3.)/2.*(S-T));
00252         if(solutions[3] == 0) /*no complex roots*/
00253         {
00254             if(solutions[1] != solutions[0] )
00255                 return 2;  /*two real roots*/
00256             else
00257                 return 1;
00258         }
00259         else /*contains two complex roots*/
00260         {
00261             return 4;
00262         }
00263     }
00264
00265     else   /*we have distinct real roots*/
00266     {
00267         double th = acos(Q/sqrt(-D*D*D));
00268         solutions[0] = 2*sqrt(-D)*cos(th/3.)-b/(3*a);
00269         solutions[1] = 2*sqrt(-D)*cos((th+2*M_PI)/3.)-b/(3*a);
00270         solutions[2] = 2*sqrt(-D)*cos((th+4*M_PI)/3.)-b/(3*a);
00271         return 3;
00272     }
00273     return 0;
00274 }
00275
00276 /*   a*x3 + b*x2 + c*x + d = 0
00277
00278 Formula:
00279   Step 1: Calculate p and q
00280           p = ( 3*c/a - (b/a)2 ) / 3
00281           q = ( 2*(b/a)3 - 9*b*c/a/a + 27*d/a ) / 27
00282   Step 2: Calculate discriminant D
00283           D = (p/3)3 + (q/2)2
00284   Step 3: Depending on the sign of D, you follow different strategy.
00285           If D<0, three distinct real roots.
00286           If D=0, three real roots of which at least two are equal.
00287           If D>0, one real and two complex roots.
00288   Step 3a: For D>0 and D=0
00289           Calculate u and v
00290           u = cubic_root(-q/2 + sqrt(D))
00291           v = cubic_root(-q/2 - sqrt(D))
00292           Find the three transformed roots
00293           y1 = u + v
00294           y2 = -(u+v)/2 + i (u-v)*sqrt(3)/2
00295           y3 = -(u+v)/2 - i (u-v)*sqrt(3)/2
00296   Step 3b: Alternately, for D<0, a trigonometric formulation is more convenient
00297           y1 =  2 * sqrt(|p|/3) * cos(phi/3)
00298           y2 = -2 * sqrt(|p|/3) * cos((phi+pi)/3)
00299           y3 = -2 * sqrt(|p|/3) * cos((phi-pi)/3)
00300           where phi = acos(-q/2/sqrt(|p|3/27))
00301                 pi  = 3.141592654...
00302   Step 4  Finally, find the three roots
00303           x = y - b/a/3
00304
00305 */
00306 int solve_ten_cubic_2(double a, double b, double c, double d, double solutions[4])
00307 {
00308     double p = (3*c/a - (b/a)*(b/a))/3.;
00309     double q = (2*pow((b/a),3) - 9*b*c/a/a + 27*d/a)/27.;
00310     double D = pow((p/3),3) + (q/2)*(q/2);
00311
00312     if(D < 0 )/*3 distinct real roots*/
00313     {
00314         double phi = acos(-q/2/sqrt(pow(fabs(p),3)/27));
00315         solutions[0] =  2 * sqrt(fabs(p)/3) * cos(phi/3);
00316         solutions[1] = -2 * sqrt(fabs(p)/3) * cos((phi+M_PI)/3);
00317         solutions[2] = -2 * sqrt(fabs(p)/3) * cos((phi-M_PI)/3);
00318     }
00319     else if(D>=0)
00320     {
00321         double u = pow((-q/2 + sqrt(D)), 1./3.);
00322         double v = pow((-q/2 - sqrt(D)), 1./3.);
00323         solutions[0] = u + v;
00324
00325         if(D == 0)
00326         {
00327             solutions[1] = -(u+v)/2;
00328             if(solutions[0] == solutions[1])
00329                 return 1;
00330             else
00331                 return 2;
00332         }
00333
00334         return 1;
00335     }
00336 }
00337
00338
00339 int solve_ten_quadratic(double a, double b, double c, double solutions[2])
00340 {
00341     double D = b*b-4*a*c;
00342     if(D>=0)
00343     {
00344         if(D==0)
00345         {
00346             solutions[0] = -b/(2*a); return 1;
00347         }
00348         else
00349         {
00350             D=sqrt(D);
00351             solutions[0] = -b/(2*a)+D;
00352             solutions[1] = -b/(2*a)-D;
00353             return 2;
00354         }
00355     }
00356     return 0;  /*there is no real root*/
00357 }
00358
00359
00360
00361 int get_sign(double x)
00362 {
00363     if(x>=0.0) return 1;
00364     else return -1;
00365 }
00366
00367 bool SameSide(icVector3 A, icVector3 B, icVector3 C, icVector3 P)
00368 {
00369     icVector3 AB = B - A ;
00370     icVector3 AC = C - A ;
00371     icVector3 AP = P - A ;
00372
00373     icVector3 v1 = cross(AB,AC) ;
00374     icVector3 v2 = cross(AB,AP) ;
00375
00376     // v1 and v2 should point to the same direction
00377     return dot(v1,v2) >= 0 ;
00378 }
00379
00380 // Same side method
00381 // Determine whether point P in triangle ABC
00382 bool PointInTriangle(icVector3 A, icVector3 B, icVector3 C, icVector3 P)
00383 {
00384     return SameSide(A, B, C, P) &&
00385         SameSide(B, C, A, P) &&
00386         SameSide(C, A, B, P) ;
00387 }
00388
00389 void BubbleSorting(int *a, int size){
00390     int i, j, temp;
00391     for ( i = 0; i < size; i++ )    // controls passes through the list
00392     {
00393         for ( j = 0; j < size - 1; j++ )   // performs adjacent comparisons
00394         {
00395             if(a[j] > a[j+1])      // determines if a swap should occur
00396             {
00397                 temp = a[j];       // swap is performed
00398                 a[j] = a[j+1];
00399                 a[j+1] = temp;
00400             }
00401         }
00402     }
00403 }
00404 void linbcg(Vec_I_DP &b, Vec_IO_DP &x, Vec_INT *ija_p,Vec_DP *sa_p,const int itol, const DP tol,const int itmax, int &iter, DP &err){
00405     DP ak,akden,bk,bkden=1.0,bknum,bnrm,dxnrm,xnrm,zm1nrm,znrm;
00406     const DP EPS=1.0e-14;
00407     int j;
00408
00409     int n=b.size();
00410     Vec_DP p(n),pp(n),r(n),rr(n),z(n),zz(n);
00411     iter=0;
00412     atimes(x,r,ija_p,sa_p,0);
00413     for (j=0;j<n;j++) {
00414         r[j]=b[j]-r[j];
00415         rr[j]=r[j];
00416     }
00417     //atimes(r,rr,0);
00418     if (itol == 1) {
00419         bnrm=snrm(b,itol);
00420         asolve(r,z,sa_p,0);
00421     }
00422     else if (itol == 2) {
00423         asolve(b,z,sa_p,0);
00424         bnrm=snrm(z,itol);
00425         asolve(r,z,sa_p,0);
00426     }
00427     else if (itol == 3 || itol == 4) {
00428         asolve(b,z,sa_p,0);
00429         bnrm=snrm(z,itol);
00430         asolve(r,z,sa_p,0);
00431         znrm=snrm(z,itol);
00432     } else printf("illegal itol in linbcg");
00433     //cout << fixed << setprecision(6);
00434     while (iter < itmax) {
00435         ++iter;
00436         asolve(rr,zz,sa_p,1);
00437         for (bknum=0.0,j=0;j<n;j++) bknum += z[j]*rr[j];
00438         if (iter == 1) {
00439             for (j=0;j<n;j++) {
00440                 p[j]=z[j];
00441                 pp[j]=zz[j];
00442             }
00443         } else {
00444             bk=bknum/bkden;
00445             for (j=0;j<n;j++) {
00446                 p[j]=bk*p[j]+z[j];
00447                 pp[j]=bk*pp[j]+zz[j];
00448             }
00449         }
00450         bkden=bknum;
00451         atimes(p,z,ija_p,sa_p,0);
00452         for (akden=0.0,j=0;j<n;j++) akden += z[j]*pp[j];
00453         if(akden != 0)
00454             ak=bknum/akden;
00455         else
00456             ak = bknum;
00457         atimes(pp,zz,ija_p,sa_p,1);
00458         for (j=0;j<n;j++) {
00459             x[j] += ak*p[j];
00460             r[j] -= ak*z[j];
00461             rr[j] -= ak*zz[j];
00462         }
00463         asolve(r,z,sa_p,0);
00464         if (itol == 1)
00465         {
00466             if(bnrm != 0)
00467                 err=snrm(r,itol)/bnrm;
00468             else
00469                 err=snrm(r,itol);
00470         }
00471         else if (itol == 2)
00472             err=snrm(z,itol)/bnrm;
00473         else if (itol == 3 || itol == 4) {
00474             zm1nrm=znrm;
00475             znrm=snrm(z,itol);
00476             if (fabs(zm1nrm-znrm) > EPS*znrm) {
00477                 dxnrm=fabs(ak)*snrm(p,itol);
00478                 err=znrm/fabs(zm1nrm-znrm)*dxnrm;
00479             } else {
00480                 err=znrm/bnrm;
00481                 continue;
00482             }
00483             xnrm=snrm(x,itol);
00484             if (err <= 0.5*xnrm) err /= xnrm;
00485             else {
00486                 err=znrm/bnrm;
00487                 continue;
00488             }
00489         }
00490        // cout << "iter=" << setw(4) << iter+1 << setw(12) << err << endl;
00491         if (err <= tol) break;
00492     }
00493 }
00494
00495 void atimes(Vec_I_DP &x, Vec_O_DP &r,Vec_INT *ija_p,Vec_DP *sa_p, const int itrnsp)
00496 {
00497     if (itrnsp) sprstx(*sa_p,*ija_p,x,r);
00498     else sprsax(*sa_p,*ija_p,x,r);
00499 }
00500
00501
00502 void asolve(Vec_I_DP &b, Vec_O_DP &x,Vec_DP *sa_p,const int itrnsp)
00503 {
00504     int i;
00505
00506     int n=b.size();
00507     for(i=0;i<n;i++) x[i]=((*sa_p)[i] != 0.0 ? b[i]/(*sa_p)[i] : b[i]);
00508 }
00509
00510
00511 /*--------------------------------------------------------------*/
00513 void sprsin(Mat_I_DP &a, const DP thresh, Vec_O_DP &sa, Vec_O_INT &ija)
00514 {
00515     int i,j,k;
00516
00517     int n=a.nrows();
00518     int nmax=sa.size();
00519     for (j=0;j<n;j++) sa[j]=a[j][j];
00520     ija[0]=n+1;
00521     k=n;
00522     for (i=0;i<n;i++) {
00523         for (j=0;j<n;j++) {
00524             if (fabs(a[i][j]) >= thresh && i != j) {
00525                 if (++k > nmax) printf("sprsin: sa and ija too small");
00526                 sa[k]=a[i][j];
00527                 ija[k]=j;
00528             }
00529         }
00530         ija[i+1]=k+1;
00531     }
00532 }
00533
00534 DP snrm(Vec_I_DP &sx, const int itol)
00535 {
00536     int i,isamax;
00537     DP ans;
00538
00539     int n=sx.size();
00540     if (itol <= 3) {
00541         ans = 0.0;
00542         for (i=0;i<n;i++) ans += sx[i]*sx[i];
00543         return sqrt(ans);
00544     } else {
00545         isamax=0;
00546         for (i=0;i<n;i++) {
00547             if (fabs(sx[i]) > fabs(sx[isamax])) isamax=i;
00548         }
00549         return fabs(sx[isamax]);
00550     }
00551 }
00552
00553
00554
00555 void sprsax(Vec_I_DP &sa, Vec_I_INT &ija, Vec_I_DP &x, Vec_O_DP &b)
00556 {
00557     int i,k;
00558
00559     int n=x.size();
00560     if (ija[0] != n+1)
00561         printf("sprsax: mismatched vector and matrix");
00562     for (i=0;i<n;i++) {
00563         b[i]=sa[i]*x[i];
00564         for (k=ija[i];k<ija[i+1];k++) {
00565             b[i] += sa[k]*x[ija[k]];
00566         }
00567     }
00568 }
00569
00570
00571 void sprstx(Vec_I_DP &sa, Vec_I_INT &ija, Vec_I_DP &x, Vec_O_DP &b)
00572 {
00573     int i,j,k;
00574
00575     int n=x.size();
00576     if (ija[0] != (n+1))
00577         printf("mismatched vector and matrix in sprstx");
00578     for (i=0;i<n;i++) b[i]=sa[i]*x[i];
00579     for (i=0;i<n;i++) {
00580         for (k=ija[i];k<ija[i+1];k++) {
00581             j=ija[k];
00582             b[j] += sa[k]*x[i];
00583         }
00584     }
00585 }
00586
00587 void Bresenham(int32 x1, int32 y1, int32 x2, int32 y2, vector<Location>& locationVec)
00588 {
00589         bool swapflag = false;
00590         if (x1 > x2){
00591                 int32 tmpx = x1;
00592                 int32 tmpy = y1;
00593                 x1 = x2;
00594                 y1 = y2;
00595                 x2 = tmpx;
00596                 y2 = tmpy;
00597                 swapflag = true;
00598         }
00599
00600         int32 dx = x2-x1;
00601         int32 dy = y2-y1;
00602         int32 x = x1;
00603         int32 y = y1;
00604         int32 sub = (dy<<1)-dx;
00605         locationVec.push_back(Location(x, y));
00606         while(x<x2){
00607                 ++x;
00608                 if (sub > 0){
00609                         sub += (dy<<1) - (dx<<1);
00610                         ++y;
00611                 }else {
00612                         sub += (dy<<1);
00613                 }
00614                 locationVec.push_back(Location(x, y));
00615         }
00616
00617         if (swapflag){
00618                 uint32 size = locationVec.size();
00619                 for (uint32 i = 0; i < size/2 ; ++i){
00620                         Location tmp = locationVec[i];
00621                         locationVec[i] = locationVec[size-i-1];
00622                         locationVec[size-i-1] = tmp;
00623                 }
00624         }
00625 }
00626
00627 void CalcShortestDistance(const Location& startPos, const Location& endPos, vector<Location>& locationVec)
00628 {
00629     if (startPos.x==endPos.x && startPos.y==endPos.y)
00630         return ;
00631
00632     if (endPos.x == startPos.x){ //x相同
00633         if (endPos.y > startPos.y){
00634             for (uint32 i = 0; i < (uint32)(endPos.y-startPos.y); ++i){
00635                 locationVec.push_back(Location(startPos.x, startPos.y+i+1));
00636             }
00637         }else{
00638             for (uint32 i = 0; i < (uint32)(startPos.y-endPos.y); ++i){
00639                 locationVec.push_back(Location(startPos.x, startPos.y-i-1));
00640             }
00641         }
00642         return ;
00643     }
00644
00645     float k = (float)(endPos.y-startPos.y)/(endPos.x-startPos.x);
00646
00647     if (k >= 0 && k <= 1){ //斜率为0~1
00648
00649         Bresenham(startPos.x,startPos.y,endPos.x,endPos.y,locationVec);
00650
00651     }else if (k > 1){ //斜率为1~无穷大
00652
00653         Bresenham(startPos.y,startPos.x,endPos.y,endPos.x,locationVec);
00654         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it){
00655             int tmp = (*it).x;
00656             (*it).x = (*it).y;
00657             (*it).y = tmp;
00658         }
00659
00660     }else if (k >= -1 && k < 0){ //斜率为-1~0
00661
00662         Bresenham(startPos.x,-startPos.y,endPos.x,-endPos.y,locationVec);
00663         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it)
00664             (*it).y = -(*it).y;
00665
00666     }else if (k < -1){ //斜率为无穷小~-1
00667
00668         Bresenham(-startPos.y,startPos.x,-endPos.y,endPos.x,locationVec);
00669         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it){
00670             int tmp = (*it).x;
00671             (*it).x = (*it).y;
00672             (*it).y = tmp;
00673             (*it).y = -(*it).y;
00674         }
00675     }
00676
00677     locationVec.erase(locationVec.begin());
00678 }
00679
00680
00681 void CalcBresenhamLocs(const Location& startPos, const Location& endPos, vector<Location>& locationVec)
00682 {
00683     if (startPos.x==endPos.x && startPos.y==endPos.y)
00684         return ;
00685
00686     if (endPos.x == startPos.x){ //x相同
00687         if (endPos.y > startPos.y){
00688             for (uint32 i = 0; i < (uint32)(endPos.y-startPos.y); ++i){
00689                 locationVec.push_back(Location(startPos.x, startPos.y+i+1));
00690             }
00691         }else{
00692             for (uint32 i = 0; i < (uint32)(startPos.y-endPos.y); ++i){
00693                 locationVec.push_back(Location(startPos.x, startPos.y-i-1));
00694             }
00695         }
00696         return ;
00697     }
00698
00699     float k = (float)(endPos.y-startPos.y)/(endPos.x-startPos.x);
00700
00701     if (k >= 0 && k <= 1){ //斜率为0~1
00702
00703         Bresenham(startPos.x,startPos.y,endPos.x,endPos.y,locationVec);
00704
00705     }else if (k > 1){ //斜率为1~无穷大
00706
00707         Bresenham(startPos.y,startPos.x,endPos.y,endPos.x,locationVec);
00708         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it){
00709             int tmp = (*it).x;
00710             (*it).x = (*it).y;
00711             (*it).y = tmp;
00712         }
00713
00714     }else if (k >= -1 && k < 0){ //斜率为-1~0
00715
00716         Bresenham(startPos.x,-startPos.y,endPos.x,-endPos.y,locationVec);
00717         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it)
00718             (*it).y = -(*it).y;
00719
00720     }else if (k < -1){ //斜率为无穷小~-1
00721
00722         Bresenham(-startPos.y,startPos.x,-endPos.y,endPos.x,locationVec);
00723         for (vector<Location>::iterator it = locationVec.begin(); it!=locationVec.end(); ++it){
00724             int tmp = (*it).x;
00725             (*it).x = (*it).y;
00726             (*it).y = tmp;
00727             (*it).y = -(*it).y;
00728         }
00729     }
00730
00731     locationVec.erase(locationVec.begin());
00732 }
00733
```

tensor_field_nav_core
Author(s): Lintao Zheng, Kai Xu
autogenerated on Thu Jun 6 2019 19:50:56