void Grid::AdjacentCost( void* node, std::vector< StateCost > *neighbors )
{
int x, y;
const int dx[8] = { 1, 1, 0, -1, -1, -1, 0, 1 };
const int dy[8] = { 0, 1, 1, 1, 0, -1, -1, -1 };
const float cost[8] = { 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f };
NodeToXY( node, &x, &y );
for( int i=0; i<8; ++i ) {
int nx = x + dx[i];
int ny = y + dy[i];
int pass = Passable( nx, ny );
if ( pass >0 ) {
StateCost nodeCost = { XYToNode( nx, ny ), cost[i] };
neighbors->push_back( nodeCost );
}
else{
// Normal floor
StateCost nodeCost = { XYToNode( nx, ny ), FLT_MAX };
neighbors->push_back( nodeCost );
}
}
}
bool Grid::PassableRect(int minX, int minY, int maxX, int maxY)
{
for(int i=minX; i<=maxX; i++){
for(int j=minY; j<=maxY; j++){
if(Passable(i,j) == 0)
return false;
}
}
return true;
}
int Grid::SetPlayer( float x, float y )
{
int i = RealToInt( x, true );
int j = RealToInt( y, false );
int result = Passable(i , j);
if( result != 0 ){
playerX = i;
playerY = j;
}
return result;
}
int Grid::SetPos( int nx, int ny )
{
int result = MicroPather::NO_SOLUTION;
if ( Passable( nx, ny ) == 1 )
{
float totalCost;
if ( showConsidered )
pather->Reset();
result = pather->Solve( XYToNode( playerX, playerY ), XYToNode( nx, ny ), &path, &totalCost );
if ( result == MicroPather::SOLVED ) {
playerX = nx;
playerY = ny;
pathIt++;
toDeleteList.clear();
//Log* pathLog = LogManager::getSingleton().createLog( "pathDebug_"+LogManager::number(pathIt)+".log", false, true);
//pathLog->logMessage(Print());
ReducePath();
//Log* pathLog2 = LogManager::getSingleton().createLog( "pathReduceDebug_"+LogManager::number(pathIt)+".log", false, true);
//pathLog2->logMessage(Print());
pathCell.clear();
for( int k=0; k<path.size(); ++k ) {
bool toInsert=true;
for(int l=0; l<toDeleteList.size(); l++){
if(k==toDeleteList[l]){
toInsert=false;
break;
}
}
if(toInsert){
int x, y;
NodeToXY( path[k], &x, &y );
Cell_grid c;
c.x = x;
c.y = y;
pathCell.insert( pathCell.end(), c);
}
}
}
}
return result;
}
int Grid::Passable( float x, float y)
{
int xInt = RealToInt( x,true);
int yInt = RealToInt( y,false);
return Passable(xInt, yInt);
}
Node Tools::FindClosestPassable(Node start, Node unpassable,bool **pathingMap)
{
Node passable;
bool found = false;
std::vector<Node> unpassableNodes;
std::vector<Node> passableNodes;
unpassableNodes.push_back(unpassable);
std::cout << unpassableNodes.size()<<" unpassable \n";
while(!found && unpassableNodes.end() != unpassableNodes.begin()) {
//std::cout << "while \n";
//std::cout << "test \n";
Node currentNode = unpassableNodes[0];
//std::cout << unpassableNodes.size()<<" unpassable \n";
if (CheckInBound(currentNode.GetBottomNode()) && Passable(currentNode.GetBottomNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetBottomNode());
}
else {
unpassableNodes.push_back(currentNode.GetBottomNode());
}
if (CheckInBound(currentNode.GetBottomLeftNode()) && Passable(currentNode.GetBottomLeftNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetBottomLeftNode());
}
else {
unpassableNodes.push_back(currentNode.GetBottomLeftNode());
}
if (CheckInBound(currentNode.GetLeftNode()) && Passable(currentNode.GetLeftNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetLeftNode());
}
else {
unpassableNodes.push_back(currentNode.GetLeftNode());
}
if (CheckInBound(currentNode.GetTopLeftNode()) && Passable(currentNode.GetTopLeftNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetTopLeftNode());
}
else {
unpassableNodes.push_back(currentNode.GetTopLeftNode());
}
if (CheckInBound(currentNode.GetTopNode()) && Passable(currentNode.GetTopNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetTopNode());
}
else {
unpassableNodes.push_back(currentNode.GetTopNode());
}
if (CheckInBound(currentNode.GetTopRightNode()) && Passable(currentNode.GetTopRightNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetTopRightNode());
}
else {
unpassableNodes.push_back(currentNode.GetTopRightNode());
}
if (CheckInBound(currentNode.GetRightNode()) && Passable(currentNode.GetRightNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetRightNode());
}
else {
unpassableNodes.push_back(currentNode.GetRightNode());
}
if (CheckInBound(currentNode.GetBottomRightNode()) && Passable(currentNode.GetBottomRightNode(),pathingMap)) {
passableNodes.push_back(currentNode.GetBottomRightNode());
}
else {
unpassableNodes.push_back(currentNode.GetBottomRightNode());
}
//std::cout << passableNodes.size()<<"\n passable \n";
//std::cout << passableNodes.size()<<"\n passable \n";
if(!passableNodes.empty()) {
found = true;
break;
}
unpassableNodes.erase(unpassableNodes.begin());
}
passable = passableNodes[0];
for(std::vector<Node>::iterator it = passableNodes.begin(); it != passableNodes.end(); ++it) {
Node currentNode = *it;
if (DistanceEuclidienne(start.GetX(),passable.GetX(),start.GetY(),passable.GetY()) > DistanceEuclidienne(start.GetX(),currentNode.GetX(),start.GetY(),currentNode.GetY())) {
passable = currentNode;
}
}
return passable;
}
bool Tools::Astar(Node start, Node destination, bool **pathingMap, std::vector<Node>& path)
{
//initialisation des structure necessaire
bool solution = false;
std::vector<Node> openNodes;
std::vector<Node> closedNodes;
std::map<Node,Node> parents;
//initialisation du cout du premier node a 0
start.SetG(0);
start.SetH(0);
//ajout au vecteur ouvert
openNodes.push_back(start);
Node current;
int currentIndex;
while (solution == false) {
//Si le vecteur ouvert est vide il n'y a pas de solution
if(openNodes.empty()) {
return false;
}
//index pour faciliter le retrer de current au vecteur ouvert
int index = 0;
//choisi le meilleur noeud dans le vecteur ouvert
current = BestNodeInVector(openNodes, index);
//ajout dans le vecteur fermer
closedNodes.push_back(current);
currentIndex = closedNodes.size() - 1;
//on suprime le node choisi du vecteur ouvert
openNodes.erase(openNodes.begin()+index);
//verification si on est a destination alors on quitte la boucle
if (current.GetX() == destination.GetX() && current.GetY() == destination.GetY()) {
solution = true;
break;
}
//Verification de la case en bas a gauche
if(CheckInBound(current.GetBottomLeftNode()) && Passable(current.GetBottomLeftNode(),pathingMap) && !VectorContainsNode(current.GetBottomLeftNode(),closedNodes))
{
if(!VectorContainsNode(current.GetBottomLeftNode(),openNodes)) {
Node nodeToAdd = current.GetBottomLeftNode();
nodeToAdd.SetG(current.GetG()+14);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomLeftNode(),openNodes, 14,parents);
}
}
//Verification de la case en bas
if(CheckInBound(current.GetBottomNode()) && Passable(current.GetBottomNode(),pathingMap) && !VectorContainsNode(current.GetBottomNode(),closedNodes))
{
if(!VectorContainsNode(current.GetBottomNode(),openNodes)) {
Node nodeToAdd = current.GetBottomNode();
nodeToAdd.SetG(current.GetG()+10);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomNode(),openNodes, 10,parents);
}
}
//Verification de la case a gauche
if(CheckInBound(current.GetLeftNode()) && Passable(current.GetLeftNode(),pathingMap) && !VectorContainsNode(current.GetLeftNode(),closedNodes))
{
if(!VectorContainsNode(current.GetLeftNode(),openNodes)) {
Node nodeToAdd = current.GetLeftNode();
nodeToAdd.SetG(current.GetG()+10);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetLeftNode(),openNodes, 10,parents);
}
}
// Verification de la case en haut a gauche
if(CheckInBound(current.GetTopLeftNode()) && Passable(current.GetTopLeftNode(),pathingMap) && !VectorContainsNode(current.GetTopLeftNode(),closedNodes))
{
if(!VectorContainsNode(current.GetTopLeftNode(),openNodes)) {
Node nodeToAdd = current.GetTopLeftNode();
nodeToAdd.SetG(current.GetG()+14);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopLeftNode(),openNodes, 14,parents);
}
}
//Verification de la case en haut
if(CheckInBound(current.GetTopNode()) && Passable(current.GetTopNode(),pathingMap) && !VectorContainsNode(current.GetTopNode(),closedNodes))
{
if(!VectorContainsNode(current.GetTopNode(),openNodes)) {
Node nodeToAdd = current.GetTopNode();
nodeToAdd.SetG(current.GetG()+10);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopNode(),openNodes, 10,parents);
}
}
// Verification de la case en haut a droite
if(CheckInBound(current.GetTopRightNode()) && Passable(current.GetTopRightNode(),pathingMap) && !VectorContainsNode(current.GetTopRightNode(),closedNodes))
{
if(!VectorContainsNode(current.GetTopRightNode(),openNodes)) {
Node nodeToAdd = current.GetTopRightNode();
nodeToAdd.SetG(current.GetG()+14);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopRightNode(),openNodes, 14,parents);
}
}
//Verification de la case a droite
if(CheckInBound(current.GetRightNode()) && Passable(current.GetRightNode(),pathingMap) && !VectorContainsNode(current.GetRightNode(),closedNodes))
{
if(!VectorContainsNode(current.GetRightNode(),openNodes)) {
Node nodeToAdd = current.GetRightNode();
nodeToAdd.SetG(current.GetG()+10);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetRightNode(),openNodes, 10,parents);
}
}
// Verification de la case en bas a droite
if(CheckInBound(current.GetBottomRightNode()) && Passable(current.GetBottomRightNode(),pathingMap) && !VectorContainsNode(current.GetBottomRightNode(),closedNodes))
{
if(!VectorContainsNode(current.GetBottomRightNode(),openNodes)) {
Node nodeToAdd = current.GetBottomRightNode();
nodeToAdd.SetG(current.GetG()+14);
nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10);
nodeToAdd.SetParent(&closedNodes[currentIndex]);
parents[nodeToAdd] = closedNodes[currentIndex];
openNodes.push_back(nodeToAdd);
}
else
{
UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomRightNode(),openNodes, 14,parents);
}
}
}
//Si on trouve une solution alors on Backtrack jusqu'au noeux d'origine
if(solution == true) {
// Implementation avec les pointeur parent directement dans les Node
// Node *currentBactrack = &closedNodes[currentIndex];
// path.push_back(*currentBactrack);
// while (currentBactrack->HasParent()){
// std::cout << "solution x : " << currentBactrack->GetX()<<" solution y : " << currentBactrack->GetY() << "\n";
// std::cout << "trackback : "<< path.size() << "\n";
// currentBactrack = currentBactrack->GetParent();
// std::cout << "test";
// path.push_back(*currentBactrack);
// std::cout << " solution x : " << currentBactrack->GetX()<<" solution y : " << currentBactrack->GetY() << "\n";
//
// }
//Implementation en utilisant une map entre les Nodes et leur parents
Node currentBactrack = closedNodes[currentIndex];
path.push_back(currentBactrack);
std::map<Node,Node>::iterator it = parents.find(currentBactrack);
while(it != parents.end()) {
currentBactrack = it->second;
//std::cout << "solution x : " << currentBactrack.GetX()<<" solution y : " << currentBactrack.GetY() << "\n";
//std::cout << "trackback : "<< path.size() << "\n";
path.push_back(currentBactrack);
it = parents.find(currentBactrack);
}
}
return true;
}
