@@ -421,11 +421,10 @@ inline void
421421NavFn::updateCell (int n)
422422{
423423 // get neighbors
424- float u, d, l, r;
425- l = potarr[n - 1 ];
426- r = potarr[n + 1 ];
427- u = potarr[n - nx];
428- d = potarr[n + nx];
424+ const float l = potarr[n - 1 ];
425+ const float r = potarr[n + 1 ];
426+ const float u = potarr[n - nx];
427+ const float d = potarr[n + nx];
429428 // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
430429 // potarr[n], l, r, u, d);
431430 // ROS_INFO("[Update] cost: %d\n", costarr[n]);
@@ -452,8 +451,8 @@ NavFn::updateCell(int n)
452451 // use quadratic approximation
453452 // might speed this up through table lookup, but still have to
454453 // do the divide
455- float d = dc / hf;
456- float v = -0.2301 * d * d + 0.5307 * d + 0.7040 ;
454+ const float div = dc / hf;
455+ const float v = -0.2301 * div * div + 0.5307 * div + 0.7040 ;
457456 pot = ta + hf * v;
458457 }
459458
@@ -496,11 +495,10 @@ inline void
496495NavFn::updateCellAstar (int n)
497496{
498497 // get neighbors
499- float u, d, l, r;
500- l = potarr[n - 1 ];
501- r = potarr[n + 1 ];
502- u = potarr[n - nx];
503- d = potarr[n + nx];
498+ float l = potarr[n - 1 ];
499+ float r = potarr[n + 1 ];
500+ float u = potarr[n - nx];
501+ float d = potarr[n + nx];
504502 // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
505503 // potarr[n], l, r, u, d);
506504 // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]);
@@ -527,8 +525,8 @@ NavFn::updateCellAstar(int n)
527525 // use quadratic approximation
528526 // might speed this up through table lookup, but still have to
529527 // do the divide
530- float d = dc / hf;
531- float v = -0.2301 * d * d + 0.5307 * d + 0.7040 ;
528+ const float div = dc / hf;
529+ const float v = -0.2301 * div * div + 0.5307 * div + 0.7040 ;
532530 pot = ta + hf * v;
533531 }
534532
@@ -834,22 +832,22 @@ NavFn::calcPath(int n, int * st)
834832 // check eight neighbors to find the lowest
835833 int minc = stc;
836834 int minp = potarr[stc];
837- int st = stcpx - 1 ;
838- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
839- st ++;
840- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
841- st ++;
842- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
843- st = stc - 1 ;
844- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
845- st = stc + 1 ;
846- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
847- st = stcnx - 1 ;
848- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
849- st ++;
850- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
851- st ++;
852- if (potarr[st ] < minp) {minp = potarr[st ]; minc = st ;}
835+ int sti = stcpx - 1 ;
836+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
837+ sti ++;
838+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
839+ sti ++;
840+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
841+ sti = stc - 1 ;
842+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
843+ sti = stc + 1 ;
844+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
845+ sti = stcnx - 1 ;
846+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
847+ sti ++;
848+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
849+ sti ++;
850+ if (potarr[sti ] < minp) {minp = potarr[sti ]; minc = sti ;}
853851 stc = minc;
854852 dx = 0 ;
855853 dy = 0 ;
0 commit comments