worldmodel.cpp

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

	if (object.getAbsVec().getMagnitude() < body->getVisibleDistance())
	{
		object.setAbsVec().setAsPolar(body->getVisibleDistance() +
				PUT_OUT_VIS_SURE_DIST, object.getAbsVec().getDirection());
		object.setBodyVec().setAsPolar(body->getVisibleDistance() +
				PUT_OUT_VIS_SURE_DIST, object.getBodyVec().getDirection());
		object.setHeadVec().setAsPolar(body->getVisibleDistance() +
				PUT_OUT_VIS_SURE_DIST, object.getHeadVec().getDirection());

		object.setPos() = body->getPos() + object.getAbsVec();

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

void WorldModel::changePosDevBySee(Object &object)
{
	const Player *player = dynamic_cast<const Player *>(&object);
	if (player && player->isGoalie() &&
		player->getTeamId() == TID_OPPONENT)
	{
		if (object.getAbsVec().getMagnitude() < object.getPosDeviation() +
				1.5 + PUT_OUT_VIS_SURE_DIST)
			object.setPosDeviation(object.getAbsVec().getMagnitude() -
				1.5 - PUT_OUT_VIS_SURE_DIST);
	}

	else if (object.getAbsVec().getMagnitude() < object.getPosDeviation() +
			body->getVisibleDistance() + PUT_OUT_VIS_SURE_DIST)
		object.setPosDeviation(object.getAbsVec().getMagnitude() -
				body->getVisibleDistance() - PUT_OUT_VIS_SURE_DIST);
}

void WorldModel::logObjects()
{
	unsigned i;

	LOG << "Body"
		<< " Pos" << body->getPos()
		<< " Vel" << body->getVel()
		<< " Ang" << Point(body->getBodyDir(), body->getHeadDir())
		<< " Poi" << body->getPointingDir() << endl;

	LOG << "Ball"
		<< " Pos" << ball->getPos()
		<< " Vel" << ball->getVel()
		<< " Ang" << Point(ball->getBodyVec().getDirection(),
						   ball->getHeadVec().getDirection())
		<< " Len" << ball->getAbsVec().getMagnitude()
		<< " PoD" << ball->getPosDeviation()
		<< " VPoD" << ball->getValidPosDeviation() << endl;

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (fullPlayers[TID_TEAMMATE][i]->isValid() &&
			!fullPlayers[TID_TEAMMATE][i]->isBody())
			LOG << "OurF" << i+1
				<< " Pos" << fullPlayers[TID_TEAMMATE][i]->getPos()
				<< " Vel" << fullPlayers[TID_TEAMMATE][i]->getVel()
				<< " Ang" << Point(fullPlayers[TID_TEAMMATE][i]->getBodyDir(),
								   fullPlayers[TID_TEAMMATE][i]->getHeadDir())
				<< " Poi" << fullPlayers[TID_TEAMMATE][i]->getPointingDir()
				<< " Sta" << fullPlayers[TID_TEAMMATE][i]->getStamina()
				<< " PoD" << fullPlayers[TID_TEAMMATE][i]->getPosDeviation()
				<< " VPoD" << fullPlayers[TID_TEAMMATE][i]->getValidPosDeviation() << endl;

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (fullPlayers[TID_OPPONENT][i]->isValid())
			LOG << "OppF" << i+1
				<< " Pos" << fullPlayers[TID_OPPONENT][i]->getPos()
				<< " Vel" << fullPlayers[TID_OPPONENT][i]->getVel()
				<< " Ang" << Point(fullPlayers[TID_OPPONENT][i]->getBodyDir(),
								   fullPlayers[TID_OPPONENT][i]->getHeadDir())
				<< " Poi" << fullPlayers[TID_OPPONENT][i]->getPointingDir()
				<< " Sta" << fullPlayers[TID_OPPONENT][i]->getStamina()
				<< " PoD" << fullPlayers[TID_OPPONENT][i]->getPosDeviation()
				<< " VPoD" << fullPlayers[TID_OPPONENT][i]->getValidPosDeviation() << endl;

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_TEAMMATE][i]->isValid())
			LOG << "OurH" << i+1
				<< " Pos" << halfPlayers[TID_TEAMMATE][i]->getPos()
				<< " Vel" << halfPlayers[TID_TEAMMATE][i]->getVel()
				<< " Ang" << Point(halfPlayers[TID_TEAMMATE][i]->getBodyDir(),
								   halfPlayers[TID_TEAMMATE][i]->getHeadDir())
				<< " Poi" << halfPlayers[TID_TEAMMATE][i]->getPointingDir()
				<< " Sta" << halfPlayers[TID_TEAMMATE][i]->getStamina()
				<< " PoD" << halfPlayers[TID_TEAMMATE][i]->getPosDeviation()
				<< " VPoD" << halfPlayers[TID_TEAMMATE][i]->getValidPosDeviation() << endl;

	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (halfPlayers[TID_OPPONENT][i]->isValid())
			LOG << "OppH" << i+1
				<< " Pos" << halfPlayers[TID_OPPONENT][i]->getPos()
				<< " Vel" << halfPlayers[TID_OPPONENT][i]->getVel()
				<< " Ang" << Point(halfPlayers[TID_OPPONENT][i]->getBodyDir(),
								   halfPlayers[TID_OPPONENT][i]->getHeadDir())
				<< " Poi" << halfPlayers[TID_OPPONENT][i]->getPointingDir()
				<< " Sta" << halfPlayers[TID_OPPONENT][i]->getStamina()
				<< " PoD" << halfPlayers[TID_OPPONENT][i]->getPosDeviation()
				<< " VPoD" << halfPlayers[TID_OPPONENT][i]->getValidPosDeviation() << endl;

	for (i = 0; i < QUARTER_PLAYERS_NUM; i++)
		if (quarterPlayers[i]->isValid())
			LOG << "Quar" << i+1
				<< " Pos" << quarterPlayers[i]->getPos()
				<< " Vel" << quarterPlayers[i]->getVel()
				<< " Ang" << Point(quarterPlayers[i]->getBodyDir(),
								   quarterPlayers[i]->getHeadDir())
				<< " Poi" << quarterPlayers[i]->getPointingDir()
				<< " Sta" << quarterPlayers[i]->getStamina()
				<< " PoD" << quarterPlayers[i]->getPosDeviation()
				<< " VPoD" << quarterPlayers[i]->getValidPosDeviation() << endl;

}

void WorldModel::updateCurInterCalculate()
{
	curInterCalculate.startSession(0);
//	curInterCalculate.setLogging(true);
	curInterCalculate.updateByWorldModel(this);
	curInterCalculate.run();
	curInterCalculate.logCheckedPlayers();
}

void WorldModel::updateCurKickCalculate()
{
	curKickCalculate.update(this);
}

// Some Utilities
bool WorldModel::isBallKickable() const
{
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();

	if (getBall().getAbsVec().getMagnitude() <= kickableArea)
		return true;
	return false;
}

bool WorldModel::isBallKickable(const Ball &theBall) const
{
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();

	if (theBall.getAbsVec().getMagnitude() <= kickableArea)
		return true;
	return false;
}

bool WorldModel::isBallCatchable() const
{
	float catchableArea;
	catchableArea = hypot(getBody().getCatchableAreaL() , 
		(getBody().getCatchableAreaW() / 2));

	if (getBall().getAbsVec().getMagnitude() < catchableArea)
		return true;
	return false;
}

bool WorldModel::isBallCatchable(const Ball &theBall) const
{
	float catchableArea;
	catchableArea = hypot(getBody().getCatchableAreaL() , 
		(getBody().getCatchableAreaW() / 2));

	if (theBall.getAbsVec().getMagnitude() < catchableArea)
		return true;
	return false;
}

bool WorldModel::isBallInTmmKickable() const
{
	register int i;
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();
	
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_TEAMMATE, i).isValid())
			if(!getFullPlayer(TID_TEAMMATE, i).isBody())
				if(getFullPlayer(TID_TEAMMATE, i).getDistance(getBall()) 
					<=	kickableArea)
					return true;
					
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (getHalfPlayer(TID_TEAMMATE, i).isValid())
			if(!getHalfPlayer(TID_TEAMMATE, i).isBody())
				if(getHalfPlayer(TID_TEAMMATE, i).getDistance(getBall()) 
					<=	kickableArea)
					return true;

	return false;
}

bool WorldModel::isBallInTmmKickable(const Ball &theBall) const
{
	register int i;
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();
	
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_TEAMMATE, i).isValid())
			if(!getFullPlayer(TID_TEAMMATE, i).isBody())
				if(getFullPlayer(TID_TEAMMATE, i).getDistance(theBall) 
					<=	kickableArea)
					return true;
					
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (getHalfPlayer(TID_TEAMMATE, i).isValid())
			if(!getHalfPlayer(TID_TEAMMATE, i).isBody())
				if(getHalfPlayer(TID_TEAMMATE, i).getDistance(theBall) 
					<=	kickableArea)
					return true;

	return false;
}

bool WorldModel::isBallInOppKickable() const
{
	register int i;
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();
	
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_OPPONENT, i).isValid())
			if(getFullPlayer(TID_OPPONENT, i).getDistance(getBall()) 
				<=	kickableArea)
				return true;
					
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (getHalfPlayer(TID_OPPONENT,i).isValid())
			if(getHalfPlayer(TID_OPPONENT,i).getDistance(getBall()) 
				<=	kickableArea)
				return true;

	return false;
}

bool WorldModel::isBallInOppKickable(const Ball &theBall) const
{
	register int i;
	float kickableArea;
	kickableArea = getBall().getSize() + getBody().getKickableMargin() + 
		getBody().getSize();
	
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_OPPONENT, i).isValid())
			if(getFullPlayer(TID_OPPONENT, i).getDistance(theBall) 
				<=	kickableArea)
				return true;
					
	for (i = 0; i < HALF_PLAYERS_NUM; i++)
		if (getHalfPlayer(TID_OPPONENT,i).isValid())
			if(getHalfPlayer(TID_OPPONENT,i).getDistance(theBall) 
				<=	kickableArea)
				return true;

	return false;
}

bool WorldModel::isBallInField() const
{
	if (getBall().getPos().getX() < 52.5 + 0.5 && 
		getBall().getPos().getX() > -52.5 - 0.5 &&
		getBall().getPos().getY() < 34 + 0.5 && 
		getBall().getPos().getY() > -34 - 0.5)
		return true;
	return false;
}

bool WorldModel::isBallInField(const Ball &theBall)
{
	if (theBall.getPos().getX() < 52.5 + 0.5 && 
		theBall.getPos().getX() > -52.5 - 0.5 &&
		theBall.getPos().getY() < 34 + 0.5 && 
		theBall.getPos().getY() > -34 - 0.5)
		return true;
	return false;
}

float WorldModel::getBallOutDistance() const
{
	float xDistance = 0;
	float yDistance = 0;
	float distance;

	if (getBall().getPos().getX() < -52.5)
		xDistance = -getBall().getPos().getX() - 52.5;
	else if (getBall().getPos().getX() > 52.5)
		xDistance = +getBall().getPos().getX() - 52.5;

	if (getBall().getPos().getY() < -34)
		yDistance = -getBall().getPos().getY() - 34;
	else if (getBall().getPos().getY() > 34)
		yDistance = +getBall().getPos().getY() - 34;

	distance = hypot(xDistance, yDistance);
	return distance;
}

float WorldModel::getOutDistance(const Point &point)
{
	float xDistance = 0;
	float yDistance = 0;

	if (point.x < -52.5)
		xDistance = -point.x - 52.5;
	else if (point.x > 52.5)
		xDistance = +point.x - 52.5;

	if (point.y < -34)
		yDistance = -point.y - 34;
	else if (point.y > 34)
		yDistance = +point.y - 34;

	return hypot(xDistance, yDistance);
}

bool WorldModel::isOppBallShooted(const Ball &theBall) const
{
	Point interceptGoalLine, interceptGoal;

	if (theBall.getVel().getX() == 0)
		return 0;

	float delatX = -52.5 - theBall.getPos().getX();
	float delatY = (theBall.getVel().getY() * delatX) / theBall.getVel().getX();

	float delatX2 = -52.5 - theBall.getPos().getX();
	float delatY2 = (theBall.getVel().getY() * delatX) / theBall.getVel().getX();

	interceptGoalLine.x = theBall.getPos().getX() + delatX;
	interceptGoalLine.y = theBall.getPos().getY() + delatY;

	interceptGoal.x = theBall.getPos().getX() + delatX2;
	interceptGoal.y = theBall.getPos().getY() + delatY2;

	if (fabs(interceptGoal.y) > 8.5)
		return 0;

	Ball simBall;
	simBall = theBall;

	while (simBall.getVel().getMagnitude() > .4)
		simBall.simulateByDynamics(getBody());

///////////////////////	float standardX = GoalieXHome;		// This part must retype (GoalieXHome ?)!!!
float standardX = -51;

	if (theBall.getPos().getX() < standardX)
		standardX = -52.5;

	if (simBall.getPos().getX() < standardX)
		return true;
	return false;
}

bool WorldModel::isOppBallShootedToTir(const Ball &theBall, float upYTirPoint, 
	float downYTirPoint, Point &shootIntercept) const
{
	Point interceptGoalLine, interceptGoal;
	
	if (theBall.getVel().getX() == 0)
		return 0;

	float deltaX = -52.5 - theBall.getPos().getX();
	float deltaY = (theBall.getVel().getY() * deltaX) / theBall.getVel().getX();

	float deltaX2 = -52.5 - theBall.getPos().getX();
	float deltaY2 = (theBall.getVel().getY() * deltaX) / theBall.getVel().getX();

	interceptGoalLine.x = theBall.getPos().getX() + deltaX;
	interceptGoalLine.y = theBall.getPos().getY() + deltaY;
	
	interceptGoal.x = theBall.getPos().getX() + deltaX2;
	interceptGoal.y = theBall.getPos().getY() + deltaY2;

	
	if (interceptGoal.y < (downYTirPoint - 1) || interceptGoal.y > (upYTirPoint + 1))
		return 0;

	Ball simBall;
	simBall = theBall;

	while(simBall.getVel().getMagnitude() > .4)
		simBall.simulateByDynamics(getBody());

	float standardX = -51.5;

	if (theBall.getPos().getX() < standardX)
		standardX = -52.5;
	
	if (simBall.getPos().getX() < standardX)
	{
		shootIntercept = interceptGoal;
		return 1; 
	}
	return 0;
}

float WorldModel::getTmmDefenseLine() const
{
	float min = 0xFFFF;

	for (unsigned i = 1; i <= 5; i++)
		if (fullPlayers[TID_TEAMMATE][i - 1]->isValid())
			min = fmin(min, fullPlayers[TID_TEAMMATE][i - 1]->getPos().getX());

	if (fullPlayers[TID_TEAMMATE][8 - 1]->isValid())
		min = fmin(min, fullPlayers[TID_TEAMMATE][8 - 1]->getPos().getX());

	if (min == 0xFFFF)
		return -52.5;
	return min;
}

const Player &WorldModel::getNearestTmmToBall() const
{
	register int i;
	int nearestPlayer = -1;

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_TEAMMATE, i).isValid())
		{
			if (nearestPlayer == -1)
				nearestPlayer = i;
			else if (getFullPlayer(TID_TEAMMATE, i).getDistance(getBall()) <
				getFullPlayer(TID_TEAMMATE, nearestPlayer).getDistance(getBall()))
				nearestPlayer = i;
		}

	if (nearestPlayer == -1)
		return getBody();
	else
		return getFullPlayer(TID_TEAMMATE, nearestPlayer);
}

const Player *WorldModel::getNearestOppToBall() const
{
	register int i;
	int nearestPlayer = -1;

	for (i = 0; i < FULL_PLAYERS_NUM; i++)
		if (getFullPlayer(TID_OPPONENT, i).isValid())
		{
			if (nearestPlayer == -1)
				nearestPlayer = i;
			else if (getFullPlayer(TID_OPPONENT, i).getDistance(getBall()) <
				getFullPlayer(TID_OPPONENT, nearestPlayer).getDistance(getBall()))
				nearestPlayer = i;
		}

	if (nearestPlayer == -1)
		return NULL;
	else
		return &getFullPlayer(TID_TEAMMATE, nearestPlayer);
}


bool WorldModel::isPlayerInOurField(const Player &player)
{
	if (player.getPos().getX() <= 0)
		return true;
	return false;
}

bool WorldModel::isPlayerInOppField(const Player &player)
{
	if (player.getPos().getX() > 0)
		return true;
	return false;
}

bool WorldModel::isPlayerInOurShootArea(const Player &player)
{
	if (player.getPos().getX() < -20 &&
		player.getPos().getY() < 25 &&
		player.getPos().getY() > -25 )
		return true;
	return false;
}

bool WorldModel::isPlayerInOppShootArea(const Player &player)
{
	if (player.getPos().getX() > 30 &&
		player.getPos().getY() < 25 &&
		player.getPos().getY() > -25 )
		return true;
	return false;
}

float WorldModel::getOppOffsideLine(bool isBallFlag) const
{
	float offsideLine;