2259 lines
72 KiB
C++
2259 lines
72 KiB
C++
#include "CKAll.h"
|
|
#include <math.h>
|
|
#include "./includes/GridManager.h"
|
|
#include "GridPathManager.h"
|
|
#include "OpenList.h"
|
|
#include "NodeLinker.h"
|
|
#include "NodeLinker.h"
|
|
|
|
const float GridPathManager::m_SuccesorIndexCost[12] =
|
|
{
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_DIAGONAL, DISTANCE_CASE_JUXTAPOSE
|
|
};
|
|
|
|
const float GridPathManager::m_SuccesorIndexCost2[6] =
|
|
{
|
|
DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_JUXTAPOSE,
|
|
DISTANCE_CASE_JUXTAPOSE
|
|
};
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GridPathManager
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
GridPathManager::GridPathManager(CKGridManager *gridManager, CKMessageManager *messageManager, CKAttributeManager *attributManager) :
|
|
m_ArrayNodeGrid(0),
|
|
m_LayerID2Index(0),
|
|
m_ArrayPathProblem(0),
|
|
m_NumPathProblem(0),
|
|
m_NumFollowProblem(0),
|
|
m_MaxCase(0),
|
|
m_FollowPathID(0),
|
|
m_GraphDone(FALSE)
|
|
{
|
|
m_GridManager = gridManager;
|
|
m_MessageManager = messageManager;
|
|
m_AttributManager = attributManager;
|
|
m_MaxPathProblem = DEFAUL_MAX_PATH_CONTEXT;
|
|
m_MaxFollowProblem = DEFAUL_MAX_FOLLOW_CONTEXT;
|
|
m_PFCAttrType = m_AttributManager->GetAttributeTypeByName("Path Finding Obstacle");
|
|
m_Reseted = FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ~GridPathManager
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
GridPathManager::~GridPathManager()
|
|
{
|
|
int i;
|
|
/***********************/
|
|
/* Path finding delete */
|
|
/***********************/
|
|
|
|
// Delete all the linker.
|
|
for (NodeLinkerIt it = m_ListNodeLinker.Begin(); it != m_ListNodeLinker.End(); it++){
|
|
if (*it){
|
|
delete *it;
|
|
}
|
|
}
|
|
if (m_LayerID2Index){
|
|
delete [] m_LayerID2Index;
|
|
}
|
|
|
|
// Delete the all path problem
|
|
for (i = 0; i < m_ArrayPathProblem.Size(); ++i){
|
|
delete m_ArrayPathProblem[i];
|
|
}
|
|
|
|
if (m_ArrayNodeGrid){
|
|
delete [] m_ArrayNodeGrid;
|
|
}
|
|
|
|
// Delete all gridinfo
|
|
for (Grid2GridInfoIt itGrid = m_ListGridInfo.Begin(); itGrid != m_ListGridInfo.End(); itGrid++){
|
|
if (*itGrid){
|
|
delete *itGrid;
|
|
}
|
|
}
|
|
|
|
/**********************/
|
|
/* Path follow delete */
|
|
/**********************/
|
|
|
|
// Delete stayed path
|
|
for (PathID2PathIt itPath = m_PathID2Path.Begin(); itPath != m_PathID2Path.End(); itPath++){
|
|
delete *itPath;
|
|
}
|
|
|
|
// Delete follow problem.
|
|
for (i = 0; i < m_ArrayFollowProblem.Size(); ++i){
|
|
delete m_ArrayFollowProblem[i];
|
|
}
|
|
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Reset
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::Reset()
|
|
{
|
|
if (m_Reseted)
|
|
return;
|
|
|
|
/****************/
|
|
/* General Data */
|
|
/****************/
|
|
|
|
{
|
|
// Delete all the linker.
|
|
NodeLinkerIt it;
|
|
for (it = m_ListNodeLinker.Begin(); it != m_ListNodeLinker.End(); ++it)
|
|
if (*it)
|
|
delete *it;
|
|
m_ListNodeLinker.Clear();
|
|
}
|
|
|
|
|
|
{
|
|
// Delete all gridinfo
|
|
Grid2GridInfoIt it;
|
|
for (it = m_ListGridInfo.Begin(); it != m_ListGridInfo.End(); ++it)
|
|
if (*it)
|
|
delete *it;
|
|
m_ListGridInfo.Clear();
|
|
}
|
|
|
|
|
|
if (m_LayerID2Index)
|
|
{
|
|
delete [] m_LayerID2Index;
|
|
m_LayerID2Index = 0;
|
|
}
|
|
|
|
if (m_ArrayNodeGrid)
|
|
{
|
|
delete [] m_ArrayNodeGrid;
|
|
m_ArrayNodeGrid = 0;
|
|
}
|
|
|
|
/****************/
|
|
/* Path problem */
|
|
/****************/
|
|
|
|
{
|
|
XHashTable <int, int>::Iterator it = m_Target2PathContext.Begin();
|
|
XHashTable <int, int>::Iterator itEnd = m_Target2PathContext.End();
|
|
while (it != itEnd) {
|
|
|
|
// Get path context
|
|
int pathContext = *it;
|
|
|
|
// Get path problem
|
|
PathProblem* pathProblem = m_ArrayPathProblem[pathContext];
|
|
|
|
// Reset the path problem.
|
|
pathProblem->Reset();
|
|
|
|
// Free the index context.
|
|
m_StackFreePathIndex.PushBack(pathContext);
|
|
|
|
++it;
|
|
}
|
|
m_Target2PathContext.Clear();
|
|
m_NumPathProblem = 0;
|
|
}
|
|
|
|
|
|
/******************/
|
|
/* Follow problem */
|
|
/******************/
|
|
|
|
// Delete stayed path
|
|
for (PathID2PathIt itPath = m_PathID2Path.Begin(); itPath != m_PathID2Path.End(); ++itPath){
|
|
delete *itPath;
|
|
}
|
|
m_PathID2Path.Clear();
|
|
|
|
{
|
|
XSHashTable <int, int>::Iterator it = m_PathID2FollowContext.Begin();
|
|
XSHashTable <int, int>::Iterator itEnd = m_PathID2FollowContext.End();
|
|
while (it != itEnd) {
|
|
|
|
// Get the follow context
|
|
int followContext = *it;
|
|
|
|
// Reset the follow problem.
|
|
m_ArrayFollowProblem[followContext]->Reset();
|
|
|
|
// Free the index context;
|
|
m_StackFreeFollowIndex.PushBack(followContext);
|
|
|
|
++it;
|
|
}
|
|
m_PathID2FollowContext.Clear();
|
|
m_NumFollowProblem = 0;
|
|
}
|
|
|
|
m_Reseted = TRUE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ConstructListNodeLinker
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::ConstructListNodeLinker(XList <int> *listLayer)
|
|
{
|
|
int i,j,x,y;
|
|
const XObjectPointerArray &ArrayGrid = m_GridManager->GetGridArray();
|
|
int nbGrid = 0;
|
|
CKGrid *grid;
|
|
CKLayer *layer;
|
|
int val;
|
|
VxVector pos;
|
|
NodeLinker *nodeLinker;
|
|
GridInfo *gridInfo;
|
|
int w,l;
|
|
int numLayer = 0;
|
|
int numGrid = 0;
|
|
|
|
if (!ArrayGrid.Size())
|
|
return FALSE;
|
|
|
|
// In this part, we create and add all the 'concerned' linkers in a list.
|
|
for (i = 0; i < ArrayGrid.Size(); ++i)
|
|
{
|
|
grid = (CKGrid *)ArrayGrid[i];
|
|
|
|
// Does the grid is a valid one ?
|
|
if (GridContainOneLayer(grid, listLayer))
|
|
{
|
|
// Create the grid's grid info
|
|
gridInfo = new GridInfo(grid);
|
|
gridInfo->m_Num = numGrid++;
|
|
m_ListGridInfo.Insert((int)grid, gridInfo, FALSE);
|
|
|
|
w = grid->GetWidth();
|
|
l = grid->GetLength();
|
|
if (w*l > m_MaxCase) m_MaxCase = w*l;
|
|
|
|
for (j = 0; j < m_GridManager->GetLayerTypeCount(); ++j)
|
|
if (m_GridManager->GetAssociatedParam(j) == CKPGUID_LINKERGRAPH_ENUM)
|
|
if (layer = grid->GetLayer(j))
|
|
{
|
|
for (x = 0; x < w; ++x)
|
|
for (y = 0; y < l; ++y)
|
|
{
|
|
layer->GetValue(x, y, &val);
|
|
if (val)
|
|
{
|
|
// Create a NodeLinker.
|
|
nodeLinker = new NodeLinker(m_MaxPathProblem, listLayer->Size());
|
|
|
|
// Fill the hash table m_Linker2Attribut
|
|
nodeLinker->m_Attribut = m_GridManager->m_Context->GetAttributeManager()->GetAttributeTypeByName(layer->GetName());
|
|
nodeLinker->m_Grid = grid;
|
|
|
|
switch(val)
|
|
{
|
|
case 1:
|
|
nodeLinker->m_TypeCase = tc_tele1;
|
|
break;
|
|
case 2:
|
|
nodeLinker->m_TypeCase = tc_tele2;
|
|
break;
|
|
case 3:
|
|
nodeLinker->m_TypeCase = tc_tele3;
|
|
break;
|
|
case 4:
|
|
nodeLinker->m_TypeCase = tc_door;
|
|
break;
|
|
default:
|
|
nodeLinker->m_TypeCase = tc_door;
|
|
break;
|
|
}
|
|
nodeLinker->m_LinkerID = j;
|
|
nodeLinker->m_X = x;
|
|
nodeLinker->m_Y = y;
|
|
|
|
// If the linker is a door we need its 3d position.
|
|
if (val == 4)
|
|
{
|
|
grid->Get3dPosFrom2dCoords(&pos, x, y);
|
|
nodeLinker->SetPos(pos);
|
|
}
|
|
gridInfo->m_ListNodeLinker.PushBack(nodeLinker);
|
|
m_ListNodeLinker.PushBack(nodeLinker);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Create m_LayerID2Index
|
|
int maxLayer = 0;
|
|
for (IntIt itl = listLayer->Begin(); itl != listLayer->End(); itl++)
|
|
if (maxLayer < *itl)
|
|
maxLayer = *itl;
|
|
m_LayerID2Index = new int [maxLayer+1];
|
|
|
|
// Create grid A* search data.
|
|
m_ArrayNodeGrid = new NodeGrid[m_MaxCase];
|
|
for (i = 0; i < m_MaxCase; ++i)
|
|
m_ArrayNodeGrid[i].Set(m_MaxPathProblem, i, numGrid);
|
|
|
|
// Fill "LinkerNode as obstacle" info.
|
|
int index;
|
|
NodeLinker *node;
|
|
for (Grid2GridInfoIt it = m_ListGridInfo.Begin(); it != m_ListGridInfo.End(); it++)
|
|
for (NodeLinkerIt it2 = (*it)->m_ListNodeLinker.Begin(); it2 != (*it)->m_ListNodeLinker.End(); it2++)
|
|
{
|
|
node = (*it2);
|
|
index = node->m_Y*node->m_Grid->GetWidth()+node->m_X;
|
|
m_ArrayNodeGrid[index].m_LinkerObs[(*it)->m_Num] = node;
|
|
}
|
|
|
|
// In this part, we connect linker each other.
|
|
NodeLinkerIt it1;
|
|
NodeLinkerIt it2;
|
|
NodeLinker *linker1;
|
|
NodeLinker *linker2;
|
|
for (it1 = m_ListNodeLinker.Begin(); it1 != m_ListNodeLinker.End(); it1++)
|
|
{
|
|
linker1 = *it1;
|
|
for (it2 = m_ListNodeLinker.Begin(); it2 != m_ListNodeLinker.End(); it2++)
|
|
{
|
|
linker2 = *it2;
|
|
if (linker1 == linker2)
|
|
continue;
|
|
if (linker1->m_LinkerID == linker2->m_LinkerID)
|
|
{
|
|
// Linker are connected.
|
|
if ((linker1->m_TypeCase == tc_door && linker2->m_TypeCase == tc_door) ||
|
|
((linker1->m_TypeCase == tc_tele1 || linker1->m_TypeCase == tc_tele3) && (linker2->m_TypeCase == tc_tele2 || linker2->m_TypeCase == tc_tele3)))
|
|
linker1->m_ListConnected.PushBack(linker2);
|
|
}
|
|
}
|
|
|
|
// For each obstacles layer, we will see if linkers wich are in the same grid,
|
|
// but wich are not connected, can have a path each other.
|
|
numLayer = 0;
|
|
for (IntIt it = listLayer->Begin(); it != listLayer->End(); it++)
|
|
{
|
|
GridContext *contextInfo;
|
|
for (it2 = it1; it2 != m_ListNodeLinker.End(); it2++)
|
|
{
|
|
linker2 = *it2;
|
|
if (linker1->m_LinkerID != linker2->m_LinkerID && linker1->m_Grid == linker2->m_Grid)
|
|
{
|
|
// Instant A star.
|
|
ResetArrayContextInfo(0, linker1->m_Grid->GetWidth()*linker1->m_Grid->GetLength());
|
|
int maxLayerValue;
|
|
contextInfo = CasesDistance(linker1->m_Grid, *it, linker1->m_X, linker1->m_Y, linker2->m_X, linker2->m_Y, maxLayerValue);
|
|
if (contextInfo)
|
|
{
|
|
IndoorLink *indooLink1 = &linker1->m_ArrayIndoor[numLayer];
|
|
indooLink1->m_LayerID = *it;
|
|
indooLink1->m_ListSameGrid.PushBack(linker2);
|
|
indooLink1->m_ListSameGridCoast.PushBack(contextInfo->m_Cost);
|
|
indooLink1->m_ListSameGridLayerCoast.PushBack((int)contextInfo->m_LayerCost);
|
|
indooLink1->m_ListSameGridMaxLayerCoast.PushBack(maxLayerValue);
|
|
|
|
IndoorLink *indooLink2 = &linker2->m_ArrayIndoor[numLayer];
|
|
indooLink2->m_LayerID = *it;
|
|
indooLink2->m_ListSameGrid.PushBack(linker1);
|
|
indooLink2->m_ListSameGridCoast.PushBack(contextInfo->m_Cost);
|
|
indooLink2->m_ListSameGridLayerCoast.PushBack((int)contextInfo->m_LayerCost);
|
|
indooLink2->m_ListSameGridMaxLayerCoast.PushBack(maxLayerValue);
|
|
}
|
|
}
|
|
}
|
|
// Init the conversion table.
|
|
m_LayerID2Index[*it] = numLayer;
|
|
++numLayer;
|
|
}
|
|
}
|
|
if (!m_ListNodeLinker.Size())
|
|
for (IntIt it = listLayer->Begin(); it != listLayer->End(); it++)
|
|
// Init the conversion table.
|
|
m_LayerID2Index[*it] = numLayer++;
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ConstructGraph
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::ConstructGraph(XList <int> *listLayer)
|
|
{
|
|
XList <int> listLayer2;
|
|
CKBOOL listLinkerOk;
|
|
int i,j;
|
|
|
|
// If a graph was allready done erase it.
|
|
Reset();
|
|
|
|
if (!listLayer)
|
|
{
|
|
// If list layer is empty all the layer are considered.
|
|
for (j = 0; j < m_GridManager->GetLayerTypeCount(); ++j)
|
|
if (m_GridManager->GetAssociatedParam(j) != CKPGUID_LINKERGRAPH_ENUM)
|
|
listLayer2.PushBack(j);
|
|
listLinkerOk = ConstructListNodeLinker(&listLayer2);
|
|
}
|
|
else
|
|
listLinkerOk = ConstructListNodeLinker(listLayer);
|
|
|
|
if (listLinkerOk)
|
|
{
|
|
// Build m_ArrayPathProblem
|
|
m_ArrayPathProblem.Resize(DEFAUL_MAX_PATH_CONTEXT);
|
|
|
|
for (i = 0; i < DEFAUL_MAX_PATH_CONTEXT; ++i)
|
|
{
|
|
m_ArrayPathProblem[i] = new PathProblem();
|
|
|
|
// Just to have acces to public attributs...
|
|
m_ArrayPathProblem[i]->m_GridPathManager = this;
|
|
|
|
// Set each open list.
|
|
m_ArrayPathProblem[i]->SetOpenList(i,(int)(m_ListNodeLinker.Size()*REALTIME_OPENLIST_BUFFER_FACTOR),(int)(m_MaxCase*REALTIME_OPENLIST_BUFFER_FACTOR));
|
|
|
|
// Fill StackFreeIndex.
|
|
m_StackFreePathIndex.PushBack(i);
|
|
|
|
// Build NodeGrid goal and end.
|
|
if (listLayer)
|
|
m_NumLayer = listLayer->Size();
|
|
else
|
|
m_NumLayer = listLayer2.Size();
|
|
m_ArrayPathProblem[i]->m_NodeLinkerDynStart = new NodeLinker(m_MaxPathProblem, m_NumLayer);
|
|
m_ArrayPathProblem[i]->m_NodeLinkerDynEnd = new NodeLinker(m_MaxPathProblem, m_NumLayer);
|
|
}
|
|
|
|
// Follow data construction.
|
|
ConstructFollowData();
|
|
|
|
m_GraphDone = TRUE;
|
|
m_Reseted = FALSE;
|
|
return TRUE;
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetTargetStatus
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GridPathManager::GetTargetStatus(CK3dEntity *target)
|
|
{
|
|
int *contextPtr;
|
|
|
|
if (!(contextPtr = m_Target2PathContext.FindPtr((int)target)))
|
|
return 0;
|
|
return m_ArrayPathProblem[*contextPtr]->m_State;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// RegisterPathProblem
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::RegisterPathProblem(CK3dEntity *target, VxVector *targetPos, CK3dEntity *goalRef, VxVector *goalPos, int obstacleLayer, int obstacleThreshold, float slowingFactor, CKBOOL linker, CKBOOL linkerObs, float heuristicCoef, float timeFrame, int heuristic, CKBOOL diagonal, CKBOOL optimize)
|
|
{
|
|
int pathID;
|
|
|
|
// The graph is build with the firt problem registration.
|
|
|
|
if (!m_GraphDone && !ConstructGraph(0))
|
|
return FALSE;
|
|
|
|
// Get a free index.
|
|
if (m_StackFreePathIndex.Size())
|
|
{
|
|
pathID = *(m_StackFreePathIndex.Begin());
|
|
m_StackFreePathIndex.PopFront();
|
|
}
|
|
else
|
|
{
|
|
// Resize all parallel context.
|
|
pathID = m_MaxPathProblem++;
|
|
ResizePathContextArray(m_MaxPathProblem);
|
|
}
|
|
|
|
++m_NumPathProblem;
|
|
m_ArrayPathProblem[pathID]->Set(target, targetPos, goalRef, goalPos, obstacleLayer, obstacleThreshold, slowingFactor, linker, linkerObs, heuristicCoef, timeFrame, heuristic, diagonal, optimize, STATE_GETSTARTENDGRIDSID);
|
|
m_Target2PathContext.Insert((int)target, pathID, FALSE);
|
|
return TRUE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ResizePathContextArray
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::ResizePathContextArray(int numberContext)
|
|
{
|
|
int oldNumberContext = m_ArrayPathProblem.Size();
|
|
int i;
|
|
|
|
m_ArrayPathProblem.Resize(numberContext);
|
|
for (i = 0; i < oldNumberContext; ++i)
|
|
{
|
|
m_ArrayPathProblem[i]->m_NodeLinkerDynStart->ResizeParallelContext(numberContext);
|
|
m_ArrayPathProblem[i]->m_NodeLinkerDynEnd->ResizeParallelContext(numberContext);
|
|
}
|
|
if (numberContext > oldNumberContext)
|
|
for (i = oldNumberContext; i < numberContext; ++i)
|
|
{
|
|
PathProblem *pathProblem = new PathProblem();
|
|
|
|
// Just to have acces to public attributs...
|
|
pathProblem->m_GridPathManager = this;
|
|
|
|
// Set each open list.
|
|
pathProblem->SetOpenList(i,(int)(m_ListNodeLinker.Size()*REALTIME_OPENLIST_BUFFER_FACTOR),(int)(m_MaxCase*REALTIME_OPENLIST_BUFFER_FACTOR));
|
|
|
|
// Build NodeGrid goal and end.
|
|
pathProblem->m_NodeLinkerDynStart = new NodeLinker(m_MaxPathProblem, m_NumLayer);
|
|
pathProblem->m_NodeLinkerDynEnd = new NodeLinker(m_MaxPathProblem, m_NumLayer);
|
|
|
|
m_ArrayPathProblem[i] = pathProblem;
|
|
}
|
|
for (NodeLinkerIt NodeIt = m_ListNodeLinker.Begin(); NodeIt != m_ListNodeLinker.End(); NodeIt++)
|
|
(*NodeIt)->ResizeParallelContext(numberContext);
|
|
for (i = 0; i < m_MaxCase; ++i)
|
|
m_ArrayNodeGrid[i].ResizeParallelContext(numberContext);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// UnregisterPathProblem
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::UnregisterPathProblem(CK3dEntity *target, CKBOOL giveToFollower)
|
|
{
|
|
PathProblem *pathProblem;
|
|
int *pathContextPtr;
|
|
int pathContext;
|
|
|
|
|
|
if (!(pathContextPtr = m_Target2PathContext.FindPtr((int)target)))
|
|
return FALSE;
|
|
pathContext = *pathContextPtr;
|
|
m_Target2PathContext.Remove((int)target);
|
|
|
|
pathProblem = m_ArrayPathProblem[pathContext];
|
|
if (giveToFollower)
|
|
{
|
|
Path *newPath = new Path();
|
|
newPath->CopyAndErase(pathProblem->m_Path);
|
|
newPath->m_ObstacleLayer = pathProblem->m_ObstacleLayer;
|
|
newPath->m_ObstacleThreshold = pathProblem->m_ObstacleThreshold;
|
|
|
|
// Debug
|
|
for (int i = 0; i < newPath->NbSubPath(); ++i)
|
|
newPath->m_ArraySubPath[i]->m_PathFID = pathProblem->m_FollowPathID;
|
|
|
|
m_PathID2Path.Insert(pathProblem->m_FollowPathID, newPath, FALSE);
|
|
}
|
|
|
|
// Reset the path problem.
|
|
pathProblem->Reset();
|
|
|
|
// Clear the dyn nodelinker of all the linker for the context.
|
|
for (NodeLinkerIt it = m_ListNodeLinker.Begin(); it != m_ListNodeLinker.End(); it++)
|
|
(*it)->m_DynNodeLinker[pathContext] = 0;
|
|
|
|
// Free the index context.
|
|
m_StackFreePathIndex.PushBack(pathContext);
|
|
|
|
--m_NumPathProblem;
|
|
return TRUE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetStartEndGridID
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetStartEndGridID(PathProblem &pathProblem, int context)
|
|
{
|
|
CKGrid *gridStart;
|
|
CKGrid *gridEnd;
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
|
|
if (!(gridStart = gridManager->m_GridManager->GetPreferredGrid(&pathProblem.m_TargetPos)))
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
if (gridManager->m_ListGridInfo.FindPtr((int)gridStart))
|
|
{
|
|
if (!(gridEnd = gridManager->m_GridManager->GetPreferredGrid(&pathProblem.m_GoalPos)))
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
if (gridManager->m_ListGridInfo.FindPtr((int)gridEnd))
|
|
{
|
|
NodeLinker *nodeStart = pathProblem.m_NodeLinkerDynStart;
|
|
NodeLinker *nodeEnd = pathProblem.m_NodeLinkerDynEnd;
|
|
pathProblem.m_GridStart = gridStart;
|
|
pathProblem.m_GridEnd = gridEnd;
|
|
|
|
// Init the dyn NodeLinkers
|
|
nodeStart->ClearConnected();
|
|
nodeStart->ClearDynConnected();
|
|
nodeStart->m_Grid = gridStart;
|
|
nodeStart->m_LinkerID = -1;
|
|
gridStart->Get2dCoordsFrom3dPos(&pathProblem.m_TargetPos, &nodeStart->m_X, &nodeStart->m_Y);
|
|
|
|
nodeEnd->ClearConnected();
|
|
nodeEnd->ClearDynConnected();
|
|
nodeEnd->m_Grid = gridEnd;
|
|
nodeEnd->m_LinkerID = -2;
|
|
gridEnd->Get2dCoordsFrom3dPos(&pathProblem.m_GoalPos, &nodeEnd->m_X, &nodeEnd->m_Y);
|
|
|
|
// If start == end path is not found...
|
|
if (nodeStart->m_X == nodeEnd->m_X && nodeStart->m_Y == nodeEnd->m_Y)
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
|
|
int val;
|
|
CKLayer * layer;
|
|
|
|
// If goal is on a obstacle path is not found.
|
|
if (pathProblem.m_LinkerObs && gridManager->CaseIsLinker(pathProblem, gridEnd, nodeEnd->m_X+nodeEnd->m_Y*gridEnd->GetWidth()))
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
if (layer = gridEnd->GetLayer(pathProblem.m_ObstacleLayer))
|
|
{
|
|
layer->GetValue(nodeEnd->m_X, nodeEnd->m_Y, &val);
|
|
if (val > pathProblem.m_ObstacleThreshold)
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// If target dont use linkers or if there is no linkers...
|
|
// Or if target as no attribut (it is not allow to take linkers).
|
|
if (!pathProblem.m_Linker || !gridManager->m_ListNodeLinker.Size() || !pathProblem.m_Target->GetAttributeCount())
|
|
{
|
|
if (gridStart == gridEnd)
|
|
{
|
|
// ...we want find a direct path.
|
|
pathProblem.m_StateFunction = STATE_DIRECTPATH;
|
|
return 1;
|
|
}
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
pathProblem.m_StateFunction = STATE_BEFORESTARTLINKER;
|
|
return 1;
|
|
}
|
|
}
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// BeforeStartLinker
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int BeforeStartLinker(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
do
|
|
{
|
|
NodeLinker *nodeStart = pathProblem.m_NodeLinkerDynStart;
|
|
NodeLinker *nodeEnd = pathProblem.m_NodeLinkerDynEnd;
|
|
|
|
if (!pathProblem.m_StartPartSearch && pathProblem.m_GridStart == pathProblem.m_GridEnd)
|
|
{
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
|
|
pathProblem.InitBeforeCase2Case(pathProblem.m_GridStart, &gridManager->m_ListGridInfo);
|
|
|
|
// Refresh the context info.
|
|
gridManager->ResetArrayContextInfo(context, pathProblem.m_GridW*pathProblem.m_GridL);
|
|
|
|
// Set the linkers.
|
|
pathProblem.SetLinker(NULL, NULL);
|
|
|
|
// We inverse the path problem (to avoid to inverse it later...)
|
|
// The dyn start linker is the start node grid.
|
|
NodeGrid *nodeGrid = &gridManager->m_ArrayNodeGrid[nodeEnd->m_Y*nodeEnd->m_Grid->GetWidth()+nodeEnd->m_X];
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0;
|
|
contextInfo->m_F = sqrtf((float)((nodeEnd->m_X-nodeStart->m_X)*(nodeEnd->m_X-nodeStart->m_X)+(nodeEnd->m_Y-nodeStart->m_Y)*(nodeEnd->m_Y-nodeStart->m_Y)));
|
|
contextInfo->m_Parent = 0;
|
|
|
|
// Init the open list.
|
|
pathProblem.m_OpenListGrid->Push(nodeGrid);
|
|
|
|
// The dyn en linker is the goal node grid.
|
|
pathProblem.m_XEndSearch = nodeStart->m_X;
|
|
pathProblem.m_YEndSearch = nodeStart->m_Y;
|
|
pathProblem.m_EndIndex = nodeStart->m_Y*nodeStart->m_Grid->GetWidth()+nodeStart->m_X;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_NodeGrideEnd = 0;
|
|
//pathProblem.m_StateFunction = STATE_AFTERENDLINKER;
|
|
return 1;
|
|
}
|
|
|
|
if (pathProblem.m_GridStart == pathProblem.m_GridEnd)
|
|
{
|
|
// Search.
|
|
if (!GetCase2CasePath(pathProblem, context))
|
|
{
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_GETLINKERFROMSTART;
|
|
return 1;
|
|
}
|
|
else if (pathProblem.m_NodeGrideEnd)
|
|
{
|
|
IndoorLink *indoorLink = &nodeStart->m_ArrayIndoor[gridManager->m_LayerID2Index[pathProblem.m_ObstacleLayer]];
|
|
indoorLink->m_ListSameGrid.PushBack(nodeEnd);
|
|
indoorLink->m_ListSameGridCoast.PushBack(pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
indoorLink->m_ListSameGridLayerCoast.PushBack(0);
|
|
indoorLink->m_ListSameGridMaxLayerCoast.PushBack(0);
|
|
|
|
// Save the path found.
|
|
pathProblem.m_HypSubPathStart2End = new SubPath(pathProblem.m_GridSearch, pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
pathProblem.BuildSubPath(context, pathProblem.m_HypSubPathStart2End);
|
|
pathProblem.m_HypSubPathStart2EndCoast = pathProblem.m_HypSubPathStart2End->m_Coast;
|
|
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_GETLINKERFROMSTART;
|
|
return 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_GETLINKERFROMSTART;
|
|
return 1;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetLinkerFromStart
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetLinkerFromStart(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
do
|
|
{
|
|
// First time we enter in GetLinkerFromStart function.
|
|
if (!pathProblem.m_StartPartSearch)
|
|
{
|
|
GridInfo *gridInfo = *gridManager->m_ListGridInfo.FindPtr((int)pathProblem.m_GridStart);
|
|
pathProblem.m_ItLinkerStart = gridInfo->m_ListNodeLinker.Begin();
|
|
pathProblem.m_ItLinkerEnd = gridInfo->m_ListNodeLinker.End();
|
|
|
|
// The search grid is the startGrid.
|
|
pathProblem.m_GridSearch = pathProblem.m_GridStart;
|
|
|
|
// The start case is the target position
|
|
pathProblem.m_GridStart->Get2dCoordsFrom3dPos(&pathProblem.m_TargetPos, &pathProblem.m_XStartSearch, &pathProblem.m_YStartSearch);
|
|
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
}
|
|
|
|
// If solution is not found
|
|
if (!pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// We search...
|
|
if (!GetCase2CasePath(pathProblem, context))
|
|
// Path not found.
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
else if (pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Path is found.
|
|
// Save the path.
|
|
SubPath *subPath = new SubPath(pathProblem.m_GridSearch, pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
pathProblem.BuildSubPath(context, subPath, FALSE);
|
|
pathProblem.m_HypSubPathStart.PushBack(subPath);
|
|
|
|
// Update connectivity between dyn nodeLinker and current linker.
|
|
NodeLinkerIt itLinker = pathProblem.m_ItLinkerStart;
|
|
itLinker--;
|
|
IndoorLink *indoorLink = &pathProblem.m_NodeLinkerDynStart->m_ArrayIndoor[gridManager->m_LayerID2Index[pathProblem.m_ObstacleLayer]];
|
|
indoorLink->m_ListSameGrid.PushBack(*itLinker);
|
|
indoorLink->m_ListSameGridCoast.PushBack(pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
indoorLink->m_ListSameGridLayerCoast.PushBack(0);
|
|
indoorLink->m_ListSameGridMaxLayerCoast.PushBack(0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (pathProblem.m_ItLinkerStart != pathProblem.m_ItLinkerEnd || !pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Test if the target have access to linker
|
|
while (!pathProblem.Linker2StartIsGood())
|
|
{
|
|
pathProblem.m_ItLinkerStart++;
|
|
if (pathProblem.m_ItLinkerStart == pathProblem.m_ItLinkerEnd)
|
|
break;
|
|
}
|
|
}
|
|
|
|
// The search is over.
|
|
if (pathProblem.m_ItLinkerStart == pathProblem.m_ItLinkerEnd && pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// If we haven't hyp path start..
|
|
if (!pathProblem.m_HypSubPathStart.Size())
|
|
{
|
|
// ... and the goal is in the same grid...
|
|
if (pathProblem.m_GridStart == pathProblem.m_GridEnd)
|
|
{
|
|
// ...we want find a direct path.
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
pathProblem.m_StateFunction = STATE_DIRECTPATH;
|
|
return 1;
|
|
}
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
pathProblem.m_StateFunction = STATE_GETLINKERFROMEND;
|
|
return 1;
|
|
}
|
|
|
|
pathProblem.InitBeforeCase2Case(pathProblem.m_GridStart, &gridManager->m_ListGridInfo);
|
|
|
|
// Refresh the context info
|
|
gridManager->ResetArrayContextInfo(context, pathProblem.m_GridW*pathProblem.m_GridL);
|
|
|
|
// The new start case is the new linker case...
|
|
NodeLinker *linkerStart = *pathProblem.m_ItLinkerStart;
|
|
pathProblem.m_XEndSearch = linkerStart->m_X;
|
|
pathProblem.m_YEndSearch = linkerStart->m_Y;
|
|
pathProblem.m_EndIndex = pathProblem.m_YEndSearch*pathProblem.m_GridStart->GetWidth()+pathProblem.m_XEndSearch;
|
|
|
|
// Set the linkers.
|
|
pathProblem.SetLinker(linkerStart, NULL);
|
|
|
|
// The initial node in the open list is the start case;
|
|
NodeGrid *nodeGrid = &gridManager->m_ArrayNodeGrid[pathProblem.m_YStartSearch*pathProblem.m_GridSearch->GetWidth()+pathProblem.m_XStartSearch];
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0;
|
|
contextInfo->m_F = sqrtf((float)((pathProblem.m_XStartSearch-pathProblem.m_XEndSearch)*(pathProblem.m_XStartSearch-pathProblem.m_XEndSearch)+(pathProblem.m_YStartSearch-pathProblem.m_YEndSearch)*(pathProblem.m_YStartSearch-pathProblem.m_YEndSearch)));
|
|
contextInfo->m_Parent = 0;
|
|
pathProblem.m_OpenListGrid->Push(nodeGrid);
|
|
|
|
// We search nothing for now.
|
|
pathProblem.m_NodeGrideEnd = 0;
|
|
|
|
// Next linker.
|
|
pathProblem.m_ItLinkerStart++;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetLinkerFromEnd
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetLinkerFromEnd(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
do
|
|
{
|
|
// First time we enter in GetLinkerFromEnd function.
|
|
if (!pathProblem.m_StartPartSearch)
|
|
{
|
|
GridInfo *gridInfo = *gridManager->m_ListGridInfo.FindPtr((int)pathProblem.m_GridEnd);
|
|
pathProblem.m_ItLinkerStart = gridInfo->m_ListNodeLinker.Begin();
|
|
pathProblem.m_ItLinkerEnd = gridInfo->m_ListNodeLinker.End();
|
|
|
|
|
|
// The search grid is the end Grid.
|
|
pathProblem.m_GridSearch = pathProblem.m_GridEnd;
|
|
|
|
// The start case is the goal position
|
|
pathProblem.m_GridEnd->Get2dCoordsFrom3dPos(&pathProblem.m_GoalPos, &pathProblem.m_XStartSearch, &pathProblem.m_YStartSearch);
|
|
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
}
|
|
|
|
// If solution is not found
|
|
if (!pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// We search...
|
|
if (!GetCase2CasePath(pathProblem, context))
|
|
// Path not found.
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
else if (pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Path is found.
|
|
// Save the path.
|
|
SubPath *subPath = new SubPath(pathProblem.m_GridSearch, pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
pathProblem.BuildSubPath(context, subPath);
|
|
pathProblem.m_HypSubPathEnd.PushBack(subPath);
|
|
|
|
// Update connectivity between dyn nodeLinker and current linker.
|
|
NodeLinkerIt itLinker = pathProblem.m_ItLinkerStart;
|
|
itLinker--;
|
|
(*itLinker)->m_DynNodeLinker[context] = pathProblem.m_NodeLinkerDynEnd;
|
|
(*itLinker)->m_DynNodeLinkerCoast[context] = pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (pathProblem.m_ItLinkerStart != pathProblem.m_ItLinkerEnd || !pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Test if the target have access to linker
|
|
while (!pathProblem.Linker2EndIsGood())
|
|
{
|
|
pathProblem.m_ItLinkerStart++;
|
|
if (pathProblem.m_ItLinkerStart == pathProblem.m_ItLinkerEnd)
|
|
break;
|
|
}
|
|
}
|
|
|
|
// The search is over.
|
|
if (pathProblem.m_ItLinkerStart == pathProblem.m_ItLinkerEnd && pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// If we haven't hyp path end..
|
|
if (!pathProblem.m_HypSubPathEnd.Size())
|
|
{
|
|
// ... and the goal is in the same grid...
|
|
if (pathProblem.m_GridStart == pathProblem.m_GridEnd)
|
|
{
|
|
// ...we want find a direct path.
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
pathProblem.m_StateFunction = STATE_DIRECTPATH;
|
|
return 1;
|
|
}
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
|
|
// We search nothing for now.
|
|
pathProblem.m_NodeLinkerEnd = 0;
|
|
|
|
// Refresh Contex info data
|
|
gridManager->ResetArrayLinkerContextInfo(context);
|
|
|
|
// Add linker to list.
|
|
LinkerContext *contextInfo = pathProblem.m_NodeLinkerDynStart->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0;
|
|
contextInfo->m_F = 0;
|
|
contextInfo->m_Parent = 0;
|
|
pathProblem.m_OpenListLinker->Push(pathProblem.m_NodeLinkerDynStart);
|
|
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_GETGRID2DRIDPATH;
|
|
return 1;
|
|
}
|
|
|
|
pathProblem.InitBeforeCase2Case(pathProblem.m_GridEnd, &gridManager->m_ListGridInfo);
|
|
|
|
// Refresh the context info.
|
|
gridManager->ResetArrayContextInfo(context, pathProblem.m_GridW*pathProblem.m_GridL);
|
|
|
|
// The new end case is the new linker case...
|
|
NodeLinker *linkerEnd = *pathProblem.m_ItLinkerStart;
|
|
pathProblem.m_XEndSearch = linkerEnd->m_X;
|
|
pathProblem.m_YEndSearch = linkerEnd->m_Y;
|
|
pathProblem.m_EndIndex = pathProblem.m_YEndSearch*pathProblem.m_GridEnd->GetWidth()+pathProblem.m_XEndSearch;
|
|
|
|
// Set the linkers.
|
|
pathProblem.SetLinker(linkerEnd, NULL);
|
|
|
|
// The initial node in the open list is the end case.
|
|
NodeGrid *nodeGrid = &gridManager->m_ArrayNodeGrid[pathProblem.m_YStartSearch*pathProblem.m_GridSearch->GetWidth()+pathProblem.m_XStartSearch];
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0;
|
|
contextInfo->m_F = sqrtf((float)((pathProblem.m_XStartSearch-pathProblem.m_XEndSearch)*(pathProblem.m_XStartSearch-pathProblem.m_XEndSearch)+(pathProblem.m_YStartSearch-pathProblem.m_YEndSearch)*(pathProblem.m_YStartSearch-pathProblem.m_YEndSearch)));
|
|
contextInfo->m_Parent = 0;
|
|
pathProblem.m_OpenListGrid->Push(nodeGrid);
|
|
|
|
// We search nothing for now.
|
|
pathProblem.m_NodeGrideEnd = 0;
|
|
|
|
// Next linker.
|
|
pathProblem.m_ItLinkerStart++;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// DirectPath
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int DirectPath(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
do
|
|
{
|
|
if (!pathProblem.m_StartPartSearch)
|
|
{
|
|
pathProblem.InitBeforeCase2Case(pathProblem.m_GridStart, &gridManager->m_ListGridInfo);
|
|
|
|
// Refresh the context info.
|
|
gridManager->ResetArrayContextInfo(context, pathProblem.m_GridW*pathProblem.m_GridL);
|
|
|
|
// Set the linkers.
|
|
pathProblem.SetLinker(NULL, NULL);
|
|
|
|
// We inverse the path problem (to avoid to inverse it later...)
|
|
// The dyn start linker is the start node grid.
|
|
NodeLinker *nodeStart = pathProblem.m_NodeLinkerDynStart;
|
|
NodeLinker *nodeEnd = pathProblem.m_NodeLinkerDynEnd;
|
|
|
|
NodeGrid *nodeGrid = &gridManager->m_ArrayNodeGrid[nodeEnd->m_Y*nodeEnd->m_Grid->GetWidth()+nodeEnd->m_X];
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0;
|
|
contextInfo->m_F = sqrtf((float)((nodeEnd->m_X-nodeStart->m_X)*(nodeEnd->m_X-nodeStart->m_X)+(nodeEnd->m_Y-nodeStart->m_Y)*(nodeEnd->m_Y-nodeStart->m_Y)));
|
|
contextInfo->m_Parent = 0;
|
|
|
|
// Init the open list.
|
|
pathProblem.m_OpenListGrid->Push(nodeGrid);
|
|
|
|
// The dyn en linker is the goal node grid.
|
|
pathProblem.m_XEndSearch = nodeStart->m_X;
|
|
pathProblem.m_YEndSearch = nodeStart->m_Y;
|
|
pathProblem.m_EndIndex = nodeStart->m_Y*nodeStart->m_Grid->GetWidth()+nodeStart->m_X;
|
|
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
pathProblem.m_NodeGrideEnd = 0;
|
|
}
|
|
else
|
|
{
|
|
// Search.
|
|
if (!GetCase2CasePath(pathProblem, context))
|
|
{
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
else if (pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Save the path found.
|
|
SubPath *subPath = new SubPath(pathProblem.m_GridSearch, pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
pathProblem.BuildSubPath(context, subPath);
|
|
pathProblem.m_Path.AddSubPathBack(subPath);
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHFOUND;
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetGrid2GridPath
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetGrid2GridPath(PathProblem &pathProblem, int context)
|
|
{
|
|
NodeLinker *nodeLinker;
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
|
|
do
|
|
{
|
|
// Main A* loop
|
|
if (pathProblem.m_OpenListLinker->Size())
|
|
{
|
|
// Pop the node with le lowest F.
|
|
nodeLinker = pathProblem.m_OpenListLinker->PopBack();
|
|
nodeLinker->m_ArrayContextInfo[context]->m_InOpen = FALSE;
|
|
|
|
// Does the linker is the goal ?
|
|
if (nodeLinker == pathProblem.m_NodeLinkerDynEnd)
|
|
{
|
|
// We find a path between grids.
|
|
pathProblem.m_NodeLinkerEnd = pathProblem.m_NodeLinkerDynEnd;
|
|
|
|
// We set the new state.
|
|
AnalyseGrid2GridPath(pathProblem, context);
|
|
return 2;
|
|
}
|
|
|
|
NodeLinkerIt it;
|
|
FloatIt it2;
|
|
IntIt it3;
|
|
IntIt it4;
|
|
// For each succesor...
|
|
// Same ID.
|
|
float cost = 1;
|
|
if (nodeLinker->m_LinkerID != -1)
|
|
{
|
|
CKParameterOut *param = pathProblem.m_Target->GetAttributeParameter(nodeLinker->m_Attribut);
|
|
if (param)
|
|
{
|
|
param->GetValue(&cost);
|
|
if (cost <= 0)
|
|
cost = 0.001f;
|
|
}
|
|
}
|
|
for (it = nodeLinker->m_ListConnected.Begin(); it != nodeLinker->m_ListConnected.End(); it++)
|
|
ManageNodeLinkerSuccesor(pathProblem, context, nodeLinker, *it, cost);
|
|
|
|
// Different ID.
|
|
if (nodeLinker->m_ArrayIndoor)
|
|
{
|
|
int layerIndex = gridManager->m_LayerID2Index[pathProblem.m_ObstacleLayer];
|
|
if (nodeLinker->m_ArrayIndoor[layerIndex].m_ListSameGrid.Size())
|
|
{
|
|
// These succesor are linkers wich are in the same grid an wich are not connected.
|
|
// But a path between them exist (it was precalculated).
|
|
IndoorLink *indoorLink = &nodeLinker->m_ArrayIndoor[layerIndex];
|
|
it = indoorLink->m_ListSameGrid.Begin();
|
|
it2 = indoorLink->m_ListSameGridCoast.Begin();
|
|
it3 = indoorLink->m_ListSameGridLayerCoast.Begin();
|
|
it4 = indoorLink->m_ListSameGridMaxLayerCoast.Begin();
|
|
float slowingDivTresh = pathProblem.m_SlowingDivTreshold;
|
|
for (it ; it != nodeLinker->m_ArrayIndoor[layerIndex].m_ListSameGrid.End(); it++, it2++, it3++, it4++)
|
|
if (*it4 <= pathProblem.m_ObstacleThreshold)
|
|
ManageNodeLinkerSuccesor(pathProblem, context, nodeLinker, *it, *it2+*it3*slowingDivTresh);
|
|
}
|
|
}
|
|
|
|
// Dynamic NodeLinker.
|
|
if (nodeLinker->m_DynNodeLinker[context])
|
|
ManageNodeLinkerSuccesor(pathProblem, context, nodeLinker, nodeLinker->m_DynNodeLinker[context], nodeLinker->m_DynNodeLinkerCoast[context]);
|
|
|
|
nodeLinker->m_ArrayContextInfo[context]->m_Closed = TRUE;
|
|
}
|
|
else
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ManageNodeLinkerSuccesor
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void ManageNodeLinkerSuccesor(PathProblem &pathProblem, int context, NodeLinker *nodeLinker, NodeLinker *nodeLinkerSuccesor, float distance)
|
|
{
|
|
// Reject the linker if target can't take it.
|
|
if (nodeLinkerSuccesor->m_LinkerID >= 0 && !pathProblem.m_Target->HasAttribute(nodeLinkerSuccesor->m_Attribut))
|
|
return;
|
|
|
|
// Reject the linker if obstacle is on the linker.
|
|
CKLayer *layer = nodeLinkerSuccesor->m_Grid->GetLayer(pathProblem.m_ObstacleLayer);
|
|
if (layer)
|
|
{
|
|
int val;
|
|
layer->GetValue(nodeLinkerSuccesor->m_X, nodeLinkerSuccesor->m_Y, &val);
|
|
if (val > pathProblem.m_ObstacleThreshold)
|
|
return;
|
|
}
|
|
|
|
// If the 2 linkers are doors distance is real distance between them.
|
|
if ((nodeLinker->m_TypeCase == tc_door && nodeLinkerSuccesor->m_TypeCase == tc_door) &&
|
|
(nodeLinker->m_LinkerID == nodeLinkerSuccesor->m_LinkerID))
|
|
distance = Magnitude(VxVector(*nodeLinker->m_Pos - *nodeLinkerSuccesor->m_Pos));
|
|
|
|
LinkerContext *contexInfoS = nodeLinkerSuccesor->m_ArrayContextInfo[context];
|
|
|
|
float newCoast = nodeLinker->m_ArrayContextInfo[context]->m_Cost + distance;
|
|
if ((contexInfoS->m_InOpen || contexInfoS->m_Closed) && contexInfoS->m_Cost <= newCoast)
|
|
return;
|
|
|
|
// Update succesor.
|
|
contexInfoS->m_Cost = newCoast;
|
|
contexInfoS->m_F = newCoast;
|
|
contexInfoS->m_Parent = nodeLinker;
|
|
|
|
// Manage list.
|
|
contexInfoS->m_Closed = FALSE;
|
|
if (!contexInfoS->m_InOpen)
|
|
{
|
|
contexInfoS->m_InOpen = TRUE;
|
|
pathProblem.m_OpenListLinker->Push(nodeLinkerSuccesor);
|
|
}
|
|
else
|
|
pathProblem.m_OpenListLinker->Update();
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// AnalyseGrid2GridPath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void AnalyseGrid2GridPath(PathProblem &pathProblem, int context)
|
|
{
|
|
NodeLinker *nodeLinker;
|
|
NodeLinker *nodeLinkerFirt = 0;
|
|
NodeLinker *nodeLinkerSec = 0;
|
|
NodeLinker *nodeLinkerThird = 0;
|
|
NodeLinker *nodeLinkerFour = 0;
|
|
NodeLinker *nodeLinkerLast;
|
|
int numNode = 0;
|
|
int i;
|
|
|
|
// Count the number of Nodegrid. And eliminate the none twin linkers.
|
|
nodeLinker = pathProblem.m_NodeLinkerDynEnd;
|
|
nodeLinkerLast = nodeLinker;
|
|
while (nodeLinker)
|
|
{
|
|
++numNode;
|
|
nodeLinkerFour = nodeLinkerThird;
|
|
nodeLinkerThird = nodeLinkerSec;
|
|
nodeLinkerSec = nodeLinkerFirt;
|
|
nodeLinkerFirt = nodeLinker;
|
|
if ((numNode & 0x00000001) && numNode >= 3 && nodeLinkerSec->m_LinkerID != nodeLinkerFirt->m_LinkerID)
|
|
{
|
|
nodeLinkerThird->m_ArrayContextInfo[context]->m_Parent = nodeLinkerFirt;
|
|
nodeLinkerSec = nodeLinkerThird;
|
|
nodeLinkerThird = nodeLinkerFour;
|
|
--numNode;
|
|
}
|
|
nodeLinker = nodeLinker->m_ArrayContextInfo[context]->m_Parent;
|
|
}
|
|
|
|
if (numNode == 2)
|
|
{
|
|
// Delete unused hyp path.
|
|
int size = pathProblem.m_HypSubPathStart.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete pathProblem.m_HypSubPathStart[i];
|
|
pathProblem.m_HypSubPathStart.Clear();
|
|
|
|
size = pathProblem.m_HypSubPathEnd.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete pathProblem.m_HypSubPathEnd[i];
|
|
pathProblem.m_HypSubPathEnd.Clear();
|
|
|
|
if (!pathProblem.m_HypSubPathStart2End)
|
|
{
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return;
|
|
}
|
|
|
|
// The path is start->end, we have already construct it.
|
|
pathProblem.m_Path.AddSubPathBack(pathProblem.m_HypSubPathStart2End);
|
|
|
|
// Because we don't want to delete it...
|
|
pathProblem.m_HypSubPathStart2End = 0;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHFOUND;
|
|
return;
|
|
}
|
|
|
|
// Find index of start and end hyp path.
|
|
FindHypPath(pathProblem, context, nodeLinkerSec, nodeLinkerLast->m_ArrayContextInfo[context]->m_Parent, &pathProblem.m_SubPathStart, &pathProblem.m_SubPathEnd);
|
|
|
|
// Layer modification when path is calculating can produce no logical situation.
|
|
if (!pathProblem.m_SubPathEnd || !pathProblem.m_SubPathStart || (pathProblem.m_SubPathStart->m_ArrayCase[0] != pathProblem.m_NodeLinkerDynStart->m_Y*pathProblem.m_NodeLinkerDynStart->m_Grid->GetWidth()+pathProblem.m_NodeLinkerDynStart->m_X))
|
|
{
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return;
|
|
}
|
|
|
|
if (numNode == 4)
|
|
{
|
|
// The path is start->linker->linker->end, we all allready construct it.
|
|
pathProblem.m_Path.AddSubPathBack(pathProblem.m_SubPathEnd);
|
|
pathProblem.m_Path.AddSubPathBack(pathProblem.m_SubPathStart);
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StateFunction = STATE_PATHFOUND;
|
|
return;
|
|
}
|
|
|
|
// We remove the nodelinker extremities.
|
|
nodeLinkerThird->m_ArrayContextInfo[context]->m_Parent = 0;
|
|
pathProblem.m_NodeLinkerEnd = pathProblem.m_NodeLinkerEnd->m_ArrayContextInfo[context]->m_Parent->m_ArrayContextInfo[context]->m_Parent;
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
pathProblem.m_StateFunction = STATE_GETALLCASE2CASEPATH;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// FindHypPathIndex
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void FindHypPath(PathProblem &pathProblem, int context, NodeLinker *nodeLinkerFirst, NodeLinker *nodeLinkerLast, SubPath **startPath, SubPath **endPath)
|
|
{
|
|
int i, size;
|
|
int index;
|
|
XArray <SubPath *> *m_HypSubPath;
|
|
|
|
index = nodeLinkerFirst->m_Y*nodeLinkerFirst->m_Grid->GetWidth()+nodeLinkerFirst->m_X;
|
|
m_HypSubPath = &pathProblem.m_HypSubPathStart;
|
|
size = m_HypSubPath->Size();
|
|
for (i = 0; i < size; ++i)
|
|
if ((*m_HypSubPath)[i]->m_ArrayCase[(*m_HypSubPath)[i]->m_ArrayCase.Size()-1] == index)
|
|
{
|
|
*startPath = (*m_HypSubPath)[i];
|
|
m_HypSubPath->Remove(*startPath);
|
|
break;
|
|
}
|
|
|
|
--size;
|
|
for (i = 0; i < size; ++i)
|
|
delete (*m_HypSubPath)[i];
|
|
m_HypSubPath->Clear();
|
|
|
|
index = nodeLinkerLast->m_Y*nodeLinkerLast->m_Grid->GetWidth()+nodeLinkerLast->m_X;
|
|
m_HypSubPath = &pathProblem.m_HypSubPathEnd;
|
|
size = m_HypSubPath->Size();
|
|
for (i = 0; i < size; ++i)
|
|
if ((*m_HypSubPath)[i]->m_ArrayCase[0] == index)
|
|
{
|
|
*endPath = (*m_HypSubPath)[i];
|
|
m_HypSubPath->Remove(*endPath);
|
|
break;
|
|
}
|
|
|
|
--size;
|
|
for (i = 0; i < size; ++i)
|
|
delete (*m_HypSubPath)[i];
|
|
m_HypSubPath->Clear();
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetCase2CasePath
|
|
// Warning :This function is time/frame based.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetCase2CasePath(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
int w = pathProblem.m_GridW;
|
|
NodeGrid *nodeGrid;
|
|
NodeGrid *nodeGridSuccesor;
|
|
int succIndex;
|
|
float newCost;
|
|
float newLayerCost;
|
|
int *m_SuccesorID = gridManager->m_SuccesorID;
|
|
float *m_SuccesorCost = gridManager->m_SuccesorCost;
|
|
int *m_SuccesorLayerCost = gridManager->m_SuccesorLayerCost;
|
|
|
|
do
|
|
{
|
|
// Main A* loop
|
|
if (pathProblem.m_OpenListGrid->Size())
|
|
{
|
|
// Pop the node with le lowest F.
|
|
nodeGrid = pathProblem.m_OpenListGrid->PopBack();
|
|
nodeGrid->m_ArrayContextInfo[context]->m_InOpen = FALSE;
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
|
|
// Does the node is the goal ?
|
|
if (nodeGrid->m_CaseIndex == pathProblem.m_EndIndex)
|
|
{
|
|
pathProblem.m_NodeGrideEnd = nodeGrid;
|
|
return 2;
|
|
}
|
|
|
|
// Get succesor index of nodeGrid.
|
|
gridManager->FillSuccessor(pathProblem, nodeGrid->m_CaseIndex);
|
|
|
|
// For each succesor...
|
|
succIndex = 0;
|
|
while (m_SuccesorID[succIndex] != -1)
|
|
{
|
|
nodeGridSuccesor = &gridManager->m_ArrayNodeGrid[m_SuccesorID[succIndex]];
|
|
|
|
// The first cost is the real distance. The second is the (layer value cost)*(factor).
|
|
GridContext *contextInfoS = nodeGridSuccesor->m_ArrayContextInfo[context];
|
|
newCost = contextInfo->m_Cost + m_SuccesorCost[succIndex];
|
|
newLayerCost = contextInfo->m_LayerCost + m_SuccesorLayerCost[succIndex]*pathProblem.m_SlowingDivTreshold;
|
|
|
|
if ((contextInfoS->m_InOpen || contextInfoS->m_Closed) && contextInfoS->m_Cost + contextInfoS->m_LayerCost <= newCost + newLayerCost)
|
|
{
|
|
++succIndex;
|
|
continue;
|
|
}
|
|
|
|
// Update succesor.
|
|
contextInfoS->m_Cost = newCost;
|
|
contextInfoS->m_LayerCost = newLayerCost;
|
|
|
|
// Heuristic.
|
|
int dx, dy;
|
|
{
|
|
div_t dt = div(nodeGridSuccesor->m_CaseIndex, w);
|
|
dy = dt.quot - pathProblem.m_YEndSearch;
|
|
dx = dt.rem - pathProblem.m_XEndSearch;
|
|
}
|
|
|
|
switch (pathProblem.m_Heuristic)
|
|
{
|
|
case HEURISTIC_EUCLIDIAN_DISTANCE:
|
|
contextInfoS->m_F = newCost + newLayerCost + sqrtf((float) dx*dx+dy*dy)*pathProblem.m_HeuristicCoef;
|
|
break;
|
|
case HEURISTIC_MANHATTAN_DISTANCE:
|
|
contextInfoS->m_F = newCost + newLayerCost + (abs(dx)+abs(dy))*pathProblem.m_HeuristicCoef;
|
|
break;
|
|
case HEURISTIC_SQUARED_EUCLIDIAN_DISTANCE:
|
|
contextInfoS->m_F = newCost + newLayerCost + (dx*dx+dy*dy)*pathProblem.m_HeuristicCoef;
|
|
break;
|
|
case HEURISTIC_OPTIMIZED_EUCLIDIAN_DISTANCE:
|
|
dx=abs(dx);
|
|
dy=abs(dy);
|
|
if (dy>dx)
|
|
{
|
|
dx=dx^dy;
|
|
dy=dx^dy;
|
|
dx=dx^dy;
|
|
}
|
|
contextInfoS->m_F = newCost + newLayerCost + (0.9604f*dx+0.3978f*dy)*pathProblem.m_HeuristicCoef;
|
|
break;
|
|
default:
|
|
contextInfoS->m_F = newCost + newLayerCost;
|
|
}
|
|
// Parent.
|
|
contextInfoS->m_Parent = nodeGrid;
|
|
|
|
// Manage list.
|
|
contextInfoS->m_Closed = FALSE;
|
|
if (!contextInfoS->m_InOpen)
|
|
{
|
|
contextInfoS->m_InOpen = TRUE;
|
|
pathProblem.m_OpenListGrid->Push(nodeGridSuccesor);
|
|
}
|
|
else
|
|
pathProblem.m_OpenListGrid->Update();
|
|
++succIndex;
|
|
}
|
|
contextInfo->m_Closed = TRUE;
|
|
}
|
|
else
|
|
{
|
|
if (pathProblem.m_StateFunction == STATE_GETGRID2DRIDPATH)
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetAllCase2CasePath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GetAllCase2CasePath(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
do
|
|
{
|
|
if (!pathProblem.m_StartPartSearch)
|
|
{
|
|
// We add the last subpath.
|
|
pathProblem.m_Path.AddSubPathFront(pathProblem.m_SubPathEnd);
|
|
|
|
pathProblem.m_NodeGrideEnd = (NodeGrid *)1;
|
|
pathProblem.m_StartPartSearch = TRUE;
|
|
}
|
|
|
|
if (!pathProblem.m_NodeLinkerEnd && pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// We add the first subpath.
|
|
pathProblem.m_Path.AddSubPathBack(pathProblem.m_SubPathStart);
|
|
|
|
// Ready for next state.
|
|
pathProblem.m_StartPartSearch = FALSE;
|
|
pathProblem.m_StateFunction = STATE_PATHFOUND;
|
|
return 1;
|
|
}
|
|
|
|
if (!pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// We search...
|
|
if (!GetCase2CasePath(pathProblem, context))
|
|
{
|
|
pathProblem.m_StateFunction = STATE_PATHNOTFOUND;
|
|
return 0;
|
|
}
|
|
else if (pathProblem.m_NodeGrideEnd)
|
|
{
|
|
// Path found.
|
|
SubPath *subPath = new SubPath(pathProblem.m_GridSearch, pathProblem.m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Cost);
|
|
pathProblem.BuildSubPath(context, subPath);
|
|
pathProblem.m_Path.AddSubPathBack(subPath);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pathProblem.InitBeforeCase2Case(pathProblem.m_NodeLinkerEnd->m_Grid, &gridManager->m_ListGridInfo);
|
|
|
|
// Refresh the context info.
|
|
gridManager->ResetArrayContextInfo(context, pathProblem.m_GridW*pathProblem.m_GridL);
|
|
|
|
|
|
NodeLinker *nodeLinkerEnd = pathProblem.m_NodeLinkerEnd;
|
|
NodeLinker *nodeLinkerParent = nodeLinkerEnd->m_ArrayContextInfo[context]->m_Parent;
|
|
|
|
// Set the linkers.
|
|
pathProblem.SetLinker(nodeLinkerEnd, nodeLinkerParent);
|
|
|
|
// Init start case.
|
|
pathProblem.m_XStartSearch = nodeLinkerEnd->m_X;
|
|
pathProblem.m_YStartSearch = nodeLinkerEnd->m_Y;
|
|
|
|
// Init end case.
|
|
pathProblem.m_XEndSearch = nodeLinkerParent->m_X;
|
|
pathProblem.m_YEndSearch = nodeLinkerParent->m_Y;
|
|
pathProblem.m_EndIndex = pathProblem.m_YEndSearch*pathProblem.m_GridW+pathProblem.m_XEndSearch;
|
|
|
|
// The initial node in the open list is the start case;
|
|
NodeGrid *nodeGrid = &gridManager->m_ArrayNodeGrid[pathProblem.m_YStartSearch*pathProblem.m_GridW+pathProblem.m_XStartSearch];
|
|
GridContext *contextInfo = nodeGrid->m_ArrayContextInfo[context];
|
|
contextInfo->m_Parent = 0;
|
|
pathProblem.m_OpenListGrid->Push(nodeGrid);
|
|
|
|
// We search nothing for now.
|
|
pathProblem.m_NodeGrideEnd = 0;
|
|
|
|
// Next Linker.
|
|
pathProblem.m_NodeLinkerEnd = nodeLinkerParent->m_ArrayContextInfo[context]->m_Parent;
|
|
}
|
|
}
|
|
while (pathProblem.TimeNotOut());
|
|
return 1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// PathFound
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int PathFound(PathProblem &pathProblem, int context)
|
|
{
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
//--gridManager->m_NumPathProblem;
|
|
|
|
// Information for Follow problem
|
|
|
|
// Give num grid to subpath.
|
|
int size = pathProblem.m_Path.NbSubPath();
|
|
for (int i = 0; i < size; ++i)
|
|
{
|
|
SubPath *subPath = pathProblem.m_Path.GetSubPath(i);
|
|
subPath->m_NumGrid = gridManager->GetGridNum(subPath->m_Grid);
|
|
}
|
|
pathProblem.m_Path.m_LastPoint = pathProblem.m_GoalPos;
|
|
pathProblem.m_Path.m_LinkerObs = pathProblem.m_LinkerObs;
|
|
|
|
if (pathProblem.m_Optimize)
|
|
gridManager->OptimizePath(pathProblem);
|
|
pathProblem.m_State = 2;
|
|
pathProblem.m_StateFunction = STATE_FINISH;
|
|
return 2;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// PathNotFound
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int PathNotFound(PathProblem &pathProblem, int context)
|
|
{
|
|
//--pathProblem.m_GridPathManager->m_NumPathProblem;
|
|
pathProblem.m_State = 3;
|
|
pathProblem.m_StateFunction = STATE_FINISH;
|
|
return 0;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Finish
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int Finish(PathProblem &pathProblem, int context)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetPath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::GetPath(CK3dEntity *target, float &coast, int &pathID)
|
|
{
|
|
int *pathContextPtr;
|
|
PathProblem *pathProblem;
|
|
|
|
// Get the pathProblem attached to the target.
|
|
if (!(pathContextPtr = m_Target2PathContext.FindPtr((int)target)))
|
|
return;
|
|
pathProblem = m_ArrayPathProblem[*pathContextPtr];
|
|
pathProblem->m_FollowPathID = m_FollowPathID;
|
|
pathID = m_FollowPathID++;
|
|
coast = pathProblem->m_Path.m_Coast;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetPath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::GetPath(CK3dEntity *target, float &coast, CKCurve *curve)
|
|
{
|
|
int *contextPtr;
|
|
PathProblem *pathProblem;
|
|
CKCurvePoint *cpoint;
|
|
VxVector pos;
|
|
int x, y;
|
|
SubPath *subPath;
|
|
CKGrid *grid;
|
|
int indexPoint = 0;
|
|
int size;
|
|
|
|
// Get the pathProblem attached to the target.
|
|
if (!(contextPtr = m_Target2PathContext.FindPtr((int)target)))
|
|
return;
|
|
pathProblem = m_ArrayPathProblem[*contextPtr];
|
|
|
|
// Fill the curve.
|
|
for (int i = pathProblem->m_Path.NbSubPath()-1; i >= 0 ; --i)
|
|
{
|
|
subPath = pathProblem->m_Path.GetSubPath(i);
|
|
grid = subPath->m_Grid;
|
|
size = subPath->m_ArrayCase.Size();
|
|
for (int j = 0; j < size; ++j)
|
|
{
|
|
if (!(cpoint = curve->GetControlPoint(indexPoint)))
|
|
{
|
|
cpoint = (CKCurvePoint *)(target->GetCKContext()->CreateObject(CKCID_CURVEPOINT, NULL, CK_OBJECTCREATION_DYNAMIC));
|
|
curve->AddControlPoint(cpoint);
|
|
}
|
|
{
|
|
div_t dt = div(subPath->GetCase(j), grid->GetWidth());
|
|
y = dt.quot;
|
|
x = dt.rem;
|
|
}
|
|
subPath->m_Grid->Get3dPosFrom2dCoords(&pos, x, y);
|
|
cpoint->SetLinear(TRUE);
|
|
cpoint->SetPosition(&pos);
|
|
++indexPoint;
|
|
}
|
|
}
|
|
while (cpoint = curve->GetControlPoint(indexPoint))
|
|
{
|
|
curve->RemoveControlPoint(cpoint);
|
|
target->GetCKContext()->DestroyObject(cpoint->GetID());
|
|
}
|
|
coast = pathProblem->m_Path.m_Coast;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetPath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::GetPath(CK3dEntity *target, float &coast, CKDataArray *dataArray)
|
|
{
|
|
int *contextPtr;
|
|
PathProblem *pathProblem;
|
|
VxVector pos;
|
|
int x, y;
|
|
SubPath *subPath;
|
|
CKGrid *grid;
|
|
int indexPoint = 0;
|
|
|
|
// Get the pathProblem attached to the target.
|
|
if (!(contextPtr = m_Target2PathContext.FindPtr((int)target)))
|
|
return;
|
|
pathProblem = m_ArrayPathProblem[*contextPtr];
|
|
|
|
// Fill the array.
|
|
for (int i = pathProblem->m_Path.NbSubPath()-1; i >= 0 ; --i)
|
|
{
|
|
subPath = pathProblem->m_Path.GetSubPath(i);
|
|
grid = subPath->m_Grid;
|
|
for (int j = 0; j < subPath->m_ArrayCase.Size(); ++j)
|
|
{
|
|
{
|
|
div_t dt = div(subPath->GetCase(j), grid->GetWidth());
|
|
y = dt.quot;
|
|
x = dt.rem;
|
|
}
|
|
subPath->m_Grid->Get3dPosFrom2dCoords(&pos, x, y);
|
|
if (dataArray->GetRowCount() <= indexPoint)
|
|
dataArray->InsertRow();
|
|
dataArray->SetElementValue(indexPoint, 0, &pos);
|
|
++indexPoint;
|
|
}
|
|
}
|
|
coast = pathProblem->m_Path.m_Coast;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ManagePathFinding
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::ManagePathFinding()
|
|
{
|
|
int NumProblem = m_NumPathProblem;
|
|
|
|
for (int i = 0; i < m_MaxPathProblem; ++i)
|
|
{
|
|
if (!NumProblem)
|
|
return;
|
|
|
|
PathProblem *pathProblem = m_ArrayPathProblem[i];
|
|
if (pathProblem->m_StateFunction != STATE_FINISH )
|
|
{
|
|
--NumProblem;
|
|
pathProblem->ResetTime();
|
|
do
|
|
{
|
|
pathProblem->m_StateFunction(*pathProblem, i);
|
|
if (pathProblem->m_State != 1)
|
|
break;
|
|
}
|
|
while (pathProblem->TimeNotOut());
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Manage
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::Manage()
|
|
{
|
|
if (!m_GraphDone)
|
|
return;
|
|
ManagePathFinding();
|
|
ManagePathFollow();
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// SequenceToBeDeleted
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKERROR GridPathManager::SequenceToBeDeleted(CK_ID *objids,int count)
|
|
{
|
|
if (!m_GraphDone)
|
|
return CK_OK;
|
|
|
|
int i = 0;
|
|
|
|
// Check if a PathProblem uses an object to be deleted
|
|
if (m_NumPathProblem)
|
|
{
|
|
for (i = 0; i < m_ArrayPathProblem.Size(); ++i)
|
|
{
|
|
PathProblem* problem = m_ArrayPathProblem[i];
|
|
if ( problem->m_StateFunction == STATE_FINISH )
|
|
continue;
|
|
|
|
if ( problem->m_Target->IsToBeDeleted() ||
|
|
(problem->m_GridStart && problem->m_GridStart->IsToBeDeleted()) ||
|
|
(problem->m_GridEnd && problem->m_GridEnd->IsToBeDeleted()) ||
|
|
(problem->m_GridSearch && problem->m_GridSearch->IsToBeDeleted())
|
|
)
|
|
{
|
|
UnregisterPathProblem(problem->m_Target,FALSE);
|
|
problem->m_StateFunction = STATE_FINISH;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check if a FollowProblem uses an object to be deleted
|
|
for (i = 0; i < m_ArrayFollowProblem.Size(); ++i)
|
|
{
|
|
FollowProblem* problem = m_ArrayFollowProblem[i];
|
|
if ( problem->m_State == finish )
|
|
continue;
|
|
|
|
if ( problem->m_Target->IsToBeDeleted() ||
|
|
problem->m_Beh->IsToBeDeleted() ||
|
|
problem->m_Charac->IsToBeDeleted() ||
|
|
problem->m_Grid->IsToBeDeleted()
|
|
)
|
|
{
|
|
UnregisterFollowProblem(problem->m_PathID);
|
|
}
|
|
}
|
|
|
|
return CK_OK;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GridContainLayer
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::GridContainOneLayer(CKGrid *grid, XList <int> *listLayer) const
|
|
{
|
|
IntIt it;
|
|
|
|
for (it = listLayer->Begin(); it != listLayer->End(); it++)
|
|
if (grid->GetLayer(*it))
|
|
return TRUE;
|
|
return FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// FillSuccessor
|
|
// Used to contruct the graph.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::FillSuccessor(CKGrid *grid, int index, int layerIndex)
|
|
{
|
|
m_NumSucc = 0;
|
|
m_Count = 0;
|
|
m_Layer = grid->GetLayer(layerIndex);
|
|
m_GridW = grid->GetWidth();
|
|
m_GridL = grid->GetLength();
|
|
m_CaseY = index/m_GridW;
|
|
m_CaseX = index-m_CaseY*m_GridW;
|
|
for (int i = m_CaseX-1; i <= m_CaseX+1; ++i)
|
|
for (int j = m_CaseY-1; j <= m_CaseY+1; ++j)
|
|
{
|
|
if (i >= 0 && i < m_GridW && j >= 0 && j < m_GridL && (m_CaseX != i || m_CaseY != j))
|
|
{
|
|
if (m_Layer)
|
|
m_Layer->GetValue(i, j, &m_LayerVal);
|
|
else
|
|
m_LayerVal = 0;
|
|
if (m_LayerVal < 255)
|
|
{
|
|
m_SuccesorID[m_NumSucc] = j*m_GridW+i;
|
|
if (m_Count == 0 || m_Count == 2 || m_Count == 6 || m_Count == 8)
|
|
m_SuccesorCost[m_NumSucc] = DISTANCE_CASE_DIAGONAL;
|
|
else
|
|
m_SuccesorCost[m_NumSucc] = DISTANCE_CASE_JUXTAPOSE;
|
|
m_SuccesorLayerCost[m_NumSucc++] = m_LayerVal;
|
|
}
|
|
}
|
|
++m_Count;
|
|
}
|
|
m_SuccesorID[m_NumSucc] = -1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// FillSuccessor
|
|
// with diagonal
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::FillSuccessor(PathProblem &pathProblem, int index)
|
|
{
|
|
// -----
|
|
// |4 6 5|
|
|
// | |
|
|
// |7 9 8|
|
|
// | |
|
|
// |1 3 2|
|
|
// -----
|
|
m_GridW = pathProblem.m_GridW;
|
|
m_GridL = pathProblem.m_GridL;
|
|
// Find pos of index in the grid.
|
|
{
|
|
div_t dt = div(index,m_GridW);
|
|
m_CaseY = dt.quot;
|
|
m_CaseX = dt.rem;
|
|
}
|
|
|
|
m_NumSucc = 0;
|
|
const float *succIndexCoast;
|
|
if (pathProblem.m_Diagonal)
|
|
{
|
|
succIndexCoast = m_SuccesorIndexCost;
|
|
if (!m_CaseY)
|
|
{
|
|
if (!m_CaseX) {m_IndexSuccStart = 1; m_IndexSuccEnd = 3;} // 1
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 7; m_IndexSuccEnd = 9;} // 2
|
|
else {m_IndexSuccStart = 7; m_IndexSuccEnd = 11;}// 3
|
|
}
|
|
else if (m_CaseY == m_GridL-1)
|
|
{
|
|
if (!m_CaseX) {m_IndexSuccStart = 3; m_IndexSuccEnd = 5;} // 4
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 5; m_IndexSuccEnd = 7;} // 5
|
|
else {m_IndexSuccStart = 3; m_IndexSuccEnd = 7;} // 6
|
|
}
|
|
else if (!m_CaseX) {m_IndexSuccStart = 1; m_IndexSuccEnd = 5;} // 7
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 5; m_IndexSuccEnd = 9;} // 8
|
|
else {m_IndexSuccStart = 0; m_IndexSuccEnd = 7;} // 9
|
|
}
|
|
else
|
|
{
|
|
succIndexCoast = m_SuccesorIndexCost2;
|
|
if (!m_CaseY)
|
|
{
|
|
if (!m_CaseX) {m_IndexSuccStart = 0; m_IndexSuccEnd = 1;} // 1
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 3; m_IndexSuccEnd = 4;} // 2
|
|
else {m_IndexSuccStart = 3; m_IndexSuccEnd = 5;} // 3
|
|
}
|
|
else if (m_CaseY == m_GridL-1)
|
|
{
|
|
if (!m_CaseX) {m_IndexSuccStart = 1; m_IndexSuccEnd = 2;} // 4
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 2; m_IndexSuccEnd = 3;} // 5
|
|
else {m_IndexSuccStart = 1; m_IndexSuccEnd = 3;} // 6
|
|
}
|
|
else if (!m_CaseX) {m_IndexSuccStart = 0; m_IndexSuccEnd = 2;} // 7
|
|
else if (m_CaseX == m_GridW-1) {m_IndexSuccStart = 2; m_IndexSuccEnd = 4;} // 8
|
|
else {m_IndexSuccStart = 0; m_IndexSuccEnd = 3;} // 9
|
|
}
|
|
|
|
int indexSucc;
|
|
CKBOOL linkerObs = pathProblem.m_LinkerObs;
|
|
int obsTresh = pathProblem.m_ObstacleThreshold;
|
|
CKSquare *square = pathProblem.m_Square;
|
|
GridPathManager *gridManager = pathProblem.m_GridPathManager;
|
|
|
|
for (int i = m_IndexSuccStart; i <= m_IndexSuccEnd; ++i)
|
|
{
|
|
// Index of the successor.
|
|
indexSucc = pathProblem.m_SuccesorIndex[i] + index;
|
|
|
|
// Test is case is a linker obstacle.
|
|
if (linkerObs && gridManager->CaseIsLinker(pathProblem, indexSucc))
|
|
continue;
|
|
|
|
// Check layer cost.
|
|
int layerCost = 0;
|
|
if (!square || (layerCost = square[indexSucc].dval) <= obsTresh)
|
|
{
|
|
// Add a successor.
|
|
m_SuccesorID[m_NumSucc] = indexSucc;
|
|
m_SuccesorCost[m_NumSucc] = succIndexCoast[i];
|
|
m_SuccesorLayerCost[m_NumSucc++] = layerCost;
|
|
}
|
|
}
|
|
m_SuccesorID[m_NumSucc] = -1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// CaseIsLinker
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::CaseIsLinker(PathProblem &pathProblem, CKGrid *grid, int index)
|
|
{
|
|
NodeLinker *nodeLinker = m_ArrayNodeGrid[index].m_LinkerObs[(*m_ListGridInfo.FindPtr((int)grid))->m_Num];
|
|
|
|
if (nodeLinker)
|
|
{
|
|
if (nodeLinker->m_TypeCase == tc_door)
|
|
return FALSE;
|
|
if (pathProblem.m_Target->HasAttribute(nodeLinker->m_Attribut))
|
|
{
|
|
if (index != pathProblem.m_EndIndex)
|
|
return TRUE;
|
|
else
|
|
return FALSE;
|
|
}
|
|
return TRUE;
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// CaseIsLinker
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::CaseIsLinker(PathProblem &pathProblem, int index)
|
|
{
|
|
NodeLinker *nodeLinker = m_ArrayNodeGrid[index].m_LinkerObs[pathProblem.m_NumGrid];
|
|
|
|
if (nodeLinker)
|
|
{
|
|
if (nodeLinker->m_TypeCase == tc_door)
|
|
return FALSE;
|
|
if (pathProblem.m_Target->HasAttribute(nodeLinker->m_Attribut))
|
|
{
|
|
if (index != pathProblem.m_EndIndex)
|
|
return TRUE;
|
|
else
|
|
return FALSE;
|
|
}
|
|
return TRUE;
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// CasesDistance
|
|
// This function do a A* search, but it is not time/frame based
|
|
// because it is used to initialyse the graph.
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
GridContext *GridPathManager::CasesDistance(CKGrid *grid, int layer, int sX, int sY, int eX, int eY, int &maxLayerValue)
|
|
{
|
|
OpenListGrid listNodeGrid(0, (int)(grid->GetWidth()*grid->GetLength()*PRECALC_OPENLIST_BUFFER_FACTOR));
|
|
int w = grid->GetWidth();
|
|
int goalIndex = eY*w+eX;
|
|
int index;
|
|
NodeGrid *nodeGrid;
|
|
NodeGrid *nodeGridSuccesor;
|
|
int succIndex;
|
|
float newCoast;
|
|
int succX, succY;
|
|
|
|
// Init start node
|
|
index = sY*grid->GetWidth()+sX;
|
|
m_ArrayNodeGrid[index].m_ArrayContextInfo[0]->m_Cost = 0;
|
|
m_ArrayNodeGrid[index].m_ArrayContextInfo[0]->m_F = sqrtf((float)(((sX-eX)*(sX-eX)+(sY-eY)*(sY-eY))));
|
|
|
|
// Add start node in open list.
|
|
listNodeGrid.Push(&m_ArrayNodeGrid[index]);
|
|
|
|
// Main A* loop
|
|
while (listNodeGrid.Size())
|
|
{
|
|
// Pop the node with le lowest F.
|
|
nodeGrid = listNodeGrid.PopBack();
|
|
nodeGrid->m_ArrayContextInfo[0]->m_InOpen = FALSE;
|
|
|
|
// Does the node is the goal ?
|
|
if (nodeGrid->m_CaseIndex == goalIndex)
|
|
{
|
|
// Search the max layer value.
|
|
NodeGrid * nodeGrid2 = nodeGrid;
|
|
maxLayerValue = 0;
|
|
while (nodeGrid2)
|
|
{
|
|
if (maxLayerValue < nodeGrid2->m_LayerValue)
|
|
maxLayerValue = nodeGrid2->m_LayerValue;
|
|
nodeGrid2 = nodeGrid2->m_ArrayContextInfo[0]->m_Parent;
|
|
}
|
|
return nodeGrid->m_ArrayContextInfo[0];
|
|
}
|
|
|
|
// Get succesor index of nodeGrid.
|
|
FillSuccessor(grid, nodeGrid->m_CaseIndex, layer);
|
|
|
|
// For each succesor...
|
|
succIndex = 0;
|
|
while (m_SuccesorID[succIndex] != -1)
|
|
{
|
|
nodeGridSuccesor = &m_ArrayNodeGrid[m_SuccesorID[succIndex]];
|
|
GridContext *gridContextS = nodeGridSuccesor->m_ArrayContextInfo[0];
|
|
newCoast = nodeGrid->m_ArrayContextInfo[0]->m_Cost + m_SuccesorCost[succIndex];
|
|
|
|
if ((gridContextS->m_InOpen || gridContextS->m_Closed) && gridContextS->m_Cost <= newCoast)
|
|
{
|
|
succIndex++;
|
|
continue;
|
|
}
|
|
|
|
// Update succesor.
|
|
gridContextS->m_Cost = newCoast;
|
|
{
|
|
div_t dt = div(nodeGridSuccesor->m_CaseIndex, w);
|
|
succY = dt.quot;
|
|
succX = dt.rem;
|
|
}
|
|
gridContextS->m_F = newCoast + sqrtf((float)(((succX-eX)*(succX-eX)+(succY-eY)*(succY-eY))));
|
|
gridContextS->m_Parent = nodeGrid;
|
|
nodeGridSuccesor->m_LayerValue = m_SuccesorLayerCost[succIndex];
|
|
|
|
// Manage list.
|
|
gridContextS->m_Closed = FALSE;
|
|
if (!gridContextS->m_InOpen)
|
|
{
|
|
gridContextS->m_InOpen = TRUE;
|
|
listNodeGrid.Push(nodeGridSuccesor);
|
|
}
|
|
else
|
|
listNodeGrid.Update();
|
|
succIndex++;
|
|
}
|
|
|
|
nodeGrid->m_ArrayContextInfo[0]->m_Closed = TRUE;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ResetArrayContextInfo
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::ResetArrayContextInfo(int context, int numCase) const
|
|
{
|
|
for (int i = 0; i < numCase; i++)
|
|
{
|
|
GridContext *contextInfo = m_ArrayNodeGrid[i].m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0.0f;
|
|
contextInfo->m_LayerCost = 0.0f;
|
|
contextInfo->m_Closed = FALSE;
|
|
contextInfo->m_InOpen = FALSE;
|
|
contextInfo->m_Parent = 0;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ResetArrayLinkerContextInfo
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::ResetArrayLinkerContextInfo(int context) const
|
|
{
|
|
for (NodeLinkerIt it = m_ListNodeLinker.Begin(); it != m_ListNodeLinker.End(); it++)
|
|
{
|
|
LinkerContext *contextInfo = (*it)->m_ArrayContextInfo[context];
|
|
contextInfo->m_Cost = 0.0f;
|
|
contextInfo->m_LayerCost = 0.0f;
|
|
contextInfo->m_Closed = FALSE;
|
|
contextInfo->m_InOpen = FALSE;
|
|
contextInfo->m_Parent = 0;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// GetGridNum
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
int GridPathManager::GetGridNum(CKGrid *grid)
|
|
{
|
|
return (*m_ListGridInfo.FindPtr((int)grid))->m_Num;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// OptimizePath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void GridPathManager::OptimizePath(PathProblem &pp)
|
|
{
|
|
CKLayer *layer;
|
|
SubPath *path;
|
|
int size = pp.m_Path.NbSubPath();
|
|
int size2;
|
|
XArray <int> *arrayCase;
|
|
|
|
for (int i = 0; i < size; ++i)
|
|
{
|
|
// For each subpath.
|
|
path = pp.m_Path.GetSubPath(i);
|
|
layer = path->m_Grid->GetLayer(pp.m_ObstacleLayer);
|
|
if (layer)
|
|
pp.m_Square = layer->GetSquareArray();
|
|
else
|
|
pp.m_Square = 0;
|
|
arrayCase = &path->m_ArrayCase;
|
|
pp.m_GridW = path->m_Grid->GetWidth();
|
|
pp.m_NumGrid = path->m_NumGrid;
|
|
int indexMax = pp.m_GridW*path->m_Grid->GetLength();
|
|
|
|
// We check obstacles between each point.
|
|
size2 = arrayCase->Size();
|
|
for (int j = 0; j < size2-2; ++j)
|
|
{
|
|
int next = j+1;
|
|
int k = j+2;
|
|
int checkLinker;
|
|
if (!j || j == size2-1)
|
|
checkLinker = 1;
|
|
else
|
|
checkLinker = 0;
|
|
while (k < size2 &&!ObsBetweenPoint(pp, (*arrayCase)[j], (*arrayCase)[k], indexMax))
|
|
{
|
|
next = k;
|
|
++k;
|
|
}
|
|
for (int l = j+1; l < next; ++l)
|
|
{
|
|
(*arrayCase).RemoveAt(j+1);
|
|
--size2;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// CheckObsBetweenPoint
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL GridPathManager::ObsBetweenPoint(PathProblem &pp, int start, int end, int indexMax)
|
|
{
|
|
int w = pp.m_GridW;
|
|
div_t dt = div(start, w);
|
|
int y1 = dt.quot;
|
|
int x1 = dt.rem;
|
|
dt = div(end, w);
|
|
int y2 = dt.quot;
|
|
int x2 = dt.rem;
|
|
int i,j,tmp;
|
|
int Y1,Y2;
|
|
int index;
|
|
unsigned int obs = pp.m_ObstacleLayer;
|
|
int numGrid = pp.m_NumGrid;
|
|
CKSquare *square = pp.m_Square;
|
|
float error = 0.0f;
|
|
|
|
if (!pp.m_LinkerObs && !square)
|
|
return FALSE;
|
|
|
|
// Vertical case.
|
|
if (x1 == x2)
|
|
{
|
|
if (y1 > y2) {tmp = y1; y1 = y2; y2 = tmp;}
|
|
for (j = y1; j <= y2; j++)
|
|
{
|
|
index = j*w+x1;
|
|
if (square && (square[index].dval > obs))
|
|
return TRUE;
|
|
if (pp.m_LinkerObs && m_ArrayNodeGrid[index].m_LinkerObs[numGrid])
|
|
return TRUE;
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
// Other case.
|
|
if (x1 > x2)
|
|
{
|
|
tmp = x1; x1 = x2; x2 = tmp;
|
|
tmp = y1; y1 = y2; y2 = tmp;
|
|
}
|
|
|
|
float x1bis = -x1+0.5f;
|
|
float x1bis2 = x1bis+1;
|
|
float dy = ((float)(y2-y1)/(x2-x1));
|
|
|
|
Y1 = y1;
|
|
Y2 = y1+(int)(ceilf((x1+x1bis)*dy));
|
|
if (Y1 > Y2) {tmp = Y1; Y1 = Y2; Y2 = tmp;}
|
|
Y1 = (int)(Y1-error);
|
|
Y2 = (int)(ceilf(Y2+error));
|
|
for (j = Y1; j <= Y2; ++j)
|
|
{
|
|
index = j*w+x1;
|
|
if (index == start || index == end || index >= indexMax) continue;
|
|
if (square && (square[index].dval > obs))
|
|
return TRUE;
|
|
if (pp.m_LinkerObs && m_ArrayNodeGrid[index].m_LinkerObs[numGrid])
|
|
return TRUE;
|
|
}
|
|
|
|
for (i = x1; i < x2-1; ++i)
|
|
{
|
|
Y1 = (int)(y1+(i+x1bis)*dy);
|
|
Y2 = (int)(ceilf(y1+(i+x1bis2)*dy));
|
|
if (Y1 > Y2) {tmp = Y1; Y1 = Y2; Y2 = tmp;}
|
|
Y1 = (int)(Y1-error);
|
|
Y2 = (int)(ceilf(Y2+error));
|
|
for (j = Y1; j <= Y2; ++j)
|
|
{
|
|
index = j*w+i+1;
|
|
if (index >= indexMax) continue;
|
|
if (square && (square[index].dval > obs))
|
|
return TRUE;
|
|
if (pp.m_LinkerObs && m_ArrayNodeGrid[index].m_LinkerObs[numGrid])
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
Y1 = (int)(ceilf(y1+(x2-2+x1bis2)*dy));
|
|
Y2 = y2;
|
|
if (Y1 > Y2) {tmp = Y1; Y1 = Y2; Y2 = tmp;}
|
|
Y1 = (int)(Y1-error);
|
|
Y2 = (int)(ceilf(Y2+error));
|
|
for (j = Y1; j <= Y2; ++j)
|
|
{
|
|
index = j*w+x2;
|
|
if (index == start || index == end || index >= indexMax) continue;
|
|
if (square && (square[index].dval > obs))
|
|
return TRUE;
|
|
if (pp.m_LinkerObs && m_ArrayNodeGrid[index].m_LinkerObs[numGrid])
|
|
return TRUE;
|
|
}
|
|
return FALSE;
|
|
}
|