33 template<
typename VolumeType>
37 if(volData->getEnclosingRegion().containsPoint(v3dPos) ==
false)
48 template<
typename VolumeType>
54 template<
typename VolumeType>
63 m_params.result->clear();
66 AllNodesContainer::iterator startNode = allNodes.insert(
Node(m_params.start.getX(), m_params.start.getY(), m_params.start.getZ())).first;
67 AllNodesContainer::iterator endNode = allNodes.insert(
Node(m_params.end.getX(), m_params.end.getY(), m_params.end.getZ())).first;
74 Node* tempStart =
const_cast<Node*
>(&(*startNode));
76 tempStart->
hVal = computeH(startNode->position, endNode->position);
78 Node* tempEnd =
const_cast<Node*
>(&(*endNode));
81 openNodes.insert(startNode);
83 float fDistStartToEnd = (endNode->position - startNode->position).length();
85 if(m_params.progressCallback)
87 m_params.progressCallback(m_fProgress);
90 while((openNodes.empty() ==
false) && (openNodes.getFirst() != endNode))
93 current = openNodes.getFirst();
94 openNodes.removeFirst();
95 closedNodes.insert(current);
98 if(m_params.progressCallback)
100 const float fMinProgresIncreament = 0.001f;
101 float fDistCurrentToEnd = (endNode->position - current->position).length();
102 float fDistNormalised = fDistCurrentToEnd / fDistStartToEnd;
103 float fProgress = 1.0f - fDistNormalised;
104 if(fProgress >= m_fProgress + fMinProgresIncreament)
106 m_fProgress = fProgress;
107 m_params.progressCallback(m_fProgress);
112 const float fFaceCost =
sqrt_1;
113 const float fEdgeCost =
sqrt_2;
114 const float fCornerCost =
sqrt_3;
118 switch(m_params.connectivity)
153 if(allNodes.size() > m_params.maxNumberOfNodes)
161 if((openNodes.empty()) || (openNodes.getFirst() != endNode))
173 Node* n =
const_cast<Node*
>(&(*endNode));
176 m_params.result->push_front(n->
position);
181 if(m_params.progressCallback)
183 m_params.progressCallback(1.0f);
187 template<
typename VolumeType>
190 bool bIsVoxelValidForPath = m_params.isVoxelValidForPath(m_params.volume, neighbourPos);
191 if(!bIsVoxelValidForPath)
196 float cost = neighbourGVal;
198 std::pair<AllNodesContainer::iterator, bool> insertResult = allNodes.insert(Node(neighbourPos.
getX(), neighbourPos.
getY(), neighbourPos.
getZ()));
199 AllNodesContainer::iterator neighbour = insertResult.first;
201 if(insertResult.second ==
true)
203 Node* tempNeighbour =
const_cast<Node*
>(&(*neighbour));
204 tempNeighbour -> hVal = computeH(neighbour->position, m_params.end);
208 if(openIter != openNodes.end())
210 if(cost < neighbour->gVal)
212 openNodes.remove(openIter);
213 openIter = openNodes.end();
219 if(closedIter != closedNodes.end())
221 if(cost < neighbour->gVal)
224 closedNodes.remove(closedIter);
225 closedIter = closedNodes.end();
229 if((openIter == openNodes.end()) && (closedIter == closedNodes.end()))
236 Node* temp =
const_cast<Node*
>(&(*neighbour));
238 openNodes.insert(neighbour);
239 temp->parent =
const_cast<Node*
>(&(*current));
243 template<
typename VolumeType>
247 uint32_t faceSteps = std::abs(a.getX()-b.getX()) + std::abs(a.getY()-b.getY()) + std::abs(a.getZ()-b.getZ());
249 return faceSteps * 1.0f;
252 template<
typename VolumeType>
259 return SixConnectedCost(a,b);
262 template<
typename VolumeType>
268 array[0] = std::abs(a.getX() - b.getX());
269 array[1] = std::abs(a.getY() - b.getY());
270 array[2] = std::abs(a.getZ() - b.getZ());
275 std::sort(&array[0], &array[3]);
278 uint32_t edgeSteps = array[1] - array[0];
279 uint32_t faceSteps = array[2] - array[1];
284 template<
typename VolumeType>
289 switch(m_params.connectivity)
292 hVal = TwentySixConnectedCost(a, b);
295 hVal = EighteenConnectedCost(a, b);
298 hVal = SixConnectedCost(a, b);
306 POLYVOX_ASSERT((a-b).length() <= TwentySixConnectedCost(a,b),
"A* heuristic error.");
307 POLYVOX_ASSERT(TwentySixConnectedCost(a,b) <= EighteenConnectedCost(a,b),
"A* heuristic error.");
308 POLYVOX_ASSERT(EighteenConnectedCost(a,b) <= SixConnectedCost(a,b),
"A* heuristic error.");
311 hVal *= m_params.hBias;
324 uint32_t aX = (a.getX() << 16) & 0x00FF0000;
325 uint32_t aY = (a.getY() << 8) & 0x0000FF00;
326 uint32_t aZ = (a.getZ() ) & 0x000000FF;
327 uint32_t hashVal = hash(aX | aY | aZ);
330 hashVal &= 0x0000FFFF;
331 float fHash = hashVal / 1000000.0f;
340 template<
typename VolumeType>
343 a = (a+0x7ed55d16) + (a<<12);
344 a = (a^0xc761c23c) ^ (a>>19);
345 a = (a+0x165667b1) + (a<<5);
346 a = (a+0xd3a2646c) ^ (a<<9);
347 a = (a+0xfd7046c5) + (a<<3);
348 a = (a^0xb55a4f09) ^ (a>>16);