00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef DF_PROPAGATION_DISTANCE_FIELD_H_
00038 #define DF_PROPAGATION_DISTANCE_FIELD_H_
00039
00040 #include <distance_field/voxel_grid.h>
00041 #include <distance_field/distance_field.h>
00042 #include <LinearMath/btVector3.h>
00043 #include <vector>
00044 #include <list>
00045 #include <ros/ros.h>
00046
00047 namespace distance_field
00048 {
00049
00053 struct PropDistanceFieldVoxel
00054 {
00055 PropDistanceFieldVoxel();
00056 PropDistanceFieldVoxel(int distance_sq);
00057
00058 int distance_square_;
00059 int location_[3];
00060 int closest_point_[3];
00061 int update_direction_;
00063 static const int UNINITIALIZED=-1;
00064 };
00065
00074 class PropagationDistanceField: public DistanceField<PropDistanceFieldVoxel>
00075 {
00076 public:
00077
00081 PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00082 double origin_x, double origin_y, double origin_z, double max_distance);
00083
00084 virtual ~PropagationDistanceField();
00085
00089 virtual void addPointsToField(const std::vector<btVector3> points);
00090
00094 virtual void reset();
00095
00096 private:
00097 std::vector<std::vector<PropDistanceFieldVoxel*> > bucket_queue_;
00098 double max_distance_;
00099 int max_distance_sq_;
00100
00101 std::vector<double> sqrt_table_;
00102
00103
00104
00105
00106
00107
00108 std::vector<std::vector<std::vector<std::vector<int> > > > neighborhoods_;
00109
00110 std::vector<std::vector<int> > direction_number_to_direction_;
00111
00112 virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00113 int getDirectionNumber(int dx, int dy, int dz) const;
00114 void initNeighborhoods();
00115 static int eucDistSq(int* point1, int* point2);
00116 };
00117
00119
00120 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_sq):
00121 distance_square_(distance_sq)
00122 {
00123 for (int i=0; i<3; i++)
00124 closest_point_[i] = PropDistanceFieldVoxel::UNINITIALIZED;
00125 }
00126
00127 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00128 {
00129 }
00130
00131 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00132 {
00133 return sqrt_table_[object.distance_square_];
00134 }
00135
00136 }
00137
00138 #endif