ainavigation.cpp

Racing and Sports AI很好的游戏AI算法源码!

			}
		}
		else
		{
			m_Route[0].m_nSRd=0;
			m_Route[0].m_nSVIdx=m_pRoads[0]->NumVerts()-1;
		}
	}
	else
	{
		m_Route[0].m_nSRd=0;
	}

	// reset vars
	m_nNumRoutes=0;
	m_fTurnCenterPtOffset=0.f;
	for(int ctr=0;ctr<MAX_NUM_ROUTES;ctr++)
	{
		m_naOffRoadRoute[ctr]=0;
		m_naBlockedRoute[ctr]=0;
	}

	if(m_nWayPtIdx<m_nNumWayPts && !InSharpTurn(0))
	{
		m_Route[0].m_nType=kRoadTarget;
		m_LPosition=m_Car.GetMatrix().Pos;
	}
	else
	{
		m_Route[0].m_nType=kTurnTarget;
	}

	m_LPos=m_LPosition;
	CalcRoadTurns();
		
	// Plot a route for the next x meters.
	EnumRoutes(1);

	// Determine the best route. 
	DetermineBestRoute();
}

void aiNavigation::DetermineBestRoute()
{
	// - First, Pick the straightest route which stays on the road, and doesn't have any 
	//   blocked targets.
	// - Next, Pick the straightest route which stays on the road.
	// - Next, pick the straightest route.
	m_nBestRoute=-1;
	float fBestCumAngle=99999.f;

	for(int ctr=0;ctr<m_nNumRoutes;ctr++)
	{
		if(!m_naOffRoadRoute[ctr] && !m_naBlockedRoute[ctr] &&
		   m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
		{
			fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
			m_nBestRoute=ctr;
		}
	}

	if(m_nBestRoute<0)
	{
		fBestCumAngle=99999.f;
		for(int ctr=0;ctr<m_nNumRoutes;ctr++)
		{
			if(!m_naOffRoadRoute[ctr] && 
				m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
			{
				fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
				m_nBestRoute=ctr;
			}
		}
	}

	if(m_nBestRoute<0)
	{
		fBestCumAngle=99999.f;
		for(int ctr=0;ctr<m_nNumRoutes;ctr++)
		{
			if(m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
			{
				fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
				m_nBestRoute=ctr;
			}
		}
	}
}

void aiNavigation::EnumRoutes(int nRouteIdx)
{
	Vector3 Position=m_Route[nRouteIdx-1].Vtx;
	if(m_bFinished)		// Are we at the end of the route?
	{
		if(m_pRoads[m_Route[nRouteIdx-1].m_nSRd])
		{
			if(int nTurnId=InSharpTurn(nRouteIdx-1))
			{
				m_Route[nRouteIdx-1].m_nTurnId=nTurnId-1;
				int nWhichHalf=CalcSharpTurnTarget(nRouteIdx,nRouteIdx-1,Position);
				if(nWhichHalf)
				{
					float fTmpSideDist;
					m_Route[nRouteIdx].m_nTurnId++;
					m_LPos=m_Route[nRouteIdx].Vtx;
					if(m_pRoads[m_Route[nRouteIdx].m_nSRd] &&
					   m_pRoads[m_Route[nRouteIdx].m_nSRd]->IsPosOnRoad(m_Route[nRouteIdx].Vtx,2.f,&fTmpSideDist)>1)
						CalcDestinationTarget(nRouteIdx,Position);
				}
			}
			else
			{
				m_LPos=m_Route[nRouteIdx-1].Vtx;
				CalcDestinationTarget(nRouteIdx,Position);
				if(int nTurnId=InSharpTurn(nRouteIdx))
				{
					m_Route[nRouteIdx].m_nTurnId=nTurnId-1;
					CalcSharpTurnTarget(nRouteIdx,nRouteIdx,Position);
				}
			}
		}
		else
		{
			SetTargetPtToDestination(nRouteIdx);
		}
	}
	else
	{
		if(int nTurnId=InSharpTurn(nRouteIdx-1))
		{
			m_Route[nRouteIdx-1].m_nTurnId=nTurnId-1;
			CalcSharpTurnTarget(nRouteIdx,nRouteIdx-1,Position);
		}
		else
		{
			m_LPos=m_Route[nRouteIdx-1].Vtx;
			CalcRoadTarget(nRouteIdx,Position);
			if(int nTurnId=InSharpTurn(nRouteIdx))
			{
				m_Route[nRouteIdx].m_nTurnId=nTurnId-1;
				CalcSharpTurnTarget(nRouteIdx,nRouteIdx,Position);
			}
		}
	}

	// By Default All position calculated by the above on on the road
	m_Route[nRouteIdx].m_nOnRoad=1;

	// Now that we know where we want to go based on the road, lets check if there are
	// any obstacles in the way, and if the target is within the road boundaries.
	short *pnSRd=&m_Route[nRouteIdx-1].m_nSRd;
	short *pnSVIdx=&m_Route[nRouteIdx-1].m_nSVIdx;
	short *pnCRd=&m_Route[nRouteIdx].m_nSRd;
	short *pnCVIdx=&m_Route[nRouteIdx].m_nSVIdx;

	int nType;
	Vector3 ObsPos;
	aiObstacle *pObstacle;
	pObstacle=IsTargetBlocked(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx,*pnSVIdx,
							  *pnSRd,*pnCVIdx+3,*pnCRd,VehicleLength()*2.f,&nType);
	if(pObstacle)
	{
		pObstacle->Position(ObsPos);
		if((nType!=kOppObstacle || (nType==kOppObstacle && m_nWayPtIdx>2 && m_Route[nRouteIdx-1].m_nType!=kTurnTarget)) &&
		   aiDist2(m_Route[nRouteIdx-1].Vtx,ObsPos)<m_fRoutePlanDist*m_fRoutePlanDist)
		{
			short Locations[10], TargetTypes[10];
			Vector3 Targets[10];
			aiObstacle *pAltObstacles[10];

			int bUseSidewalk=true;
			int nNumTargets=CalcObstacleAvoidPoints(pObstacle,nRouteIdx,bUseSidewalk,Targets,
													pAltObstacles,Locations,TargetTypes);
			for(int ctr=0;ctr<nNumTargets;ctr++)
				SaveTarget(nRouteIdx,Targets[ctr],pAltObstacles[ctr],Locations[ctr],nType,TargetTypes[ctr]);
		}
		else
		{
			ContinueCheck(nRouteIdx);
		}
	}
	else
	{
		ContinueCheck(nRouteIdx);
	}
}

// ASSUMPTION:
// Will always return at least one route when no acceptable targets exist, 
// which is the current road target .
int aiNavigation::CalcObstacleAvoidPoints(aiObstacle *pObstacle,int nRouteIdx,int bUseSidewalk,
												Vector3* pTargets,aiObstacle **ppAltObstacles,
												short* pLocations,short* pTargetTypes)
{
	int nNumTargets=0;
	if(m_Route[nRouteIdx-1].m_nType!=kTurnTarget)
	{
		float fZ;
		Vector3 p, LTarget, RTarget;

		short *pnSRd=&m_Route[nRouteIdx-1].m_nSRd;
		short *pnSVIdx=&m_Route[nRouteIdx-1].m_nSVIdx;

		if(nRouteIdx==1)
			pObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,-m_Car.GetMatrix().ZDir,
								m_fRSideDist+2.f,LTarget,RTarget);
		else
			pObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx,
								m_fRSideDist+2.f,LTarget,RTarget);
		
		// Now calc the left avoid points
		p=LTarget-m_Route[nRouteIdx-1].Vtx;
		if(nRouteIdx==1)
		{
			fZ=aiDot(p,-m_Car.GetMatrix().ZDir);
		}
		else
		{
			if(m_bDir[*pnSRd])
				fZ=aiDot(p,(-(m_pRoads[*pnSRd]->VertZDir(*pnSVIdx))));
			else
				fZ=aiDot(p,m_pRoads[*pnSRd]->VertZDir(m_pRoads[*pnSRd]->NumVerts()-*pnSVIdx-1));
		}

		if(fZ>=0.f)
		{
			int nNumTries=0;
			EnumTargets(LTarget,pObstacle,nRouteIdx,*pnSRd,*pnSVIdx,-1,bUseSidewalk,nNumTries,
						pTargets,ppAltObstacles,pLocations,pTargetTypes,&nNumTargets);
		}

		// Now calc the right avoid points
		p=RTarget-m_Route[nRouteIdx-1].Vtx;
		if(nRouteIdx==1)
		{
			fZ=aiDot(p,-m_Car.GetMatrix().ZDir);
		}
		else
		{
			if(m_bDir[*pnSRd])
				fZ=aiDot(p,(-(m_pRoads[*pnSRd]->VertZDir(m_pRoads[*pnSRd]->NumVerts()-*pnSVIdx-1))));
			else
				fZ=aiDot(p,m_pRoads[*pnSRd]->VertZDir(*pnSVIdx));
		}

		if(fZ>=0.f)
		{
			int nNumTries=0;
			EnumTargets(RTarget,pObstacle,nRouteIdx,*pnSRd,*pnSVIdx,1,bUseSidewalk,nNumTries,
						pTargets,ppAltObstacles,pLocations,pTargetTypes,&nNumTargets);
		}

		// Bailout: When no routes are found, ignore the obstacle and plan on braking.
		if(!nNumTargets)
		{
			pTargets[nNumTargets]=m_Route[nRouteIdx].Vtx;
			*ppAltObstacles=pObstacle;
			pTargetTypes[nNumTargets]=kBlockedTarget;
			nNumTargets=1;
		}

		return nNumTargets;
	}

	pTargets[nNumTargets]=m_Route[nRouteIdx].Vtx;
	pTargetTypes[nNumTargets]=kTurnTarget;
	*ppAltObstacles=pObstacle;
	nNumTargets=1;

	return nNumTargets;
}

void aiNavigation::EnumTargets(Vector3& OrigTarget,aiObstacle *pOrigObstacle,int nRouteIdx,
									 int nSRd,int nSVIdx,int nSide,int bUseSidewalk,int nNumTries,
									 Vector3* pTargets,aiObstacle **ppAltObstacles,short* pLocations,
									 short* pTargetTypes,int *pnNumTargets)
{
	nNumTries++;
	if(nNumTries==_nMaxTrys)
		return;

	int nCRdVtx;
	int nCRdIdx=pOrigObstacle->CurrentRoadIdx(m_pRoads,m_bDir,&nCRdVtx);
	if(nCRdIdx<0 || !m_pRoads[nCRdIdx])
		return;

	float fSideDist;
	int nOnRoad=m_pRoads[nCRdIdx]->IsPosOnRoad(OrigTarget,m_fRSideDist,&fSideDist);
	if(nOnRoad==1 || (bUseSidewalk && nOnRoad==2))
	{
		// Finally check if any other ambients block the route
		int nType;
		aiObstacle *pNewObstacle;
		pNewObstacle=IsTargetBlocked(m_Route[nRouteIdx-1].Vtx,OrigTarget,nSVIdx,nSRd,
									 nCRdVtx+2,nCRdIdx,VehicleLength()*2.f,&nType);
		if(pNewObstacle && nType!=kOppObstacle)
		{
			Vector3 Obs1Pos, Obs2Pos, LTarget, RTarget;
			if(nRouteIdx==1)
				pNewObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,-m_Car.GetMatrix().ZDir,
									   m_fRSideDist+2.f,LTarget,RTarget);
			else
				pNewObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx,
									   m_fRSideDist+2.f,LTarget,RTarget);

			// Check the distance between the two obstacles
			pOrigObstacle->Position(Obs1Pos);
			pNewObstacle->Position(Obs2Pos);
			float fSepDist=aiDist2(Obs1Pos,Obs2Pos);
			float fOrigObsDist=aiDist2(Obs1Pos,m_Car.GetMatrix().Pos);
			float fNewObsDist=aiDist2(Obs2Pos,m_Car.GetMatrix().Pos);
			if(fSepDist>225.f && fNewObsDist<fOrigObsDist)
			{
				float fZ;
				float fCheckDist=m_fRoutePlanDist*m_fRoutePlanDist;
				if(nSide==-1)
				{
					Vector3 p=RTarget-m_Route[nRouteIdx-1].Vtx;
					if(m_bDir[nSRd])
					{
						fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
					}
					else
					{
						int nNumVerts=m_pRoads[nSRd]->NumVerts();
						fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
					}

					if(fZ>=0.f &&
					   aiDist2(m_Route[nRouteIdx-1].Vtx,Obs2Pos)<fCheckDist &&
					   nType==kAmbObstacle)
					{
						ppAltObstacles[*pnNumTargets]=pNewObstacle;
						pTargets[*pnNumTargets]=RTarget;
						pLocations[*pnNumTargets]=nOnRoad;
						pTargetTypes[*pnNumTargets]=kObstacleTarget;
						(*pnNumTargets)++;
					}
				}
				else
				{
					Vector3 p=LTarget-m_Route[nRouteIdx-1].Vtx;
					if(m_bDir[nSRd])
					{
						fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
					}
					else
					{
						int nNumVerts=m_pRoads[nSRd]->NumVerts();
						fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
					}

					if(fZ>=0.f &&
					   aiDist2(m_Route[nRouteIdx-1].Vtx,Obs2Pos)<fCheckDist &&
					   nType==kAmbObstacle)
					{
						ppAltObstacles[*pnNumTargets]=pNewObstacle;
						pTargets[*pnNumTargets]=LTarget;
						pLocations[*pnNumTargets]=nOnRoad;
						pTargetTypes[*pnNumTargets]=kObstacleTarget;
						(*pnNumTargets)++;
					}
				}
			}

			// Now calc the specified avoid points.
			float fZ;
			if(nSide==-1)
			{
				Vector3 p=LTarget-m_Route[nRouteIdx-1].Vtx;
				if(m_bDir[nSRd])
				{
					fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
				}
				else
				{
					int nNumVerts=m_pRoads[nSRd]->NumVerts();
					fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
				}

				if(fZ>=0.f)
				{
					EnumTargets(LTarget,pNewObstacle,nRouteIdx,nSRd,nSVIdx,-1,bUseSidewalk,nNumTries,
								pTargets,ppAltObstacles,pLocations,pTargetTypes,pnNumTargets);
				}
			}
			else
			{
				Vector3 p=RTarget-m_Route[nRouteIdx-1].Vtx;
				if(m_bDir[nSRd])
				{
					fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
				}
				else
				{
					int nNumVerts=m_pRoads[nSRd]->NumVerts();
					fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
				}

				if(fZ>=0.f)
				{
					EnumTargets(RTarget,pNewObstacle,nRouteIdx,nSRd,nSVIdx,1,bUseSidewalk,nNumTries,
								pTargets,ppAltObstacles,pLocations,pTargetTypes,pnNumTargets);
				}
			}
		}
		else
		{
			// Save off this target.
			ppAltObstacles[*pnNumTargets]=pOrigObstacle;
			pTargets[*pnNumTargets]=OrigTarget;
			pLocations[*pnNumTargets]=nOnRoad;
			pTargetTypes[*pnNumTargets]=kObstacleTarget;
			(*pnNumTargets)++;
		}
	}
}

int aiNavigation::CalcSharpTurnTarget(int& nRouteIdx,int nLastIdx,Vector3& Position)
{
	Vector3 XDir, ZDir;
	if(m_bDir[m_Route[nLastIdx].m_nTurnId])
	{
		XDir=m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertXDir(m_pRoads[m_Route[nLastIdx].m_nTurnId]->NumVerts()-1);
		ZDir=m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertZDir(m_pRoads[m_Route[nLastIdx].m_nTurnId]->NumVerts()-1);
	}
	else
	{
		XDir=-m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertXDir(0);
		ZDir=-m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertZDir(0);
	}

	Vector3 StartPt=m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId]+XDir*m_fTurn[m_Route[nLastIdx].m_nTurnId]*m_fRadius[m_Route[nLastIdx].m_nTurnId];

	Vector3 MyDir=m_Route[nRouteIdx-1].Vtx-m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId];
	Vector3 StartPerp;
	StartPerp.Set(-m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].z,
				  m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].y,
				  m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].x);
	float fX=aiDot(MyDir,StartPerp);
	float fZ=aiDot(MyDir,m_TurnStartDir[m_Route[nLastIdx].m_nTurnId]);
	float fMyAngle=atan2f(fX,fZ);
	m_Route[nRouteIdx].m_nTurnId=m_Route[nLastIdx].m_nTurnId;

	if(m_fTurn[m_Route[nLastIdx].m_nTurnId]<0.f)
		fMyAngle*=-1.f;
	
	if(fMyAngle<-0.1f)
	{
		m_Route[nRouteIdx].Vtx=StartPt;
		SaveTurnTarget(nRouteIdx,1);
		m_Route[nRouteIdx].m_nSRd=m_Route[nRouteIdx].m_nTurnId;
		m_Route[nRouteIdx].m_nSVIdx=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1;