Go to the documentation of this file.00001
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
00035
00036
00037
00038 #include <dynamicEDT3D/dynamicEDT3D.h>
00039
00040 #include <iostream>
00041 #include <stdlib.h>
00042
00043 int main( int , char** ) {
00044
00045
00046 int sizeX, sizeY, sizeZ;
00047 sizeX=100;
00048 sizeY=100;
00049 sizeZ=100;
00050
00051 bool*** map;
00052 map = new bool**[sizeX];
00053 for(int x=0; x<sizeX; x++){
00054 map[x] = new bool*[sizeY];
00055 for(int y=0; y<sizeY; y++){
00056 map[x][y] = new bool[sizeZ];
00057 for(int z=0; z<sizeZ; z++){
00058 if(x<2 || x > sizeX-3 || y < 2 || y > sizeY-3 || z<2 || z > sizeZ-3)
00059 map[x][y][z] = 1;
00060 else
00061 map[x][y][z] = 0;
00062 }
00063 }
00064 }
00065
00066 map[51][45][67] = 1;
00067 map[50][50][68] = 1;
00068
00069
00070 int maxDistInCells = 20;
00071 DynamicEDT3D distmap(maxDistInCells*maxDistInCells);
00072 distmap.initializeMap(sizeX, sizeY, sizeZ, map);
00073
00074
00075 distmap.update();
00076
00077
00078 int numPoints = 20;
00079 for (int frame=1; frame<=10; frame++) {
00080 std::cout<<"\n\nthis is frame #"<<frame<<std::endl;
00081 std::vector<IntPoint3D> newObstacles;
00082 for (int i=0; i<numPoints; i++) {
00083 double x = 2+rand()/(double)RAND_MAX*(sizeX-4);
00084 double y = 2+rand()/(double)RAND_MAX*(sizeY-4);
00085 double z = 2+rand()/(double)RAND_MAX*(sizeZ-4);
00086 newObstacles.push_back(IntPoint3D(x,y,z));
00087 }
00088
00089
00090 distmap.exchangeObstacles(newObstacles);
00091
00092
00093 distmap.update();
00094
00095
00096 float dist = distmap.getDistance(30,67,33);
00097 int distSquared = distmap.getSQCellDistance(30,67,33);
00098 std::cout<<"distance at 30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
00099 if(distSquared == maxDistInCells*maxDistInCells)
00100 std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
00101
00102
00103 IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
00104 if(closest.x == DynamicEDT3D::invalidObstData)
00105 std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
00106 else
00107 std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
00108
00109 }
00110
00111
00112 std::cout<<"\n\nthis is the last frame"<<std::endl;
00113
00114
00115 std::vector<IntPoint3D> empty;
00116 distmap.exchangeObstacles(empty);
00117 distmap.update();
00118
00119
00120
00121 float dist = distmap.getDistance(30,67,33);
00122 int distSquared = distmap.getSQCellDistance(30,67,33);
00123 std::cout<<"distance at 30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
00124 if(distSquared == maxDistInCells*maxDistInCells)
00125 std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
00126
00127
00128 IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
00129 if(closest.x == DynamicEDT3D::invalidObstData)
00130 std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
00131 else
00132 std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
00133
00134 return 0;
00135 }