dribble.cpp

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

		dribblePoint2.x = bodyNoDash.getPos().getX() + dribbleVector.getX();
		dribblePoint2.y = bodyNoDash.getPos().getY() + dribbleVector.getY();
	}
	
	localCycleNums = cycleNums[dribbleCount];

	Vector ballVector;
	ballVector.setByPoints(Point(ballNoDash.getPos().getX(), 
		ballNoDash.getPos().getY()), dribblePoint);

	float deltaAngle;
	deltaAngle = ballVector.getDirection() - worldModel->getBody().getBodyDir();
	deltaAngle = Degree::normalizeAngle(deltaAngle);

	if (fabs(deltaAngle) > angleNoise[dribbleCount] && 
		ballNoDash.getBodyVec().getMagnitude() + 
		kickableNoise[dribbleCount] < kickableArea)
		localCycleNums = 2;	 

	///LOG << "dribblePoint: " << dribblePoint << endl;
	///LOG << "angle: " << angle[dribbleCount] << endl;
	///LOG << "noise1: " << noise1[dribbleCount] << endl;
	///LOG << "noise2: " << noise2[dribbleCount] << endl;
	///LOG << "angleNoise: " << angleNoise[dribbleCount] << endl;
	///LOG << "maxDist: " << maxDist[dribbleCount] << endl;
	///LOG << "kickableNoise: " << kickableNoise[dribbleCount] << endl;
	///LOG << "Dribble : cycleNums : " << localCycleNums << endl;	

	if (ballInOppKickable())
		maxDist[dribbleCount] = 0;
		
	float collisionDist;
	collisionDist = hypot(bodyByDash.getPos().getX() - 
		ballByDash.getPos().getX(), bodyByDash.getPos().getY() - 
		ballByDash.getPos().getY());

#ifdef DRIBBLE_LOGLEVEL1
//	LOG << "collisionDist : " << collisionDist << endl;
	LOG << "dribbleCount : " << dribbleCount << endl;
//	LOG << "maxDist[dribbleCount] : " << maxDist[dribbleCount] << endl;
//	LOG << "ballByDash.getBodyVec().getMagnitude() : " << 
//		ballByDash.getBodyVec().getMagnitude() << endl;
//	LOG << "kickableNoise[dribbleCount] : " << kickableNoise[dribbleCount] << endl;
//	LOG << "kickableArea : " << kickableArea << endl;
	LOG << "OpponentInSense : " << opponentInSence << endl;
//	LOG << "kickPower[dribbleCount] : " << kickPower[dribbleCount] << endl;
#endif


#ifdef DRIBBLE_LOGLEVEL1
/////////////////////////NEW LOGS///////////////////////
	LOG << "angle[DC]         : " << angle[dribbleCount] << endl;
	LOG << "virtualWeight[DC] : " << virtualWeight[dribbleCount] << endl;
	LOG << "noise1[DC]        : " << noise1[dribbleCount] << endl;
	LOG << "noise2[DC]        : " << noise2[dribbleCount] << endl;
	LOG << "dashPower[DC]     : " << dashPower[dribbleCount] << endl;
	LOG << "maxDist[DC]       : " << maxDist[dribbleCount] << endl;
	LOG << "cycleNums[DC]     : " << cycleNums[dribbleCount] << endl;
	LOG << "kickableNoise[DC] : " << kickableNoise[dribbleCount] << endl;
	LOG << "angleNoise[DC]    : " << angleNoise[dribbleCount] << endl;
	LOG << "kickPower[DC]     : " << kickPower[dribbleCount] << endl;
	LOG << "kickableArea      : " << kickableArea << endl;
/////////////////////////NEW LOGS END///////////////////
#endif


	
	if (ballNoDash.getBodyVec().getMagnitude() + .1 >= kickableArea)
	{
		if (ballByDash.getBodyVec().getMagnitude() + 
			kickableNoise[dribbleCount] < kickableArea && 
			hypot(dribblePoint.x - ballPoint.x, dribblePoint.y - ballPoint.y) < 
			(maxDist[dribbleCount] * kickableArea))
		{
//			delete dribbleCommand;
#ifdef DRIBBLE_LOGLEVEL1
			LOG << "Dribble : Status1" << endl;
#endif
			dribbleCommand = new DashCommand(AT_DRIBBLE, localDashPower);
			
			nextBody.simulateByAction(dribbleCommand);
			nextBody.simulateByDynamics();
			nextBall.simulateByDynamics(nextBody);
		}	
		else
		{
			dribbleCommand = KickToPointCycles(AT_DRIBBLE, dribblePoint2, 
				localCycleNums, worldModel->getBall(), 
				worldModel->getBody()).getCommand();			
				
			nextBody.simulateByDynamics();
			nextBall.simulateByAction(nextBody, dribbleCommand);
			nextBall.simulateByDynamics(nextBody);

#ifdef DRIBBLE_LOGLEVEL1
			LOG << "Dribble : Status2" << endl;		   
			LOG << "dribblePoint2 : " << dribblePoint2 << endl;
//			LOG << "dribblePoint2 of Selected dribbleCount :::::::: : " << 
//				drbblePoint2 << endl;
//			LOG << "DribblePoint1 of Selected dribbleCount :::::::: : " << 
//				dribblePoint << endl;
#endif
		}
	}
	else
	{
		if (hypot(dribblePoint.x - ballPoint.x, dribblePoint.y - ballPoint.y) > 
			(maxDist[dribbleCount] * kickableArea)
			|| ballByDash.getBodyVec().getMagnitude() + .1 >= kickableArea
			|| collisionDist <= .4)
		{
			dribbleCommand = KickToPointCycles(AT_DRIBBLE, dribblePoint2, 
				localCycleNums, worldModel->getBall(), 
				worldModel->getBody()).getCommand();

#ifdef DRIBBLE_LOGLEVEL1
			LOG << "Dribble : Status3" << endl;
			LOG << "dribblePoint2 : " << dribblePoint2 << endl;
#endif

			nextBody.simulateByDynamics();
			nextBall.simulateByAction(nextBody, dribbleCommand);
			nextBall.simulateByDynamics(nextBody);
		}
		else 
		{
#ifdef DRIBBLE_LOGLEVEL1
			LOG << "Dribble : Status4" << endl;
#endif

//			delete dribbleCommand;
			dribbleCommand = new DashCommand(AT_DRIBBLE, localDashPower);
			nextBody.simulateByAction(dribbleCommand);
			nextBody.simulateByDynamics();
			nextBall.simulateByDynamics(nextBody);
		}
	}

	checkTargetBall = ballNoDash;
	checkTargetBody = bodyNoDash;
	checkDribbleTarget(dribbleCount);

//	LOG << "dribbleCount Latest : " << dribbleCount << endl;
	
	float dribbleWeight;
	dribbleWeight = 35 - ((dribbleCount / 2) * 11);

	float pathWeight = 0;
	float deltaPathAngle;
	Vector currentPathVector, idealPathVector;

	currentPathVector.setByPoints(Point(worldModel->getBody().getPos().getX(), 
		worldModel->getBody().getPos().getY()), dribbleTarget);

	idealPathVector.setByPoints(Point(worldModel->getBody().getPos().getX(), 
		worldModel->getBody().getPos().getY()), idealDribbleTarget);

	deltaPathAngle = currentPathVector.getDirection() - idealPathVector.getDirection();
	deltaPathAngle = Degree::normalizeAngle(deltaPathAngle);
	deltaPathAngle = fabs(deltaPathAngle);
	
	if (deltaPathAngle < 40)
		pathWeight = (40.00 - deltaPathAngle) / 1.00;
	
	if (dribbleCount == 4 || dribbleCount == 5)
		dribbleWeight -= 20;
	
	dribbleWeight += pathWeight;	
	
	float inlineDribbleWeight = 0;
	inlineDribbleWeight = getInlineDribbleWeight(dribbleCount, 2.8);
	inlineDribbleWeight = pow((inlineDribbleWeight * 3.6), 2) * 3;
	LOG << "inlineDribbleWeight : " << inlineDribbleWeight << endl;
	dribbleWeight -= inlineDribbleWeight;

	delete dashCommand;	
	return dribbleWeight + getBlockedWeight() + getBodyCrossWeight() + 5; //MEISAM2005
}

float Dribble::getBlockedWeight()
{
	float min = DRIBBLE_BLOCK_MAX_WEIGHT;

	for (unsigned i = 0; i < FULL_PLAYERS_NUM; i++)
	{
		if (!worldModel->getFullPlayer(TID_OPPONENT, i).isValid())
			continue;

		float cur = getPlayerBlockWeight(
				worldModel->getFullPlayer(TID_OPPONENT, i));
		if (cur < min)
			min = cur;
	}

	for (unsigned i = 0; i < HALF_PLAYERS_NUM; i++)
	{
		if (!worldModel->getHalfPlayer(TID_OPPONENT, i).isValid())
			continue;

		float cur = getPlayerBlockWeight(
				worldModel->getHalfPlayer(TID_OPPONENT, i));
		if (cur < min)
			min = cur;
	}

	for (unsigned i = 0; i < QUARTER_PLAYERS_NUM; i++)
	{
		if (!worldModel->getQuarterPlayer(i).isValid())
			continue;

		float cur = getPlayerBlockWeight(
				worldModel->getQuarterPlayer(i));
		if (cur < min)
			min = cur;
	}

	LOG << "DribbleBlockedWeight: " << min << endl;
	return min;
}

float Dribble::getPlayerBlockWeight(const Player &player)
{
	if (player.getAbsVec().getMagnitude() > 5)
		return DRIBBLE_BLOCK_MAX_WEIGHT - 35;

LOG << "Dribble::getPlayerBlockWeight" << endl;
LOG << "Player: " << player.getTeamId() << " " << player.getUniNum() << endl;

	if (player.getAbsVec().getMagnitude() < 2.5 &&
		fabs(player.getBodyVec().getDirection()) > 125)
	{
		LOG << "MIN_WEIGHT: in forbidden area." << endl;
		return 0 - 35;
	}

	float deltaAngle;

	if (player.getVel().getMagnitude() > 0.2)
	{
		float bodyDir =
				Degree::normalizeAngle(180 + player.getAbsVec().getDirection());
		deltaAngle = fabs(Degree::getDeltaAngle(bodyDir,
				player.getVel().getDirection()));
	}
	else
		deltaAngle = 180;

LOG << "deltaAngle: " << deltaAngle << endl;

	float distValue = /*sqrt(*/player.getAbsVec().getMagnitude()/*)*/;

LOG << "distValue: " << distValue << endl;

	float distWeight = Basics::reRate(distValue, /*1.20, SQRT_5*/ 2, 5, 0,
			DRIBBLE_BLOCK_MAX_WEIGHT * DRIBBLE_BLOCK_DIST_RATE, RRM_DIRECT);

LOG << "dist from 1.2 to " << SQRT_5 << endl;
LOG << "distWeight: " << distWeight << endl;

	float dirWeight = Basics::reRate(deltaAngle, 20, 80, 0,
			DRIBBLE_BLOCK_MAX_WEIGHT * DRIBBLE_BLOCK_DIR_RATE, RRM_DIRECT);

LOG << "dirWeight: " << dirWeight << endl;
LOG << "Weight: " << distWeight + dirWeight - 35 << endl;

	return distWeight + dirWeight - 35;
}

float Dribble::getBodyCrossWeight()
{
	float shootDir = Pass::getShootDirValue(
			worldModel->getBody().getPos(), worldModel);
	float shootDist = hypot(worldModel->getBody().getPos().getX() - 52.5,
			fmin(worldModel->getBody().getPos().getY() - 4,
				 worldModel->getBody().getPos().getY() + 4));

	float shootValue = shootDir - fmax(shootDist - 10, 0) * 1.15;

	float shootWeight = fmax (0, shootValue * 2);
	LOG << "DribbleShootWeight: " << shootWeight << endl;

	return shootWeight;
}


// class DribblePath
DribblePath::DribblePath(const WorldModel *worldModel):	worldModel(worldModel)
{
}

void DribblePath::setAngleWeight(int targetCount)
{
	float angleWeight = 0;
	float deltaAngle = targetAngles[targetCount] - targetAngles[0];
	deltaAngle = Degree::normalizeAngle(deltaAngle);

	Vector tempVector;
	tempVector.setAsPolar(cautionLength, deltaAngle);

	angleWeight += ((180 - fabs(deltaAngle)) / 30.00) * .15;
	 
	targetWeights[targetCount] += angleWeight * angleWeightRate;
}

void DribblePath::setCraudWeight(int targetCount)
{
	float beginAngle = targetAngles[targetCount] - 90;
	beginAngle = Degree::normalizeAngle(beginAngle);
	Vector tempVector;
	
	float currentAngle;
	float craudWeight = 0;
	float playerDist;
	float negWeight = 0;
	float localNegWeight;
	Vector playerVector;
	
	for (int j = 0; j < FULL_PLAYERS_NUM; j++)
		if (worldModel->getFullPlayer(TID_OPPONENT, j).isValid())
		{
			playerVector.setByPoints(beginPoint, 
				worldModel->getFullPlayer(TID_OPPONENT, j).getPos());
			
			currentAngle = playerVector.getDirection() - beginAngle;	
			currentAngle = Degree::normalizeAngle(currentAngle);

			playerDist = worldModel->getFullPlayer(TID_OPPONENT, j).getDistance(beginPoint);			
			tempVector.setAsPolar(playerDist, currentAngle);

			if (playerDist < cautionLength * 1.2)
				isOpponent = 1;

			if (tempVector.getY() <= cautionLength && tempVector.getY() > 0 && 
					fabs(tempVector.getX()) <= (cautionLength * xRate))
			{
				localNegWeight = -(tempVector.getY() - fabs(tempVector.getX()));
				localNegWeight *= (fabs(tempVector.getX()) / cautionLength) / xRate;

				if (playerDist < 5)
				{
					float rate = 5 - playerDist;
					if (rate < 1.5)
						rate = 1.5;
	
					localNegWeight *= rate;
				}

				if (localNegWeight > 0)
					localNegWeight = 0;

				if (localNegWeight < negWeight)
					negWeight = localNegWeight;
			}
		}	

	for (int j = 0; j < HALF_PLAYERS_NUM; j++)
		if (worldModel->getHalfPlayer(TID_OPPONENT, j).isValid())
		{
			playerVector.setByPoints(beginPoint, 
				worldModel->getHalfPlayer(TID_OPPONENT, j).getPos());
			
			currentAngle = playerVector.getDirection() - beginAngle;	
			currentAngle = Degree::normalizeAngle(currentAngle);

			playerDist = worldModel->getHalfPlayer(TID_OPPONENT, j).getDistance(beginPoint);			
			tempVector.setAsPolar(playerDist, currentAngle);

			if (playerDist < cautionLength * 1.2)
				isOpponent = 1;

			if (tempVector.getY() <= cautionLength && tempVector.getY() > 0 && 
					fabs(tempVector.getX()) <= (cautionLength * xRate))
			{
				localNegWeight = -(tempVector.getY() - fabs(tempVector.getX()));
				localNegWeight *= (fabs(tempVector.getX()) / cautionLength) / xRate;

				if (playerDist < 5)
				{
					float rate = 5 - playerDist;
					if (rate < 1.5)
						rate = 1.5;
	
					localNegWeight *= rate;
				}

				if (localNegWeight > 0)
					localNegWeight = 0;

				if (localNegWeight < negWeight)
					negWeight = localNegWeight;
			}
		}	



	for (int j = 0; j < QUARTER_PLAYERS_NUM; j++)
		if (worldModel->getQuarterPlayer(j).isAlive())
		{
			playerVector.setByPoints(beginPoint, 
				worldModel->getQuarterPlayer(j).getPos());
			currentAngle = playerVector.getDirection() - beginAngle;	
			currentAngle = Degree::normalizeAngle(currentAngle);
	
			playerDist = worldModel->getQuarterPlayer(j).getDistance(beginPoint);			
			tempVector.setAsPolar(playerDist, currentAngle);
	
			if (playerDist < cautionLength * 1.2)
				isOpponent = 1;
	
			if (tempVector.getY() <= cautionLength && tempVector.getY() > 0 && 
					fabs(tempVector.getX()) <= (cautionLength * xRate))
			{
				localNegWeight = -(tempVector.getY() - fabs(tempVector.getX()));
				localNegWeight *= (fabs(tempVector.getX()) / cautionLength) / xRate;

				if (playerDist < 5)
				{
					float rate = 5 - playerDist;
					if (rate < 1.5)
						rate = 1.5;

					localNegWeight *= rate;
				}

				if (localNegWeight > 0)
					localNegWeight = 0;

				if (localNegWeight < negWeight)
					negWeight = localNegWeight;
			}
		}
		
	craudWeight = negWeight;
	targetWeights[targetCount] += craudWeight * craudWeightRate;	
}

void DribblePath::setTargetWeight(int targetCount)
{
	setCraudWeight(targetCount);
	setAngleWeight(targetCount);
//	LOG << "TotalWeight : " << targetAngles[targetCount] << " " << 
//		targetWeights[targetCount] << endl; 
}


void DribblePath::setDribbleTarget()
{
	Vector tempVector;
	
	tempVector.setByPoints(Point(beginPoint.x, 
			beginPoint.y), dribbleTarget);
	
	float beginAngle = tempVector.getDirection();   
	
	for (int i = 0; i < targetNums; i++) 
	{
		targetAngles[i] = beginAngle + (i * 10); 	
		targetAngles[i] = Degree::normalizeAngle(targetAngles[i]);
		setTargetWeight(i);
	}

	if (beginPoint.y >= 31)
		for (int i = 0; i < targetNums; i++)
			if (targetAngles[i] > 0 || (beginPoint.y >= 
				32 && targetAngles[i] >= 0))
				targetWeights[i] -= 1000;

	if (beginPoint.y <= -31)
		for (int i = 0; i < targetNums; i++)
			if (targetAngles[i] < 0 || (beginPoint.y <= 
				-32 && targetAngles[i] <= 0))
				targetWeights[i] -= 1000;
	
	if (worldModel->getBall().getPos().getX() >= 50)
		for (int i = 0; i < targetNums; i++)
			if (fabs(targetAngles[i]) < 90 || 
				(worldModel->getBall().getPos().getX() >= 51 && 
				fabs(targetAngles[i]) <= 90))
				targetWeights[i] -= 1000;

	bestDribbleTarget = 0;

	for (int i = 0; i < targetNums; i++)
		if (targetWeights[i] > targetWeights[bestDribbleTarget])
			bestDribbleTarget = i;

	Vector lastVector;
	lastVector.setAsPolar(10, targetAngles[bestDribbleTarget]);

	dribbleTarget.x = beginPoint.x + lastVector.getX();
	dribbleTarget.y = beginPoint.y + lastVector.getY();

//	LOG << "MainDribbletarget : " << dribbleTarget << endl;
}


void DribblePath::setDefaultParams()
{
	isOpponent = 0;	

	float firstXPathStep = 42;

	dribbleTarget.x = 52.5;

	if (worldModel->getBall().getPos().getX() < firstXPathStep)
		dribbleTarget.y = worldModel->getBall().getPos().getY();
	else
		dribbleTarget.y = 0;	

	targetNums = 36;
	cautionLength = 12;
	angleWeightRate = 1.6;
	craudWeightRate = 8;
	xRate = .25;
}

void DribblePath::setPathParams(int targetNumsArg, float cautionLengthArg,
					float angleWeightRateArg, float craudWeightRateArg,
					float xRateArg)
{
	float firstXPathStep = 42;

	dribbleTarget.x = 52.5;

	if (worldModel->getBall().getPos().getX() < firstXPathStep)
		dribbleTarget.y = worldModel->getBall().getPos().getY();
	else
		dribbleTarget.y = 0;	
	isOpponent = 0;

	targetNums = targetNumsArg;
	cautionLength = cautionLengthArg;
	angleWeightRate = angleWeightRateArg;
	craudWeightRate = craudWeightRateArg;
	xRate = xRateArg;
}					

void DribblePath::setBeginPoint(Point beginPointArg)
{
	beginPoint = beginPointArg;
}

float DribblePath::getBestTargetWeight()
{
	setDribbleTarget();
	return targetWeights[bestDribbleTarget];
}

float DribblePath::getTargetWeight(int targetCount)
{
	setDribbleTarget();
	return targetWeights[targetCount];
}

float DribblePath::getBestTargetDir()
{
	return targetAngles[bestDribbleTarget];
}