body.cpp

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

	posDeviation += 0.5;
	if (posDeviation > MAX_POS_DEV)
		posDeviation = MAX_POS_DEV;

	validPosDeviation += 0.5;
	if (validPosDeviation > MAX_POS_DEV)
		validPosDeviation = MAX_POS_DEV;

	velDeviation += 0.02;
	if (velDeviation > MAX_VEL_DEV)
		velDeviation = MAX_VEL_DEV;

	if (!realBody)
	{
		stamina += staminaIncMax;
		if (stamina > staminaMax)
			stamina = staminaMax;
	}

	setCollisionStatus(TA_DONT_KHOW);
}

void Body::completeParseSenseBody()
{
	// updating velocity and checking for body collision.
	Vector lastVelocity(velocity);
	velocity.setAsPolar(speedAmount,headDir + speedDir);

	if (lastVelocity.getMagnitude() > 0.02 ||
		velocity.getMagnitude() > 0.02)
	{
		Vector deltaVelocity(lastVelocity - velocity);

//LOG << "COLL: deltaVelocity: " << deltaVelocity << endl;
//LOG << "COLL: velocity.getMagnitude() / 5.0: "
//	<< velocity.getMagnitude() / 5.0 << endl;
//LOG << "COLL: deltaDir: " << getDeltaAngle(lastVelocity.getDirection(),
//					velocity.getDirection()) << endl;

		if (deltaVelocity.getMagnitude() >
				fmax(velocity.getMagnitude() / 5.0, 0.05) ||
			fabs(getDeltaAngle(lastVelocity.getDirection(),
					velocity.getDirection())) > 45)
		{
			setCollisionStatus(TA_YES);
			LOG << "*** Body Collision ***" << endl;
		}
		else
			setCollisionStatus(TA_NO);
	}
	else
		setCollisionStatus(TA_DONT_KHOW);

	// updating body pointing dir
	pointingDir = Degree::normalizeAngle(headDir + armTargetDir);
}

void Body::simulateByAction(const Command *bodyCycleCommand, bool realBody)
{
	// Dash
	if (dynamic_cast<const DashCommand *>(bodyCycleCommand))
	{
		Vector dashAcclerate;
		const DashCommand *dashCommand =
				dynamic_cast<const DashCommand *>(bodyCycleCommand);

		dashAcclerate.setAsPolar(dashCommand->getPower() *
				dashPowerRate * effort, bodyDir);
		if (dashAcclerate.getMagnitude() > accelMax)
			dashAcclerate.setAsPolar(accelMax, dashAcclerate.getDirection());
		
		velocity += dashAcclerate;
		if (velocity.getMagnitude() > speedMax)
			velocity.setAsPolar(speedMax, velocity.getDirection());

		if (!realBody)
		{
			if (dashCommand->getPower() >= 0)
				stamina -= dashCommand->getPower();
			else
				stamina += 2 * dashCommand->getPower();

			if (stamina < 0)
				stamina = 0;
		}
	}

	// Turn
	else if (dynamic_cast<const TurnCommand *>(bodyCycleCommand))
	{
		float turnAngle;
		const TurnCommand *turnCommand =
				dynamic_cast<const TurnCommand *>(bodyCycleCommand);

		turnAngle = turnCommand->getDirection();

		// CHECK: check maxMoment & minMoment
		if (turnAngle > maxMoment ||
			turnAngle < minMoment)
			assert(0);

		// Actual Turn Angle formula.
		turnAngle = turnAngle /
					(1 + inertiaMoment * velocity.getMagnitude());
		assert(turnAngle <= maxMoment);
		assert(turnAngle >= minMoment);

		bodyDir = normalizeAngle(bodyDir + turnAngle);
		headDir = normalizeAngle(bodyDir + relativeHeadDir);
	}

	// Move
	else if (dynamic_cast<const MoveCommand *>(bodyCycleCommand))
	{
		const MoveCommand *moveCommand =
				dynamic_cast<const MoveCommand *>(bodyCycleCommand);

		position.setAsCartesian(moveCommand->getX(), moveCommand->getY());
	}

	// Tackle
	else if (dynamic_cast<const TackleCommand *>(bodyCycleCommand))
	{
//		const TackleCommand *tackleCommand =
//				dynamic_cast<const TackleCommand *>(bodyCycleCommand);
		// TODO: 
	}

	// PointTo
/*	else if (typeid(*bodyCycleCommand) == typeid(PointToCommand))
	{
		const PointToCommand *pointToCommand =
				dynamic_cast<const PointToCommand *>(bodyCycleCommand);
		// TODO: 
	}*/
}

void Body::setServerParamVars(const Param &serverParam)
{
	Player::setServerParamVars(serverParam);

	visibleDistance = serverParam["visible_distance"].asFloat();
}

float Body::viewWidthToViewAngle(ViewModeWidth width) const
{
	switch (width)
	{
	case VMW_NARROW:
		return 22.5;
		
	case VMW_NORMAL:
		return 45;
	
	case VMW_WIDE:
		return 90;
	}

	return 0;
}

// Counting functions
bool Body::isKickCountChanged() const
{
	if (kickCount == lastKickCount)
		return false;
	return true;
}

bool Body::isDashCountChanged() const
{
	if (dashCount == lastDashCount)
		return false;
	return true;
}

bool Body::isTurnCountChanged() const
{
	if (turnCount == lastTurnCount)
		return false;
	return true;
}

bool Body::isSayCountChanged() const
{
	if (sayCount == lastSayCount)
		return false;
	return true;
}

bool Body::isTurnNeckCountChanged() const
{
	if (turnNeckCount == lastTurnNeckCount)
		return false;
	return true;
}

bool Body::isCatchCountChanged() const
{
	if (catchCount == lastCatchCount)
		return false;
	return true;
}

bool Body::isMoveCountChanged() const
{
	if (moveCount == lastMoveCount)
		return false;
	return true;
}

bool Body::isChangeViewCountChanged() const
{
	if (changeViewCount == lastChangeViewCount)
		return false;
	return true;
}

bool Body::isArmExpiresChanged() const
{
	if (armExpires == lastArmExpires)
		return false;
	return true;
}

bool Body::isArmCountChanged() const
{
	if (armCount == lastArmCount)
		return false;
	return true;
}

bool Body::isFocusCountChanged() const
{
	if (focusCount == lastFocusCount)
		return false;
	return true;
}

bool Body::isTackleExpiresChanged() const
{
	if (tackleExpires == lastTackleExpires)
		return false;
	return true;
}

bool Body::isTackleCountChanged() const
{
	if (tackleCount == lastTackleCount)
		return false;
	return true;
}

// Getting functions
float Body::getViewAngle() const
{
	return viewAngle;
}

TriAnswer Body::getCollisionStatus() const
{
	return collisionStatus;
}

float Body::getEffort() const
{
	return effort;
}

float Body::getSpeedAmount() const
{
	return speedAmount;
}

float Body::getSpeedDir() const
{
	return speedDir;
}

float Body::getArmTargetDist() const
{
	return armTargetDist;
}

float Body::getArmTargetDir() const
{
	return armTargetDir;
}

unsigned Body::getKickCount() const
{
	return kickCount;
}

unsigned Body::getDashCount() const
{
	return dashCount;
}

unsigned Body::getTurnCount() const
{
	return turnCount;
}

unsigned Body::getSayCount() const
{
	return sayCount;
}

unsigned Body::getTurnNeckCount() const
{
	return turnNeckCount;
}

unsigned Body::getCatchCount() const
{
	return catchCount;
}

unsigned Body::getMoveCount() const
{
	return moveCount;
}

unsigned Body::getSenseBodyCycle() const
{
	return senseBodyCycle;
}

unsigned Body::getChangeViewCount() const
{
	return changeViewCount;
}

unsigned Body::getArmMovable() const
{
	return armMovable;
}

unsigned Body::getArmExpires() const
{
	return armExpires;
}

unsigned Body::getArmCount() const
{
	return armCount;
}

unsigned Body::getTackleExpires() const
{
	return tackleExpires;
}

unsigned Body::getTackleCount() const
{
	return tackleCount;
}

unsigned Body::getFocusTargetUniNum() const
{
	return focusTargetUniNum;
}

unsigned Body::getFocusCount() const
{
	return focusCount;
}

TeamId Body::getFocusTargetTeamId() const
{
	return focusTargetTeamId;
}

ViewModeWidth Body::getVMWidth() const
{
	return vmWidth;
}

ViewModeQuality Body::getVMQuality() const
{
	return vmQuality;
}

float Body::getVisibleDistance() const
{
	return visibleDistance;
}

// Setting functions
void Body::setViewAngle(float viewAngleArg)
{
	viewAngle = viewAngleArg;
}

void Body::setCollisionStatus(TriAnswer collisionStatusArg)
{
	collisionStatus = collisionStatusArg;
}