00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 namespace PolyVox
00025 {
00031 template< template<typename> class VolumeType, typename VoxelType>
00032 bool aStarDefaultVoxelValidator(const VolumeType<VoxelType>* volData, const Vector3DInt32& v3dPos)
00033 {
00034
00035 if(volData->getEnclosingRegion().containsPoint(v3dPos) == false)
00036 {
00037 return false;
00038 }
00039
00040
00041 VoxelType voxel = volData->getVoxelAt(v3dPos);
00042 if(voxel.getDensity() >= VoxelType::getThreshold())
00043 {
00044 return false;
00045 }
00046
00047 return true;
00048 }
00049
00051
00053 template< template<typename> class VolumeType, typename VoxelType>
00054 AStarPathfinder<VolumeType, VoxelType>::AStarPathfinder(const AStarPathfinderParams<VolumeType, VoxelType>& params)
00055 :m_params(params)
00056 {
00057 }
00058
00059 template< template<typename> class VolumeType, typename VoxelType>
00060 void AStarPathfinder<VolumeType, VoxelType>::execute()
00061 {
00062
00063 allNodes.clear();
00064 openNodes.clear();
00065 closedNodes.clear();
00066
00067
00068 m_params.result->clear();
00069
00070
00071 AllNodesContainer::iterator startNode = allNodes.insert(Node(m_params.start.getX(), m_params.start.getY(), m_params.start.getZ())).first;
00072 AllNodesContainer::iterator endNode = allNodes.insert(Node(m_params.end.getX(), m_params.end.getY(), m_params.end.getZ())).first;
00073
00074
00075
00076
00077
00078
00079 Node* tempStart = const_cast<Node*>(&(*startNode));
00080 tempStart->gVal = 0;
00081 tempStart->hVal = computeH(startNode->position, endNode->position);
00082
00083 Node* tempEnd = const_cast<Node*>(&(*endNode));
00084 tempEnd->hVal = 0.0f;
00085
00086 openNodes.insert(startNode);
00087
00088 float fDistStartToEnd = (endNode->position - startNode->position).length();
00089 m_fProgress = 0.0f;
00090 if(m_params.progressCallback)
00091 {
00092 m_params.progressCallback(m_fProgress);
00093 }
00094
00095 while((openNodes.empty() == false) && (openNodes.getFirst() != endNode))
00096 {
00097
00098 current = openNodes.getFirst();
00099 openNodes.removeFirst();
00100 closedNodes.insert(current);
00101
00102
00103 if(m_params.progressCallback)
00104 {
00105 const float fMinProgresIncreament = 0.001f;
00106 float fDistCurrentToEnd = (endNode->position - current->position).length();
00107 float fDistNormalised = fDistCurrentToEnd / fDistStartToEnd;
00108 float fProgress = 1.0f - fDistNormalised;
00109 if(fProgress >= m_fProgress + fMinProgresIncreament)
00110 {
00111 m_fProgress = fProgress;
00112 m_params.progressCallback(m_fProgress);
00113 }
00114 }
00115
00116
00117 const float fFaceCost = sqrt_1;
00118 const float fEdgeCost = sqrt_2;
00119 const float fCornerCost = sqrt_3;
00120
00121
00122
00123 switch(m_params.connectivity)
00124 {
00125 case TwentySixConnected:
00126 processNeighbour(current->position + arrayPathfinderCorners[0], current->gVal + fCornerCost);
00127 processNeighbour(current->position + arrayPathfinderCorners[1], current->gVal + fCornerCost);
00128 processNeighbour(current->position + arrayPathfinderCorners[2], current->gVal + fCornerCost);
00129 processNeighbour(current->position + arrayPathfinderCorners[3], current->gVal + fCornerCost);
00130 processNeighbour(current->position + arrayPathfinderCorners[4], current->gVal + fCornerCost);
00131 processNeighbour(current->position + arrayPathfinderCorners[5], current->gVal + fCornerCost);
00132 processNeighbour(current->position + arrayPathfinderCorners[6], current->gVal + fCornerCost);
00133 processNeighbour(current->position + arrayPathfinderCorners[7], current->gVal + fCornerCost);
00134
00135 case EighteenConnected:
00136 processNeighbour(current->position + arrayPathfinderEdges[ 0], current->gVal + fEdgeCost);
00137 processNeighbour(current->position + arrayPathfinderEdges[ 1], current->gVal + fEdgeCost);
00138 processNeighbour(current->position + arrayPathfinderEdges[ 2], current->gVal + fEdgeCost);
00139 processNeighbour(current->position + arrayPathfinderEdges[ 3], current->gVal + fEdgeCost);
00140 processNeighbour(current->position + arrayPathfinderEdges[ 4], current->gVal + fEdgeCost);
00141 processNeighbour(current->position + arrayPathfinderEdges[ 5], current->gVal + fEdgeCost);
00142 processNeighbour(current->position + arrayPathfinderEdges[ 6], current->gVal + fEdgeCost);
00143 processNeighbour(current->position + arrayPathfinderEdges[ 7], current->gVal + fEdgeCost);
00144 processNeighbour(current->position + arrayPathfinderEdges[ 8], current->gVal + fEdgeCost);
00145 processNeighbour(current->position + arrayPathfinderEdges[ 9], current->gVal + fEdgeCost);
00146 processNeighbour(current->position + arrayPathfinderEdges[10], current->gVal + fEdgeCost);
00147 processNeighbour(current->position + arrayPathfinderEdges[11], current->gVal + fEdgeCost);
00148
00149 case SixConnected:
00150 processNeighbour(current->position + arrayPathfinderFaces[0], current->gVal + fFaceCost);
00151 processNeighbour(current->position + arrayPathfinderFaces[1], current->gVal + fFaceCost);
00152 processNeighbour(current->position + arrayPathfinderFaces[2], current->gVal + fFaceCost);
00153 processNeighbour(current->position + arrayPathfinderFaces[3], current->gVal + fFaceCost);
00154 processNeighbour(current->position + arrayPathfinderFaces[4], current->gVal + fFaceCost);
00155 processNeighbour(current->position + arrayPathfinderFaces[5], current->gVal + fFaceCost);
00156 }
00157
00158 if(allNodes.size() > m_params.maxNumberOfNodes)
00159 {
00160
00161
00162 break;
00163 }
00164 }
00165
00166 if((openNodes.empty()) || (openNodes.getFirst() != endNode))
00167 {
00168
00169 throw std::runtime_error("No path found");
00170 }
00171 else
00172 {
00173
00174
00175
00176
00177
00178 Node* n = const_cast<Node*>(&(*endNode));
00179 while(n != 0)
00180 {
00181 m_params.result->push_front(n->position);
00182 n = n->parent;
00183 }
00184 }
00185
00186 if(m_params.progressCallback)
00187 {
00188 m_params.progressCallback(1.0f);
00189 }
00190 }
00191
00192 template< template<typename> class VolumeType, typename VoxelType>
00193 void AStarPathfinder<VolumeType, VoxelType>::processNeighbour(const Vector3DInt32& neighbourPos, float neighbourGVal)
00194 {
00195 bool bIsVoxelValidForPath = m_params.isVoxelValidForPath(m_params.volume, neighbourPos);
00196 if(!bIsVoxelValidForPath)
00197 {
00198 return;
00199 }
00200
00201 float cost = neighbourGVal;
00202
00203 std::pair<AllNodesContainer::iterator, bool> insertResult = allNodes.insert(Node(neighbourPos.getX(), neighbourPos.getY(), neighbourPos.getZ()));
00204 AllNodesContainer::iterator neighbour = insertResult.first;
00205
00206 if(insertResult.second == true)
00207 {
00208 Node* tempNeighbour = const_cast<Node*>(&(*neighbour));
00209 tempNeighbour -> hVal = computeH(neighbour->position, m_params.end);
00210 }
00211
00212 OpenNodesContainer::iterator openIter = openNodes.find(neighbour);
00213 if(openIter != openNodes.end())
00214 {
00215 if(cost < neighbour->gVal)
00216 {
00217 openNodes.remove(openIter);
00218 openIter = openNodes.end();
00219 }
00220 }
00221
00222
00223 ClosedNodesContainer::iterator closedIter = closedNodes.find(neighbour);
00224 if(closedIter != closedNodes.end())
00225 {
00226 if(cost < neighbour->gVal)
00227 {
00228
00229 closedNodes.remove(closedIter);
00230 closedIter = closedNodes.end();
00231 }
00232 }
00233
00234 if((openIter == openNodes.end()) && (closedIter == closedNodes.end()))
00235 {
00236
00237
00238
00239
00240
00241 Node* temp = const_cast<Node*>(&(*neighbour));
00242 temp->gVal = cost;
00243 openNodes.insert(neighbour);
00244 temp->parent = const_cast<Node*>(&(*current));
00245 }
00246 }
00247
00248 template< template<typename> class VolumeType, typename VoxelType>
00249 float AStarPathfinder<VolumeType, VoxelType>::SixConnectedCost(const Vector3DInt32& a, const Vector3DInt32& b)
00250 {
00251
00252 uint32_t faceSteps = abs(a.getX()-b.getX()) + abs(a.getY()-b.getY()) + abs(a.getZ()-b.getZ());
00253
00254 return faceSteps * 1.0f;
00255 }
00256
00257 template< template<typename> class VolumeType, typename VoxelType>
00258 float AStarPathfinder<VolumeType, VoxelType>::EighteenConnectedCost(const Vector3DInt32& a, const Vector3DInt32& b)
00259 {
00260
00261
00262
00263
00264 return SixConnectedCost(a,b);
00265 }
00266
00267 template< template<typename> class VolumeType, typename VoxelType>
00268 float AStarPathfinder<VolumeType, VoxelType>::TwentySixConnectedCost(const Vector3DInt32& a, const Vector3DInt32& b)
00269 {
00270
00271
00272 uint32_t array[3];
00273 array[0] = abs(a.getX() - b.getX());
00274 array[1] = abs(a.getY() - b.getY());
00275 array[2] = abs(a.getZ() - b.getZ());
00276
00277
00278
00279
00280 std::sort(&array[0], &array[3]);
00281
00282 uint32_t cornerSteps = array[0];
00283 uint32_t edgeSteps = array[1] - array[0];
00284 uint32_t faceSteps = array[2] - array[1];
00285
00286 return cornerSteps * sqrt_3 + edgeSteps * sqrt_2 + faceSteps * sqrt_1;
00287 }
00288
00289 template< template<typename> class VolumeType, typename VoxelType>
00290 float AStarPathfinder<VolumeType, VoxelType>::computeH(const Vector3DInt32& a, const Vector3DInt32& b)
00291 {
00292 float hVal;
00293
00294 switch(m_params.connectivity)
00295 {
00296 case TwentySixConnected:
00297 hVal = TwentySixConnectedCost(a, b);
00298 break;
00299 case EighteenConnected:
00300 hVal = EighteenConnectedCost(a, b);
00301 break;
00302 case SixConnected:
00303 hVal = SixConnectedCost(a, b);
00304 break;
00305 default:
00306 assert(false);
00307 }
00308
00309
00310
00311 assert((a-b).length() <= TwentySixConnectedCost(a,b));
00312 assert(TwentySixConnectedCost(a,b) <= EighteenConnectedCost(a,b));
00313 assert(EighteenConnectedCost(a,b) <= SixConnectedCost(a,b));
00314
00315
00316 hVal *= m_params.hBias;
00317
00318
00319
00320
00321
00322
00323 polyvox_hash<uint32_t> uint32Hash;
00324 uint32_t hashValX = uint32Hash(a.getX());
00325 uint32_t hashValY = uint32Hash(a.getY());
00326 uint32_t hashValZ = uint32Hash(a.getZ());
00327 uint32_t hashVal = hashValX ^ hashValY ^ hashValZ;
00328
00329 hashVal &= 0x0000FFFF;
00330 float fHash = hashVal / 1000000.0f;
00331
00332
00333 hVal += fHash;
00334 return hVal;
00335 }
00336 }