worldmodel.cpp

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

	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		quarterPlayers[i]->setReadyToUpdate(false);

	// Killing a few players
	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		quarterPlayers[i]->kill();

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
	{
		if (halfPlayers[TID_TEAMMATE][i]->isAlive() &&
			halfPlayers[TID_TEAMMATE][i]->canBodySee(body->getPos(),
				body->getViewAngle(), body->getVisibleDistance()))
			halfPlayers[TID_TEAMMATE][i]->kill();

		if (halfPlayers[TID_OPPONENT][i]->isAlive() &&
			halfPlayers[TID_OPPONENT][i]->canBodySee(body->getPos(),
				body->getViewAngle(), body->getVisibleDistance()))
			halfPlayers[TID_OPPONENT][i]->kill();
	}
}

void WorldModel::replaceBodyInPlayersList(unsigned bodyUniNum)
{
	delete fullPlayers[TID_TEAMMATE][bodyUniNum - 1];
	fullPlayers[TID_TEAMMATE][bodyUniNum - 1] = body;

	setBodyReplaced(true);
}

void WorldModel::updateObjects()
{
	unsigned i;

	for (i = 0; i < flagsNum; i++)
		flags[i]->update(fieldData);

	for (i = 0; i < goalsNum; i++)
		goals[i]->update(fieldData);

	for (i = 0; i < linesNum; i++)
		lines[i]->update(fieldData);

	body->update(flags, goals, lines, flagsNum, goalsNum, linesNum);

	if (ball->isReadyToUpdate())
		ball->update(*body);
	else if (ball->isValid())
		ball->virtualUpdate(*body);

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (fullPlayers[TID_TEAMMATE][i]->isReadyToUpdate())
			fullPlayers[TID_TEAMMATE][i]->update(*body, playMode);
		else if (i != body->getUniNum() - 1 && // this function do not virtual update body.
				 fullPlayers[TID_TEAMMATE][i]->isAlive())
			fullPlayers[TID_TEAMMATE][i]->virtualUpdate(*body);

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (fullPlayers[TID_OPPONENT][i]->isReadyToUpdate())
			fullPlayers[TID_OPPONENT][i]->update(*body, playMode);
		else if (fullPlayers[TID_OPPONENT][i]->isAlive())
			fullPlayers[TID_OPPONENT][i]->virtualUpdate(*body);

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_TEAMMATE][i]->isReadyToUpdate())
			halfPlayers[TID_TEAMMATE][i]->update(*body, playMode);
		else if (halfPlayers[TID_TEAMMATE][i]->isValid())
			halfPlayers[TID_TEAMMATE][i]->virtualUpdate(*body);

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_OPPONENT][i]->isReadyToUpdate())
			halfPlayers[TID_OPPONENT][i]->update(*body, playMode);
		else if (halfPlayers[TID_OPPONENT][i]->isValid())
			halfPlayers[TID_OPPONENT][i]->virtualUpdate(*body);

	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		if (quarterPlayers[i]->isReadyToUpdate())
			quarterPlayers[i]->update(*body, playMode);
		else if (quarterPlayers[i]->isValid())
			quarterPlayers[i]->virtualUpdate(*body);

	completeUpdateObjects();
}

void WorldModel::simulateObjects(const Body &lastBody, const Command *bodyCycleCommand)
{
	unsigned i;

	if (getPlayMode() == PM_FREE_KICK &&
		getVirtualPlayMode() ==	VPM_GOALIE_CATCH_BALL && body->isGoalie())
		ball->simulateByAction(lastBody, bodyCycleCommand, true);
	else
		ball->simulateByAction(lastBody, bodyCycleCommand);

	ball->simulateByDynamics(*body, 2, ballStatus, curTime);

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (i != body->getUniNum() - 1 && // this function do not simulate body.
			fullPlayers[TID_TEAMMATE][i]->isAlive())
			fullPlayers[TID_TEAMMATE][i]->simulateByDynamics(*body);
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (fullPlayers[TID_OPPONENT][i]->isAlive())
			fullPlayers[TID_OPPONENT][i]->simulateByDynamics(*body);

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_TEAMMATE][i]->isValid())
			halfPlayers[TID_TEAMMATE][i]->simulateByDynamics(*body);
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_OPPONENT][i]->isValid())
			halfPlayers[TID_OPPONENT][i]->simulateByDynamics(*body);

	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		if (quarterPlayers[i]->isValid())
			quarterPlayers[i]->simulateByDynamics(*body);
}

void WorldModel::completeUpdateObjects()
{
	int i, j;
	Player *playerPtr;

	// Removing same Halves
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
	{
		// Teammates
		if (halfPlayers[TID_TEAMMATE][i]->isValid() &&
			halfPlayers[TID_TEAMMATE][i]->getSeeTime() != getCurTime())
		{

			playerPtr = NULL;
			for (j = 0; j < HALF_PLAYERS_NUM; j++)
				if (halfPlayers[TID_TEAMMATE][j]->isValid() &&
					halfPlayers[TID_TEAMMATE][j]->getSeeTime() == getCurTime() &&
					halfPlayers[TID_TEAMMATE][j]->getDistance(*halfPlayers[TID_TEAMMATE][i]) <=
					halfPlayers[TID_TEAMMATE][j]->getPosDeviation() + halfPlayers[TID_TEAMMATE][i]->getPosDeviation() + 1) // One meter for sure
				{
					if (!playerPtr)
						playerPtr = halfPlayers[TID_TEAMMATE][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
	
			if (playerPtr)
			{
				for (j = 0; j < HALF_PLAYERS_NUM; j++)
					if (i != j && halfPlayers[TID_TEAMMATE][j]->isValid() &&
						halfPlayers[TID_TEAMMATE][j]->getSeeTime() != getCurTime() &&
						halfPlayers[TID_TEAMMATE][j]->getDistance(*playerPtr) <=
						halfPlayers[TID_TEAMMATE][j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
				{
					j = -1;
					break;
				}

				if (j != -1)
					halfPlayers[TID_TEAMMATE][i]->kill();
			}
		}

		// Opponents
		if (halfPlayers[TID_OPPONENT][i]->isValid() &&
			halfPlayers[TID_OPPONENT][i]->getSeeTime() != getCurTime())
		{

			playerPtr = NULL;
			for (j = 0; j < HALF_PLAYERS_NUM; j++)
				if (halfPlayers[TID_OPPONENT][j]->isValid() &&
					halfPlayers[TID_OPPONENT][j]->getSeeTime() == getCurTime() &&
					halfPlayers[TID_OPPONENT][j]->getDistance(*halfPlayers[TID_OPPONENT][i]) <=
					halfPlayers[TID_OPPONENT][j]->getPosDeviation() + halfPlayers[TID_OPPONENT][i]->getPosDeviation() + 1) // One meter for sure
				{
					if (!playerPtr)
						playerPtr = halfPlayers[TID_OPPONENT][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
	
			if (playerPtr)
			{
				for (j = 0; j < HALF_PLAYERS_NUM; j++)
					if (i != j && halfPlayers[TID_OPPONENT][j]->isValid() &&
						halfPlayers[TID_OPPONENT][j]->getSeeTime() != getCurTime() &&
						halfPlayers[TID_OPPONENT][j]->getDistance(*playerPtr) <=
						halfPlayers[TID_OPPONENT][j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
				{
					j = -1;
					break;
				}

				if (j != -1)
					halfPlayers[TID_OPPONENT][i]->kill();
			}
		}

	}

	// Changing Quarters to Halves
	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		if (quarterPlayers[i]->isValid())
		{
			playerPtr = NULL;
			for (j = 0; j < HALF_PLAYERS_NUM; j++)
			{
				// Teammates
				if (halfPlayers[TID_TEAMMATE][j]->isValid() &&
					halfPlayers[TID_TEAMMATE][j]->getSeeTime() != getCurTime() &&
					halfPlayers[TID_TEAMMATE][j]->getDistance(*quarterPlayers[i]) <=
					halfPlayers[TID_TEAMMATE][j]->getPosDeviation() + quarterPlayers[i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = halfPlayers[TID_TEAMMATE][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}

				// Opponents
				if (halfPlayers[TID_OPPONENT][j]->isValid() &&
					halfPlayers[TID_OPPONENT][j]->getSeeTime() != getCurTime() &&
					halfPlayers[TID_OPPONENT][j]->getDistance(*quarterPlayers[i]) <=
					halfPlayers[TID_OPPONENT][j]->getPosDeviation() + quarterPlayers[i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = halfPlayers[TID_OPPONENT][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
			}
	
			if (playerPtr != NULL)
			{
				for (j = 0; j < QUARTER_PLAYERS_NUM; j++)
					if (i != j && quarterPlayers[j]->isValid() &&
						quarterPlayers[j]->getDistance(*playerPtr) <=
						quarterPlayers[j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
					{
						j = -1;
						break;
					}

				if (j != -1)
				{
					playerPtr->setByAnotherPlayer(*quarterPlayers[i], *body);
					quarterPlayers[i]->kill();
				}
			}
		}

	// Changing Quarters to Fulls
	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		if (quarterPlayers[i]->isValid())
		{
			playerPtr = NULL;
			for (j = 0; j < FULL_PLAYERS_NUM; j++)
			{
				// Teammates
				if (fullPlayers[TID_TEAMMATE][j]->isAlive() &&
					!fullPlayers[TID_TEAMMATE][j]->isBody() &&
					fullPlayers[TID_TEAMMATE][j]->getSeeTime() != getCurTime() &&
					fullPlayers[TID_TEAMMATE][j]->getDistance(*quarterPlayers[i]) <=
					fullPlayers[TID_TEAMMATE][j]->getPosDeviation() + quarterPlayers[i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = fullPlayers[TID_TEAMMATE][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}

				// Opponents
				if (fullPlayers[TID_OPPONENT][j]->isAlive() &&
					fullPlayers[TID_OPPONENT][j]->getSeeTime() != getCurTime() &&
					fullPlayers[TID_OPPONENT][j]->getDistance(*quarterPlayers[i]) <=
					fullPlayers[TID_OPPONENT][j]->getPosDeviation() + quarterPlayers[i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = fullPlayers[TID_OPPONENT][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
			}
	
			if (playerPtr != NULL)
			{
				for (j = 0; j < QUARTER_PLAYERS_NUM; j++)
					if (i != j && quarterPlayers[j]->isValid() &&
						quarterPlayers[j]->getDistance(*playerPtr) <=
						quarterPlayers[j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
					{
						j = -1;
						break;
					}

				if (j != -1)
				{
					playerPtr->setByAnotherPlayer(*quarterPlayers[i], *body);
					quarterPlayers[i]->kill();

//LOG << "Tran qua to " << playerPtr->getTeamId() << " " << playerPtr->getUniNum() << endl;
//LOG << "qu ti: " << quarterPlayers[i]->getSeeTime() << " fu ti: " << playerPtr->getSeeTime() << " cur ti: " << curTime << endl;

				}
			}
		}

	// Changing Halves to Fulls
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
	{
		// Teammates
		if (halfPlayers[TID_TEAMMATE][i]->isValid())
		{
			playerPtr = NULL;
			for (j = 0; j < FULL_PLAYERS_NUM; j++)
				if (fullPlayers[TID_TEAMMATE][j]->isAlive() &&
					!fullPlayers[TID_TEAMMATE][j]->isBody() &&
					fullPlayers[TID_TEAMMATE][j]->getSeeTime() != getCurTime() &&
					fullPlayers[TID_TEAMMATE][j]->getDistance(*halfPlayers[TID_TEAMMATE][i]) <=
					fullPlayers[TID_TEAMMATE][j]->getPosDeviation() + halfPlayers[TID_TEAMMATE][i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = fullPlayers[TID_TEAMMATE][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
	
			if (playerPtr != NULL)
			{
				for (j = 0; j < HALF_PLAYERS_NUM; j++)
					if (i != j && halfPlayers[TID_TEAMMATE][j]->isValid() &&
						halfPlayers[TID_TEAMMATE][j]->getDistance(*playerPtr) <=
						halfPlayers[TID_TEAMMATE][j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
					{
						j = -1;
						break;
					}

				if (j != -1)
				{
					playerPtr->setByAnotherPlayer(*halfPlayers[TID_TEAMMATE][i], *body);
					halfPlayers[TID_TEAMMATE][i]->kill();
				}
			}
		}

		// Opponents
		if (halfPlayers[TID_OPPONENT][i]->isValid())
		{
			playerPtr = NULL;
			for (j = 0; j < FULL_PLAYERS_NUM; j++)
				if (fullPlayers[TID_OPPONENT][j]->isAlive() &&
					fullPlayers[TID_OPPONENT][j]->getSeeTime() != getCurTime() &&
					fullPlayers[TID_OPPONENT][j]->getDistance(*halfPlayers[TID_OPPONENT][i]) <=
					fullPlayers[TID_OPPONENT][j]->getPosDeviation() + halfPlayers[TID_OPPONENT][i]->getPosDeviation() + 1) // One meter for sure
				{
					if (playerPtr == NULL)
						playerPtr = fullPlayers[TID_OPPONENT][j];
					else
					{
						playerPtr = NULL;
						break;
					}
				}
	
			if (playerPtr != NULL)
			{
				for (j = 0; j < HALF_PLAYERS_NUM; j++)
					if (i != j && halfPlayers[TID_OPPONENT][j]->isValid() &&
						halfPlayers[TID_OPPONENT][j]->getDistance(*playerPtr) <=
						halfPlayers[TID_OPPONENT][j]->getPosDeviation() + playerPtr->getPosDeviation() + 1) // One meter for sure
					{
						j = -1;
						break;
					}

				if (j != -1)
				{
					playerPtr->setByAnotherPlayer(*halfPlayers[TID_OPPONENT][i], *body);
					halfPlayers[TID_OPPONENT][i]->kill();
				}
			}
		}

	}

	// Players change place
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
	{
		if (fullPlayers[TID_TEAMMATE][i]->isAlive() &&
			!fullPlayers[TID_TEAMMATE][i]->isBody() &&
			fullPlayers[TID_TEAMMATE][i]->getSeeTime() != getCurTime())
		{
			putOutOfVisibleDistance(*fullPlayers[TID_TEAMMATE][i]);

			if (fabs(fullPlayers[TID_TEAMMATE][i]->getHeadVec().getDirection()) <
					body->getViewAngle() - CHANGE_PLACE_SURE_ANGLE &&
				fullPlayers[TID_TEAMMATE][i]->getAbsVec().getMagnitude() <
					UNUM_FAR_LENGTH - CHANGE_PLACE_SURE_DIST)
				putOutOfVisibleAngle(*fullPlayers[TID_TEAMMATE][i]);

			changePosDevBySee(*fullPlayers[TID_TEAMMATE][i]);
		}

		if (fullPlayers[TID_OPPONENT][i]->isAlive() &&
			fullPlayers[TID_OPPONENT][i]->getSeeTime() != getCurTime())
		{
			putOutOfVisibleDistance(*fullPlayers[TID_OPPONENT][i]);

			if (fabs(fullPlayers[TID_OPPONENT][i]->getHeadVec().getDirection()) <
					body->getViewAngle() - CHANGE_PLACE_SURE_ANGLE &&
				fullPlayers[TID_OPPONENT][i]->getAbsVec().getMagnitude() <
					UNUM_FAR_LENGTH - CHANGE_PLACE_SURE_DIST)
				putOutOfVisibleAngle(*fullPlayers[TID_OPPONENT][i]);

			changePosDevBySee(*fullPlayers[TID_OPPONENT][i]);
		}
	}

	for (i = 0; i <= HALF_PLAYERS_NUM; i++)
	{
		if (halfPlayers[TID_TEAMMATE][i]->isValid() &&
			halfPlayers[TID_TEAMMATE][i]->getSeeTime() != getCurTime())
		{
			putOutOfVisibleDistance(*halfPlayers[TID_TEAMMATE][i]);

			if (fabs(halfPlayers[TID_TEAMMATE][i]->getHeadVec().getDirection()) <
					body->getViewAngle() - CHANGE_PLACE_SURE_ANGLE &&
				halfPlayers[TID_TEAMMATE][i]->getAbsVec().getMagnitude() <
					TEAM_FAR_LENGTH - CHANGE_PLACE_SURE_DIST)
				putOutOfVisibleAngle(*halfPlayers[TID_TEAMMATE][i]);

			changePosDevBySee(*halfPlayers[TID_TEAMMATE][i]);
		}

		if (halfPlayers[TID_OPPONENT][i]->isValid() &&
			halfPlayers[TID_OPPONENT][i]->getSeeTime() != getCurTime())
		{
			putOutOfVisibleDistance(*halfPlayers[TID_OPPONENT][i]);

			if (fabs(halfPlayers[TID_OPPONENT][i]->getHeadVec().getDirection()) <
					body->getViewAngle() - CHANGE_PLACE_SURE_ANGLE &&
				halfPlayers[TID_OPPONENT][i]->getAbsVec().getMagnitude() <
					TEAM_FAR_LENGTH - CHANGE_PLACE_SURE_DIST)
				putOutOfVisibleAngle(*halfPlayers[TID_OPPONENT][i]);

				changePosDevBySee(*halfPlayers[TID_OPPONENT][i]);
		}
	}

	// Ball change place
	if (ball->isValid() && ball->getSeeTime() != getCurTime())
	{
		putOutOfVisibleDistance(*ball);
		changePosDevBySee(*ball);
	}

}

void WorldModel::putOutOfVisibleAngle(Object &object)
{
	float rotateAngle;

LOG << "obj.angle: " << object.getHeadVec().getDirection() << endl;

	if (object.getHeadVec().getDirection() > 0)
		rotateAngle = body->getViewAngle() - 
				object.getHeadVec().getDirection() +
				CHANGE_PLACE_SURE_ANGLE;
	else
		rotateAngle = -body->getViewAngle() -
				object.getHeadVec().getDirection() -
				CHANGE_PLACE_SURE_ANGLE;

	object.setAbsVec().rotate(rotateAngle);
	object.setBodyVec().rotate(rotateAngle);
	object.setHeadVec().rotate(rotateAngle);
	object.setPos() = body->getPos() + object.getAbsVec();

	if (dynamic_cast<Ball *>(&object))
		LOG << "Complete updates: ball putted out of visible angle." << endl;
	if (Player *player = dynamic_cast<Player *>(&object))
		LOG << "Complete updates: player " << player->getTeamId() << " "
			<< player->getUniNum() << " putted out of visible angle." << endl;
}

void WorldModel::putOutOfVisibleDistance(Object &object)
{