basicdash.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

		body.getPos().getY() > rectY1)
		sourcePlace = 3;
	if (body.getPos().getX() <= rectX1 && body.getPos().getY() <= rectY1)
		sourcePlace = 4;
	if (body.getPos().getX() > rectX1 && body.getPos().getY() <= rectY1)
		sourcePlace = 5;

#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint realTargetPlace = " << realTargetPlace << endl;
LOG << "FAGotoPoint sourcePlace" << sourcePlace <<  endl;
#endif
		
	if (sourcePlace == realTargetPlace)
		goModel = 0;
	else if (abs(sourcePlace - realTargetPlace) == 1)
		goModel = 0;
	else if ((sourcePlace == 1 && realTargetPlace == 3) || 
			 (sourcePlace == 1 && realTargetPlace == 4) ||
			 (sourcePlace == 1 && realTargetPlace == 5))
		goModel = 1;
	else if (sourcePlace == 2 && realTargetPlace == 4)
		goModel = 0;
	else if (sourcePlace == 2 && realTargetPlace == 5)
		goModel = 2;
	else if (sourcePlace == 3 && realTargetPlace == 1)
		goModel = 1;
	else if (sourcePlace == 3 && realTargetPlace == 5)
		goModel = 2;
	else if (sourcePlace == 4 && realTargetPlace == 1)
		goModel = 1;
	else if (sourcePlace == 4 && realTargetPlace == 2)
		goModel = 0;
	else if ((sourcePlace == 5 && realTargetPlace == 1) || 
			 (sourcePlace == 5 && realTargetPlace == 2) ||
			 (sourcePlace == 5 && realTargetPlace == 3))
		goModel = 2;

#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint goModel = " << goModel << endl;
#endif
	
	command = NULL;

	if (goModel == 0)
	{
		DashNormalGotoPoint doGotoPoint(creator, realTargetPoint, 1.4, body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
	}
	if (goModel == 1)
	{
		goAroundFlag = true;
		DashNormalGotoPoint doGotoPoint(creator, Point(rectX1 - 1.5,rectY2 + 1.5), 1,
				body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
	}
	if (goModel == 2)
	{
		goAroundFlag = true;
		DashNormalGotoPoint doGotoPoint(creator, Point(rectX1 - 1.5,rectY1 - 1.5), 1,
				body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
	}
}

ForbiddenAreaGotoPoint::ForbiddenAreaGotoPoint(ActionType creator, const Point &point, Point &rectNW,
		 Point &rectSE, const Body &body): BasicAction(creator)
{
	ForbiddenAreaGotoPoint(creator, rectNW.x, rectNW.y, rectSE.y, body);
}

ForbiddenAreaGotoPoint::ForbiddenAreaGotoPoint(ActionType creator, const Point &point,
		 Point circleCenter, float circleRadius, const Body &body,
		 bool forbiddenX): BasicAction(creator)
{
	Vector sourceCenter;
	Vector targetCenter;
	Vector sourceTarget;
	Vector moveVector;
	float formulaA = 0;
	bool intersectFlag;	
	forbiddenSourceFlag = false;
	forbiddenTargetFlag = false;
	realTargetPlace = 0;
	realTargetPoint = point;
	intersectFlag = false;
	goAroundFlag = false;

	command = NULL;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "ForbiddenAreaGotoPoint Circle" << endl;
#endif
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint Point = " << point << endl;	
#endif
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint Center = " << circleCenter << endl;	
#endif
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint Radius = " << circleRadius << endl;	
#endif
	targetCenter.setByPoints(point, circleCenter);
	sourceCenter.setByPoints(body.getPos(), circleCenter);
	if (forbiddenX && point.x > circleCenter.x - 1.5)
	{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint forbiddenTargetX = true" << endl;
#endif
		forbiddenTargetFlag = true;
		realTargetPoint.y = point.y;
		realTargetPoint.x = circleCenter.x - 1.5;
		targetCenter.setByPoints(realTargetPoint, circleCenter);
		
	}
	if (targetCenter.getMagnitude() < circleRadius)
	{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint forbiddenTarget = true" << endl;
#endif
		forbiddenTargetFlag = true;
		moveVector = targetCenter;
		moveVector *= -1;
		moveVector.setAsPolar(circleRadius + 0.5, moveVector.getDirection());
		realTargetPoint = (Vector)circleCenter + moveVector;
		targetCenter.setByPoints(realTargetPoint, circleCenter);
	}
	sourceTarget.setByPoints(body.getPos(), realTargetPoint);

	if (body.getDistance(circleCenter) < circleRadius - 1.3)
	{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint forbiddenSource = true" << endl;
#endif
		forbiddenSourceFlag = true;
		targetCenter.setByPoints(body.getPos(), circleCenter);
		moveVector = targetCenter;
		moveVector *= -1;
		moveVector.setAsPolar(circleRadius + 0.5, moveVector.getDirection());
		realTargetPoint = (Vector)circleCenter + moveVector;

		DashNormalGotoPoint doGotoPoint(creator, realTargetPoint, 1.5, body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
		return;
		/*
		command = new EmptyCommand();
		status = 0;
		return;
		*/
	}

	command = NULL;
	
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint newPoint = " << realTargetPoint << endl;	
#endif

	Line goLine;
	goLine.setByPoints(body.getPos(), realTargetPoint);
	formulaA = goLine.getPointDist(circleCenter);
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint formulaA = " << formulaA << endl;	
#endif
	if (formulaA < circleRadius - 0.07 &&
	   	sourceTarget.getMagnitude() > sourceCenter.getMagnitude())
	{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint <radius " << endl;	
#endif
		Point tangence1, tangence2;
		float tangenceStandDistance;
		Point goPoint;
		bool tangencePossible1, tangencePossible2;
		Vector tangenceVector1;
		Vector tangenceVector2;
		bool directDash1, directDash2;

		float centerTargetAngle;
		float centerSourceAngle;
		float middleAngle;

		goAroundFlag = true;
		centerSourceAngle = normalizeAngle(sourceCenter.getDirection() + 180);
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint centerSourceAngle=" << centerSourceAngle << endl;	
#endif
		centerTargetAngle = normalizeAngle(targetCenter.getDirection() + 180);
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint centerTargetAngle=" << centerTargetAngle << endl;
#endif
		middleAngle = (centerTargetAngle - centerSourceAngle) / 2;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint middleAngle=" << middleAngle << endl;	
#endif
		
		directDash1 = directDash2 = true;
		intersectFlag = true;
		tangencePossible1 = true;
		tangencePossible2 = true;
		tangenceStandDistance = Basics::reRate(sourceCenter.getMagnitude() - circleRadius,
				0, 10, 0, 10, RRM_REVERSE);
		moveVector.setAsPolar(circleRadius + tangenceStandDistance,
				centerSourceAngle + middleAngle);
		tangence1 = (Vector)circleCenter + moveVector;
		moveVector.setAsPolar(circleRadius + tangenceStandDistance,
				centerSourceAngle + middleAngle + 180);
		tangence2 = (Vector)circleCenter + moveVector;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint tp1" << tangence1 << endl;
#endif
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint tp2" << tangence2 << endl;
#endif
		if (fabs(tangence1.x) > 56.5 || fabs(tangence1.y) > 38 ||
				(forbiddenX && tangence1.x > circleCenter.x))
			tangencePossible1 = false;
		if (fabs(tangence2.x) > 56.5 || fabs(tangence2.y) > 38 ||
				(forbiddenX && tangence2.x > circleCenter.x))
			tangencePossible2 = false;
		tangenceVector1.setByPoints(body.getPos(), tangence1);
		tangenceVector2.setByPoints(body.getPos(), tangence2);
/*
		if (tangenceVector1.getMagnitude() < 0.45)
		{
			tangencePossible1 = true;
			moveVector.setAsPolar(circleRadius + 2.8,
					targetCenter.getDirection() + 45);
			tangence1 = (Vector)circleCenter + moveVector;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPointnew tp1" << tangence1 << endl;
#endif
			if (fabs(tangence1.x) > 56.5 || fabs(tangence1.y) > 38)
				tangencePossible1 = false;
			tangenceVector1.setByPoints(body.getPos(),tangence1);
		}
		
		if (tangenceVector2.getMagnitude() < 0.45)
		{
			tangencePossible2 = true;
			moveVector.setAsPolar(circleRadius + 2.8,
					targetCenter.getDirection() - 45);
			tangence2 = (Vector)circleCenter + moveVector;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPointnew tp2" << tangence2 << endl;
#endif
			if (fabs(tangence2.x) > 56.5 || fabs(tangence2.y) > 38)
				tangencePossible2 = false;
			tangenceVector2.setByPoints(body.getPos(),tangence2);
		}
*/
		if (!tangencePossible1)
		{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select2 mode 1" << endl;
#endif
			goPoint = tangence2;
		}
		else if (!tangencePossible2)
		{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select1 mode 1" << endl;
#endif
			goPoint = tangence1;
		}
		else if (tangenceVector1.getMagnitude() < tangenceVector2.getMagnitude()		-1 &&
				tangencePossible1)
		{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select1 mode 2" << endl;
#endif
			goPoint = tangence1;
		}
		else if (tangenceVector2.getMagnitude() < tangenceVector1.getMagnitude()		-1 &&
				tangencePossible2)
		{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select2 mode 2" << endl;
#endif
			goPoint = tangence2;
		}
		else
		{
			DashNormalGotoPoint checkGotoPoint1 (creator, tangence1, 0.9, body);
			DashNormalGotoPoint checkGotoPoint2 (creator, tangence2, 0.9, body);
			delete checkGotoPoint1.getCommand();
			delete checkGotoPoint2.getCommand();
			if (checkGotoPoint1.getMode() == CT_TURN)
				directDash1 = false;
			else
				directDash1 = true;
			if (checkGotoPoint2.getMode() == CT_TURN)
				directDash2 = false;
			else
				directDash2 = true;
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint directdashes" << directDash1 << " " << directDash2 << endl;
#endif
			if (tangencePossible1 && directDash1 && !directDash2)
			{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select1 mode 3" << endl;
#endif
				goPoint = tangence1;
			}
			else if (tangencePossible2 && directDash2 && !directDash1)
			{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select2 mode 3" << endl;
#endif
				goPoint = tangence2;
			}
			else if (fabs(tangence1.y) < fabs(tangence2.y))
			{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select1 mode 4" << endl;
#endif
				goPoint = tangence1;
			}
			else
			{
#ifdef FORBIDDENAREA_GOTO_POINT_LOG
LOG << "FAGotoPoint select2 mode 4" << endl;
#endif
				goPoint = tangence2;
			}
		}
		DashNormalGotoPoint doGotoPoint(creator, goPoint, 1.1, body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
	}
	else
	{
		DashNormalGotoPoint doGotoPoint(creator, realTargetPoint, 1.4, body, 90);
		command = doGotoPoint.getCommand();
		if (doGotoPoint.getMode() == CT_EMPTY)
			status = 0;
		else
			status = 1;	
	}
}

Command *ForbiddenAreaGotoPoint::getCommand()
{
	return command;
}

bool ForbiddenAreaGotoPoint::isForbiddenSource() const
{
	return forbiddenSourceFlag;
}

bool ForbiddenAreaGotoPoint::isForbiddenTarget() const
{
	return forbiddenTargetFlag;
}

int ForbiddenAreaGotoPoint::getRealTargetPlace() const
{
	return realTargetPlace;
}

Point ForbiddenAreaGotoPoint::getRealTargetPoint() const
{
	return realTargetPoint;
}

bool ForbiddenAreaGotoPoint::isClockwise() const
{
	return clockwiseFlag;
}

int ForbiddenAreaGotoPoint::getStatus() const
{
	return status;
}