390 lines
11 KiB
C++
390 lines
11 KiB
C++
#include "CKAll.h"
|
|
#include "PathProblem.h"
|
|
#include "float.h"
|
|
#include "GridInfo.h"
|
|
|
|
#ifdef PSP
|
|
#undef _MASTER
|
|
#endif
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// PathProblem
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
PathProblem::PathProblem()
|
|
:m_Target(0),
|
|
m_GoalPos(0,0,0),
|
|
m_ObstacleLayer(-1),
|
|
m_ObstacleThreshold(0),
|
|
m_SlowingFactor(0),
|
|
m_Linker(0),
|
|
m_HeuristicCoef(0),
|
|
m_TimeFrame(0),
|
|
m_OpenListLinker(0),
|
|
m_OpenListGrid(0),
|
|
m_State(1),
|
|
m_GridStart(0),
|
|
m_GridEnd(0),
|
|
m_GridSearch(0),
|
|
m_NodeGrideEnd(0),
|
|
m_NodeLinkerEnd(0),
|
|
m_StartPartSearch(FALSE),
|
|
m_NodeLinkerDynStart(0),
|
|
m_NodeLinkerDynEnd(0),
|
|
m_HypSubPathStart2End(0),
|
|
m_HypSubPathStart2EndCoast(FLT_MAX),
|
|
m_SubPathStart(0),
|
|
m_SubPathEnd(0),
|
|
m_FollowPathID(-1)
|
|
{
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ~PathProblem
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
PathProblem::~PathProblem()
|
|
{
|
|
int i;
|
|
int size;
|
|
|
|
size = m_HypSubPathStart.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete m_HypSubPathStart[i];
|
|
m_HypSubPathStart.Clear();
|
|
|
|
size = m_HypSubPathEnd.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete m_HypSubPathEnd[i];
|
|
m_HypSubPathEnd.Clear();
|
|
|
|
if (m_OpenListLinker)
|
|
delete m_OpenListLinker;
|
|
if (m_OpenListGrid)
|
|
delete m_OpenListGrid;
|
|
if (m_NodeLinkerDynStart)
|
|
delete m_NodeLinkerDynStart;
|
|
if (m_NodeLinkerDynEnd)
|
|
delete m_NodeLinkerDynEnd;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Set
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::Set(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, StatePathFunction stateFunction)
|
|
{
|
|
m_Target = target;
|
|
m_Target->Transform(&m_TargetPos, targetPos);
|
|
|
|
if (goalRef)
|
|
goalRef->Transform(&m_GoalPos, goalPos);
|
|
else
|
|
m_GoalPos = *goalPos;
|
|
|
|
m_ObstacleLayer = obstacleLayer;
|
|
if (obstacleThreshold > 255)
|
|
m_ObstacleThreshold = 255;
|
|
else if (m_ObstacleThreshold < 0)
|
|
m_ObstacleThreshold = 0;
|
|
else
|
|
m_ObstacleThreshold = obstacleThreshold;
|
|
m_SlowingFactor = (float)fabs(slowingFactor);
|
|
m_SlowingDivTreshold = m_SlowingFactor/(float)(m_ObstacleThreshold+1);
|
|
m_Linker = linker;
|
|
m_LinkerObs = linkerObs;
|
|
if (heuristicCoef < 0)
|
|
m_HeuristicCoef = 0;
|
|
else
|
|
m_HeuristicCoef = heuristicCoef;
|
|
m_TimeFrame = timeFrame;
|
|
|
|
m_Heuristic = heuristic;
|
|
m_Diagonal = diagonal;
|
|
m_Optimize = optimize;
|
|
|
|
// Problem is waiting for solution.
|
|
m_StateFunction = stateFunction;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// SetOpenList
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::SetOpenList(int numContext, int buffLinker, int buffGrid)
|
|
{
|
|
m_OpenListLinker = new OpenListLinker(numContext, buffLinker);
|
|
m_OpenListGrid = new OpenListGrid(numContext, buffGrid);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Reset
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::Reset()
|
|
{
|
|
int i;
|
|
int size;
|
|
|
|
// Init the solution data.
|
|
m_State = 1;
|
|
m_GridStart = 0;
|
|
m_GridEnd = 0;
|
|
|
|
// We clear the open list just in case if the search was stopped.
|
|
if (m_OpenListGrid)
|
|
m_OpenListGrid->Clear();
|
|
if (m_OpenListLinker)
|
|
m_OpenListLinker->Clear();
|
|
m_StartPartSearch = FALSE;
|
|
|
|
// Clear the dyn nodelinker
|
|
m_NodeLinkerDynStart->ClearConnected();
|
|
m_NodeLinkerDynStart->ClearDynConnected();
|
|
m_NodeLinkerDynEnd->ClearConnected();
|
|
m_NodeLinkerDynEnd->ClearDynConnected();
|
|
|
|
// Clear the path
|
|
size = m_Path.m_ArraySubPath.Size();
|
|
for (i = 0; i < size; i++)
|
|
delete m_Path.m_ArraySubPath[i];
|
|
m_Path.m_ArraySubPath.Clear();
|
|
m_Path.m_Coast = 0;
|
|
|
|
|
|
size = m_HypSubPathStart.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete m_HypSubPathStart[i];
|
|
m_HypSubPathStart.Clear();
|
|
|
|
size = m_HypSubPathEnd.Size();
|
|
for (i = 0; i < size; ++i)
|
|
delete m_HypSubPathEnd[i];
|
|
m_HypSubPathEnd.Clear();
|
|
|
|
m_SubPathStart = 0;
|
|
m_SubPathEnd = 0;
|
|
|
|
if(m_HypSubPathStart2End)
|
|
{
|
|
delete m_HypSubPathStart2End;
|
|
m_HypSubPathStart2End = 0;
|
|
}
|
|
m_HypSubPathStart2EndCoast = FLT_MAX;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ResetOpenList
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::ResetOpenList(CKBOOL linker)
|
|
{
|
|
if (linker)
|
|
{
|
|
if (m_OpenListLinker)
|
|
m_OpenListLinker->Clear();
|
|
}
|
|
else
|
|
{
|
|
if (m_OpenListGrid)
|
|
m_OpenListGrid->Clear();
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// BuildHypPath
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::BuildSubPath(int context, SubPath *subPath, CKBOOL back) const
|
|
{
|
|
NodeGrid *node;
|
|
NodeGrid *prevNode;
|
|
int width = subPath->m_Grid->GetWidth();
|
|
int oldCaseIndex;
|
|
int lastCase = -1;
|
|
int oldDir;
|
|
int dir;
|
|
int diffCase;
|
|
|
|
subPath->SetLinker(m_Linker1, m_Linker2);
|
|
|
|
if (!m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Parent)
|
|
{
|
|
// The SubPath is just one case. (but a path is one goal and one end...)
|
|
subPath->PushBack(m_NodeGrideEnd->m_CaseIndex);
|
|
subPath->PushFront(m_NodeGrideEnd->m_CaseIndex);
|
|
return;
|
|
}
|
|
|
|
// Push the first index;
|
|
subPath->PushBack(m_NodeGrideEnd->m_CaseIndex);
|
|
|
|
node = m_NodeGrideEnd->m_ArrayContextInfo[context]->m_Parent;
|
|
diffCase = node->m_CaseIndex - m_NodeGrideEnd->m_CaseIndex;
|
|
if (diffCase == 1)
|
|
oldDir = 1;
|
|
else if (diffCase == -1)
|
|
oldDir = 2;
|
|
else if (diffCase == width)
|
|
oldDir = 3;
|
|
else if (diffCase == -width)
|
|
oldDir = 4;
|
|
else if (diffCase == width + 1)
|
|
oldDir = 5;
|
|
else if (diffCase == width - 1)
|
|
oldDir = 6;
|
|
else if (diffCase == -width + 1)
|
|
oldDir = 7;
|
|
else if (diffCase == -width - 1)
|
|
oldDir = 8;
|
|
oldCaseIndex = node->m_CaseIndex;
|
|
node = node->m_ArrayContextInfo[context]->m_Parent;
|
|
|
|
if (!node)
|
|
{
|
|
// There is only 2 nodes.
|
|
if (back)
|
|
subPath->PushBack(oldCaseIndex);
|
|
else
|
|
subPath->PushFront(oldCaseIndex);
|
|
return;
|
|
}
|
|
|
|
while (node)
|
|
{
|
|
diffCase = node->m_CaseIndex - oldCaseIndex;
|
|
|
|
// Determine the dir(ection) of the node.
|
|
if (diffCase == 1)
|
|
dir = 1;
|
|
else if (diffCase == -1)
|
|
dir = 2;
|
|
else if (diffCase == width)
|
|
dir = 3;
|
|
else if (diffCase == -width)
|
|
dir = 4;
|
|
else if (diffCase == width + 1)
|
|
dir = 5;
|
|
else if (diffCase == width - 1)
|
|
dir = 6;
|
|
else if (diffCase == -width + 1)
|
|
dir = 7;
|
|
else if (diffCase == -width - 1)
|
|
dir = 8;
|
|
if (dir != oldDir || !m_Optimize)
|
|
{
|
|
if (back)
|
|
subPath->PushBack(oldCaseIndex);
|
|
else
|
|
subPath->PushFront(oldCaseIndex);
|
|
oldDir = dir;
|
|
lastCase = oldCaseIndex;
|
|
}
|
|
oldCaseIndex = node->m_CaseIndex;
|
|
prevNode = node;
|
|
node = node->m_ArrayContextInfo[context]->m_Parent;
|
|
}
|
|
|
|
if (lastCase != prevNode->m_CaseIndex)
|
|
{
|
|
// Push the last index;
|
|
if (back)
|
|
subPath->PushBack(prevNode->m_CaseIndex);
|
|
else
|
|
subPath->PushFront(prevNode->m_CaseIndex);
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ResetTime
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::ResetTime()
|
|
{
|
|
m_Time.Reset();
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// TimeNotOut
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL PathProblem::TimeNotOut()
|
|
{
|
|
if (m_Time.Current() < m_TimeFrame)
|
|
return TRUE;
|
|
return FALSE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ItLinkerStartIsGood
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL PathProblem::Linker2StartIsGood()
|
|
{
|
|
int dx = m_NodeLinkerDynStart->m_X - (*m_ItLinkerStart)->m_X;
|
|
int dy = m_NodeLinkerDynStart->m_Y - (*m_ItLinkerStart)->m_Y;
|
|
|
|
if (sqrt((float)(dx*dx+dy*dy)) > m_HypSubPathStart2EndCoast)
|
|
return FALSE;
|
|
if (!m_Target->HasAttribute((*m_ItLinkerStart)->m_Attribut))
|
|
return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ItLinkerEndIsGood
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
CKBOOL PathProblem::Linker2EndIsGood()
|
|
{
|
|
int dx = m_NodeLinkerDynEnd->m_X - (*m_ItLinkerStart)->m_X;
|
|
int dy = m_NodeLinkerDynEnd->m_Y - (*m_ItLinkerStart)->m_Y;
|
|
|
|
if (sqrt((float)(dx*dx+dy*dy)) > m_HypSubPathStart2EndCoast)
|
|
return FALSE;
|
|
if (!m_Target->HasAttribute((*m_ItLinkerStart)->m_Attribut))
|
|
return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// SetLinker
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::SetLinker(NodeLinker *linker1, NodeLinker *linker2)
|
|
{
|
|
m_Linker1 = linker1;
|
|
m_Linker2 = linker2;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// InitCase2Case
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
void PathProblem::InitBeforeCase2Case(CKGrid *grid, XSHashTable <GridInfo *, int> *listGridInfo)
|
|
{
|
|
ResetOpenList(FALSE);
|
|
m_GridSearch = grid;
|
|
m_NumGrid = (*listGridInfo->FindPtr((int)m_GridSearch))->m_Num;
|
|
m_GridW = m_GridSearch->GetWidth();
|
|
m_GridL = m_GridSearch->GetLength();
|
|
|
|
CKLayer *layer = m_GridSearch->GetLayer(m_ObstacleLayer);
|
|
if (layer)
|
|
m_Square = layer->GetSquareArray();
|
|
else
|
|
m_Square = 0;
|
|
|
|
if (m_Diagonal)
|
|
{
|
|
m_SuccesorIndex[0] = m_GridW-1;
|
|
m_SuccesorIndex[1] = m_GridW;
|
|
m_SuccesorIndex[2] = m_GridW+1;
|
|
m_SuccesorIndex[3] = 1;
|
|
m_SuccesorIndex[4] = -m_GridW+1;
|
|
m_SuccesorIndex[5] = -m_GridW;
|
|
m_SuccesorIndex[6] = -m_GridW-1;
|
|
m_SuccesorIndex[7] = -1;
|
|
m_SuccesorIndex[8] = m_GridW-1;
|
|
m_SuccesorIndex[9] = m_GridW;
|
|
m_SuccesorIndex[10] = m_GridW+1;
|
|
m_SuccesorIndex[11] = 1;
|
|
}
|
|
else
|
|
{
|
|
m_SuccesorIndex[0] = m_GridW;
|
|
m_SuccesorIndex[1] = 1;
|
|
m_SuccesorIndex[2] = -m_GridW;
|
|
m_SuccesorIndex[3] = -1;
|
|
m_SuccesorIndex[4] = m_GridW;
|
|
m_SuccesorIndex[5] = 1;
|
|
}
|
|
}
|