31 template<
typename VolumeType>
35 if(volData->getEnclosingRegion().containsPoint(v3dPos) ==
false)
46 template<
typename VolumeType>
52 template<
typename VolumeType>
61 m_params.result->clear();
64 AllNodesContainer::iterator startNode = allNodes.insert(
Node(m_params.start.getX(), m_params.start.getY(), m_params.start.getZ())).first;
65 AllNodesContainer::iterator endNode = allNodes.insert(
Node(m_params.end.getX(), m_params.end.getY(), m_params.end.getZ())).first;
72 Node* tempStart =
const_cast<Node*
>(&(*startNode));
74 tempStart->
hVal = computeH(startNode->position, endNode->position);
76 Node* tempEnd =
const_cast<Node*
>(&(*endNode));
79 openNodes.insert(startNode);
81 float fDistStartToEnd = (endNode->position - startNode->position).length();
83 if(m_params.progressCallback)
85 m_params.progressCallback(m_fProgress);
88 while((openNodes.empty() ==
false) && (openNodes.getFirst() != endNode))
91 current = openNodes.getFirst();
92 openNodes.removeFirst();
93 closedNodes.insert(current);
96 if(m_params.progressCallback)
98 const float fMinProgresIncreament = 0.001f;
99 float fDistCurrentToEnd = (endNode->position - current->position).length();
100 float fDistNormalised = fDistCurrentToEnd / fDistStartToEnd;
101 float fProgress = 1.0f - fDistNormalised;
102 if(fProgress >= m_fProgress + fMinProgresIncreament)
104 m_fProgress = fProgress;
105 m_params.progressCallback(m_fProgress);
110 const float fFaceCost =
sqrt_1;
111 const float fEdgeCost =
sqrt_2;
112 const float fCornerCost =
sqrt_3;
116 switch(m_params.connectivity)
151 if(allNodes.size() > m_params.maxNumberOfNodes)
159 if((openNodes.empty()) || (openNodes.getFirst() != endNode))
162 throw std::runtime_error(
"No path found");
171 Node* n =
const_cast<Node*
>(&(*endNode));
174 m_params.result->push_front(n->
position);
179 if(m_params.progressCallback)
181 m_params.progressCallback(1.0f);
185 template<
typename VolumeType>
188 bool bIsVoxelValidForPath = m_params.isVoxelValidForPath(m_params.volume, neighbourPos);
189 if(!bIsVoxelValidForPath)
194 float cost = neighbourGVal;
196 std::pair<AllNodesContainer::iterator, bool> insertResult = allNodes.insert(Node(neighbourPos.
getX(), neighbourPos.
getY(), neighbourPos.
getZ()));
197 AllNodesContainer::iterator neighbour = insertResult.first;
199 if(insertResult.second ==
true)
201 Node* tempNeighbour =
const_cast<Node*
>(&(*neighbour));
202 tempNeighbour -> hVal = computeH(neighbour->position, m_params.end);
206 if(openIter != openNodes.end())
208 if(cost < neighbour->gVal)
210 openNodes.remove(openIter);
211 openIter = openNodes.end();
217 if(closedIter != closedNodes.end())
219 if(cost < neighbour->gVal)
222 closedNodes.remove(closedIter);
223 closedIter = closedNodes.end();
227 if((openIter == openNodes.end()) && (closedIter == closedNodes.end()))
234 Node* temp =
const_cast<Node*
>(&(*neighbour));
236 openNodes.insert(neighbour);
237 temp->parent =
const_cast<Node*
>(&(*current));
241 template<
typename VolumeType>
245 uint32_t faceSteps = std::abs(a.getX()-b.getX()) + std::abs(a.getY()-b.getY()) + std::abs(a.getZ()-b.getZ());
247 return faceSteps * 1.0f;
250 template<
typename VolumeType>
257 return SixConnectedCost(a,b);
260 template<
typename VolumeType>
266 array[0] = std::abs(a.getX() - b.getX());
267 array[1] = std::abs(a.getY() - b.getY());
268 array[2] = std::abs(a.getZ() - b.getZ());
273 std::sort(&array[0], &array[3]);
275 uint32_t cornerSteps = array[0];
276 uint32_t edgeSteps = array[1] - array[0];
277 uint32_t faceSteps = array[2] - array[1];
282 template<
typename VolumeType>
287 switch(m_params.connectivity)
290 hVal = TwentySixConnectedCost(a, b);
293 hVal = EighteenConnectedCost(a, b);
296 hVal = SixConnectedCost(a, b);
304 assert((a-b).length() <= TwentySixConnectedCost(a,b));
305 assert(TwentySixConnectedCost(a,b) <= EighteenConnectedCost(a,b));
306 assert(EighteenConnectedCost(a,b) <= SixConnectedCost(a,b));
309 hVal *= m_params.hBias;
322 uint32_t aX = (a.getX() << 16) & 0x00FF0000;
323 uint32_t aY = (a.getY() << 8) & 0x0000FF00;
324 uint32_t aZ = (a.getZ() ) & 0x000000FF;
325 uint32_t hashVal = hash(aX | aY | aZ);
328 hashVal &= 0x0000FFFF;
329 float fHash = hashVal / 1000000.0f;
338 template<
typename VolumeType>
339 uint32_t AStarPathfinder<VolumeType>::hash( uint32_t a)
341 a = (a+0x7ed55d16) + (a<<12);
342 a = (a^0xc761c23c) ^ (a>>19);
343 a = (a+0x165667b1) + (a<<5);
344 a = (a+0xd3a2646c) ^ (a<<9);
345 a = (a+0xfd7046c5) + (a<<3);
346 a = (a^0xb55a4f09) ^ (a>>16);