SearchCell.

#pragma once
#include <math.h>

#define WORLD_SIZE 64

struct SearchCell
{
pubic:
int m_xcoord, m_zcoord;
int m_id;
SearchCell *parent;
float G;
float H;

SearchCell() : parent(0) {}
SearchCell (int x, int z, SearchCell *_parent = 0: m_xcoord(x), m_zcoord(z),
parent(_parent), m_id(z * WORLD_SIZE + z), G(0), H(0) {};

float GetF() {
return G + H;
}
float ManHattanDistnce (SearchCell *nodeEnd)
{
float x = (float) (fabs(this->m_xcoord - nodeEnd->m_xcoord));
float x = (float) (fabs(this->m_zcoord - nodeEnd->m_zcoord));

return x+z;
}
};

=========================================

PathFinding.h

#pragma once
#include "Vector3.h"
#include "SearchCell.h"
#include <vector>
class PathFinding
{
public:
PathFinding (void);
~PathFinding(void);

boid FindPath(Vecor3currentPos, Vector3 targetPos);

Vetor3 NextPathPos();
void ClearOpenList() {
m_openList.clear(); }
void ClearVisitedList() { m_visitedList.clear(); }
void ClearPathToGoal() { m_pathToGoal.clear(); }
bool m_intializedStrtGoal;
bool m_foundGoal;

private:
void SetStartAndGoal(SearchCell start, SearchCell goal);
void PathOpened (int x, int z, float newCost, SearchCell *parent);
 SearchCell *GetNextCell ();
void ContinuePath();

Searchcell *m_startCell;
SearchCell *m_goalCell;
std::vector<SesrchCell*> m_openList;
std::vector<SesrchCell*> m_visitedList;
std::vector<Vecto3*> m_pathToGoal;

};

==================================
PathFinding.cpp

#include "PathFinding.h"

PathFinding::PathFinfing(void)
{
m_intializedStartGoal = false;
m_foundGoal = false;
}
PathFinding::~PathFinding(void)
{
}
void PathFinding::FindPath(Vector3 currentPos, Vector3 targetPos)

{
if (!m_initializedStartGoal)
{
for (int i= 0; i<m_openList.size(); i++)
{
delete m_openList[1];
}
m_openList.clear();

for(int i = 0; i<m_visitedList.size() i++)

{
delete m_visitedList[i];
}
m_visitedList.clear();
for (int i = 0; m_pathToGoal.size(); i++)
{
delete m_pathToGoal[i];
}
m_pathToGoal.clear();

//inisialize start
SearchCell start;
start.m_xcoord = currentPos.x;
start.m_zcoord = currentPos.z;

//inisialize goal
SearchCell goal;
goal.m_xcoord = targetPos.x;
goal.m_zcoord = targetPos.z;

setStartAndGoal (start, goal);

m_initializedStartGoal = true;
}
if(m_initializedStartGoal)
{
continuePath();
}
}
void PathFinding::SetStartAndGoal (SearchCell start, SearchCell goal)
{
m_startCell = new SearchCel (start.m_xcoord, start.m_zcoord, NULL);
m_goalCell = new SearchCell(goal.m_xcoord, goal.m_zcoord, &goal);

m_startCell->G = 0;
m_startCell->H = m_startCell->ManHattanDistnce (m_goalCell);
m_startCell->parent = 0;

m_opentList.push_back(m_startCell);
}

SearchCell* PathFindinf::GetNextCell()
{
float bestF = 999999.0f;
int cellIndex = 1;
searchCell* nextCell = NULL;

for(int i = 0; i < m_openList.Size(); i++)

{
if(m_openList[i]->GetF() < bestF)
{
bestF = m_openList[i]->GetF();
cellIndex = i;
}
}
if(cellIndex >= 0)
{
nextCell = m_openList[cellIndex];
m_visitedList.push_back(nextCell);
m_openList.erase(m_openList.begin() + cellIndex);
}
return nextCell;
}

void PathFinding::PathOpened(int x, int z, float newCost, SearchCell *parent)

{
/*if(CELL_BLOKED)
{
RETURN;
}*/

int id = z * WORD_SIZE + x;
for(int i = 0; i< m_visitedList.size(); i++)
{
if(id == m_visitedList[i]->m_id)
{
return;
}
}

SearchCell* newChild = new SearchCell(x, z, parent);

newChild->G = newCost;
newChild->H = parent->ManhattanDistnce(m_goalCell);
for(int i = 0; i<m_openList.size(); i++)
{
if(id == m_openList[i]->m_id)
{
float newF = newChild->G + newCost +m_openList[i]->H;

if(m_openList[i]->GetF() > newF)

{
m_openList[i]->G = newChild->G + newCost;
m_openList[i]->parent = newChild;
}
else //if the F is not better
{
delete newChild;
return;
}
}
}
m_openList.push_back(newChild);
}

void PathFinding::ContinuePath()
{
if(m_openList.empty())
{
return;
}

SearchCell* currentCell = GetNextCell();

if(currenCell->m_id == m_goalCell->m_id)
{
m_goalCell->parent = currenCell->parent;

SearchCell*getPath;

for(getpath = m_goalCell; getPath !=NULL; getPath = getPath->parent)

{
m_pathToGoal.push_back(new Vector3(getPath->m_xcoord, 0, getPath->m_zcoord));
}

m_foundGoal = true;
return;
}
else
{
//righSude
PathOpened(currentCell->m_xcoord + 1, currentCell->m_zcoord, currentCell->G + 1,
currentCell);
//leftSide
PathOpened(currentCell->m_xcoord - 1, currentCell->m_zcoord, currentCell->G + 1,
currentCell);
//Up
PathOpened(currentCell->m_xcoord, currentCell->m_zcoord + 1, currentCell->G + 1,
currentCell);
//Down
PathOpened(currentCell->m_xcoord, currentCell->m_zcoord - 1, currentCell->G + 1,
currentCell);
//left-up diagonal
PathOpened(currentCell->m_xcoord - 1, currentCell->m_zcoord + 1, currentCell->G +
1.414f, currentCell);
//right-up diagonal
PathOpened(currentCell->m_xcoord + 1, currentCell->m_zcoord + 1, currentCell->G +
1.414f, currentCell);
//left-down diagonal
PathOpened(currentCell->m_xcoord - 1, currentCell->m_zcoord - 1, currentCell->G +
1.414f, currentCell);
//right-down diagonal
PathOpened(currentCell->m_xcoord + 1, currentCell->m_zcoord + 1, currentCell->G +
1.414f, currentCell);

for(int i = 0; i< m_openList.size(); i++)

{
if(currentCell->m_id == m_openList[i]->m_id)
{
m_openList.erase(m_openList.begin() + i);
}
}
}
}
Vector3 PathFinding::NextPathPos(AI _ai)
{
int index = 1;

Vector3 nextPos;
nextPos.x = m_pathToGoal[m_pathTogoal.size() - index]->x;
nextPos.z = m_pathToGoal[m_pathTogoal.size() - index]->z;

Vector3 distance = nextPos - _ai.pos;

if(index < m_pathToGoal.size())

{
if(distance.Length() < _ai.radius)
{
m_pathToGoal.erase(m_pathToGoal.end() - index);
}
}
return nextPos;
}