ainavigation.cpp

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

		m_Route[nRouteIdx].m_nType=kRoadTarget;
		return 0;
	}
	else
	{
		float fCircumfrence=m_fRadius[m_Route[nLastIdx].m_nTurnId]*m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId];
		int nNumSteps=(int)(fCircumfrence*.1f);
		nNumSteps+=(nNumSteps%2);	// Make always even
		if(nNumSteps<2)
			nNumSteps=2;

		float fStepAngle=(m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId])/nNumSteps;
		for(int i=1;i<nNumSteps+1;i++)
		{
			if(fMyAngle<(fStepAngle*i-.5f*fStepAngle))
			{
				float fX=m_fRadius[m_Route[nLastIdx].m_nTurnId]*cosf(fStepAngle*i);
				float fZ=m_fRadius[m_Route[nLastIdx].m_nTurnId]*sinf(fStepAngle*i);
				Vector3 Target=m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId]-ZDir*fZ+
							   XDir*m_fTurn[m_Route[nLastIdx].m_nTurnId]*fX;
				Vector3 p=Target-m_Route[nRouteIdx-1].Vtx;
				float fDist=(float)fabsf(aiDot(p,m_Car.GetMatrix().XDir));

				m_Route[nRouteIdx].Vtx=Target;

				SaveTurnTarget(nRouteIdx,0);
				if(i*fStepAngle<m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId]*.5f)
				{
					m_Route[nRouteIdx].m_nSRd=m_Route[nLastIdx].m_nTurnId;
					m_Route[nRouteIdx].m_nSVIdx=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1;
				}
				else
				{
					m_Route[nRouteIdx].m_nSRd=m_Route[nLastIdx].m_nTurnId+1;
					int nRdId=m_Route[nRouteIdx].m_nSRd;
					m_Route[nRouteIdx].m_nSVIdx=m_pRoads[nRdId]->RoadVertice(m_Route[nRouteIdx].Vtx,m_bDir[nRdId],-1);
					if(m_Route[nRouteIdx].m_nSVIdx>=m_pRoads[nRdId]->NumVerts())
					{
						m_Route[nRouteIdx].m_nSRd++;
						m_Route[nRouteIdx].m_nSVIdx=0;
					}
		
					if(nRdId>=m_nRdIdx)
						m_bFinished=true;
				}

				m_Route[nRouteIdx].m_nTurnId=m_Route[nLastIdx].m_nTurnId;

				float fTmpSideDist;
				if(i*2>nNumSteps && 
				   m_pRoads[m_Route[nRouteIdx].m_nTurnId+1]->IsPosOnRoad(m_Route[nRouteIdx].Vtx,0.f,&fTmpSideDist)<3)
				{
					m_Route[nRouteIdx].m_nTurnId++;
					m_LPos=m_Route[nRouteIdx].Vtx;
					break;
				}

				if(i<nNumSteps)
					nRouteIdx++;
			}
		}
	}

	if(m_pRoads[m_Route[nRouteIdx].m_nSRd])
		m_Route[nRouteIdx].m_nSVIdx=aiClamp(m_Route[nRouteIdx].m_nSVIdx,0,
										    m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1);
	return 1;
}

void aiNavigation::SaveTurnTarget(int nRouteIdx,int bDoAngleCalc)
{
	m_Route[nRouteIdx].m_nType=kTurnTarget;
	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(bDoAngleCalc)
	{
		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);
		}
	}
}

void aiNavigation::InitRoadTurns()
{
	// Calc the road angles
	for(int i=0;i<2;i++)
	{
		if(m_pRoads[i] && m_pRoads[i+1])
		{
			float fX, fZ;
			Vector3 TargetDir;
			int nNumVerts=m_pRoads[i]->NumVerts();
			int nNNumVerts=m_pRoads[i+1]->NumVerts();
			if(m_bDir[i+1])
				TargetDir=m_pRoads[i+1]->CenterVertice(1)-m_pRoads[i+1]->CenterVertice(0);
			else
				TargetDir=m_pRoads[i+1]->CenterVertice(nNNumVerts-2)-m_pRoads[i+1]->CenterVertice(nNNumVerts-1);

			if(m_bDir[i])
			{
				fX=aiDot(TargetDir,(-(m_pRoads[i]->VertXDir(nNumVerts-1))));
				fZ=aiDot(TargetDir,(-(m_pRoads[i]->VertZDir(nNumVerts-1))));
			}
			else
			{
				fX=aiDot(TargetDir,m_pRoads[i]->VertXDir(0));
				fZ=aiDot(TargetDir,m_pRoads[i]->VertZDir(0));
			}
			m_fRdAngle[i]=atan2f(fX,fZ);

			// Calc the direction of the turn
			if(m_fRdAngle[i]>=_fTurnThreshold)
				m_fTurn[i]=1.f;
			else if(m_fRdAngle[i]<=-_fTurnThreshold)
				m_fTurn[i]=-1.f;
			else
				m_fTurn[i]=0.f;
		}
		else
		{
			m_fRdAngle[i]=0.f;
			m_fTurn[i]=0.f;
		}
	}
}

void aiNavigation::CalcRoadTurns()
{
	for(int i=0;i<2;i++)
	{
		if(m_fTurn[i] && m_pRoads[i] && m_pRoads[i+1])
		{
			// Calc the intersection point of the current and next roads.
			float fWidth=CalcTurnIntersection(i);
		
			float fTheta=3.14f-fabsf(m_fRdAngle[i]);
			float fHalfTheta=fTheta*.5f;

			m_fRadius[i]=fWidth/(1.f-sinf(fHalfTheta));
			m_fTurnSetback[i]=m_fRadius[i]*cosf(fHalfTheta);

			if(m_bDir[i])
				m_TurnCenterPt[i]=m_IntersectionPt[i]+
								  m_pRoads[i]->VertZDir(m_pRoads[i]->NumVerts()-1)*m_fTurnSetback[i]-
								  m_pRoads[i]->VertXDir(m_pRoads[i]->NumVerts()-1)*m_fTurn[i]*(m_fRadius[i]-fWidth);
			else
				m_TurnCenterPt[i]=m_IntersectionPt[i]+
								  -m_pRoads[i]->VertZDir(0)*m_fTurnSetback[i]-
								  -m_pRoads[i]->VertXDir(0)*m_fTurn[i]*(m_fRadius[i]-fWidth);

			Vector3 XDir, ZDir;
			if(m_bDir[i])
			{
				XDir=m_pRoads[i]->VertXDir(m_pRoads[i]->NumVerts()-1);
				ZDir=m_pRoads[i]->VertZDir(m_pRoads[i]->NumVerts()-1);
			}
			else
			{
				XDir=-m_pRoads[i]->VertXDir(0);
				ZDir=-m_pRoads[i]->VertZDir(0);
			}

			Vector3 StartPt=m_TurnCenterPt[i]+XDir*m_fTurn[i]*m_fRadius[i];
			m_TurnStartDir[i]=StartPt-m_TurnCenterPt[i];
			m_TurnStartDir[i].y=0.f;
			m_TurnStartDir[i].Normalize();

			float fX=m_fRadius[i]*cosf(m_fRdAngle[i]*m_fTurn[i]);
			float fZ=m_fRadius[i]*sinf(m_fRdAngle[i]*m_fTurn[i]);
			Vector3 EndPt=m_TurnCenterPt[i]-ZDir*fZ+XDir*m_fTurn[i]*fX;
			m_TurnEndDir[i]=EndPt-m_TurnCenterPt[i];
			m_TurnEndDir[i].y=0.f;
			m_TurnEndDir[i].Normalize();
		}
	}
}

float aiNavigation::CalcTurnIntersection(int nWhichTurn)
{
	int nNumVerts=m_pRoads[nWhichTurn]->NumVerts();
	int nNNumVerts=m_pRoads[nWhichTurn+1]->NumVerts();

	// Calc the turn offsets. This could be moved away a little from the sidewalk
	float fCRdOffset=0.f;
	float fNRdOffset=0.f;

	float t;
	Vector3 P0, P1, Q0, Q1;
	if(m_fTurn[nWhichTurn]>0.f)	// right turn
	{
		int nVerts=0;
		if(m_bDir[nWhichTurn])
		{
			P0=m_pRoads[nWhichTurn]->RBoundary(nNumVerts-1+nVerts)+m_pRoads[nWhichTurn]->VertXDir(nNumVerts-1)*fCRdOffset; 
			P1=-(m_pRoads[nWhichTurn]->VertZDir(nNumVerts-1));
		}
		else
		{
			P0=m_pRoads[nWhichTurn]->LBoundary(0+nVerts)-m_pRoads[nWhichTurn]->VertXDir(0)*fCRdOffset; 
			P1=-(m_pRoads[nWhichTurn]->VertZDir(0));
		}

		if(m_bDir[nWhichTurn+1])
		{
			Q0=m_pRoads[nWhichTurn+1]->RBoundary(0+nVerts)+m_pRoads[nWhichTurn+1]->VertXDir(0)*fNRdOffset; 
			Q1=m_pRoads[nWhichTurn+1]->VertZDir(0);
			m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->RBoundary(0).y;
		}
		else
		{
			Q0=m_pRoads[nWhichTurn+1]->LBoundary(nNNumVerts-1+nVerts)-m_pRoads[nWhichTurn+1]->VertXDir(nNNumVerts-1)*fNRdOffset; 
			Q1=m_pRoads[nWhichTurn+1]->VertZDir(nNNumVerts-1);
			m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->LBoundary(nNNumVerts-1).y;
		}
	}
	else
	{
		int nVerts=0;
		if(m_bDir[nWhichTurn])
		{
			P0=m_pRoads[nWhichTurn]->LBoundary(nNumVerts-1+nVerts)-m_pRoads[nWhichTurn]->VertXDir(nNumVerts-1)*fCRdOffset;
			P1=-(m_pRoads[nWhichTurn]->VertZDir(nNumVerts-1));
		}
		else
		{
			P0=m_pRoads[nWhichTurn]->RBoundary(0+nVerts)+m_pRoads[nWhichTurn]->VertXDir(0)*fCRdOffset;
			P1=-(m_pRoads[nWhichTurn]->VertZDir(0));
		}

		if(m_bDir[nWhichTurn+1])
		{
			Q0=m_pRoads[nWhichTurn+1]->LBoundary(0+nVerts)-m_pRoads[nWhichTurn+1]->VertXDir(0)*fNRdOffset;
			Q1=-(m_pRoads[nWhichTurn+1]->VertZDir(0));
			m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->LBoundary(0).y;
		}
		else
		{
			Q0=m_pRoads[nWhichTurn+1]->RBoundary(nNNumVerts-1+nVerts)+m_pRoads[nWhichTurn+1]->VertXDir(nNNumVerts-1)*fNRdOffset;
			Q1=m_pRoads[nWhichTurn+1]->VertZDir(nNNumVerts-1);
			m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->RBoundary(nNNumVerts-1).y;
		}
	}

	t=(P1.x*Q0.z-P1.x*P0.z-P1.z*Q0.x+P1.z*P0.x)/(P1.z*Q1.x-P1.x*Q1.z);
	m_IntersectionPt[nWhichTurn].x=Q0.x+t*Q1.x;
	m_IntersectionPt[nWhichTurn].z=Q0.z+t*Q1.z;

	// Add 1.f to the width in order to make the turn grow as the opp
	// gets closer to the turn, but only if more than 10m to the turn.
	float fOffset=0.f;
	float fDist2=(m_fTurnSetback[nWhichTurn]+12.f)*(m_fTurnSetback[nWhichTurn]+12.f);
	if(aiDist2(m_LPos,m_IntersectionPt[nWhichTurn])>fDist2)
		fOffset=1.f;

	float fWidth;
	Vector3 p=m_LPos-m_IntersectionPt[nWhichTurn];
	if(m_bDir[nWhichTurn]==1)
		fWidth=m_fTurn[nWhichTurn]*aiDot(p,m_pRoads[nWhichTurn]->VertXDir(m_pRoads[nWhichTurn]->NumVerts()-1))+fOffset;
	else
		fWidth=m_fTurn[nWhichTurn]*aiDot(p,-m_pRoads[nWhichTurn]->VertXDir(0))+fOffset;

	return fWidth;
}

int aiNavigation::InSharpTurn(int nRouteIdx)
{
	int i;
	Vector3 MyDir;

	for(i=m_Route[nRouteIdx].m_nSRd;i<3;i++)
	{
		// Test the intersections
		if(i==0 || i==1 && m_Route[nRouteIdx].m_nTurnId==1 ||
		  (i==1 && -_fMaxStraightAngle<m_fRdAngle[0] && m_fRdAngle[0]<_fMaxStraightAngle))
		{
			float fRadius2=(m_fRadius[i]+15.f)*(m_fRadius[i]+15.f);
			if(m_fTurn[i] && m_pRoads[i] && m_pRoads[i+1] && aiDist2(m_Route[nRouteIdx].Vtx,m_TurnCenterPt[i])<fRadius2)
			{
				MyDir=m_Route[nRouteIdx].Vtx-m_TurnCenterPt[i];
				
				Vector3 StartPerp;
				StartPerp.Set(-m_TurnStartDir[i].z,m_TurnStartDir[i].y,m_TurnStartDir[i].x);
				float fSX=aiDot(MyDir,StartPerp*m_fTurn[i]);
				
				Vector3 EndPerp;
				EndPerp.Set(m_TurnEndDir[i].z,m_TurnEndDir[i].y,-m_TurnEndDir[i].x);
				float fEX=aiDot(MyDir,EndPerp*m_fTurn[i]);
				if(fSX>=-0.01f && fEX>=0.01f)
					return i+1;
			}
		}
	}

	return 0;
}

void aiNavigation::SaveTarget(int nRouteIdx,Vector3& Target,aiObstacle *pObstacle,short& Location,
									int nObsType,short& TargetType)
{
	int nNVIdx;
	int nCRd=pObstacle->CurrentRoadIdx(m_pRoads,m_bDir,&nNVIdx);
	if(!m_pRoads[nCRd])
	{
		nCRd--;
		if(nCRd<0)
			return;
		nNVIdx=m_pRoads[nCRd]->NumVerts()-1;
	}
	nNVIdx=aiClamp(nNVIdx,0,m_pRoads[nCRd]->NumVerts()-1);

	m_Route[nRouteIdx].m_nType=TargetType;
	m_Route[nRouteIdx].m_nObsType=nObsType;
	m_Route[nRouteIdx].m_pObstacle=pObstacle;
	m_Route[nRouteIdx].m_nOnRoad=Location;
	m_Route[nRouteIdx].Vtx=Target;
	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);
	}
	m_Route[nRouteIdx].m_nSVIdx=nNVIdx;
	m_Route[nRouteIdx].m_nSRd=nCRd;
	ContinueCheck(nRouteIdx);
}

void aiNavigation::ContinueCheck(int nRouteIdx)
{
	// Test if we're at the end of the route and are now ready to navigate to the destination
	// point.
	if(m_Route[nRouteIdx].m_fCumDist<m_fRoutePlanDist && 
	   m_pRoads[m_Route[nRouteIdx].m_nSRd] &&
	   (m_Route[nRouteIdx].m_nSRd==m_nRdIdx ||
	   m_Route[nRouteIdx].m_nSVIdx>=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1 &&
	   m_Route[nRouteIdx].m_nSRd+1<=2 && m_pRoads[m_Route[nRouteIdx].m_nSRd+1]==0) &&
	   m_nNumRoutes<10 &&
	   nRouteIdx<=_nMaxRouteSplits)
	{
		nRouteIdx++;
		m_bFinished=true;
		EnumRoutes(nRouteIdx);
	}
	else if(m_Route[nRouteIdx].m_fCumDist<m_fRoutePlanDist && m_pRoads[2] &&
		    !(m_Route[nRouteIdx].m_nSRd==2 && m_Route[nRouteIdx].m_nSVIdx==m_pRoads[2]->NumVerts()) &&
			m_nNumRoutes<10 &&
			nRouteIdx<=_nMaxRouteSplits)
	{
		nRouteIdx++;
		EnumRoutes(nRouteIdx);
	}
	else
	{
		// Save off all the routes to view as alternatives
		for(int i=0;i<=nRouteIdx;i++)
		{
			m_AltRoute[m_nNumRoutes][i]=m_Route[i];
			if(m_Route[i].m_nOnRoad==2)
				m_naOffRoadRoute[m_nNumRoutes]=1;
			if(m_Route[i].m_nType==kBlockedTarget)
				m_naBlockedRoute[m_nNumRoutes]=1;
		}
		m_naNumRouteVerts[m_nNumRoutes]=nRouteIdx+1;
		m_nNumRoutes++;
	}
}

void aiNavigation::CalcDestinationTarget(int nRouteIdx,Vector3& Position)
{
	Vector3 p, LTarget, RTarget;
	float fLX, fRX, fX, fZ, fLAngle, fRAngle, fLViewAngle, fRViewAngle;
	short nCRd, nLRd, nRRd, nCVIdx, nLVIdx, nRVIdx;

	nCRd=m_Route[nRouteIdx-1].m_nSRd;
	nCVIdx=m_Route[nRouteIdx-1].m_nSVIdx;
	int nNumVerts=m_pRoads[nCRd]->NumVerts();

	if(nRouteIdx>1 && m_Route[nRouteIdx-2].m_nType!=kTurnTarget)
	{
		nCVIdx++;
		if(nCVIdx==m_pRoads[nCRd]->NumVerts())
		{
			if(m_pRoads[nCRd+1])
			{
				nCVIdx=0;
				nCRd++;
				nNumVerts=m_pRoads[nCRd]->NumVerts();
			}
			else
				nCVIdx--;
		}
	}

	switch(m_nDestMapCompType)
	{
		case kRoad:
		{

			int nIndex=nNumVerts-nCVIdx;
			if(nIndex==nNumVerts)
				nIndex--;

			Position=m_Route[nRouteIdx-1].Vtx;

			if(m_bDir[nCRd])
			{
				// Init 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;
				p=LTarget-m_Route[nRouteIdx-1].Vtx;

				fLX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
				fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
				if(-0.01<fLX && fLX<0.01f)
					fLX=-1.f;
				fLViewAngle=atan2f(fLX,fZ);

				// Now init the right side
				RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+
						(m_pRoads[nCRd]->VertXDir(nCVIdx))*m_fRSideDist;
				p=RTarget-m_Route[nRouteIdx-1].Vtx;

				fRX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
				fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
				if(-0.01<fRX && fRX<0.01f)
					fRX=1.f;
				fRViewAngle=atan2f(fRX,fZ);
			}
			else
			{