Hello,
I'm trying to implement A-Star Pahtfinding on 3D enviroment (triangles).
I have a set of triangles (a triangle represents a NODE) with connections between them (neighbours) all setup and ok.
Now, to make it more faster i've created a NxN matrice in wich i have precalculated distances between center's of the triangles (distances between nodes, i.e cost) and stored these distance in this "matrice" i've mentioned. (it is the 3d distance between two 3D points).
Here is this function wich precalculates all the distances between nodes:
(all working ok until now...)
void CAStarSearch::PrecalculateMatrix()
{
// get number of nodes from list
int counter = Oct.m_NavNodeList.size();
// allocate "NxN" grid of floats.
this->matrix = (float **)malloc(sizeof(float) * counter );
for(WORD x = 0; x<counter; x++)
{
matrix[x] = (float *)malloc(sizeof(float) * counter );
Oct.m_NavNodeList[x].m_ID = x;
}
// loop thru this grid of float's.
// for every column
for(WORD b=0; b<counter; b++)
{
// for every row
for(WORD a=0; a<counter; a++)
{
// if a==b this means that distance between nodes is zero (i.e same node).
// else the "matrix" will be filled with distances between nodes.
if(a == b) matrix[a] = 0;
else
// compute distance between nodes
matrix[a] = Oct.m_NavNodeList[a].m_Center.GetDistance( Oct.m_NavNodeList.m_Center );
}
}
// DisplayMatrix();
}
OK, now after that i'm trying to do 3d Pathfinding like so:
(i've commented the lines to be more readable)
1) getting the G value (cost from ENTRY to current NODE):
float CAStarSearch::GetG(CNavNode* last_closednode, CNavNode *node)
{
// return distance from last_closednode to node ( and cumulate this in "ClosedListG" variabile. )
return ClosedListG += this->GetMatrixValue( last_closednode, node);
}
NOTE: i keep track and store in a global "ClosedListG" variable the SUM of distances starting grom ENTRY to current NODE. (ClosedListG = the distance from ENTRY to current node )
2) getting the H value (cost from current NODE to EXIT node):
float CAStarSearch::GetH(CNavNode* current_opennode, CNavNode *target)
{
// return distance between current_opennode and target node.
return this->GetMatrixValue( current_opennode, target);
}
NOTE: this also just returns distance from "current_opennode" to "target" node looking up in "matrix" table .
3) given a (open) list of nodes , select the one with lowest cost "F" ,
assuming that all nodes in this list have F, G and H values calculated
already:
CNavNode* CAStarSearch::GetLowestF(CNavNode *target)
{
list<CNavNode*>::iterator next = m_OpenList.begin();
// select first node on open.
CNavNode* SelectedNode = *next;
// loop thru all nodes in OPEN list.
for(list<CNavNode*>::iterator it = m_OpenList.begin(); it!=m_OpenList.end(); ++it)
{
// chosen = current item in open.
CNavNode* chosen = *it;
// get lowest value here.
if( chosen->m_F < SelectedNode->m_F)
{
// select this one because it has a LOW value than previous one.
SelectedNode = chosen;
}
}
return SelectedNode;
}
4) the A-Star seach function now:
void CAStarSearch::void CAStarSearch::FindPath(CVector3 *Start, CVector3 *Target)
{
// get current NODE for entry, result is stored in "entry" variable (entry = start position in nodes list)
CNavNode entry;
bool foundEntry = false;
Oct.FindNode(*Start, Oct.m_pRoot , entry , foundEntry );
// get current NODE for exit, result is stored in "exit" variable (exit = goal in nodes list)
CNavNode exit;
bool foundExit = false;
Oct.FindNode(*Target, Oct.m_pRoot , exit , foundExit );
// GetMatrixValue returns distance between 2 nodes. (all values have been precalculaded before
// entering this function - and stored in a "N x N" grid (of floats).
if(this->GetMatrixValue(&entry,&exit)==0) return;
// clear CLOSED list and OPEN list.
m_ClosedList.clear();
m_OpenList.clear();
// ClosedListG = path from ENTRY point to CURRENT node.
// (it is the G value from A-star, and represents distance from ENTRY point node to CURRENT node.
// For now ClosedListG is 0 ( it's value is computed and kept track in GetG() function )
this->ClosedListG = 0;
// add ENTRY yo CLOSED list.
m_ClosedList.push_back(&entry);
// add ENTRY's CONNECTIONS to OPEN list.
for(BYTE c=0; c<entry.m_NoConnections; c++)
{
entry.pConnection[c]->m_G = GetG(&entry, entry.pConnection[c]);
entry.pConnection[c]->m_H = GetH(entry.pConnection[c], &exit);
entry.pConnection[c]->m_F = entry.pConnection[c]->m_G + entry.pConnection[c]->m_H;
m_OpenList.push_back(entry.pConnection[c]);
}
// as long as OPEN list is NOT empty (0).
while(!m_OpenList.empty())
{
// select a NODE with lowest F value from OPEN list.
CNavNode *current = this->GetLowestF(&exit);
// have we reached exit (goal node) ??
if(this->GetMatrixValue(current ,&exit)==0) break;
else
{
// add current NODE to CLOSED list.
m_ClosedList.push_back(current);
// remove current NODE from OPEN list.
m_OpenList.remove(current);
// for every CONNECTION in current NODE.
for(BYTE c=0; c<current->m_NoConnections; c++)
{
// does it exist in OPEN list ? if YES -> skip
if(Exist(current->pConnection[c], m_OpenList)) continue;
// does it exist in CLOSED list ? if YES -> skip
if(Exist(current->pConnection[c], m_ClosedList)) continue;
// compute current's CONNECTION's -> F,G,H values.
current->pConnection[c]->m_G = GetG(current, current->pConnection[c]);
current->pConnection[c]->m_H = GetH(current->pConnection[c], &exit);
current->pConnection[c]->m_F = current->pConnection[c]->m_G + current->pConnection[c]->m_H;
// add above selected (low score) node to CLOSED list.
m_OpenList.push_back( current->pConnection[c] );
}
}
}
// used "nodez" variable here - just to display how many nodes were found when reached goal node.
extern int nodez;
nodez = m_ClosedList.size();
// draw all nodes in CLOSED list.
DrawClosedList();
}
NOTE: i call "FindPath" function every frame !
5) and last function that checks if a node exists in list:
bool CAStarSearch::Exist( CNavNode *test_node, list<CNavNode *>lst )
{
// test if empty list
if(lst.size()==0) return false;
list<CNavNode*>::iterator it = lst.begin();
// loop thru all nodes in "lst"
while(it!=lst.end())
{
// check if equal here , if YES return true.
CNavNode *chosen = *it;
if(chosen == test_node) return true;
it++;
}
return false;
}
PROBLEMS:
1) Search time (from ENTRY to EXIT) is slow (if m_ClosedList has more than 3 to max 50 nodes - the FPS drops from 1400fps to 60fps). Is it because of the STL list functions wich are somekind slowing things down ?? What i'm doing wrong ?
2) Don't know if i'm using the correct algorithm/function for calculating H value (i.e. GetH()) . I just used DISTANCES from a node to another node for calculating this value (and stored them in "matrix" , i.e float distances between nodes).
3) I think i'm doing something wrong in A-Star Search Function because
it seems that "m_ClosedList" ends with some nodes that DONT belong to path (this i think is due to the H value i've mentioned above).
I know i've posted a lot of code and i appoligise for that !!
(i could upload a screenshot but i dont know how...)
Thank you , any sugestions will be apreciated. :)
[Edited by - redbaseone on December 26, 2009 2:25:19 PM]
