void SolarSimulation::runSimulation ( ) throw (SimulationInvalidScenarioException, SimulationException)
{
SolarScenario *scen = getSolarScenario();
processScenario(*scen);
std::vector<GFX::Vertex *> verts = figure.getVertices();
if ( verts.size() < 1 ) {
throw SimulationException();
}
std::vector<GFX::Vertex *>::iterator vi;
double thetaRes = input.getAzimuthResolution() * PI / 180.0;
double phiRes = input.getZenithResolution() * PI / 180.0;
try {
int timesteps = radTran->initialize();
while ( radTran->getNextTimeStep() ) {
int i = 0;
for ( vi = verts.begin(); vi != verts.end(); vi++ ) {
if ( scen->getDiffuse() ) {
traceDiffuse(*vi, thetaRes, phiRes);
}
if ( scen->getDirect() ) {
traceDirect(*vi);
}
// stub for new reflective component, not yet implemented
if ( CFG::InternalConfig::ENABLE_REFLECTIVE ) {
if ( scen->getReflective() ) {
traceReflective(*vi, scen->getReflectiveGround());
}
}
percentComplete = static_cast<double>(++i) / verts.size();
}
}
normalizeVertices(verts, 1.0 / timesteps);
} catch ( SimulationException &e ) {
//Logger::error(e.what());
}
// TODO move this somewhere more... appropriate
std::ofstream of("log/verts.csv");
for ( vi = verts.begin(); vi != verts.end(); ++vi ) {
std::map<const unsigned int, double> irrad = (*vi)->getIrradiance();
std::map<const unsigned int, double>::const_iterator mi;
for ( mi = irrad.begin(); mi != irrad.end(); ++mi ) {
of << (*mi).second << ",";
}
of << std::endl;
}
of.close();
percentComplete = 1.0;
fireCompleteEvent();
}
score EvaluationModule::aimAtTeamMate(Robot::robotID shootingRobotID, Robot::robotID goalRobotID, double * rotationToTarget){
GameStatePtr gameState( new GameState() );
if( video.updateGameState( gameState ) < 0){
std::ostringstream s;
s<<__FILE__<<":"<<__LINE__;
throw SimulationException( s.str() );
}
score score_ = 0;
Pose shootingRobotPose=gameState->getRobotPos( shootingRobotID );
Pose goalRobotPose=gameState->getRobotPos( goalRobotID );
//obliczam wspolrzedna srodka robota do ktorego kierowane jest podanie w ukladzie wsp zw z robotem strzelajacym
RotationMatrix rm( shootingRobotPose.get<2>() );
Pose recvPassRelPos = goalRobotPose.transform( shootingRobotPose.getPosition() , rm );
if( rotationToTarget )
*rotationToTarget = calculateProperAngleToTarget( shootingRobotPose, goalRobotPose );
//jesli wsp x jest bliska 0 i roboty maja przeciwna rotace to mozna zrealizowac podanie
if( pow(recvPassRelPos.get<0>(),2) < 0.05*recvPassRelPos.get<1>() ){
//roboty maja rotacje odpowiednia do siebie gdy roznica jest rowna -M_PI
//double deviation = pow( fabs( shootingRobotPose.get<2>() - goalRobotPose.get<2>() ) - M_PI, 2 ) ;
double deviation = fabs( shootingRobotPose.get<2>() - goalRobotPose.get<2>() ) - M_PI ;
double threshold = 0.1*recvPassRelPos.get<1>();
if( deviation < threshold ){// 4 stopnie
LOG_DEBUG( log," mozna podac deviation="<<deviation<<" threshold "<<threshold );
//std::cout<<" mozna podac deviation="<<deviation<<std::endl;
score_ = 1.0 - deviation;
}
else{
LOG_INFO(log, "deviation "<<deviation );
LOG_INFO(log, "shootingRobotPose.get<2>() "<<shootingRobotPose.get<2>()<<" goalRobotPose.get<2>() "<< goalRobotPose.get<2>() );
}
}
else{
if(rotationToTarget){
LOG_INFO(log, "!!!!!!!!! bad recvPassRelPos "<<recvPassRelPos<<" rotationToTarget "<<*rotationToTarget<<" currRotation "<<shootingRobotPose.get<2>() );
}
else{
LOG_INFO(log, "!!!!!!!!! bad recvPassRelPos "<<recvPassRelPos<<" currRotation "<<shootingRobotPose.get<2>() )
}
}
return score_;
}
void FollowLine::execute(void *){
LOG_INFO(log,"create start FollowLine tactic. Line "<<p1<<" "<<p2);
Task::status taskStatus = Task::not_completed;
Pose goalPose = Pose(this->p1,0);
Pose nextGoalPose = Pose(this->p2,0);
//znajdz punkt na odcinku (p1,p2) bedacy najblizej pilki
//i nie nalezacy do zadnej przeszkody
//r-nie prostej przechodzacej przez P1 i P2
// double a = -( p2.y - p1.y )/( p2.x - p1.x );
// double b = 1;
// double c = (-1)*p1.x * a - p1.y;
GameStatePtr gameState ( new GameState() );
if( Videoserver::getInstance().updateGameState( gameState ) < 0 )
throw SimulationException("FollowLine::execute");
while(true){
{
LockGuard l(this->mutex);
if(this->stop)
break;
}
taskStatus = Task::not_completed;
if( Videoserver::getInstance().updateGameState( gameState ) < 0)
throw SimulationException("FollowLine::execute");
Pose currPose = gameState->getRobotPos(this->robot.getRobotID());
Vector2D diff = currPose.getPosition() - goalPose.getPosition();
//podazaj do kranca zdefiniowanego odcinka
if( ( fabs(diff.x) < 0.05 ) && ( fabs(diff.y) < 0.05 ) ){
Pose tmp = goalPose;
goalPose = nextGoalPose;
nextGoalPose = tmp;
}
Vector2D robotCurrentGlobalVel=gameState->getRobotGlobalVelocity( robot.getRobotID() );
//robotNewVel=calculateVelocity( robotCurrentVel, Pose(targetRelPosition.x,targetRelPosition.y,0));
bool haveBall= false;
Vector2D robotNewGlobalVel=this->robot.calculateVelocity( robotCurrentGlobalVel, currPose, goalPose, haveBall );
//double w = robot->calculateAngularVel(*currGameState,robot->getRobotID(), goalPose);
double w = robot.calculateAngularVel( gameState->getRobotPos( robot.getRobotID() ), goalPose, gameState->getSimTime(), haveBall );
//this->robot.setRelativeSpeed( Vector2D( 1.0,0.0 ) * sgn( diff.x ) ,0 );
this->robot.setGlobalSpeed( robotNewGlobalVel ,w , currPose.get<2>());
/*
while( ( fabs(diff.x) > 0.05 ) && ( fabs(diff.y) > 0.05 ) ){
// newTask = this->currentTask->nextTask();
if( diff.x < 0 )
this->robot.setRelativeSpeed( Vector(1.0,0.0),0 );
if(){
this->currentTask = TaskSharedPtr( newTask );
}
break;
}
*/
}
this->finished = false;
}
BallState::ballState EvaluationModule::getBallState(Robot::robotID id, bool * iAmCloserToBall ){
GameStatePtr gameState( new GameState() );
if( video.updateGameState( gameState ) < 0){
std::ostringstream s;
s<<__FILE__<<":"<<__LINE__;
throw SimulationException( s.str() );
}
const std::vector<std::string> blueTeam=Config::getInstance().getBlueTeam();
BallState::ballState bs=BallState::free;
BallState::ballState ballState_=BallState::free;
Pose p;
Pose ballPose = gameState->getBallPos();
//pobierz ostatni stan z servera wideo,
//wazne w przypadku autu badz zdobycia bramki
ballState_ = gameState->getBallState();//getBallState( ballPose.getPosition() );
if( ballState_ != BallState::free){
//LOG_FATAL( log,"Ball state is "<<ballState_ );
LOG_FATAL( log,"Ball 123position is "<<ballPose<<" ballState "<<ballState_ );
return ballState_;
}
if( Robot::isBlue( id ) ){
bs = BallState::occupied_our;
}
else
bs = BallState::occupied_theirs;
double distanceToBall;
double angleToBall;
//dla kazdego robota z naszej druzyny sprawdz czy nie jest posiadaczem pilki
//dla kazdego robota z druzyny przeciwnej sprawdz czy nie jest posiadaczem pilki
//odleglosc najblizszego robota do pilki
double shortestDistance = numeric_limits<double>::max( );
Robot::robotID closerToBallRobotID;
Robot::robotID robotID;
BOOST_FOREACH(std::string modelName,blueTeam){
robotID = Robot::getRobotID( modelName);
p = gameState->getRobotPos( robotID );
//if( p.distance(ballPose) < ( Config::getInstance().getRobotMainCylinderRadious() + 0.04 ) ){
if( this->isRobotOwnedBall( Robot::getRobotID( modelName ), gameState , distanceToBall, angleToBall ) ){
ballState_ = bs;
if( Robot::getRobotID( modelName) == id )
return BallState::mine;
else
return ballState_;
}
if( distanceToBall < shortestDistance ){
shortestDistance=distanceToBall;
closerToBallRobotID = robotID;
}
}
