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 #include <distance_field/propagation_distance_field.h>
00038 #include <visualization_msgs/Marker.h>
00039
00040 namespace distance_field
00041 {
00042
00043 PropagationDistanceField::~PropagationDistanceField()
00044 {
00045 }
00046
00047 PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00048 double origin_x, double origin_y, double origin_z, double max_distance):
00049 DistanceField<PropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, PropDistanceFieldVoxel(max_distance))
00050 {
00051 max_distance_ = max_distance;
00052 int max_dist_int = ceil(max_distance_/resolution);
00053 max_distance_sq_ = (max_dist_int*max_dist_int);
00054 initNeighborhoods();
00055
00056 bucket_queue_.resize(max_distance_sq_+1);
00057
00058
00059 sqrt_table_.resize(max_distance_sq_+1);
00060 for (int i=0; i<=max_distance_sq_; ++i)
00061 sqrt_table_[i] = sqrt(double(i))*resolution;
00062 }
00063
00064 int PropagationDistanceField::eucDistSq(int3 point1, int3 point2)
00065 {
00066 int dx = point1.x() - point2.x();
00067 int dy = point1.y() - point2.y();
00068 int dz = point1.z() - point2.z();
00069 return dx*dx + dy*dy + dz*dz;
00070 }
00071
00072 void PropagationDistanceField::updatePointsInField(const std::vector<tf::Vector3>& points, bool iterative)
00073 {
00074 if( iterative )
00075 {
00076 VoxelSet points_added;
00077 VoxelSet points_removed(object_voxel_locations_);
00078
00079
00080
00081 for( unsigned int i=0; i<points.size(); i++)
00082 {
00083
00084 int3 voxel_loc;
00085 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00086 voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00087 if( valid )
00088 {
00089 if( iterative )
00090 {
00091 bool already_obstacle_voxel = ( object_voxel_locations_.find(voxel_loc) != object_voxel_locations_.end() );
00092 if( !already_obstacle_voxel )
00093 {
00094
00095 object_voxel_locations_.insert(voxel_loc);
00096
00097
00098 points_added.insert(voxel_loc);
00099 }
00100 else
00101 {
00102
00103 points_removed.erase(voxel_loc);
00104 }
00105 }
00106 else
00107 {
00108 object_voxel_locations_.insert(voxel_loc);
00109 points_added.insert(voxel_loc);
00110 }
00111 }
00112 }
00113
00114 removeObstacleVoxels( points_removed );
00115 addNewObstacleVoxels( points_added );
00116 }
00117
00118 else
00119 {
00120 VoxelSet points_added;
00121 reset();
00122
00123 for( unsigned int i=0; i<points.size(); i++)
00124 {
00125
00126 int3 voxel_loc;
00127 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00128 voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00129 if( valid )
00130 {
00131 object_voxel_locations_.insert(voxel_loc);
00132 points_added.insert(voxel_loc);
00133 }
00134 }
00135 addNewObstacleVoxels( points_added );
00136 }
00137 }
00138
00139 void PropagationDistanceField::addPointsToField(const std::vector<tf::Vector3>& points)
00140 {
00141 VoxelSet voxel_locs;
00142
00143 for( unsigned int i=0; i<points.size(); i++)
00144 {
00145
00146 int3 voxel_loc;
00147 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00148 voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00149
00150 if( valid )
00151 {
00152 bool already_obstacle_voxel = ( object_voxel_locations_.find(voxel_loc) != object_voxel_locations_.end() );
00153 if( !already_obstacle_voxel )
00154 {
00155
00156 object_voxel_locations_.insert(voxel_loc);
00157
00158
00159 voxel_locs.insert(voxel_loc);
00160 }
00161 }
00162 }
00163
00164 addNewObstacleVoxels( voxel_locs );
00165 }
00166
00167 void PropagationDistanceField::addNewObstacleVoxels(const VoxelSet& locations)
00168 {
00169 int x, y, z;
00170 int initial_update_direction = getDirectionNumber(0,0,0);
00171 bucket_queue_[0].reserve(locations.size());
00172
00173 VoxelSet::const_iterator it = locations.begin();
00174 for( it=locations.begin(); it!=locations.end(); ++it)
00175 {
00176 int3 loc = *it;
00177 x = loc.x();
00178 y = loc.y();
00179 z = loc.z();
00180 bool valid = isCellValid( x, y, z);
00181 if (!valid)
00182 continue;
00183 PropDistanceFieldVoxel& voxel = getCell(x,y,z);
00184 voxel.distance_square_ = 0;
00185 voxel.closest_point_ = loc;
00186 voxel.location_ = loc;
00187 voxel.update_direction_ = initial_update_direction;
00188 bucket_queue_[0].push_back(&voxel);
00189 }
00190
00191 propogate();
00192 }
00193
00194 void PropagationDistanceField::removeObstacleVoxels(const VoxelSet& locations )
00195 {
00196 std::vector<int3> stack;
00197 int initial_update_direction = getDirectionNumber(0,0,0);
00198
00199 stack.reserve( num_cells_[DIM_X] * num_cells_[DIM_Y] * num_cells_[DIM_Z] );
00200 bucket_queue_[0].reserve(locations.size());
00201
00202
00203 VoxelSet::const_iterator it = locations.begin();
00204 for( it=locations.begin(); it!=locations.end(); ++it)
00205 {
00206 int3 loc = *it;
00207 bool valid = isCellValid( loc.x(), loc.y(), loc.z());
00208 if (!valid)
00209 continue;
00210 PropDistanceFieldVoxel& voxel = getCell(loc.x(), loc.y(), loc.z());
00211 voxel.distance_square_ = max_distance_sq_;
00212 voxel.closest_point_ = loc;
00213 voxel.location_ = loc;
00214 voxel.update_direction_ = initial_update_direction;
00215 stack.push_back(loc);
00216 }
00217
00218
00219 while(stack.size() > 0)
00220 {
00221 int3 loc = stack.back();
00222 stack.pop_back();
00223
00224 for( int neighbor=0; neighbor<27; neighbor++ )
00225 {
00226 int3 diff = getLocationDifference(neighbor);
00227 int3 nloc( loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z() );
00228
00229 if( isCellValid(nloc.x(), nloc.y(), nloc.z()) )
00230 {
00231 PropDistanceFieldVoxel& nvoxel = getCell(nloc.x(), nloc.y(), nloc.z());
00232 int3& close_point = nvoxel.closest_point_;
00233 PropDistanceFieldVoxel& closest_point_voxel = getCell( close_point.x(), close_point.y(), close_point.z() );
00234
00235 if( closest_point_voxel.distance_square_ != 0 )
00236 {
00237 if( nvoxel.distance_square_!=max_distance_sq_)
00238 {
00239 nvoxel.distance_square_ = max_distance_sq_;
00240 nvoxel.closest_point_ = nloc;
00241 nvoxel.location_ = nloc;
00242 nvoxel.update_direction_ = initial_update_direction;
00243 stack.push_back(nloc);
00244 }
00245 }
00246 else
00247 {
00248 bucket_queue_[0].push_back(&nvoxel);
00249 }
00250 }
00251 }
00252 }
00253
00254 propogate();
00255 }
00256
00257 void PropagationDistanceField::propogate()
00258 {
00259 int x, y, z, nx, ny, nz;
00260 int3 loc;
00261
00262
00263 for (unsigned int i=0; i<bucket_queue_.size(); ++i)
00264 {
00265 std::vector<PropDistanceFieldVoxel*>::iterator list_it = bucket_queue_[i].begin();
00266 while(list_it!=bucket_queue_[i].end())
00267 {
00268 PropDistanceFieldVoxel* vptr = *list_it;
00269
00270 x = vptr->location_.x();
00271 y = vptr->location_.y();
00272 z = vptr->location_.z();
00273
00274
00275 std::vector<int3 >* neighborhood;
00276 int D = i;
00277 if (D>1)
00278 D=1;
00279
00280 if (vptr->update_direction_<0 || vptr->update_direction_>26)
00281 {
00282
00283 ++list_it;
00284 continue;
00285 }
00286
00287 neighborhood = &neighborhoods_[D][vptr->update_direction_];
00288
00289 for (unsigned int n=0; n<neighborhood->size(); n++)
00290 {
00291 int dx = (*neighborhood)[n].x();
00292 int dy = (*neighborhood)[n].y();
00293 int dz = (*neighborhood)[n].z();
00294 nx = x + dx;
00295 ny = y + dy;
00296 nz = z + dz;
00297 if (!isCellValid(nx,ny,nz))
00298 continue;
00299
00300
00301
00302 PropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00303 loc.x() = nx;
00304 loc.y() = ny;
00305 loc.z() = nz;
00306 int new_distance_sq = eucDistSq(vptr->closest_point_, loc);
00307 if (new_distance_sq > max_distance_sq_)
00308 continue;
00309 if (new_distance_sq < neighbor->distance_square_)
00310 {
00311
00312 neighbor->distance_square_ = new_distance_sq;
00313 neighbor->closest_point_ = vptr->closest_point_;
00314 neighbor->location_ = loc;
00315 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00316
00317
00318 bucket_queue_[new_distance_sq].push_back(neighbor);
00319 }
00320 }
00321
00322 ++list_it;
00323 }
00324 bucket_queue_[i].clear();
00325 }
00326
00327 }
00328
00329 void PropagationDistanceField::getOccupiedVoxelMarkers(const std::string & frame_id,
00330 const ros::Time stamp,
00331 const tf::Transform& cur,
00332 visualization_msgs::Marker& inf_marker)
00333 {
00334 inf_marker.points.clear();
00335 inf_marker.header.frame_id = frame_id;
00336 inf_marker.header.stamp = stamp;
00337 inf_marker.ns = "distance_field_occupied_cells";
00338 inf_marker.id = 1;
00339 inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00340 inf_marker.action = 0;
00341 inf_marker.scale.x = resolution_[DIM_X];
00342 inf_marker.scale.y = resolution_[DIM_Y];
00343 inf_marker.scale.z = resolution_[DIM_Z];
00344 inf_marker.color.r = 0.0;
00345 inf_marker.color.g = 0.0;
00346 inf_marker.color.b = 1.0;
00347 inf_marker.color.a = 0.1;
00348
00349
00350 inf_marker.points.reserve(100000);
00351
00352 VoxelSet::iterator iter;
00353 for(iter = object_voxel_locations_.begin(); iter != object_voxel_locations_.end(); iter++)
00354 {
00355 int last = inf_marker.points.size();
00356 inf_marker.points.resize(last + 1);
00357 double nx, ny, nz;
00358 int x = (*iter).x();
00359 int y = (*iter).y();
00360 int z = (*iter).z();
00361 this->gridToWorld(x,y,z,nx, ny, nz);
00362 tf::Vector3 vec(nx,ny,nz);
00363 vec = cur*vec;
00364 inf_marker.points[last].x = vec.x();
00365 inf_marker.points[last].y = vec.y();
00366 inf_marker.points[last].z = vec.z();
00367 }
00368 }
00369
00370
00371 void PropagationDistanceField::reset()
00372 {
00373 VoxelGrid<PropDistanceFieldVoxel>::reset(PropDistanceFieldVoxel(max_distance_sq_));
00374 }
00375
00376 void PropagationDistanceField::initNeighborhoods()
00377 {
00378
00379 direction_number_to_direction_.resize(27);
00380 for (int dx=-1; dx<=1; ++dx)
00381 {
00382 for (int dy=-1; dy<=1; ++dy)
00383 {
00384 for (int dz=-1; dz<=1; ++dz)
00385 {
00386 int direction_number = getDirectionNumber(dx, dy, dz);
00387 int3 n_point( dx, dy, dz);
00388 direction_number_to_direction_[direction_number] = n_point;
00389 }
00390 }
00391 }
00392
00393 neighborhoods_.resize(2);
00394 for (int n=0; n<2; n++)
00395 {
00396 neighborhoods_[n].resize(27);
00397
00398 for (int dx=-1; dx<=1; ++dx)
00399 {
00400 for (int dy=-1; dy<=1; ++dy)
00401 {
00402 for (int dz=-1; dz<=1; ++dz)
00403 {
00404 int direction_number = getDirectionNumber(dx, dy, dz);
00405
00406 for (int tdx=-1; tdx<=1; ++tdx)
00407 {
00408 for (int tdy=-1; tdy<=1; ++tdy)
00409 {
00410 for (int tdz=-1; tdz<=1; ++tdz)
00411 {
00412 if (tdx==0 && tdy==0 && tdz==0)
00413 continue;
00414 if (n>=1)
00415 {
00416 if ((abs(tdx) + abs(tdy) + abs(tdz))!=1)
00417 continue;
00418 if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0)
00419 continue;
00420 }
00421 int3 n_point(tdx,tdy,tdz);
00422 neighborhoods_[n][direction_number].push_back(n_point);
00423 }
00424 }
00425 }
00426
00427 }
00428 }
00429 }
00430 }
00431
00432 }
00433
00434 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00435 {
00436 return (dx+1)*9 + (dy+1)*3 + dz+1;
00437 }
00438
00439 int3 PropagationDistanceField::getLocationDifference(int directionNumber) const
00440 {
00441 return direction_number_to_direction_[ directionNumber ];
00442 }
00443
00444 SignedPropagationDistanceField::~SignedPropagationDistanceField()
00445 {
00446 }
00447
00448 SignedPropagationDistanceField::SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00449 double origin_x, double origin_y, double origin_z, double max_distance):
00450 DistanceField<SignedPropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, SignedPropDistanceFieldVoxel(max_distance,0))
00451 {
00452 max_distance_ = max_distance;
00453 int max_dist_int = ceil(max_distance_/resolution);
00454 max_distance_sq_ = (max_dist_int*max_dist_int);
00455 initNeighborhoods();
00456
00457
00458 sqrt_table_.resize(max_distance_sq_+1);
00459 for (int i=0; i<=max_distance_sq_; ++i)
00460 sqrt_table_[i] = sqrt(double(i))*resolution;
00461 }
00462
00463 int SignedPropagationDistanceField::eucDistSq(int3 point1, int3 point2)
00464 {
00465 int dx = point1.x() - point2.x();
00466 int dy = point1.y() - point2.y();
00467 int dz = point1.z() - point2.z();
00468 return dx*dx + dy*dy + dz*dz;
00469 }
00470
00471 void SignedPropagationDistanceField::addPointsToField(const std::vector<tf::Vector3>& points)
00472 {
00473
00474 positive_bucket_queue_.resize(max_distance_sq_+1);
00475 negative_bucket_queue_.resize(max_distance_sq_+1);
00476
00477 positive_bucket_queue_[0].reserve(points.size());
00478 negative_bucket_queue_[0].reserve(points.size());
00479
00480 for(int x = 0; x < num_cells_[0]; x++)
00481 {
00482 for(int y = 0; y < num_cells_[1]; y++)
00483 {
00484 for(int z = 0; z < num_cells_[2]; z++)
00485 {
00486 SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z);
00487 voxel.closest_negative_point_.x() = x;
00488 voxel.closest_negative_point_.y() = y;
00489 voxel.closest_negative_point_.z() = z;
00490 voxel.negative_distance_square_ = 0;
00491 }
00492 }
00493 }
00494
00495
00496 int x, y, z, nx, ny, nz;
00497 int3 loc;
00498 int initial_update_direction = getDirectionNumber(0,0,0);
00499 for (unsigned int i=0; i<points.size(); ++i)
00500 {
00501 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00502 if (!valid)
00503 continue;
00504 SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z);
00505 voxel.positive_distance_square_ = 0;
00506 voxel.negative_distance_square_ = max_distance_sq_;
00507 voxel.closest_positive_point_.x() = x;
00508 voxel.closest_positive_point_.y() = y;
00509 voxel.closest_positive_point_.z() = z;
00510 voxel.closest_negative_point_.x() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00511 voxel.closest_negative_point_.y() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00512 voxel.closest_negative_point_.z() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00513 voxel.location_.x() = x;
00514 voxel.location_.y() = y;
00515 voxel.location_.z() = z;
00516 voxel.update_direction_ = initial_update_direction;
00517 positive_bucket_queue_[0].push_back(&voxel);
00518 }
00519
00520
00521 for (unsigned int i=0; i<positive_bucket_queue_.size(); ++i)
00522 {
00523 std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = positive_bucket_queue_[i].begin();
00524 while(list_it!=positive_bucket_queue_[i].end())
00525 {
00526 SignedPropDistanceFieldVoxel* vptr = *list_it;
00527
00528 x = vptr->location_.x();
00529 y = vptr->location_.y();
00530 z = vptr->location_.z();
00531
00532
00533 std::vector<int3 >* neighborhood;
00534 int D = i;
00535 if (D>1)
00536 D=1;
00537
00538 if (vptr->update_direction_<0 || vptr->update_direction_>26)
00539 {
00540
00541 ++list_it;
00542 continue;
00543 }
00544
00545 neighborhood = &neighborhoods_[D][vptr->update_direction_];
00546
00547 for (unsigned int n=0; n<neighborhood->size(); n++)
00548 {
00549 int dx = (*neighborhood)[n].x();
00550 int dy = (*neighborhood)[n].y();
00551 int dz = (*neighborhood)[n].z();
00552 nx = x + dx;
00553 ny = y + dy;
00554 nz = z + dz;
00555 if (!isCellValid(nx,ny,nz))
00556 continue;
00557
00558
00559
00560 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00561 loc.x() = nx;
00562 loc.y() = ny;
00563 loc.z() = nz;
00564 int new_distance_sq = eucDistSq(vptr->closest_positive_point_, loc);
00565 if (new_distance_sq > max_distance_sq_)
00566 continue;
00567 if (new_distance_sq < neighbor->positive_distance_square_)
00568 {
00569
00570 neighbor->positive_distance_square_ = new_distance_sq;
00571 neighbor->closest_positive_point_ = vptr->closest_positive_point_;
00572 neighbor->location_ = loc;
00573 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00574
00575
00576 positive_bucket_queue_[new_distance_sq].push_back(neighbor);
00577 }
00578 }
00579
00580 ++list_it;
00581 }
00582 positive_bucket_queue_[i].clear();
00583 }
00584
00585
00586 for(unsigned int i = 0; i < points.size(); i++)
00587 {
00588 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00589 if(!valid)
00590 continue;
00591
00592 for(int dx = -1; dx <= 1; dx ++)
00593 {
00594 for(int dy = -1; dy<= 1; dy ++)
00595 {
00596 for(int dz = -1; dz <= 1; dz++)
00597 {
00598 nx = x + dx;
00599 ny = y + dy;
00600 nz = z + dz;
00601
00602 if(!isCellValid(nx, ny, nz))
00603 continue;
00604
00605 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00606
00607 if(neighbor->closest_negative_point_.x() != SignedPropDistanceFieldVoxel::UNINITIALIZED)
00608 {
00609 neighbor->update_direction_ = initial_update_direction;
00610 negative_bucket_queue_[0].push_back(neighbor);
00611 }
00612 }
00613 }
00614 }
00615
00616 }
00617
00618 for (unsigned int i=0; i<negative_bucket_queue_.size(); ++i)
00619 {
00620 std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = negative_bucket_queue_[i].begin();
00621 while(list_it!=negative_bucket_queue_[i].end())
00622 {
00623 SignedPropDistanceFieldVoxel* vptr = *list_it;
00624
00625 x = vptr->location_.x();
00626 y = vptr->location_.y();
00627 z = vptr->location_.z();
00628
00629
00630 std::vector<int3 >* neighborhood;
00631 int D = i;
00632 if (D>1)
00633 D=1;
00634
00635 if (vptr->update_direction_<0 || vptr->update_direction_>26)
00636 {
00637
00638 ++list_it;
00639 continue;
00640 }
00641
00642 neighborhood = &neighborhoods_[D][vptr->update_direction_];
00643
00644 for (unsigned int n=0; n<neighborhood->size(); n++)
00645 {
00646 int dx = (*neighborhood)[n].x();
00647 int dy = (*neighborhood)[n].y();
00648 int dz = (*neighborhood)[n].z();
00649 nx = x + dx;
00650 ny = y + dy;
00651 nz = z + dz;
00652 if (!isCellValid(nx,ny,nz))
00653 continue;
00654
00655
00656
00657 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00658 loc.x() = nx;
00659 loc.y() = ny;
00660 loc.z() = nz;
00661 int new_distance_sq = eucDistSq(vptr->closest_negative_point_, loc);
00662 if (new_distance_sq > max_distance_sq_)
00663 continue;
00664 if (new_distance_sq < neighbor->negative_distance_square_)
00665 {
00666
00667 neighbor->negative_distance_square_ = new_distance_sq;
00668 neighbor->closest_negative_point_ = vptr->closest_negative_point_;
00669 neighbor->location_ = loc;
00670 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00671
00672
00673 negative_bucket_queue_[new_distance_sq].push_back(neighbor);
00674 }
00675 }
00676
00677 ++list_it;
00678 }
00679 negative_bucket_queue_[i].clear();
00680 }
00681
00682 }
00683
00684 void SignedPropagationDistanceField::reset()
00685 {
00686 VoxelGrid<SignedPropDistanceFieldVoxel>::reset(SignedPropDistanceFieldVoxel(max_distance_sq_, 0));
00687 }
00688
00689 void SignedPropagationDistanceField::initNeighborhoods()
00690 {
00691
00692 direction_number_to_direction_.resize(27);
00693 for (int dx=-1; dx<=1; ++dx)
00694 {
00695 for (int dy=-1; dy<=1; ++dy)
00696 {
00697 for (int dz=-1; dz<=1; ++dz)
00698 {
00699 int direction_number = getDirectionNumber(dx, dy, dz);
00700 int3 n_point( dx, dy, dz);
00701 direction_number_to_direction_[direction_number] = n_point;
00702 }
00703 }
00704 }
00705
00706 neighborhoods_.resize(2);
00707 for (int n=0; n<2; n++)
00708 {
00709 neighborhoods_[n].resize(27);
00710
00711 for (int dx=-1; dx<=1; ++dx)
00712 {
00713 for (int dy=-1; dy<=1; ++dy)
00714 {
00715 for (int dz=-1; dz<=1; ++dz)
00716 {
00717 int direction_number = getDirectionNumber(dx, dy, dz);
00718
00719 for (int tdx=-1; tdx<=1; ++tdx)
00720 {
00721 for (int tdy=-1; tdy<=1; ++tdy)
00722 {
00723 for (int tdz=-1; tdz<=1; ++tdz)
00724 {
00725 if (tdx==0 && tdy==0 && tdz==0)
00726 continue;
00727 if (n>=1)
00728 {
00729 if ((abs(tdx) + abs(tdy) + abs(tdz))!=1)
00730 continue;
00731 if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0)
00732 continue;
00733 }
00734 int3 n_point(tdx,tdy,tdz);
00735 neighborhoods_[n][direction_number].push_back(n_point);
00736 }
00737 }
00738 }
00739
00740 }
00741 }
00742 }
00743 }
00744
00745
00746
00747 }
00748
00749 int SignedPropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00750 {
00751 return (dx+1)*9 + (dy+1)*3 + dz+1;
00752 }
00753
00754
00755 }