navigation源码中Costmap2d中使用的Breshenham代码解析

因为更新的是直线所经历过的栅格而不是点了，所以需要考虑选择右边的还是右上的

从最原始的计算开始，现在只考虑第一想象的，直线的斜率k小于1， 从起点开始，x增加一个单位，因为是一个单位，所以y增加了斜率k, 如上图， d1 > d2, 就选择右上的格子，d1 < d2就选择右边格子，d1==d2就设定为选者右上格子，即d1>= 0.5时选择右上格子。实际代码中设置一个比较值变量e=0.5, k-e>=0 则选者右上格子, (yn - y0 / (xn -x0)) - e = dy/dx - e > 0 =  dy/dx -0.5 >= 0选择右上格子，实际代码中避免用除法和小数，不等式两边同时乘以dx, 有

                                            dy - 0.5dx >= 0

                                             dy +   dx/2  >=  dx,

代码里用的是这个公式判断x增加一个单位时，y变化

然后考虑4个像限的情况和斜率大于1情况，x占主导还是y占主导

X占主导时  即k < 1 
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,int offset_b, unsigned int offset, unsigned int max_length)
    {
        


        
      unsigned int end = std::min(max_length, abs_da);
      for (unsigned int i = 0; i < end; ++i)
      {
        at(offset);//更新栅格代价为空闲
        offset += offset_a;//x 或y方向加一个单位, x or y的一个单位所移动的格子数是不同的
        
         //这里用的就是公式  dy +   dx/2  >=  dx
        error_b += abs_db;//
        if ((unsigned int)error_b >= abs_da)//满足就是k >=0.5
        {
          offset += offset_b;// y方向增加一个单位，对于map下就是增加了一个行即这里的offset_b

          error_b -= abs_da;//更新error_b， 因为下次计算y时  x已经增加了一个单位了
        }
      }
      at(offset);//更新栅格代价为空闲
    }