ainavigation.cpp

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

#include "aiNavigation.h"
#include "aiMap.h"
#include "aiObstacle.h"

static int _nMaxTrys=10;
static int _nMaxRouteSplits=5;
static float _fFriction=1.2f;
static float _fTurnThreshold=.7f;
static float _fMaxStraightAngle=.2f;

extern aiMap AIMAP;

inline short aiNavigation::CheckPt(int nIdx)
{
	if(nIdx<0 || nIdx>=m_nNumWayPts)
	{
		return -1;
	}
	return m_panWayPts[nIdx];
}

aiNavigation::aiNavigation()
{
}

aiRouteNode::aiRouteNode() {
	Reset();
}

void aiRouteNode::Reset() {
	m_pObstacle=0;
	Vtx.Zero();
	m_fCumAngle=0.0f;
	m_fCumDist=0.0f;
	m_nSRd=0;
	m_nSVIdx=0;
	m_nType=0;
	m_nTurnId=0;
	m_nObsType=0;
	m_nOnRoad=0;	// 0: Off the road, 1: On the road, 2: On the sidewalk
}

aiNavigation::~aiNavigation()
{
}

void aiNavigation::Init(int nId,char *pGeoFile) 
{
	m_Car.Init(nId,pGeoFile); 

	Vector3 Min=m_Car.GetBoxMin();
	Vector3 Max=m_Car.GetBoxMax();
	m_fFrontBumperDistance=-Min.z;
	m_fBackBumperDistance=Max.z;
	m_fLSideDist=-Min.x;
	m_fRSideDist=Max.x;

	Reset();
}

void aiNavigation::Reset() 
{
	m_nLastState=-1;
	m_nState=kForward;
	m_Car.Reset();
	m_bFinished=false;

	// Not sure if these are good default values, but for replays sake...
	m_nWayPtIdx=1;
	m_nCurMapCompType=3;
	m_nCurMapCompIdx=1;
	m_nDestMapCompType=3;
	m_nDestMapCompId=1;
	m_fTurnCenterPtOffset=0.f;
	m_nNumWayPts=1;
	m_nBestRoute=-1;
	m_nNumRoutes=0;
	m_bDir[0]=m_bDir[1]=m_bDir[2]=0;
	m_fRdAngle[0]=m_fRdAngle[1]=0.f;
	m_fTurn[0]=m_fTurn[1]=0.f;
	m_LPos.Set(0.f,0.f,0.f);
	m_LPosition.Set(0.f,0.f,0.f);
	m_Destination.Set(0.f,0.f,0.f);
	m_DestHeading.Set(0.f,0.f,0.f);
	m_IntersectionPt[0].Set(0.f,0.f,0.f);
	m_IntersectionPt[1].Set(0.f,0.f,0.f);
	m_TurnCenterPt[0].Set(0.f,0.f,0.f);
	m_TurnCenterPt[1].Set(0.f,0.f,0.f);
	m_TurnStartDir[0].Set(0.f,0.f,0.f);
	m_TurnStartDir[1].Set(0.f,0.f,0.f);
	m_TurnEndDir[0].Set(0.f,0.f,0.f);
	m_TurnEndDir[1].Set(0.f,0.f,0.f);
	m_fTurnSetback[0]=m_fTurnSetback[1]=0.f;
	m_fRadius[0]=m_fRadius[1]=0.f;

	m_fBrake=0.0f;
	m_fThrottle=0.0f;
	m_fSteering=0.0f;

	for(int i=0;i<3;i++)
		m_pRoads[i]=0;

	for(int k=0;k<MAX_ROUTE_NODES;k++) {
		m_Route[k].Reset();
	}

	for(int j=0; j< MAX_NUM_ROUTES; j++) {
		m_naNumRouteVerts[j]=0;
		for(int i=0;i<MAX_ROUTE_NODES;i++) {
			m_AltRoute[j][i].Reset();
		}
	}
}

void aiNavigation::RegisterRoute(short *panRoute,short nNumRouteNodes,const Vector3& Destination,
									 const Vector3& DestHeading,int nNumRepeats,float fTargetSpeed,
									 float fDestOffset,float fTurnVelFactor,float fBrakeThreshold,
									 float fRoutePlanDist)
{
	aiIntersection *pInt0, *pInt1;

	m_fTurnVelFactor=fTurnVelFactor;
	m_fBrakeThreshold=fBrakeThreshold;
	m_fRoutePlanDist=fRoutePlanDist;
	
	m_panWayPts=panRoute;
	m_nNumWayPts=nNumRouteNodes;
	m_pPrevRoad=0;
	m_LPosition=m_Car.GetMatrix().Pos;
	m_LPos=m_Car.GetMatrix().Pos;
	m_nBestRoute=-1;
	m_TargetPt=m_Car.GetMatrix().Pos;

	m_pRoads[0]=0;
	m_pRoads[1]=0;
	m_pRoads[2]=0;

	m_fRdAngle[0]=0.f;
	m_fRdAngle[1]=0.f;

	m_Route[0].m_nSVIdx=0;
	m_Route[0].m_nSRd=0;

	m_Destination=Destination;
	m_DestHeading=DestHeading;
	m_fDestSpeed=fTargetSpeed;
	m_fDestOffset=fDestOffset;
	DestMapComponent(m_Destination,&m_nDestMapCompId,&m_nDestMapCompType);

	int nTargetId=-1;
	if(m_pRoads[0])
		nTargetId=m_pRoads[0]->Id();

	AIMAP.MapComponent(m_Car.GetMatrix().Pos,&m_nCurMapCompIdx,&m_nCurMapCompType,
					   m_Car.GetRoomId(),nTargetId);
	switch(m_nCurMapCompType)
	{
		case kIntersection:
		{
			m_nRdIdx=0;

			m_nWayPtIdx=0;
			m_pRoads[0]=AIMAP.Intersection(m_nCurMapCompIdx)->Road(0);
			if(m_pRoads[0]->Destination()==AIMAP.Intersection(m_nCurMapCompIdx))
				m_bDir[0]=1;	// The direction that the vehicle will traverse the road
			else
				m_bDir[0]=0;

			if(m_nNumWayPts==2)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				m_nWayPtIdx=1;
				m_nRdIdx=1;
			}
			else if(m_nNumWayPts==3)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				pInt0=AIMAP.Intersection(CheckPt(1));
				pInt1=AIMAP.Intersection(CheckPt(2));
				m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
				m_nWayPtIdx=1;
				m_nRdIdx=2;
			}
			else if(m_nNumWayPts>3)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				pInt0=AIMAP.Intersection(CheckPt(1));
				pInt1=AIMAP.Intersection(CheckPt(2));
				m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
				pInt0=AIMAP.Intersection(CheckPt(2));
				pInt1=AIMAP.Intersection(CheckPt(3));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
				m_nWayPtIdx=1;
				m_nRdIdx=3;
			}

			m_nState=kForward;
			if(m_bDir[0])
			{
				Vector3 p=m_pRoads[0]->CenterVertice(0)-m_Car.GetMatrix().Pos;
				m_fSideDist=aiDot(p,m_pRoads[0]->VertXDir(0));
			}
			else
			{
				int nNumVerts=m_pRoads[0]->NumVerts();
				Vector3 p=m_pRoads[0]->CenterVertice(nNumVerts-1)-m_Car.GetMatrix().Pos;
				m_fSideDist=aiDot(p,-m_pRoads[0]->VertXDir(nNumVerts-1));
			}
			break;
		}
		case kRoad:
		{
			int nCompIdx=m_nCurMapCompIdx;
			m_nWayPtIdx=0;
			m_bDir[0]=1;
			m_pRoads[0]=AIMAP.Path(nCompIdx);

			m_nRdIdx=0;
			if(m_nNumWayPts==2)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				m_nWayPtIdx=1;
				m_nRdIdx=1;
			}
			else if(m_nNumWayPts==3)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				pInt0=AIMAP.Intersection(CheckPt(1));
				pInt1=AIMAP.Intersection(CheckPt(2));
				m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
				m_nWayPtIdx=1;
				m_nRdIdx=2;
			}
			else if(m_nNumWayPts>3)
			{
				pInt0=AIMAP.Intersection(CheckPt(0));
				pInt1=AIMAP.Intersection(CheckPt(1));
				m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
				pInt0=AIMAP.Intersection(CheckPt(1));
				pInt1=AIMAP.Intersection(CheckPt(2));
				m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
				pInt0=AIMAP.Intersection(CheckPt(2));
				pInt1=AIMAP.Intersection(CheckPt(3));
				m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
				m_nWayPtIdx=1;
				m_nRdIdx=3;
			}

			m_nState=kForward;
			int nVIdx=aiClamp(m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[0],-1),
							0,m_pRoads[0]->NumVerts()-1);
			if(m_bDir[0])
			{
				Vector3 p=m_pRoads[0]->CenterVertice(nVIdx)-m_Car.GetMatrix().Pos;
				m_fSideDist=aiDot(p,m_pRoads[0]->VertXDir(nVIdx));
			}
			else
			{
				int nNumVerts=m_pRoads[0]->NumVerts();
				Vector3 p=m_pRoads[0]->CenterVertice(nNumVerts-nVIdx-1)-m_Car.GetMatrix().Pos;
				m_fSideDist=aiDot(p,-m_pRoads[0]->VertXDir(nNumVerts-nVIdx-1));
			}
			break;
		}
	}
}

void aiNavigation::DriveRoute()
{
	switch(m_nState)
	{
		case kBackup:
		{
			if(m_nLastState!=m_nState)
			{
				InitBackup();
				m_nLastState=m_nState;
			}
			Backup();
			break;
		}
		case kForward:
		{
			if(m_nLastState!=m_nState)
			{
				InitForward();
				m_nLastState=m_nState;
			}
			Forward();
			break;
		}
		case kStop:
		{
			Stop();
			break;
		}
	}
}

void aiNavigation::Stop()
{
	m_TargetPt=m_Destination;
	Vector3 TargetHeading=m_Destination-m_Car.GetMatrix().Pos;
	float fX=aiDot(TargetHeading,m_Car.GetMatrix().XDir);
	float fZ=aiDot(TargetHeading,m_Car.GetMatrix().ZDir);
	float fAngle=atan2f(fX,-fZ)*1.33f;
	m_fSteering=aiClamp(fAngle,-1.f,1.f);
	m_Car.SetSteering(m_fSteering);

	m_fThrottle=0.f;
	m_fBrake=1.f;
	m_Car.SetThrottle(0.f);
	m_Car.SetBrake(1.f);

	m_naNumRouteVerts[0]=0;
	m_nNumRoutes=0;
}

void aiNavigation::InitForward()
{
	m_fSteering=0.f;
	m_Car.SetSteering(0.f);
	m_Car.SetThrottle(0.f);
	m_Car.SetBrake(0.f);
	m_TargetPt=m_Car.GetMatrix().Pos;

	m_nNumRoutes=0;
	m_nBestRoute=-1;
	m_fTurnCenterPtOffset=0.f;
	for(int ctr=0;ctr<MAX_NUM_ROUTES;ctr++)
	{
		m_naOffRoadRoute[ctr]=0;
		m_naBlockedRoute[ctr]=0;
		m_naNumRouteVerts[ctr]=0;
	}
}

void aiNavigation::Forward()
{
	if(m_Car.Stuck())
	{
		PlanRoute();
		m_nState=kBackup;
		return;
	}

	// Determine our road segment and lane
	short nTargetRdId=-1;
	if(m_nWayPtIdx<m_nNumWayPts)
	{
		bool bDir;
		if(m_nWayPtIdx==0)
		{
			aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nNumWayPts-1));
			aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			aiPath *pCurRoad=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&bDir);
			if(pCurRoad)
				nTargetRdId=pCurRoad->Id();
		}
		else
		{
			aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx-1));
			aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
			aiPath *pCurRoad=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&bDir);
			if(pCurRoad)
				nTargetRdId=pCurRoad->Id();
		}
	}
	else
	{
		if(m_nDestMapCompType==kRoad)
		{
			nTargetRdId=m_nDestMapCompId;
		}
	}
	AIMAP.MapComponent(m_Car.GetMatrix().Pos,&m_nCurMapCompIdx,
					   &m_nCurMapCompType,m_Car.GetRoomId(),nTargetRdId);
	SolveRoadTargetPoint();

	// if the destination is behind me, and the dest heading is similar
	Vector3 TargetHeading;
	if(m_AltRoute[m_nBestRoute][1].m_fCumDist==9999.f &&		// going for the destination
	   aiDist2(m_Car.GetMatrix().Pos,m_Destination)<625.f &&
	   aiDot(-m_Car.GetMatrix().ZDir,m_DestHeading)>0.f &&
	   aiDot((m_Destination-m_Car.GetMatrix().Pos),-m_Car.GetMatrix().ZDir)<0.f)
	{
		TargetHeading=-(m_TargetPt-m_Car.GetMatrix().Pos);
		m_fBrake=1.f;
		m_fThrottle=0.f;
	}
	else
	{
		TargetHeading=m_TargetPt-m_Car.GetMatrix().Pos;
	}

	float fX=aiDot(TargetHeading,m_Car.GetMatrix().XDir);
	float fZ=aiDot(TargetHeading,m_Car.GetMatrix().ZDir);
	float fAngle=atan2f(fX,-fZ)*1.33f;

	fAngle*=1.428f;
	m_fSteering=aiClamp(fAngle,-1.f,1.f);

	m_Car.SetSteering(m_fSteering);
	m_Car.SetBrake(m_fBrake);
	m_Car.SetThrottle(m_fThrottle);
}

void aiNavigation::InitBackup()
{
	m_nBackupCount=0;
	m_TargetPt=m_AltRoute[m_nBestRoute][1].Vtx;
	m_Car.SetTransmissionToReverse();
}

void aiNavigation::Backup()
{
	// Calc turn angle to reach the target.
	Vector3 TargetHeading=m_TargetPt-m_Car.GetMatrix().Pos;

	float fX=TargetHeading^m_Car.GetMatrix().XDir;
	float fZ=TargetHeading^-m_Car.GetMatrix().ZDir;

	float fAngle=atan2f(fX,fZ);
	if(fAngle>0.1f || fAngle<-.1f)
	{
		float fSteering=-fAngle/.35f;
		fSteering=aiClamp(fSteering,-1.f,1.f);
		m_Car.SetSteering(fSteering);
	}
	else
	{
		m_Car.SetSteering(0.f);
		FinishedBackingUp();
		return;
	}

	if(m_nBackupCount>65)
	{
		FinishedBackingUp();
		return;
	}
	else
	{