int NumElemFromPoint(MacroMesh *m, real *xphy, real *xref0)
{
int num = -1;
int ino = NearestNode(m, xphy);
real xref[3];
// TO DO: remove nodes that do not belong to any element
assert(m->node2elem[0 + m->max_node2elem * ino] != -1);
int ii = 0;
while(m->node2elem[ii + m->max_node2elem * ino] != -1) {
int ie = m->node2elem[ii + m->max_node2elem * ino];
if(IsInElem(m, ie, xphy, xref)) {
if(xref0 != NULL){
xref0[0] = xref[0];
xref0[1] = xref[1];
xref0[2] = xref[2];
}
return ie;
}
ii++;
}
return num;
}
void Pathfinder::UpdatePath() {
//need to set all parents to null before recalculating the path.
//if not, there's an infinite loop in BuildPath()
for (std::vector<PathNode>::iterator itr = m_nodes.begin(); itr != m_nodes.end(); ++itr) {
itr->SetParent(nullptr);
}
//set StartNode and EndNode
m_startNode = NearestNode(m_StartPosition);
m_endNode = NearestNode(m_EndPosition);
//recalculate all estimated costs
int16_t cost = 0;
PathNode * node;
for (std::vector<PathNode>::iterator itr = m_nodes.begin(); itr != m_nodes.end(); ++itr) {
node = &(*itr);
if (node->GetCost() != -1) {
//estimated based on pixels distance
float cost = node->GetCost();
float estimated = (sqrt(pow((m_endNode->GetPos().mX - node->GetPos().mX), 2)
+ pow(m_endNode->GetPos().mY - node->GetPos().mY, 2)));
node->SetEstimatedCost(estimated);
node->SetCost(cost + estimated);
node->SetTotalCost(cost + estimated);
}
}
//A*
m_openNodes.clear();
m_closedNodes.clear();
m_startNode->SetCost(0);
m_startNode->SetTotalCost(0);
m_openNodes.push_back(m_startNode);
bool objectiveFound = false;
while (!m_openNodes.empty() && !objectiveFound) {
PathNode * node = *(m_openNodes.begin());
m_openNodes.erase(m_openNodes.begin());
if (node->GetPos().mX == m_endNode->GetPos().mX
&& node->GetPos().mY == m_endNode->GetPos().mY) {
BuildPath(node);
} else {
for (std::vector<Polygon::Edge>::iterator itr = node->GetPolygon()->m_edges.begin();
itr != node->GetPolygon()->m_edges.end(); ++itr) {
printf("EDGE: %d - %d\n", node->GetPolygon()->m_id, itr->m_neighbour->m_id);
PathNode * nextNode = nullptr;
for (std::vector<PathNode>::iterator pathNodeItr = m_nodes.begin();
pathNodeItr != m_nodes.end(); ++pathNodeItr) {
if (pathNodeItr->GetPolygon()->m_id == itr->m_neighbour->m_id) {
nextNode = &(*pathNodeItr);
}
}
if (nextNode == m_endNode) {
nextNode->SetParent(node);
nextNode->SetTotalCost(node->GetTotalCost() + nextNode->GetCost());
BuildPath(nextNode);
objectiveFound = true;
break;
}
if (nextNode->GetCost() != -1) {
if (node == nextNode) {
continue;
} else if (find(m_closedNodes.begin(), m_closedNodes.end(), nextNode)
!= m_closedNodes.end()) {
continue;
} else if (find(m_openNodes.begin(), m_openNodes.end(), nextNode)
!= m_openNodes.end()) {
if (nextNode->GetTotalCost() > 0
&& nextNode->GetTotalCost() >
node->GetTotalCost() + nextNode->GetCost()) {
nextNode->SetTotalCost(node->GetTotalCost() + nextNode->GetCost());
nextNode->SetParent(node);
}
} else {
nextNode->SetParent(node);
m_openNodes.push_back(nextNode);
}
}
}
std::vector<PathNode *>::iterator el = find(m_openNodes.begin(),
m_openNodes.end(), node);
if (el != m_openNodes.end()) {
m_openNodes.erase(el);
}
m_closedNodes.push_back(node);
}
}
}
