@@ -461,105 +461,7 @@ class Costmap2D
461461 */
462462 virtual void initMaps (unsigned int size_x, unsigned int size_y);
463463
464- /* *
465- * @brief Raytrace a line and apply some action at each step
466- * @param at The action to take... a functor
467- * @param x0 The starting x coordinate
468- * @param y0 The starting y coordinate
469- * @param x1 The ending x coordinate
470- * @param y1 The ending y coordinate
471- * @param max_length The maximum desired length of the segment...
472- * allows you to not go all the way to the endpoint
473- * @param min_length The minimum desired length of the segment
474- */
475- template <class ActionType >
476- inline void raytraceLine (
477- ActionType at, unsigned int x0, unsigned int y0, unsigned int x1,
478- unsigned int y1,
479- unsigned int max_length = UINT_MAX, unsigned int min_length = 0 )
480- {
481- int dx_full = x1 - x0;
482- int dy_full = y1 - y0;
483-
484- // we need to chose how much to scale our dominant dimension,
485- // based on the maximum length of the line
486- double dist = std::hypot (dx_full, dy_full);
487- if (dist < min_length) {
488- return ;
489- }
490-
491- unsigned int min_x0, min_y0;
492- if (dist > 0.0 ) {
493- // Adjust starting point and offset to start from min_length distance
494- min_x0 = (unsigned int )(x0 + dx_full / dist * min_length);
495- min_y0 = (unsigned int )(y0 + dy_full / dist * min_length);
496- } else {
497- // dist can be 0 if [x0, y0]==[x1, y1].
498- // In this case only this cell should be processed.
499- min_x0 = x0;
500- min_y0 = y0;
501- }
502- unsigned int offset = min_y0 * size_x_ + min_x0;
503-
504- int dx = x1 - min_x0;
505- int dy = y1 - min_y0;
506-
507- unsigned int abs_dx = abs (dx);
508- unsigned int abs_dy = abs (dy);
509-
510- int offset_dx = sign (dx);
511- int offset_dy = sign (dy) * size_x_;
512-
513- double scale = (dist == 0.0 ) ? 1.0 : std::min (1.0 , max_length / dist);
514- // if x is dominant
515- if (abs_dx >= abs_dy) {
516- int error_y = abs_dx / 2 ;
517-
518- bresenham2D (
519- at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int )(scale * abs_dx));
520- return ;
521- }
522-
523- // otherwise y is dominant
524- int error_x = abs_dy / 2 ;
525-
526- bresenham2D (
527- at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int )(scale * abs_dy));
528- }
529-
530464private:
531- /* *
532- * @brief A 2D implementation of Bresenham's raytracing algorithm...
533- * applies an action at each step
534- */
535- template <class ActionType >
536- inline void bresenham2D (
537- ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b,
538- int offset_a,
539- int offset_b, unsigned int offset,
540- unsigned int max_length)
541- {
542- unsigned int end = std::min (max_length, abs_da);
543- for (unsigned int i = 0 ; i < end; ++i) {
544- at (offset);
545- offset += offset_a;
546- error_b += abs_db;
547- if ((unsigned int )error_b >= abs_da) {
548- offset += offset_b;
549- error_b -= abs_da;
550- }
551- }
552- at (offset);
553- }
554-
555- /* *
556- * @brief get the sign of an int
557- */
558- inline int sign (int x)
559- {
560- return x > 0 ? 1.0 : -1.0 ;
561- }
562-
563465 mutex_t * access_;
564466
565467protected:
0 commit comments