// Given the specified position and velocity vectors for a given orbit, retuns the position
// and velocity vectors after a specified time
void PredictPosVelVectors(const VECTOR3 &Pos, const VECTOR3 &Vel, double a, double Mu,
double Time, VECTOR3 &NewPos, VECTOR3 &NewVel, double &NewVelMag)
{
double SqrtMu = sqrt(Mu);
// Variables for computation
double X = (SqrtMu * Time) / a; // Initial guesses for X
double Z = CalcZ(X, a); // and Z
double C, S; // C(Z) and S(Z)
double F, FDot, G, GDot;
// Calculate the X and Z for the specified time of flight
CalcXandZ(X, Z, Pos, Vel, a, Time, SqrtMu);
// Calculate C(Z) and S(Z)
C = CalcC(Z);
S = CalcS(Z);
// Calculate the new position and velocity vectors
F = CalcF(X, C, Mag(Pos));
G = CalcG(Time, X, S, SqrtMu);
NewPos = (Pos * F) + (Vel * G);
FDot = CalcFDot(SqrtMu, Mag(Pos), Mag(NewPos), X, Z, S);
GDot = CalcGDot(X, C, Mag(NewPos));
NewVel = (Pos * FDot) + (Vel * GDot);
NewVelMag = Mag(NewVel);
}
void NotFoundPoint(CPoint* tmpStart, CPoint* end, CPoint* point)
{
point->m_parentPoint = tmpStart;
point->G = CalcG(tmpStart, point);
point->G = CalcH(end, point);
point->CalcF();
m_openVec.push_back(point);
}
void KPathFinder::NotFoundPoint(KCellEx* pCurrent, KCellEx* pDest, KCellEx* pSurround)
{
pSurround->pWayFindingData->pParent = pCurrent;
pSurround->pWayFindingData->G = CalcG(pCurrent, pSurround);
pSurround->pWayFindingData->H = CalcH(pSurround, pDest);
pSurround->pWayFindingData->UpdateF();
m_setOpenPoint.insert(pSurround);
pSurround->pWayFindingData->bOpenPoint = true;
}
void KPathFinder::FoundPoint(KCellEx* pCurrent, KCellEx* pSurround)
{
int G = CalcG(pCurrent, pSurround);
if (G < pSurround->pWayFindingData->G)
{
pSurround->pWayFindingData->pParent = pCurrent;
pSurround->pWayFindingData->G = G;
pSurround->pWayFindingData->UpdateF();
}
}
void RefreshPoint(CPoint* tmpStart, CPoint* point)
{
int valueG = CalcG(tmpStart, point);
if(valueG < point->G)
{
point->m_parentPoint = tmpStart;
point->G = valueG;
point->CalcF();
}
}
void CPathFind::InitNode(int startx,int starty,int endx,int endy)
{
m_startnode = new Node();
m_startnode->x = startx;
m_startnode->y = starty ;
m_startnode->G = CalcG(startx,starty);
m_startnode->H = 0 ;
m_startnode->F = m_startnode->G+ m_startnode->H;
m_startnode->father = NULL;
m_endnode = new Node();
m_endnode->x = endx;
m_endnode->y = endy ;
m_endnode->G = CalcG(endx,endy);
m_endnode->H = 0 ;
m_endnode->F = m_endnode->G+ m_endnode->H;
m_endnode->father = NULL;
}
pNode CPathFind::CreateFirstNode(int x, int y)
{
pNode node = new Node;
node->x = x;
node->y = y;
node->H = 0;
node->G = CalcG(x, y);
node->F = node->H + node->G;
node->father = NULL;
return node;
}
bool CPathFind::PathFind(int startx, int starty, int endx, int endy) {
InitNode(startx,starty,endx,endy);
//首先做几个预先判断
if ((startx == endx) && (starty == endy)) {
cout << "起点就是终点";
}
m_startx = startx;
m_starty = starty;
m_endx = endx;
m_endy = endy;
pNode node = CreateFirstNode(startx, starty);
//将这个节点放入openlist
m_openlist.push_back(node);
//对这个地方进行排序
push_heap(m_openlist.begin(),m_openlist.end(),NodeSort);
pNode tmpnode = NULL;
for(;;)
{
if(m_openlist.empty())
{
cout<<"错误:不能找到目标节点"<<endl;
return false;
}
tmpnode = m_openlist.front();
++m_step;
pop_heap(m_openlist.begin(),m_openlist.end(),NodeSort);
m_openlist.pop_back();
if (!CheckIsDestination(tmpnode))
{
//这里不是是目标地点
for(int i = 0 ;i<8 ;i++)
{
int nextx ;
int nexty ;
GetShiftByDirectory(i,&nextx,&nexty);
nextx = tmpnode->x + nextx;
nexty = tmpnode->y + nexty;
//判断这个点是可以通过的
cout<<"next is"<<nextx<<":"<<nexty<<endl;
if(isIllegle(nextx,nexty))
{
//这里可以通过
//计算这个点的G值
int newGvalue;
if(i % 2 ==0)
newGvalue = tmpnode->G+10;
else
newGvalue = tmpnode->G+14;
vector<pNode>::iterator OpenIt;
//说明该节点在OPEN表中
for(OpenIt=m_openlist.begin();OpenIt<m_openlist.end();OpenIt++)
{
if (((*OpenIt)->x == nextx)&&((*OpenIt)->y ==nexty))
{
break;
}
}
if(OpenIt != m_openlist.end())
{
if ((*OpenIt)->G <= newGvalue)
continue;
}
//说明该节点在close表中
vector<pNode>::iterator CloseIt;
for(CloseIt=m_closelist.begin();CloseIt<m_closelist.end();CloseIt++)
{
if (((*CloseIt)->x == nextx)&&((*CloseIt)->y ==nexty))
{
break;
}
}
if(CloseIt != m_closelist.end())
{
if ((*CloseIt)->G <= newGvalue)
continue;
}
//如果都不满足上边的条件那么说明这个节点是最优节点
Node *bestNode = new Node;
bestNode->x = nextx;
bestNode->y = nexty;
bestNode->father = tmpnode;
bestNode->G = newGvalue;
bestNode->H = CalcG(nextx,nexty);
bestNode->F = bestNode->G + bestNode->H;
if (CloseIt != m_closelist.end())
{
delete(*CloseIt);
m_closelist.erase(CloseIt);
}
if (OpenIt != m_openlist.end())
{
delete(*OpenIt);
m_openlist.erase(OpenIt);
make_heap(m_openlist.begin(),m_openlist.end(),NodeSort);
}
m_openlist.push_back(bestNode);
push_heap(m_openlist.begin(),m_openlist.end(),NodeSort);
for(vector<pNode>::iterator k = m_openlist.begin() ;k<m_openlist.end();k++)
{
cout<<"x:"<<(*k)->x<<",y:"<<(*k)->y<<endl;
}
cout<<" isIlleglea is true"<<endl;
}
else
{
cout<<" isIlleglea is false"<<endl;
//不能通过
}
}
m_closelist.push_back(tmpnode);
}
else
{
generatePath();
return true;
}
}
}