ainavigation.cpp

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

		m_Car.SetThrottle(0.85f);
		m_Car.SetBrake(0.f);
	}
	m_nBackupCount++;
}

void aiNavigation::FinishedBackingUp()
{
	// Restore the state before the backup.
	m_nState=kForward;
	m_Car.SetThrottle(0.0f);
	m_Car.SetBrake(1.f);
}

//------Privates-----------------------------------------------------------------

void aiNavigation::CalcSpeed()
{
	m_fThrottle=1.f;
	m_fBrake=0.f;

	// Check if near the final destination which occurs at the end of the race.
	float fDist=aiDist2(m_Car.GetMatrix().Pos,m_Destination);
	if(fDist<5000.f && m_nWayPtIdx>=m_nNumWayPts)
	{
		float fTargetVel=m_fDestSpeed;
		float fDistToTarget=(float)sqrt(fDist)-m_fDestOffset;
		float fTargetTime=fDistToTarget/m_Car.Speed();
		float fBrake=0.f;
		if(m_Car.Speed()>fTargetVel)
			fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);

		if(fBrake>fDistToTarget*.014)
		{
			m_fThrottle=0.f;
			m_fBrake=fBrake;
			m_fBrake=aiClamp(m_fBrake,0.f,1.f);
		}
		if(fDistToTarget<2.5f && m_fDestSpeed<=0.f)
		{
			m_fThrottle=0.f;
			m_fBrake=1.f;
		}
		return;
	}

	// Check the intersection turns.
	if(m_pRoads[0] && (m_fRdAngle[0]<-_fTurnThreshold || _fTurnThreshold<m_fRdAngle[0]) &&
	   aiDist2(m_Car.GetMatrix().Pos,m_IntersectionPt[0])<CheckDistance(0))
	{
		float fTargetVel=(float)sqrt(_fFriction*19.8f*m_fRadius[0])*
						 m_fTurnVelFactor;

		// Set the brake and throttle
		float fBrake=0.f;
		if(m_Car.Speed()>fTargetVel)
		{
			Vector3 p=m_AltRoute[m_nBestRoute][0].Vtx-m_IntersectionPt[0];
			float fDistToTurn;
			if(m_bDir[0])
				fDistToTurn=aiDot(p,m_pRoads[0]->VertZDir(m_pRoads[0]->NumVerts()-1))-m_fTurnSetback[0];
			else
				fDistToTurn=aiDot(p,-m_pRoads[0]->VertZDir(0))-m_fTurnSetback[0];

			float fTargetTime=fDistToTurn/m_Car.Speed();

			fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);
		}

		if(fBrake>m_fBrakeThreshold && fBrake>m_fBrake)
		{
			m_fThrottle=0.f;
			m_fBrake=fBrake;
			m_fBrake=aiClamp(m_fBrake,0.f,1.f);
		}
	}
	else if(m_pRoads[1] && (m_fRdAngle[1]<-_fTurnThreshold || _fTurnThreshold<m_fRdAngle[1]) &&
			aiDist2(m_Car.GetMatrix().Pos,m_IntersectionPt[1])<CheckDistance(1))
	{
		float fTargetVel=(float)sqrt(_fFriction*19.8f*m_fRadius[1])*
						 m_fTurnVelFactor;

		// Set the brake and throttle
		float fBrake=0.f;
		if(m_Car.Speed()>fTargetVel)
		{
			float fDistToTurn=aiDist(m_AltRoute[m_nBestRoute][0].Vtx,m_IntersectionPt[1])-m_fTurnSetback[1];
				
			float fTargetTime=fDistToTurn/m_Car.Speed();
			fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);
		}

		if(fBrake>m_fBrakeThreshold && fBrake>m_fBrake)
		{
			m_fThrottle=0.f;
			m_fBrake=fBrake;
			m_fBrake=aiClamp(m_fBrake,0.f,1.f);
		}
	}
}

float aiNavigation::CheckDistance(int nId)
{
	return (m_fRoutePlanDist+m_fTurnSetback[nId])*(m_fRoutePlanDist+m_fTurnSetback[nId]);
}

void aiNavigation::SolveRoadTargetPoint()
{
	if(m_nWayPtIdx>=m_nNumWayPts)
		m_bFinished=true;
	else
		m_bFinished=false;

	// Plan out a rough route consisting of the next three road segs
	if(PlanRoute())
		return;

	// Process all the things that need to done when changing roads.
	if(m_pPrevRoad!=m_pRoads[0])
	{
		m_LPos=m_Car.GetMatrix().Pos;
		m_LPosition=m_Car.GetMatrix().Pos;
		m_pPrevRoad=m_pRoads[0];
		InitRoadTurns();
		CalcRoadTurns();
	}

	// Calc the exact route through the traffic
	CalcRoute();

	m_TargetPt=m_AltRoute[m_nBestRoute][1].Vtx;

	// Set the throttle and brake
	CalcSpeed();
}

int aiNavigation::PlanRoute()
{
	// Check if time to increment the destination intersection index
	if(m_nWayPtIdx<m_nNumWayPts && 
	  (m_nCurMapCompType==kIntersection && m_nCurMapCompIdx==CheckPt(m_nWayPtIdx)))
	{
		// Do this in case the road leading to this intersection is very short.
		// i.e. two intersections which are practically next to each other. What
		// happens is the opponents will go from one int to another without ever
		// registering on the conecting road.
		if(m_nWayPtIdx>0)
		{
			bool bDir;
			aiPath *pPath=AIMAP.DetRdSegBetweenInts(AIMAP.Intersection(CheckPt(m_nWayPtIdx-1)),
													AIMAP.Intersection(CheckPt(m_nWayPtIdx)),&bDir);
			if(m_pRoads[0]!=pPath)
				LocateWayPtFromRoad(pPath);
		}

		// Lets move on to the next intersection.
		m_nWayPtIdx++;
	}

	switch(m_nCurMapCompType)
	{
		case kRoad:
		{
			LocateWayPtFromRoad(AIMAP.Path(m_nCurMapCompIdx));
			break;
		}
		case kIntersection:
		{
			if(m_nRdIdx<3)
			{
				if(m_nDestMapCompType==kRoad)
				{
					if(m_nNumWayPts>0)
					{
						for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
						{
							if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
							   AIMAP.Path(m_nDestMapCompId))
							{
								m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
								if(m_pRoads[m_nRdIdx]->Departure()==AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
									m_bDir[m_nRdIdx]=1;
								else
									m_bDir[m_nRdIdx]=0;
								return 0;
							}
						}
					}
					else
					{
						for(int i=0;i<AIMAP.Intersection(m_nCurMapCompIdx)->NumRoads();i++)
						{
							if(AIMAP.Intersection(m_nCurMapCompIdx)->Road(i)==
							   AIMAP.Path(m_nDestMapCompId))
							{
								m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_nCurMapCompIdx)->Road(i);
								if(m_pRoads[m_nRdIdx]->Departure()==AIMAP.Intersection(m_nCurMapCompIdx))
									m_bDir[m_nRdIdx]=1;
								else
									m_bDir[m_nRdIdx]=0;
								return 0;
							}
						}
					}
				}
			}
		}
	}
	return 0;
}

int aiNavigation::LocateWayPtFromRoad(aiPath *pCallLink)
{
	if(m_nWayPtIdx>=m_nNumWayPts)
	{
		m_pRoads[0]=pCallLink;
		m_pRoads[1]=m_pRoads[2]=0;

		int nIdx=m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,1,-1);
		nIdx=aiClamp(nIdx,0,m_pRoads[0]->NumVerts()-1);
		float fDir=aiDot(m_pRoads[0]->VertZDir(nIdx),m_Car.GetMatrix().ZDir);
		if(fDir>=0.f)
			m_bDir[0]=1;
		else
			m_bDir[0]=0;
		return true;
	}

	// Is the destination int the next goal? Handle missed intersections
	if(pCallLink->Destination()->Id()==CheckPt(m_nWayPtIdx+1) ||
	   pCallLink->Departure()->Id()==CheckPt(m_nWayPtIdx+1) ||
	   (m_nWayPtIdx==m_nNumWayPts-1 && m_nDestMapCompId==m_nCurMapCompIdx))
		m_nWayPtIdx++;

	// Is the destination int the current goal?
	if(pCallLink->Destination()->Id()==CheckPt(m_nWayPtIdx))
	{
		m_bDir[0]=1;
		m_pRoads[0]=pCallLink;

		m_nRdIdx=0;
		aiIntersection *pInt0, *pInt1, *pInt2;
		if(m_nWayPtIdx==m_nNumWayPts-1) 
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=0;
			pInt2=0;
			m_nRdIdx=1;
		}
		else if(m_nWayPtIdx==m_nNumWayPts-2)
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
			pInt2=0;
			m_nRdIdx=2;
		}
		else
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
			pInt2=AIMAP.Intersection(CheckPt(m_nWayPtIdx+2));
			m_nRdIdx=3;
		}

		m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
		m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt1,pInt2,&m_bDir[2]);

		if(m_nDestMapCompType==kRoad && 0<m_nRdIdx && m_nRdIdx<3)
		{
			for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
			{
				if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
				   AIMAP.Path(m_nDestMapCompId))
				{
					m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
					if(m_pRoads[m_nRdIdx]->Departure()==
					   AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
						m_bDir[m_nRdIdx]=1;
					else
						m_bDir[m_nRdIdx]=0;
					break;
				}
			}
		}
		return true;
	}

	// Check for wrong way down a one way road.
	if(pCallLink->Departure()->Id()==CheckPt(m_nWayPtIdx)) 
	{
		m_bDir[0]=0;
		m_pRoads[0]=pCallLink;

		m_nRdIdx=0;
		aiIntersection *pInt0, *pInt1, *pInt2;
		if(m_nWayPtIdx==m_nNumWayPts-1) 
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=0;
			pInt2=0;
			m_nRdIdx=1;
		}
		else if(m_nWayPtIdx==m_nNumWayPts-2)
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
			pInt2=0;
			m_nRdIdx=2;
		}
		else
		{
			pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
			pInt2=AIMAP.Intersection(CheckPt(m_nWayPtIdx+2));
			m_nRdIdx=3;
		}

		m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
		m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt1,pInt2,&m_bDir[2]);

		if(m_nDestMapCompType==kRoad && 0<m_nRdIdx && m_nRdIdx<3)
		{
			for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
			{
				if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
				   AIMAP.Path(m_nDestMapCompId))
				{
					m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
					if(m_pRoads[m_nRdIdx]->Departure()==
					   AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
						m_bDir[m_nRdIdx]=1;
					else
						m_bDir[m_nRdIdx]=0;
					break;
				}
			}
		}
		return true;
	}

	// Check the destination intersection's intersections
	for(int ctr=0;ctr<pCallLink->Destination()->NumRoads();ctr++)
	{
		if(pCallLink->Destination()->Road(ctr)->Destination()->Id()==CheckPt(m_nWayPtIdx))
		{
			m_pRoads[0]=pCallLink;
			m_bDir[0]=1;
			m_pRoads[1]=pCallLink->Destination()->Road(ctr);
			m_bDir[1]=1;
			if(m_nWayPtIdx+1<m_nNumWayPts)
			{
				aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
				aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
			}
			else
				m_pRoads[2]=0;
			return true;
		}
		if(pCallLink->Destination()->Road(ctr)->Departure()->Id()==CheckPt(m_nWayPtIdx))
		{
			m_pRoads[0]=pCallLink;
			m_bDir[0]=1;
			m_pRoads[1]=pCallLink->Destination()->Road(ctr);
			m_bDir[1]=0;
			if(m_nWayPtIdx+1<m_nNumWayPts)
			{
				aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
				aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
			}
			else
				m_pRoads[2]=0;
			return true;
		}
	}
	
	// Check the departure intersection's intersections
	for(int ctr=0;ctr<pCallLink->Departure()->NumRoads();ctr++)
	{
		if(pCallLink->Departure()->Road(ctr)->Destination()->Id()==CheckPt(m_nWayPtIdx))
		{
			m_pRoads[0]=pCallLink;
			m_bDir[0]=0;
			m_pRoads[1]=pCallLink->Departure()->Road(ctr);
			m_bDir[1]=1;
			if(m_nWayPtIdx+1<m_nNumWayPts)
			{
				aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
				aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
			}
			else
				m_pRoads[2]=0;
			return true;
		}
		if(pCallLink->Departure()->Road(ctr)->Departure()->Id()==CheckPt(m_nWayPtIdx))
		{
			m_pRoads[0]=pCallLink;
			m_bDir[0]=0;
			m_pRoads[1]=pCallLink->Departure()->Road(ctr);
			m_bDir[1]=0;
			if(m_nWayPtIdx+1<m_nNumWayPts)
			{
				aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
				aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
			}
			else
				m_pRoads[2]=0;
			return true;
		}
	}

	return false;
}

void aiNavigation::CalcRoute()
{
	// Initialize the route
	m_Route[0].Vtx=m_Car.GetMatrix().Pos;
	m_Route[0].Vtx.y+=1.f;
	m_Route[0].m_fCumAngle=0.f;
	m_Route[0].m_fCumDist=0.f;
	m_Route[0].m_nTurnId=0;

	if(!m_pRoads[0])
	{
		m_nBestRoute=0;
		m_AltRoute[m_nBestRoute][1].Vtx=m_Destination;
		m_AltRoute[m_nBestRoute][1].m_nTurnId=0;
		return;
	}

	m_Route[0].m_nSVIdx=m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[0],-1);
	if(m_Route[0].m_nSVIdx==m_pRoads[0]->NumVerts())
	{
		// Check if we're coming up to a sharp turn.
		if(m_pRoads[1])
		{
			m_Route[0].m_nSRd=1;
			m_Route[0].m_nSVIdx=m_pRoads[1]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[1],-1);
			if(m_Route[0].m_nSVIdx==m_pRoads[1]->NumVerts())
			{
				if(m_pRoads[2])
				{
					m_Route[0].m_nSRd=2;
					m_Route[0].m_nSVIdx=m_pRoads[2]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[2],-1);
				}
				else
				{
					m_Route[0].m_nSVIdx=m_pRoads[1]->NumVerts()-1;
				}