📄 ainavigation.cpp
字号:
Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
VDir.Normalize();
// Init the left side
LTarget=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
VDir*(m_fLSideDist+1.f);
p=LTarget-m_Route[nRouteIdx-1].Vtx;
fLX=aiDot(p,m_pRoads[nCRd]->VertXDir(nIndex));
fZ=aiDot(p,m_pRoads[nCRd]->VertZDir(nIndex));
if(fLX>0.f && nRouteIdx==1)
{
Position=m_Route[nRouteIdx-1].Vtx+m_pRoads[nCRd]->VertXDir(nIndex)*fLX;
fLX=0.f;
}
if(fZ<1.f && nRouteIdx==1)fZ=1.f;
fLViewAngle=atan2f(fLX,fZ);
// Now init the right side
RTarget=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)-
VDir*(m_fRSideDist+1.f);
p=RTarget-m_Route[nRouteIdx-1].Vtx;
fRX=aiDot(p,m_pRoads[nCRd]->VertXDir(nIndex));
fZ=aiDot(p,m_pRoads[nCRd]->VertZDir(nIndex));
if(fRX<0.f && nRouteIdx==1)
{
Position=m_Route[nRouteIdx-1].Vtx-m_pRoads[nCRd]->VertXDir(nIndex)*fRX;
fRX=0.f;
}
if(fZ<1.f && nRouteIdx==1)fZ=1.f;
fRViewAngle=atan2f(fRX,fZ);
}
// Check see if we can see the destination.
int bKnownDir=false;
nLVIdx=nRVIdx=nCVIdx;
nLRd=nRRd=nCRd;
nCVIdx++;
if(nCVIdx==m_pRoads[nCRd]->NumVerts())
{
if(m_pRoads[nCRd+1])
{
nCVIdx=0;
nCRd++;
nNumVerts=m_pRoads[nCRd]->NumVerts();
}
else
nCVIdx--;
}
for(nCVIdx;nCVIdx<m_pRoads[nCRd]->NumVerts();nCVIdx++)
{
if(m_bDir[nCRd])
{
// First check the left side
if(m_pRoads[nCRd]->Divider()==1) // Do this just for jersey barriers.
LTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
else
LTarget=m_pRoads[nCRd]->LBoundary(nCVIdx)-
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
p=LTarget-Position;
if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd]) // reverse
{
int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
nTmpIndex--;
fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
}
else // forward
{
fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
}
if(-0.01<fX && fX<0.01f)
fX=-1.f;
fLAngle=atan2f(fX,fZ);
if(fLAngle>fRViewAngle) // Road must be going right
{
bKnownDir=true;
if(nRouteIdx>1 && nRRd==m_Route[nRouteIdx-1].m_nSRd && nRVIdx==m_Route[nRouteIdx-1].m_nSVIdx)
{
nRVIdx++;
if(nRVIdx>m_pRoads[nRRd]->NumVerts()-1)
{
nRVIdx--;
}
}
nCRd=nRRd;
nCVIdx=nRVIdx;
if(m_pRoads[nCRd]->Divider()==1) // Do this just for jersey barriers.
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
else
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nCVIdx)+
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
break;
}
else if(fLAngle>fLViewAngle-.001f) // Tighten up the left side of the view frustrum
{
fLViewAngle=fLAngle;
nLVIdx=nCVIdx;
nLRd=nCRd;
}
// Now check the right side
RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
p=RTarget-Position;
if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd]) // reverse
{
int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
nTmpIndex--;
fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
}
else // forward
{
fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
}
if(-0.01<fX && fX<0.01f)
fX=1.f;
fRAngle=atan2f(fX,fZ);
if(fRAngle<fLViewAngle) // Road must be going left
{
bKnownDir=true;
if(nRouteIdx>1 && nLRd==m_Route[nRouteIdx-1].m_nSRd && nLVIdx==m_Route[nRouteIdx-1].m_nSVIdx)
{
nLVIdx++;
if(nLVIdx>m_pRoads[nLRd]->NumVerts()-1)
{
nLVIdx--;
}
}
nCRd=nLRd;
nCVIdx=nLVIdx;
if(m_pRoads[nCRd]->Divider()==1) // Do this just for jersey barriers.
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
else
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nCVIdx)-
(m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
break;
}
else if(fRAngle<fRViewAngle+.001f) // Tighten up the right side of the view frustrum
{
fRViewAngle=fRAngle;
nRVIdx=nCVIdx;
nRRd=nCRd;
}
}
else
{
// First check the left side
Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
VDir.Normalize();
LTarget=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
VDir*(m_fLSideDist+1.f);
p=LTarget-Position;
if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd]) // reverse
{
int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
nTmpIndex--;
fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
}
else // forward
{
fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
}
if(-0.01<fX && fX<0.01f)
fX=-1.f;
if(fZ<1.f)fZ=1.f;
fLAngle=atan2f(fX,fZ);
if(fLAngle>fRViewAngle) // Road must be going right
{
bKnownDir=true;
nCRd=nRRd;
nCVIdx=nRVIdx;
nNumVerts=m_pRoads[nCRd]->NumVerts();
if(m_bDir[nCRd])
{
Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
m_pRoads[nCRd]->RBoundary(nCVIdx);
Dir.Normalize();
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nCVIdx)+
Dir*(m_fRSideDist+1.f);
}
else
{
Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1);
Dir.Normalize();
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)+
Dir*(m_fRSideDist+1.f);
}
break;
}
else if(fLAngle>fLViewAngle-.001f) // Tighten up the left side of the view frustrum
{
fLViewAngle=fLAngle;
nLVIdx=nCVIdx;
nLRd=nCRd;
}
// Check the right side.
VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
VDir.Normalize();
RTarget=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)-
(m_pRoads[nCRd]->VertXDir(nIndex))*(m_fRSideDist+1.f);
p=RTarget-Position;
if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd]) // reverse
{
int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
nTmpIndex--;
fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
}
else // forward
{
fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
}
if(-0.01<fX && fX<0.01f)
fX=1.f;
if(fZ<1.f)fZ=1.f;
fRAngle=atan2f(fX,fZ);
if(fRAngle<fLViewAngle) // Road must be going left
{
bKnownDir=true;
nCRd=nLRd;
nCVIdx=nLVIdx;
nNumVerts=m_pRoads[nCRd]->NumVerts();
if(m_bDir[nCRd])
{
Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
m_pRoads[nCRd]->LBoundary(nCVIdx);
Dir.Normalize();
if(m_pRoads[nCRd]->Divider()==1) // Do this just for jersey barriers.
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+
Dir*(m_fLSideDist+1.f);
else
{
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nCVIdx)+
Dir*(m_fLSideDist+1.f);
}
}
else
{
Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
Dir.Normalize();
m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
Dir*(m_fLSideDist+1.f);
}
break;
}
else if(fRAngle<fRViewAngle+.001f) // Tighten up the right side of the view frustrum
{
fRViewAngle=fRAngle;
nRVIdx=nCVIdx;
nRRd=nCRd;
}
}
}
if(nLVIdx<nRVIdx)
nCVIdx=nLVIdx;
else
nCVIdx=nRVIdx;
if(!bKnownDir)
{
if(!m_bDir[nRRd] &&
(nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1)) // road goes to the right
{
int nRd=m_Route[nRouteIdx-1].m_nSRd;
int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
if(nVert<1)
nVert=1;
if(m_bDir[nRd])
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)+
m_pRoads[nRd]->VertXDir(nVert)*fMySide;
}
else
{
nNumVerts=m_pRoads[nRd]->NumVerts();
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)-
m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
}
nCRd=nRRd;
nCVIdx=nRVIdx;
m_bFinished=false;
}
else if(!m_bDir[nLRd] &&
(nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1)) // road goes to the left
{
int nRd=m_Route[nRouteIdx-1].m_nSRd;
int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
if(nVert<1)
nVert=1;
if(m_bDir[nRd])
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)+
m_pRoads[nRd]->VertXDir(nVert)*fMySide;
}
else
{
nNumVerts=m_pRoads[nRd]->NumVerts();
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)-
m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
}
nCRd=nLRd;
nCVIdx=nLVIdx;
m_bFinished=false;
}
else if(m_bDir[nRRd] &&
(nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1)) // road goes to the right
{
float fMySide;
int nRd=m_Route[nRouteIdx-1].m_nSRd;
int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
if(m_bDir[nRd])
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
}
else
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
}
if(fMySide<m_fRSideDist)
fMySide=m_fRSideDist;
m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->RBoundary(nRVIdx)+
m_pRoads[nRRd]->VertXDir(nRVIdx)*fMySide;
nCRd=nRRd;
nCVIdx=nRVIdx;
m_bFinished=false;
}
else if(m_bDir[nLRd] &&
(nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1)) // road goes to the left
{
float fMySide;
int nRd=m_Route[nRouteIdx-1].m_nSRd;
int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
if(m_bDir[nRd])
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nVert);
fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nVert));
}
else
{
p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
}
if(fMySide<m_fLSideDist)
fMySide=m_fLSideDist;
m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(nLVIdx)-
m_pRoads[nLRd]->VertXDir(nLVIdx)*fMySide;
nCRd=nLRd;
nCVIdx=nLVIdx;
m_bFinished=false;
}
}
int nDestIdx=m_pRoads[nCRd]->RoadVertice(m_Destination,m_bDir[nCRd],-1);
if(nCVIdx>=nDestIdx)
{
SetTargetPtToDestination(nRouteIdx);
return;
}
m_Route[nRouteIdx].m_nType=kRoadTarget;
m_Route[nRouteIdx].m_nSVIdx=nCVIdx;
m_Route[nRouteIdx].m_nSRd=nCRd;
m_Route[nRouteIdx].m_nTurnId=nCRd;
m_Route[nRouteIdx].Vtx.y+=1.f;
m_Route[nRouteIdx].m_fCumDist=m_Route[nRouteIdx-1].m_fCumDist+
aiDist(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx);
if(nRouteIdx==1)
{
Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
}
else
{
Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
PTargetDir.Angle(TargetDir);
}
break;
}
case kIntersection:
{
SetTargetPtToDestination(nRouteIdx);
return;
}
}
}
void aiNavigation::SetTargetPtToDestination(int nRouteIdx)
{
if(m_fDestOffset==0.f)
m_Route[nRoute⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -