void quickWorldModelTest() {
gSystem->Load("lib/libPropagation.so");
// BedmapTable *iceTable = new BedmapTable (RyansCons::kBathymetry);
WorldModel *myWorld = new WorldModel();
TH2F *iceHist = new TH2F("iceHist","Ice Thickness",1000,-3e6,3e6,1000,-3e6,3e6);
TH2F *iceHist2 = new TH2F("iceHist2","Ice Thickness (Bedmap)",1000,-3e6,3e6,1000,-3e6,3e6);
TH2F *surfaceHist = new TH2F("surfaceHist","Surface",1000,-3e6,3e6,1000,-3e6,3e6);
// int goodFlag;
for(int binx=1;binx<=iceHist->GetNbinsX();binx++) {
Double_t x=iceHist->GetXaxis()->GetBinCenter(binx);
for(int biny=1;biny<=iceHist->GetNbinsY();biny++) {
Double_t y=iceHist->GetYaxis()->GetBinCenter(biny);
Double_t value=myWorld->getCrust2Data(x,y,RyansCons::kThicknessOfIce);
iceHist->SetBinContent(binx,biny,value);
value=myWorld->getIceThickness(x,y);
iceHist2->SetBinContent(binx,biny,value);
value=myWorld->getSurface(x,y);
surfaceHist->SetBinContent(binx,biny,value/1e6);
if(value>1e7 || value<1e6) cout << x << "\t" << y << "\t" << value << endl;
}
}
TCanvas *can = new TCanvas("can","can");
can->Divide(1,2);
can->cd(1);
iceHist->Draw("colz");
can->cd(2);
iceHist2->Draw("colz");
TCanvas *can2 = new TCanvas("can2","can2");
surfaceHist->Draw("colz");
}
//=================================================================
bool TileAssembler::convertRawFile(const std::string& pModelFilename)
{
bool success = true;
std::string filename = iSrcDir;
if (filename.length() >0)
filename.push_back('/');
filename.append(pModelFilename);
WorldModel_Raw raw_model;
if (!raw_model.Read(filename.c_str()))
return false;
// write WorldModel
WorldModel model;
model.setRootWmoID(raw_model.RootWMOID);
if (!raw_model.groupsArray.empty())
{
std::vector<GroupModel> groupsArray;
uint32 groups = raw_model.groupsArray.size();
for (uint32 g = 0; g < groups; ++g)
{
GroupModel_Raw& raw_group = raw_model.groupsArray[g];
groupsArray.push_back(GroupModel(raw_group.mogpflags, raw_group.GroupWMOID, raw_group.bounds ));
groupsArray.back().setMeshData(raw_group.vertexArray, raw_group.triangles);
groupsArray.back().setLiquidData(raw_group.liquid);
}
model.setGroupModels(groupsArray);
}
success = model.writeFile(iDestDir + "/" + pModelFilename + ".vmo");
//std::cout << "readRawFile2: '" << pModelFilename << "' tris: " << nElements << " nodes: " << nNodes << std::endl;
return success;
}
/*!
*/
void
CrossGenerator::updateOpponents( const WorldModel & wm )
{
const double opponent_dist_thr2 = std::pow( 20.0, 2 );
const Vector2D goal = ServerParam::i().theirTeamGoalPos();
const AngleDeg goal_angle_from_ball = ( goal - M_first_point ).th();
for ( AbstractPlayerCont::const_iterator
p = wm.theirPlayers().begin(),
end = wm.theirPlayers().end();
p != end;
++p )
{
AngleDeg opponent_angle_from_ball = ( (*p)->pos() - M_first_point ).th();
if ( ( opponent_angle_from_ball - goal_angle_from_ball ).abs() > 90.0 )
{
continue;
}
if ( (*p)->pos().dist2( M_first_point ) > opponent_dist_thr2 )
{
continue;
}
M_opponents.push_back( *p );
#ifdef DEBUG_PRINT
dlog.addText( Logger::PASS,
"Cross opponent %d pos(%.1f %.1f)",
(*p)->unum(),
(*p)->pos().x, (*p)->pos().y );
#endif
}
}
HOBJECT WorldModel::AttachObject( HOBJECT hObj )
{
if( !hObj ) return LTNULL;
// Make sure the object is detached first.
DetachObject( hObj );
LTVector vPos, vParentPos;
LTRotation rRot, rParentRot;
// Get our position and rotation
g_pLTServer->GetObjectPos( m_hObject, &vParentPos );
g_pLTServer->GetObjectRotation( m_hObject, &rParentRot );
LTMatrix mRot;
rParentRot.ConvertToMatrix( mRot );
// Get the attachment object pos / rot
g_pLTServer->GetObjectPos( hObj, &vPos );
g_pLTServer->GetObjectRotation( hObj, &rRot );
// Calculate the offsets...
LTVector vPosOffset( ~mRot * (vPos - vParentPos) );
LTRotation rRotOffset = rRot * ~rParentRot;
// Attach it...
HATTACHMENT hAttachment;
LTRESULT LTRes = g_pLTServer->CreateAttachment( m_hObject, hObj, LTNULL,
&vPosOffset, &rRotOffset, &hAttachment );
if( LTRes != LT_OK )
return NULL;
LTObjRefNotifier ref( *this );
ref = hObj;
m_AttachmentList.push_back( ref );
// If the object is a WorldModel set it's activate parent...
if( IsWorldModel( hObj ))
{
WorldModel *pWorldModel = dynamic_cast<WorldModel*>(g_pLTServer->HandleToObject( hObj ));
if( pWorldModel )
{
// Send any activate messages the WorldModel recevies to us...
pWorldModel->SetActivateParent( m_hObject );
// Add the WorldModel to our list of objects to inherit our ActivateType...
m_ActivateTypeHandler.InheritObject( hObj );
}
}
return hObj;
}
void Decision::Freeman_Say()
{
wm.MySayInfo.SayToWho = 6;
wm.MySayInfo.WhoAmI = wm.GetUnum();
wm.MySayInfo.State = -1;
wm.MySayInfo.CurrentTar = 0;
wm.MySayInfo.SuggestMate1 = 1;
robot.Say(wm.GenerateSayInfomation());
}
void WorldModel::DetachObject( HOBJECT hObj )
{
if( !hObj ) return;
// Remove the object from our list of attachments. Should only be in here once.
for( ObjRefNotifierList::iterator iter = m_AttachmentList.begin( ); iter != m_AttachmentList.end( ); iter++ )
{
if( hObj == *iter )
{
// Remove it from our list.
m_AttachmentList.erase( iter );
break;
}
}
HATTACHMENT hAttachment = NULL;
if( g_pLTServer->FindAttachment( m_hObject, hObj, &hAttachment ) != LT_OK )
return;
g_pLTServer->RemoveAttachment( hAttachment );
// If the object is a WorldModel set it's activate parent...
if( IsWorldModel( hObj ))
{
WorldModel *pWorldModel = dynamic_cast<WorldModel*>(g_pLTServer->HandleToObject( hObj ));
if( pWorldModel )
{
// No longer send any activate messages the WorldModel recevies to us...
pWorldModel->SetActivateParent( LTNULL );
// Remove the WorldModel from our list of object to inherit our ActivateType...
m_ActivateTypeHandler.DisownObject( hObj );
}
}
// Don't just let the object float
uint32 dwFlags;
g_pCommonLT->GetObjectFlags( hObj, OFT_Flags, dwFlags );
if( (IsKindOf(hObj, "Prop" )) && (dwFlags & FLAG_GRAVITY) )
{
LTVector vVel;
g_pPhysicsLT->GetVelocity( hObj, &vVel );
if( vVel.y > -0.1f )
{
vVel.y -= 10.0f;
g_pPhysicsLT->SetVelocity( hObj, &vVel );
}
}
}
void lidarlocalizorapp(){
printf("starting\n");
Lidar lidar;
lidar.init("lidar/lidarconfig.conf");
LidarManager lidarManager;
lidarManager.lidar=&lidar;
lidarManager.init("lidar/lidarpositionconfig.conf");
LidarLocalizer localizer;
localizer.loadPresetMap();
WorldModel model;
model.init("indoorlidarconf.conf");
model.lidar=&lidarManager;
model.lidarLocalizer=&localizer;
Network network;
model.network=&network;
Display display=Display(&model);
//start timer
TimeKeeper::start_time();
printf("made all objects\n");
int key = 0;
long start=0;
double x;
double y;
try {
while((key&255)!=27){
start=TimeKeeper::GetTime();
// printf("loop start\n");
model.updateModel();
// localizer.estimatePosition(&state,&scanpoints,fake_scan_point_count);
// printf("model updated\n");
//display.displayModel();
key=cvWaitKey(15);
cout<<"position "<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRot<<endl;
cout<<"time elapsed "<<(TimeKeeper::GetTime()-start)/1000000.0<<endl;
}
}
catch(...) {
cerr << "we are catching an uncaught exception. bad things happened" << endl;
}
}
/*!
*/
Strategy::BallArea
Strategy::get_ball_area( const WorldModel & wm )
{
int ball_step = 1000;
ball_step = std::min( ball_step, wm.interceptTable()->teammateReachCycle() );
ball_step = std::min( ball_step, wm.interceptTable()->opponentReachCycle() );
ball_step = std::min( ball_step, wm.interceptTable()->selfReachCycle() );
return get_ball_area( wm.ball().inertiaPoint( ball_step ) );
}
/*!
*/
void
CrossGenerator::updateReceivers( const WorldModel & wm )
{
static const double shootable_dist2 = std::pow( 16.0, 2 ); // Magic Number
static const double min_cross_dist2
= std::pow( ServerParam::i().defaultKickableArea() * 2.2, 2 );
static const double max_cross_dist2
= std::pow( inertia_n_step_distance( ServerParam::i().ballSpeedMax(),
9,
ServerParam::i().ballDecay() ),
2 );
const Vector2D goal = ServerParam::i().theirTeamGoalPos();
const bool is_self_passer = ( M_passer->unum() == wm.self().unum() );
for ( AbstractPlayerCont::const_iterator
p = wm.ourPlayers().begin(),
end = wm.ourPlayers().end();
p != end;
++p )
{
if ( *p == M_passer ) continue;
if ( is_self_passer )
{
if ( (*p)->isGhost() ) continue;
if ( (*p)->posCount() >= 4 ) continue;
if ( (*p)->pos().x > wm.offsideLineX() ) continue;
}
else
{
// ignore other players
if ( (*p)->unum() != wm.self().unum() )
{
continue;
}
}
if ( (*p)->pos().dist2( goal ) > shootable_dist2 ) continue;
double d2 = (*p)->pos().dist2( M_first_point );
if ( d2 < min_cross_dist2 ) continue;
if ( max_cross_dist2 < d2 ) continue;
M_receiver_candidates.push_back( *p );
#ifdef DEBUG_UPDATE_OPPONENT
dlog.addText( Logger::CROSS,
"Cross receiver %d pos(%.1f %.1f)",
(*p)->unum(),
(*p)->pos().x, (*p)->pos().y );
#endif
}
}
/*!
*/
int
TackleGenerator::predictOpponentsReachStep( const WorldModel & wm,
const Vector2D & first_ball_pos,
const Vector2D & first_ball_vel,
const AngleDeg & ball_move_angle )
{
int first_min_step = 50;
#if 1
const ServerParam & SP = ServerParam::i();
const Vector2D ball_end_point = inertia_final_point( first_ball_pos,
first_ball_vel,
SP.ballDecay() );
if ( ball_end_point.absX() > SP.pitchHalfLength()
|| ball_end_point.absY() > SP.pitchHalfWidth() )
{
Rect2D pitch = Rect2D::from_center( 0.0, 0.0, SP.pitchLength(), SP.pitchWidth() );
Ray2D ball_ray( first_ball_pos, ball_move_angle );
Vector2D sol1, sol2;
int n_sol = pitch.intersection( ball_ray, &sol1, &sol2 );
if ( n_sol == 1 )
{
first_min_step = SP.ballMoveStep( first_ball_vel.r(), first_ball_pos.dist( sol1 ) );
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"(predictOpponent) ball will be out. step=%d reach_point=(%.2f %.2f)",
first_min_step,
sol1.x, sol1.y );
#endif
}
}
#endif
int min_step = first_min_step;
for ( AbstractPlayerCont::const_iterator
o = wm.theirPlayers().begin(),
end = wm.theirPlayers().end();
o != end;
++o )
{
int step = predictOpponentReachStep( *o,
first_ball_pos,
first_ball_vel,
ball_move_angle,
min_step );
if ( step < min_step )
{
min_step = step;
}
}
return ( min_step == first_min_step
? 1000
: min_step );
}
bool StaticMapTree::InitMap(const std::string& fname, VMapManager2* vm)
{
DEBUG_FILTER_LOG(LOG_FILTER_MAP_LOADING, "Initializing StaticMapTree '%s'", fname.c_str());
bool success = true;
std::string fullname = iBasePath + fname;
FILE* rf = fopen(fullname.c_str(), "rb");
if (!rf)
return false;
else
{
char chunk[8];
// general info
if (!readChunk(rf, chunk, VMAP_MAGIC, 8)) success = false;
char tiled;
if (success && fread(&tiled, sizeof(char), 1, rf) != 1) success = false;
iIsTiled = !!tiled;
// Nodes
if (success && !readChunk(rf, chunk, "NODE", 4)) success = false;
if (success) success = iTree.readFromFile(rf);
if (success)
{
iNTreeValues = iTree.primCount();
iTreeValues = new ModelInstance[iNTreeValues];
}
if (success && !readChunk(rf, chunk, "GOBJ", 4)) success = false;
// global model spawns
// only non-tiled maps have them, and if so exactly one (so far at least...)
ModelSpawn spawn;
#ifdef VMAP_DEBUG
DEBUG_LOG("Map isTiled: %u", static_cast<uint32>(iIsTiled));
#endif
if (!iIsTiled && ModelSpawn::readFromFile(rf, spawn))
{
WorldModel* model = vm->acquireModelInstance(iBasePath, spawn.name);
DEBUG_FILTER_LOG(LOG_FILTER_MAP_LOADING, "StaticMapTree::InitMap(): loading %s", spawn.name.c_str());
if (model)
{
model->setModelFlags(spawn.flags);
// assume that global model always is the first and only tree value (could be improved...)
iTreeValues[0] = ModelInstance(spawn, model);
iLoadedSpawns[0] = 1;
}
else
{
success = false;
ERROR_LOG("StaticMapTree::InitMap() could not acquire WorldModel pointer for '%s'!", spawn.name.c_str());
}
}
fclose(rf);
}
return success;
}
/*!
*/
void
Strategy::update( const WorldModel & wm )
{
static GameTime s_update_time( -1, 0 );
if ( s_update_time == wm.time() )
{
return;
}
s_update_time = wm.time();
updateSituation( wm );
updatePosition( wm );
}
/*!
*/
void
TackleGenerator::generate( const WorldModel & wm )
{
static GameTime s_update_time( 0, 0 );
if ( s_update_time == wm.time() )
{
// dlog.addText( Logger::CLEAR,
// __FILE__": already updated" );
return;
}
s_update_time = wm.time();
clear();
if ( wm.self().isKickable() )
{
// dlog.addText( Logger::CLEAR,
// __FILE__": kickable" );
return;
}
if ( wm.self().tackleProbability() < 0.001
&& wm.self().foulProbability() < 0.001 )
{
// dlog.addText( Logger::CLEAR,
// __FILE__": never tacklable" );
return;
}
if ( wm.time().stopped() > 0 )
{
// dlog.addText( Logger::CLEAR,
// __FILE__": time stopped" );
return;
}
if ( wm.gameMode().type() != GameMode::PlayOn
&& ! wm.gameMode().isPenaltyKickMode() )
{
// dlog.addText( Logger::CLEAR,
// __FILE__": illegal playmode " );
return;
}
#ifdef DEBUG_PROFILE
MSecTimer timer;
#endif
calculate( wm );
#ifdef DEBUG_PROFILE
dlog.addText( Logger::CLEAR,
__FILE__": PROFILE. elapsed=%.3f [ms]",
timer.elapsedReal() );
#endif
}
QuestTickResult ScanPlanetQuest::Tick(float, const WorldModel &worldModel) {
int scanned = worldModel.GetMetaData(targetPlanet).GetValue(metaDataMarker);
if (scanned >= 100) {
//TODO improve sponsor relationship
return QuestTickResult(QuestModelAction(QuestActionType::SUCCEED, GetId()));
}
return QuestTickResult(GetId());
}
static void CreateServerMark(CLIENTWEAPONFX & theStruct)
{
// If this isn't a GameBase object, return...
if (!IsWorldModel(theStruct.hObj)) return;
// See if we should create a mark, or simply move one of the GameBase's
// marks.
// If the GameBase has the max number of marks or this mark is very close
// to a pre-existing mark, just move that mark to the new position.
WorldModel* pObj = (WorldModel*) g_pLTServer->HandleToObject(theStruct.hObj);
if (!pObj) return;
pObj->CreateServerMark( theStruct );
}
WorldModel* VMapManager2::acquireModelInstance(const std::string &basepath, const std::string &filename)
{
ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
if (model == iLoadedModelFiles.end())
{
WorldModel *worldmodel = new WorldModel();
if (!worldmodel->readFile(basepath + filename + ".vmo"))
{
ERROR_LOG("VMapManager2: could not load '%s%s.vmo'!", basepath.c_str(), filename.c_str());
delete worldmodel;
return NULL;
}
DEBUG_LOG("VMapManager2: loading file '%s%s'.", basepath.c_str(), filename.c_str());
model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
model->second.setModel(worldmodel);
}
model->second.incRefCount();
return model->second.getModel();
}
void ExplosionView::notify(Subject* s)
{
WorldModel* m;
m = dynamic_cast<WorldModel *>(s);
if(start_animation)
{
float current_radius = timer.GetElapsedTime() * radius_per_second;
int circle_complete = (current_radius / max_radius) * 100;
int blue_value = (circle_complete * 255) / 100;
std::cout << circle_complete << '\n';
std::cout << blue_value << '\n';
circle = sf::Shape::Circle(m->get_x_position(), m->get_y_position(), current_radius, sf::Color(255, blue_value, 0));
app->Draw(circle);
draw();
if(current_radius > max_radius)
m->dispose();
}
}
/*!
*/
void
ActionChainHolder::update( const WorldModel & wm )
{
static GameTime s_update_time( 0, 0 );
static FieldEvaluator::ConstPtr s_update_evaluator;
static ActionGenerator::ConstPtr s_update_generator;
if ( s_update_time == wm.time()
&& s_update_evaluator == M_evaluator
&& s_update_generator == M_generator )
{
return;
}
s_update_time = wm.time();
s_update_evaluator = M_evaluator;
s_update_generator = M_generator;
M_graph = ActionChainGraph::Ptr( new ActionChainGraph( M_evaluator, M_generator ) );
M_graph->calculate( wm );
}
/*!
*/
void
DebugClient::writeAll( const WorldModel & world,
const ActionEffector & effector )
{
if ( M_on )
{
this->toStr( world, effector );
if ( M_connected )
{
this->send();
}
if ( M_write_mode
&& world.time().stopped() == 0 )
{
this->write( world.time().cycle() );
}
this->clear();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, std::string("worldmodel"));
ros::NodeHandle n;
WorldModel worldmodel;
ros::Rate loop_rate(1);
int count = 0;
while (ros::ok())
{
worldmodel.step();
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
WorldModel* VMapManager2::acquireModelInstance(const std::string& basepath, const std::string& filename)
{
//! Critical section, thread safe access to iLoadedModelFiles
std::lock_guard<std::mutex> lock(LoadedModelFilesLock);
ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
if (model == iLoadedModelFiles.end())
{
WorldModel* worldmodel = new WorldModel();
if (!worldmodel->readFile(basepath + filename + ".vmo"))
{
VMAP_ERROR_LOG("misc", "VMapManager2: could not load '%s%s.vmo'", basepath.c_str(), filename.c_str());
delete worldmodel;
return NULL;
}
VMAP_DEBUG_LOG("maps", "VMapManager2: loading file '%s%s'", basepath.c_str(), filename.c_str());
model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
model->second.setModel(worldmodel);
}
model->second.incRefCount();
return model->second.getModel();
}
WorldModel* VMapManager2::acquireModelInstance(const std::string& basepath, const std::string& filename)
{
//! Critical section, thread safe access to iLoadedModelFiles
TRINITY_GUARD(ACE_Thread_Mutex, LoadedModelFilesLock);
ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
if (model == iLoadedModelFiles.end())
{
WorldModel* worldmodel = new WorldModel();
if (!worldmodel->readFile(basepath + filename + ".vmo"))
{
sLog->outError("VMapManager2: could not load '%s%s.vmo'", basepath.c_str(), filename.c_str());
delete worldmodel;
return NULL;
}
sLog->outDebug(LOG_FILTER_MAPS, "VMapManager2: loading file '%s%s'", basepath.c_str(), filename.c_str());
model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
model->second.setModel(worldmodel);
}
model->second.incRefCount();
return model->second.getModel();
}
/*!
*/
void
Strategy::updateSituation( const WorldModel & wm )
{
M_current_situation = Normal_Situation;
if ( wm.gameMode().type() != GameMode::PlayOn )
{
if ( wm.gameMode().isPenaltyKickMode() )
{
dlog.addText( Logger::TEAM,
__FILE__": Situation PenaltyKick" );
M_current_situation = PenaltyKick_Situation;
}
else if ( wm.gameMode().isPenaltyKickMode() )
{
dlog.addText( Logger::TEAM,
__FILE__": Situation OurSetPlay" );
M_current_situation = OurSetPlay_Situation;
}
else
{
dlog.addText( Logger::TEAM,
__FILE__": Situation OppSetPlay" );
M_current_situation = OppSetPlay_Situation;
}
return;
}
int self_min = wm.interceptTable()->selfReachCycle();
int mate_min = wm.interceptTable()->teammateReachCycle();
int opp_min = wm.interceptTable()->opponentReachCycle();
int our_min = std::min( self_min, mate_min );
if ( opp_min <= our_min - 2 )
{
dlog.addText( Logger::TEAM,
__FILE__": Situation Defense" );
M_current_situation = Defense_Situation;
return;
}
if ( our_min <= opp_min - 2 )
{
dlog.addText( Logger::TEAM,
__FILE__": Situation Offense" );
M_current_situation = Offense_Situation;
return;
}
dlog.addText( Logger::TEAM,
__FILE__": Situation Normal" );
}
void Decision::EstimateSituationUpdate_FreeMan()
{
SITUATION_FREEMAN situation_decision = UNKNOWN_SITUATION;
Vector m_vBallPosm_vBallPos = wm.m_vBallPos;
bool bTeammate = false;
double dClosestDistTeammate=0;double dClosestDistPlayer=0;
int iClosestTeammate=0; int iClosestPlayer=0; int iClosestOpp=0;
dClosestDistTeammate = wm.GetClosestToBallTeammate(iClosestTeammate);
dClosestDistPlayer = wm.GetClosestToBallPlayer(iClosestPlayer,bTeammate);
//if ( (iClosestTeammate==wm.GetUnum() || (iClosestTeammate!=wm.GetUnum() && wm.IsTeammateAbnormal(iClosestTeammate) && fabs(dClosestDistTeammate-wm.GetMeToBallDistInXY())<1.0 ) ) && !wm.IsBallInOurFA())
if (1)
{
situation_decision = ATTACK;
goto CheckSituation_Change;
}
if ( iClosestTeammate!=wm.GetUnum() && ((wm.IsBallInOurFA() && !wm.IsTeammateAbnormal(iClosestTeammate)) || !wm.IsBallInOurFA()) )
{
situation_decision = ASSIST_ATTACK;
goto CheckSituation_Change;
}
if ((wm.IsBallInOurFA() && wm.IsTeammateAbnormal(3)) || (wm.GetClosestToBallOpp(iClosestOpp)<0.3 && wm.IsTeammateAbnormal(iClosestTeammate)))
{
situation_decision = URGENCY_DEFEND;
goto CheckSituation_Change;
}
else
situation_decision = ASSIST_DEFEND;
CheckSituation_Change:
if (situation_decision != Freeman_Situation) //When Situation Changed , Consider about Action Over
{
if (Motion_State == WALK_PERIOD)
Motion_State = STOP_WALK_PERIOD;
else if (Motion_State == MICRO_PERIOD)
Motion_State = STOP_MICRO_PERIOD;
else
Motion_State = ACTION_READY;
}
Freeman_Situation = situation_decision;
return;
}
void recorder(){
ofstream logfile;
logfile.open("logfile.txt");
ofstream lidarLogfile;
lidarLogfile.open("lidarlogfile.txt");
Lidar lidar;
lidar.init("lidar/lidarconfig.conf");
LidarManager lidarManager;
lidarManager.lidar=&lidar;
lidarManager.init("lidar/lidarpositionconfig.conf");
WorldModel model;
model.init("visionworldmodelconfig.conf");
model.lidar=&lidarManager;
Network network;
model.network=&network;
char buffer [50];
Camera a;
a.init(0);
a.loadCalib("vision/calibration/Intrinsic.xml","vision/calibration/Distortion.xml","vision/calibration/H.xml");
CameraManager manager;
manager.init();
manager.addCam(&a);
manager.startThread();
VisionInterpreter vision= VisionInterpreter(&manager);
vision.init("vision/visioninterpreter.conf");
vision.manager=&manager;
model.vision=&vision;
Display display=Display(&model);
//start timer
TimeKeeper::start_time();
printf("made all objects\n");
int key = 0;
long start=0;
try {
while((key&255)!=27){
start=TimeKeeper::GetTime();
model.updateModel();
display.displayModel();
key=cvWaitKey(3);
//write a timestamp to the logfile
logfile<<start<<endl;
//write the crio state to the logfile
logfile<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRotRad<<endl;
//write the lidar data to the logfile
lidarLogfile<<start<<endl;
lidarLogfile<<lidar.num_values<<endl;
for(int i=0;i<lidar.num_values;i++){
lidarLogfile<<lidar.values[i]<<" ";
}
lidarLogfile<<endl;
sprintf (buffer, "output/output%.7ld.bmp",start);
if(!cvSaveImage(buffer,a.image,0)) printf("Could not save: %s\n",buffer);
else printf("picture taken!!!\n");
cout<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRotRad<<endl;
cout<<TimeKeeper::GetTime()-start<<endl;
if((key&255) ==32){
if(!cvSaveImage("output/vision map.jpg",model.worldModelVision,0)) printf("Could not save");
else printf("picture taken!!!\n");
}
}
}
catch(...) {
cerr << "we are catching an uncaught exception. bad things happened" << endl;
}
lidarLogfile.close();
logfile.close();
}
//=================================================================
bool TileAssembler::convertRawFile(const std::string& pModelFilename)
{
bool success = true;
std::string filename = iSrcDir;
if (filename.length() >0)
filename.append("/");
filename.append(pModelFilename);
FILE *rf = fopen(filename.c_str(), "rb");
if (!rf)
{
printf("ERROR: Can't open model file in form: %s",pModelFilename.c_str());
printf("... or form: %s",filename.c_str() );
return false;
}
char ident[8];
int readOperation = 1;
// temporary use defines to simplify read/check code (close file and return at fail)
#define READ_OR_RETURN(V,S) if(fread((V), (S), 1, rf) != 1) { \
fclose(rf); printf("readfail, op = %i\n", readOperation); return(false); }readOperation++;
#define CMP_OR_RETURN(V,S) if(strcmp((V),(S)) != 0) { \
fclose(rf); printf("cmpfail, %s!=%s\n", V, S);return(false); }
READ_OR_RETURN(&ident, 8);
CMP_OR_RETURN(ident, "VMAP003");
// we have to read one int. This is needed during the export and we have to skip it here
uint32 tempNVectors;
READ_OR_RETURN(&tempNVectors, sizeof(tempNVectors));
uint32 groups;
uint32 RootWMOID;
char blockId[5];
blockId[4] = 0;
int blocksize;
READ_OR_RETURN(&groups, sizeof(uint32));
READ_OR_RETURN(&RootWMOID, sizeof(uint32));
std::vector<GroupModel> groupsArray;
for (uint32 g=0; g<groups; ++g)
{
std::vector<MeshTriangle> triangles;
std::vector<Vector3> vertexArray;
uint32 mogpflags, GroupWMOID;
READ_OR_RETURN(&mogpflags, sizeof(uint32));
READ_OR_RETURN(&GroupWMOID, sizeof(uint32));
float bbox1[3], bbox2[3];
READ_OR_RETURN(bbox1, sizeof(float)*3);
READ_OR_RETURN(bbox2, sizeof(float)*3);
uint32 liquidflags;
READ_OR_RETURN(&liquidflags, sizeof(uint32));
// will this ever be used? what is it good for anyway??
uint32 branches;
READ_OR_RETURN(&blockId, 4);
CMP_OR_RETURN(blockId, "GRP ");
READ_OR_RETURN(&blocksize, sizeof(int));
READ_OR_RETURN(&branches, sizeof(uint32));
for (uint32 b=0; b<branches; ++b)
{
uint32 indexes;
// indexes for each branch (not used jet)
READ_OR_RETURN(&indexes, sizeof(uint32));
}
// ---- indexes
READ_OR_RETURN(&blockId, 4);
CMP_OR_RETURN(blockId, "INDX");
READ_OR_RETURN(&blocksize, sizeof(int));
uint32 nindexes;
READ_OR_RETURN(&nindexes, sizeof(uint32));
if (nindexes >0)
{
uint16 *indexarray = new uint16[nindexes];
READ_OR_RETURN(indexarray, nindexes*sizeof(uint16));
for (uint32 i=0; i<nindexes; i+=3)
{
triangles.push_back(MeshTriangle(indexarray[i], indexarray[i+1], indexarray[i+2]));
}
delete[] indexarray;
}
// ---- vectors
READ_OR_RETURN(&blockId, 4);
CMP_OR_RETURN(blockId, "VERT");
READ_OR_RETURN(&blocksize, sizeof(int));
uint32 nvectors;
READ_OR_RETURN(&nvectors, sizeof(uint32));
if (nvectors >0)
{
float *vectorarray = new float[nvectors*3];
READ_OR_RETURN(vectorarray, nvectors*sizeof(float)*3);
for (uint32 i=0; i<nvectors; ++i)
{
vertexArray.push_back( Vector3(vectorarray + 3*i) );
}
delete[] vectorarray;
}
// ----- liquid
WmoLiquid *liquid = 0;
if (liquidflags& 1)
{
WMOLiquidHeader hlq;
READ_OR_RETURN(&blockId, 4);
CMP_OR_RETURN(blockId, "LIQU");
READ_OR_RETURN(&blocksize, sizeof(int));
READ_OR_RETURN(&hlq, sizeof(WMOLiquidHeader));
liquid = new WmoLiquid(hlq.xtiles, hlq.ytiles, Vector3(hlq.pos_x, hlq.pos_y, hlq.pos_z), hlq.type);
uint32 size = hlq.xverts*hlq.yverts;
READ_OR_RETURN(liquid->GetHeightStorage(), size*sizeof(float));
size = hlq.xtiles*hlq.ytiles;
READ_OR_RETURN(liquid->GetFlagsStorage(), size);
}
groupsArray.push_back(GroupModel(mogpflags, GroupWMOID, AABox(Vector3(bbox1), Vector3(bbox2))));
groupsArray.back().setMeshData(vertexArray, triangles);
groupsArray.back().setLiquidData(liquid);
// drop of temporary use defines
#undef READ_OR_RETURN
#undef CMP_OR_RETURN
}
fclose(rf);
// write WorldModel
WorldModel model;
model.setRootWmoID(RootWMOID);
if (groupsArray.size())
{
model.setGroupModels(groupsArray);
success = model.writeFile(iDestDir + "/" + pModelFilename + ".vmo");
}
//std::cout << "readRawFile2: '" << pModelFilename << "' tris: " << nElements << " nodes: " << nNodes << std::endl;
return success;
}
/*!
*/
Vector2D
Bhv_ChainAction::getKeepBallVel( const WorldModel & wm )
{
static GameTime s_update_time( 0, 0 );
static Vector2D s_best_ball_vel( 0.0, 0.0 );
if ( s_update_time == wm.time() )
{
return s_best_ball_vel;
}
s_update_time = wm.time();
//
//
//
const int ANGLE_DIVS = 12;
const ServerParam & SP = ServerParam::i();
const PlayerType & ptype = wm.self().playerType();
const double collide_dist2 = std::pow( ptype.playerSize()
+ SP.ballSize(),
2 );
const double keep_dist = ptype.playerSize()
+ ptype.kickableMargin() * 0.5
+ ServerParam::i().ballSize();
const Vector2D next_self_pos
= wm.self().pos() + wm.self().vel();
const Vector2D next2_self_pos
= next_self_pos
+ wm.self().vel() * ptype.playerDecay();
//
// create keep target point
//
Vector2D best_ball_vel = Vector2D::INVALIDATED;
int best_opponent_step = 0;
double best_ball_speed = 1000.0;
for ( int a = 0; a < ANGLE_DIVS; ++a )
{
Vector2D keep_pos
= next2_self_pos
+ Vector2D::from_polar( keep_dist,
360.0/ANGLE_DIVS * a );
if ( keep_pos.absX() > SP.pitchHalfLength() - 0.2
|| keep_pos.absY() > SP.pitchHalfWidth() - 0.2 )
{
continue;
}
Vector2D ball_move = keep_pos - wm.ball().pos();
double ball_speed = ball_move.r() / ( 1.0 + SP.ballDecay() );
Vector2D max_vel
= KickTable::calc_max_velocity( ball_move.th(),
wm.self().kickRate(),
wm.ball().vel() );
if ( max_vel.r2() < std::pow( ball_speed, 2 ) )
{
continue;
}
Vector2D ball_next_next = keep_pos;
Vector2D ball_vel = ball_move.setLengthVector( ball_speed );
Vector2D ball_next = wm.ball().pos() + ball_vel;
if ( next_self_pos.dist2( ball_next ) < collide_dist2 )
{
ball_next_next = ball_next;
ball_next_next += ball_vel * ( SP.ballDecay() * -0.1 );
}
#ifdef DEBUG_PRINT
dlog.addText( Logger::TEAM,
__FILE__": (getKeepBallVel) %d: ball_move th=%.1f speed=%.2f max=%.2f",
a,
ball_move.th().degree(),
ball_speed,
max_vel.r() );
dlog.addText( Logger::TEAM,
__FILE__": __ ball_next=(%.2f %.2f) ball_next2=(%.2f %.2f)",
ball_next.x, ball_next.y,
ball_next_next.x, ball_next_next.y );
#endif
//
// check opponent
//
int min_step = 1000;
for ( PlayerPtrCont::const_iterator o = wm.opponentsFromSelf().begin();
o != wm.opponentsFromSelf().end();
++o )
{
if ( (*o)->distFromSelf() > 10.0 )
{
break;
}
int o_step = FieldAnalyzer::predict_player_reach_cycle( *o,
ball_next_next,
(*o)->playerTypePtr()->kickableArea(),
0.0, // penalty distance
1, // body count thr
1, // default turn step
0, // wait cycle
true );
if ( o_step <= 0 )
{
break;
}
if ( o_step < min_step )
{
min_step = o_step;
}
}
#ifdef DEBUG_PRINT
dlog.addText( Logger::TEAM,
__FILE__": (getKeepBallVel) %d: keepPos=(%.2f %.2f)"
" ballNext2=(%.2f %.2f) ballVel=(%.2f %.2f) speed=%.2f o_step=%d",
a,
keep_pos.x, keep_pos.y,
ball_next_next.x, ball_next_next.y,
ball_vel.x, ball_vel.y,
ball_speed,
min_step );
#endif
if ( min_step > best_opponent_step )
{
best_ball_vel = ball_vel;
best_opponent_step = min_step;
best_ball_speed = ball_speed;
}
else if ( min_step == best_opponent_step )
{
if ( best_ball_speed > ball_speed )
{
best_ball_vel = ball_vel;
best_opponent_step = min_step;
best_ball_speed = ball_speed;
}
}
}
s_best_ball_vel = best_ball_vel;
return s_best_ball_vel;
}
/*!
*/
void
PredictState::init( const WorldModel & wm )
{
//
// initialize world
//
M_world = &wm;
//
// initialize spend_time
//
M_spend_time = 0;
//
// initialize ball pos & vel
//
M_ball.assign( wm.ball().pos(), wm.ball().vel() );
//
// initialize self_unum
//
M_self_unum = wm.self().unum();
//
// initialize ball holder
//
const AbstractPlayerObject * h = wm.getTeammateNearestToBall( VALID_PLAYER_THRESHOLD );
if ( h
&& wm.ball().pos().dist2( h->pos() ) < wm.ball().pos().dist2( wm.self().pos() ) )
{
M_ball_holder_unum = h->unum();
}
else
{
M_ball_holder_unum = wm.self().unum();
}
//
// initialize all teammates
//
M_our_players.reserve( 11 );
for ( int n = 1; n <= 11; ++n )
{
PredictPlayerObject::Ptr ptr;
if ( n == M_self_unum )
{
ptr = PredictPlayerObject::Ptr( new PredictPlayerObject( wm.self() ) );
}
else
{
const AbstractPlayerObject * t = wm.ourPlayer( n );
if ( t )
{
ptr = PredictPlayerObject::Ptr( new PredictPlayerObject( *t ) );
}
else
{
ptr = PredictPlayerObject::Ptr( new PredictPlayerObject() );
}
}
M_our_players.push_back( ptr );
#ifndef STRICT_LINE_UPDATE
if ( ptr->isValid()
&& M_our_offense_player_line_x < ptr->pos().x )
{
M_our_offense_player_line_x = ptr->pos().x;
}
#endif
}
updateLines();
}
void pointandclick(){
Lidar lidar;
lidar.init("lidar/lidarconfig.conf");
LidarManager lidarManager;
lidarManager.lidar=&lidar;
lidarManager.init("lidar/lidarpositionconfig.conf");
WorldModel model;
model.init("visionworldmodelconfig.conf");
model.lidar=&lidarManager;
Network network;
model.network=&network;
printf("made network\n");
Camera a;
a.init(0);
a.loadCalib("vision/calibration/Intrinsic.xml","vision/calibration/Distortion.xml","vision/calibration/H.xml");
CameraManager manager;
manager.init();
manager.addCam(&a);
manager.startThread();
printf("made camera\n");
VisionInterpreter vision= VisionInterpreter(&manager);
printf("pre init\n");
vision.init("vision/visioninterpreter.conf");
printf("post init\n");
vision.manager=&manager;
model.vision=&vision;
printf("made vision interpreter\n");
PointAndClickDisplay display=PointAndClickDisplay(&model);
printf("made all objects\n");
//start timer
TimeKeeper::start_time();
int key = 0;
long start=0;
try {
while((key&255)!=27){
start=TimeKeeper::GetTime();
cout<<"update model\n";
model.updateModel();
cout<<"update disp\n";
display.displayModel();
key=cvWaitKey(3);
}
// cout<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRot<<endl;
// cout<<TimeKeeper::GetTime()-start<<endl;
}
catch(...) {
cerr << "we are catching an uncaught exception. bad things happened" << endl;
}
}
/*!
*/
void
ShootGenerator::evaluateCourses( const WorldModel & wm )
{
const double y_dist_thr2 = std::pow( 8.0, 2 );
const ServerParam & SP = ServerParam::i();
const PlayerObject * goalie = wm.getOpponentGoalie();
const AngleDeg goalie_angle = ( goalie
? ( goalie->pos() - M_first_ball_pos ).th()
: 180.0 );
const Container::iterator end = M_courses.end();
for ( Container::iterator it = M_courses.begin();
it != end;
++it )
{
double score = 1.0;
if ( it->kick_step_ == 1 )
{
score += 50.0;
}
if ( it->goalie_never_reach_ )
{
score += 100.0;
}
if ( it->opponent_never_reach_ )
{
score += 100.0;
}
double goalie_rate = 1.0;
if ( goalie )
{
#if 1
double variance2 = ( it->goalie_never_reach_
? 1.0 // 1.0*1.0
: std::pow( 10.0, 2 ) );
double angle_diff = ( it->ball_move_angle_ - goalie_angle ).abs();
goalie_rate = 1.0 - std::exp( - std::pow( angle_diff, 2 )
/ ( 2.0 * variance2 ) );
#else
double angle_diff = ( it->ball_move_angle_ - goalie_angle ).abs();
goalie_rate = 1.0 - std::exp( - std::pow( angle_diff * 0.1, 2 )
// / ( 2.0 * 90.0 * 0.1 ) );
// / ( 2.0 * 40.0 * 0.1 ) ); // 2009-07
// / ( 2.0 * 90.0 * 0.1 ) ); // 2009-12-13
/ ( 2.0 * 20.0 * 0.1 ) ); // 2010-06-09
#endif
}
double y_rate = 1.0;
if ( it->target_point_.dist2( M_first_ball_pos ) > y_dist_thr2 )
{
double y_dist = std::max( 0.0, it->target_point_.absY() - 4.0 );
y_rate = std::exp( - std::pow( y_dist, 2.0 )
/ ( 2.0 * std::pow( SP.goalHalfWidth() - 1.5, 2 ) ) );
}
#ifdef DEBUG_PRINT_EVALUATE
dlog.addText( Logger::SHOOT,
"(shoot eval) %d: score=%f(%f) pos(%.2f %.2f) speed=%.3f goalie_rate=%f y_rate=%f",
it->index_,
score * goalie_rate * y_rate, score,
it->target_point_.x, it->target_point_.y,
it->first_ball_speed_,
goalie_rate,
y_rate );
#endif
score *= goalie_rate;
score *= y_rate;
it->score_ = score;
}
}
