worldmodel.cpp

mersad源码 03年robocup 季军 可以研究一下大家

		virtualPlayMode = VPM_NONE;
	}
	else if (at->toString() == "penalty_ready_r")
	{
		if (fieldSide == SI_RIGHT)
			playMode = PM_PENALTY_READY;
		else
			playMode = PM_PENALTY_READY_OPP;

		playModeRemainCycle = 50;
		virtualPlayMode = VPM_NONE;
	}

	else if (at->toString().substr(0, 6) == "goal_l")
	{
		if (fieldSide == SI_LEFT)
			playMode = PM_GOAL;
		else
			playMode = PM_GOAL_OPP;

		playModeRemainCycle = 50;
	}
	else if (at->toString().substr(0, 6) == "goal_r")
	{
		if (fieldSide == SI_RIGHT)
			playMode = PM_GOAL;
		else
			playMode = PM_GOAL_OPP;

		playModeRemainCycle = 50;
	}

	// virtual PlayModes
	else if (at->toString() == "drop_ball")
	{
		virtualPlayMode = VPM_DROP_BALL;
		ball->setPosDeviation(MAX_POS_DEV);
		ball->setValidPosDeviation(MAX_POS_DEV);
	}

	else if (at->toString() == "time_up_without_a_team")
		playMode = PM_TIME_UP_WITHOUT_A_TEAM;

	else if (at->toString() == "time_up")
		playMode = PM_TIME_UP;

	else if (at->toString() == "time_over")
		playMode = PM_TIME_UP;
	
	else if (at->toString() == "half_time")
		virtualPlayMode = VPM_HALF_TIME;

	else if (at->toString() == "time_extended")
		virtualPlayMode = VPM_TIME_EXTENDED;

	else if (at->toString() == "penalty_draw")
		virtualPlayMode = VPM_PENALTY_DRAW;

	else if (at->toString() == "goalie_catch_ball_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_GOALIE_CATCH_BALL;
		else
			virtualPlayMode = VPM_GOALIE_CATCH_BALL_OPP;
	}
	else if (at->toString() == "goalie_catch_ball_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_GOALIE_CATCH_BALL;
		else
			virtualPlayMode = VPM_GOALIE_CATCH_BALL_OPP;
	}

	else if (at->toString() == "foul_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_FOUL;
		else
			virtualPlayMode = VPM_FOUL_OPP;
	}
	else if (at->toString() == "foul_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_FOUL;
		else
			virtualPlayMode = VPM_FOUL_OPP;
	}

	else if (at->toString() == "back_pass_l")
	{
		if (fieldSide == SI_LEFT)
			playMode = PM_BACK_PASS;
		else
			playMode = PM_BACK_PASS_OPP;
	}
	else if (at->toString() == "back_pass_r")
	{
		if (fieldSide == SI_RIGHT)
			playMode = PM_BACK_PASS;
		else
			playMode = PM_BACK_PASS_OPP;
	}

	else if (at->toString() == "penalty_onfield_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_ON_FIELD;
		else
			virtualPlayMode = VPM_PENALTY_ON_FIELD_OPP;
	}
	else if (at->toString() == "penalty_onfield_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_ON_FIELD;
		else
			virtualPlayMode = VPM_PENALTY_ON_FIELD_OPP;
	}

	else if (at->toString() == "penalty_miss_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_MISS;
		else
			virtualPlayMode = VPM_PENALTY_MISS_OPP;
	}
	else if (at->toString() == "penalty_miss_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_MISS;
		else
			virtualPlayMode = VPM_PENALTY_MISS_OPP;
	}

	else if (at->toString() == "penalty_score_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_SCORE;
		else
			virtualPlayMode = VPM_PENALTY_SCORE_OPP;
	}
	else if (at->toString() == "penalty_score_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_SCORE;
		else
			virtualPlayMode = VPM_PENALTY_SCORE_OPP;
	}

	else if (at->toString() == "penalty_winner_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_WINNER;
		else
			virtualPlayMode = VPM_PENALTY_WINNER_OPP;
	}
	else if (at->toString() == "penalty_winner_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_WINNER;
		else
			virtualPlayMode = VPM_PENALTY_WINNER_OPP;
	}

	else if (at->toString() == "penalty_foul_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_FOUL;
		else
			virtualPlayMode = VPM_PENALTY_FOUL_OPP;
	}
	else if (at->toString() == "penalty_foul_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_FOUL;
		else
			virtualPlayMode = VPM_PENALTY_FOUL_OPP;
	}

	else if (at->toString() == "penalty_taken_l")
	{
		if (fieldSide == SI_LEFT)
			virtualPlayMode = VPM_PENALTY_TAKEN;
		else
			virtualPlayMode = VPM_PENALTY_TAKEN_OPP;
	}
	else if (at->toString() == "penalty_taken_r")
	{
		if (fieldSide == SI_RIGHT)
			virtualPlayMode = VPM_PENALTY_TAKEN;
		else
			virtualPlayMode = VPM_PENALTY_TAKEN_OPP;
	}

	else
		assert(0);
}

void WorldModel::selfHear(const std::string &message)
{
	if ((message[1] == 'A' || message[1] == 'B') &&
		curTime != refereeHearTime + 1)
	{
		hearedHeader = message[1];
		hearedSender = body->getUniNum();
		hearedInterceptNum = SayEncoding::getCharNum(message[8]);
		hearedRequestPassNum = SayEncoding::getCharNum(message[9]);
	}
}

void WorldModel::teammateHear(float hearDir, unsigned senderNum,
		const string &message)
{
	hearedHeader = message[1];

	switch (message[1])
	{
	case 'A':
		beforePassTmmHear(message, senderNum);
		break;

	case 'B':
		beforePassRelatedTmmHear(message, senderNum);
		break;

	case 'C':
		suggestPassTmmHear(message, senderNum);

	case 'Y':
		if (curTime != refereeHearTime + 1)
			routinTmmHearWithoutBall(message, senderNum);
		break;

	case 'Z':
		if (curTime != refereeHearTime + 1)
			routinTmmHear(message, senderNum);
		break;

	case 'F':
		if (curTime != refereeHearTime + 1)
			freeKickKickerHear(message, senderNum);
		break;

	default:
		assert(0);
	}

	logObjects();
}

void WorldModel::opponentHear(float hearDir, const string &message)
{
}

void WorldModel::routinTmmHear(const std::string &message,
		unsigned senderNum)
{
	float x, y, velMag, velDir;

	if (ball->getSeePosDeviation() > 2 && !isBallKickable() &&
		ball->isUpdateByHearPermitted())
	{
		SayEncoding::decodeObjectPosition(message, 2, x, y);
		SayEncoding::decodeObjectVelocity(message, 5, velMag, velDir);

		ball->updateByHear(x, y, velMag, velDir, *body);
	}

	hearedSender = senderNum;
	hearedInterceptNum = SayEncoding::getCharNum(message[8]);
	hearedRequestPassNum = SayEncoding::getCharNum(message[9]);

	mustAttention = SayEncoding::getCharNum(message[10]);
}

void WorldModel::routinTmmHearWithoutBall(const std::string &message,
		unsigned senderNum)
{
	hearedSender = senderNum;
	hearedInterceptNum = SayEncoding::getCharNum(message[2]);
	hearedRequestPassNum = SayEncoding::getCharNum(message[3]);

	mustAttention = SayEncoding::getCharNum(message[4]);
}

void WorldModel::beforePassTmmHear(const std::string &message,
		unsigned senderNum)
{
	float x, y, velMag, velDir;

	SayEncoding::decodeObjectPosition(message, 2, x, y);
	SayEncoding::decodeObjectVelocity(message, 5, velMag, velDir);

	hearedSender = senderNum;
	hearedInterceptNum = SayEncoding::getCharNum(message[8]);

	ball->updateByHear(x, y, velMag, velDir, *body);
}

void WorldModel::beforePassRelatedTmmHear(const std::string &message,
		unsigned senderNum)
{
	float x, y, velMag, velDir;

	SayEncoding::decodeObjectPosition(message, 2, x, y);
	SayEncoding::decodeObjectVelocity(message, 5, velMag, velDir);

	hearedSender = senderNum;
	hearedInterceptNum = SayEncoding::getCharNum(message[8]);

	unsigned num = hearedInterceptNum;
	if (num > 11)
		num -= 11;

	if (fullPlayers[TID_TEAMMATE][num - 1]->isValid())
		ball->updateByHear(
			fullPlayers[TID_TEAMMATE][num - 1]->getPos().getX() + x,
			fullPlayers[TID_TEAMMATE][num - 1]->getPos().getY() + y,
			velMag, velDir, *body);
}

void WorldModel::suggestPassTmmHear(const std::string &message,
		unsigned senderNum)
{
	float magnitude, direction, weight;

	SayEncoding::decodeObjectVelocity(message, 2, magnitude, direction);
	SayEncoding::decodeWeight(message, 5, weight, -50, 150);

	hearedSender = senderNum;
	hearedSuggestPassNum = SayEncoding::getCharNum(message[8]);
	hearedSuggestPassDir = direction;
	hearedSuggestPassSpeed = magnitude;
	hearedSuggestPassWeight = weight;

	hearedSuggestPassTime = curTime;
}

void WorldModel::parseTeammateCoachHear(const SExpression &exp)
{
	string messageString;
	string expString;
	unsigned dummyPtr = 0;

	int i, j;
	int typeNum;

	SExpression *exp2;
	SExpression *exp3;
	SExpAtomic *at;
	SExpAtomic *at2;

	LOG << "WorldModel::parseTeammateCoachHear" << endl;

	if (dynamic_cast<const SExpAtomic *>(exp[0])->toString() == "freeform")
	{
		expString = dynamic_cast<const SExpAtomic *>(exp[1])->toString();
		expString = "(" + expString.substr(1, expString.length() - 2) + ")";
		exp2 = new SExpression(expString, dummyPtr);
		for (i = 0; i < exp2->size(); i++)
		{
			exp3 = dynamic_cast<SExpression *>((*exp2)[i]);
			at = dynamic_cast<SExpAtomic *>((*exp3)[0]);
			assert(at);

			// Opponent Types (T)
			if (at->toString() == "T")
			{
				at2 = dynamic_cast<SExpAtomic *>((*exp3)[1]);
				assert(at2);
				messageString = at2->toString();
				assert(messageString.length() == 11);
				for (j = 0; j < 11; j++)
				{
					typeNum = messageString[j] - '0';
					setFullPlayer(TID_OPPONENT, j).setType(typeNum,	playerTypes[typeNum]);
					LOG << "\tAOTR: Opp Num " << j + 1 << " setted to type " << typeNum << endl;
				}
			}
		}
	}
	
}

void WorldModel::freeKickKickerHear(const std::string &message,
		unsigned senderNum)
{
	unsigned adviceNumber;
	
	adviceNumber = SayEncoding::getCharNum(message[2]);

	LOG << "WorldModel::freeKickAdvicerHear sender=" << senderNum <<
		" adviceNumber=" << adviceNumber << endl;

	freeKickAdvicerSayCycle = curTime;
	freeKickAdvicerSayNumber = adviceNumber;
}

void WorldModel::initViewPartDirs()
{
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		viewPartDirs[i] = i * (360 / VIEW_PARTS_NUM);
}

void WorldModel::resetSeeDeltaCycles()
{
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		viewPartCycles[i] = MAX_SEE_DELTA_CYCLE;
}

void WorldModel::updateSeeDeltaCyclesAfterSee()
{
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		if (getSeeViewPartValue(i, body->getHeadDir(),
				body->getViewAngle()) >= 0.75)
			viewPartCycles[i] = 0;
}

void WorldModel::updateSeeDeltaCyclesAfterSenseBody()
{
	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
		if (viewPartCycles[i] < MAX_SEE_DELTA_CYCLE)
			viewPartCycles[i]++;
}

float WorldModel::getSeeDeltaCycle(float dir, float minRate) const
{
	int pairNum;
	float deltaAngle;

	for (unsigned i = 0; i < VIEW_PARTS_NUM; i++)
	{
		deltaAngle = getDeltaAngle(dir, viewPartDirs[i]);

		if (fabs(deltaAngle) < EXTRA_VIEW_SIZE)
			return viewPartCycles[i];

		if (fabs(deltaAngle) < VIEW_PART_SIZE / 2)
		{
			if (deltaAngle > 0)
				pairNum = i + 1;
			else
				pairNum = i - 1;

			if (pairNum == VIEW_PARTS_NUM)
				pairNum = 0;
			else if (pairNum == -1)
				pairNum = VIEW_PARTS_NUM - 1;

			if (fabs(deltaAngle) > VIEW_PART_SIZE - EXTRA_VIEW_SIZE / 2.00)
				return viewPartCycles[i] * minRate +
						viewPartCycles[pairNum] * (1.00 - minRate);

			return viewPartCycles[i] * (1.00 - minRate) +
					viewPartCycles[pairNum] * minRate;
		}
	}

	assert(0);
	return MAX_SEE_DELTA_CYCLE;
}

float WorldModel::getSeeViewPartValue(unsigned viewPartNum,
		float headDir, float viewAngle) const
{
	float deltaAngle;

	deltaAngle = fabs(getDeltaAngle(viewPartDirs[viewPartNum], headDir));
	deltaAngle += VIEW_PART_SIZE / 2.0 - viewAngle;

	if (deltaAngle <= EXTRA_VIEW_SIZE + 5) // 5 Degrees for sure
		return reRate(deltaAngle, 0, EXTRA_VIEW_SIZE + 5, 0.75, 1, RRM_REVERSE);

	return reRate(deltaAngle, EXTRA_VIEW_SIZE + 5, 45, 0, 0.75, RRM_REVERSE);
}

void WorldModel::updatePlayModeRemainCycleAfterSenseBody()
{
	if (playModeRemainCycle > 0)
		playModeRemainCycle--;
}

void WorldModel::updateBallStatus()
{
	float checkedTmmNum = curInterCalculate.getTmmCheckedPlayersNum();
	float checkedOppNum = curInterCalculate.getOppCheckedPlayersNum();
	bool kickableTmm = isBallInTmmKickable();
	bool kickableOpp = isBallInOppKickable();
	bool kickableBody = isBallKickable();

	if (!curInterCalculate.existFastestPlayer())
		ballStatus = BS_FREE_BALL;
	else if (kickableBody)
	{
		if (kickableOpp)
			ballStatus = BS_KICKABLE_BODY_OPP;
		else if (kickableTmm)
			ballStatus = BS_KICKABLE_BODY_TMM;
		else
			ballStatus = BS_KICKABLE_BODY;
	}
	else if (kickableTmm || kickabl