Пример #1
0
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");
    
}
Пример #2
0
    //=================================================================
    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;
    }
Пример #3
0
/*!

 */
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
    }
}
Пример #4
0
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;
}
Пример #5
0
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());
}
Пример #6
0
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 );
		}
	}
}
Пример #7
0
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;
  }
  
}
Пример #8
0
/*!

 */
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 ) );
}
Пример #9
0
/*!

 */
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 );
}
Пример #11
0
    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;
    }
Пример #12
0
/*!

 */
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

}
Пример #14
0
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());
}
Пример #15
0
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 );
}
Пример #16
0
 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();
 }
Пример #17
0
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 );
}
Пример #19
0
/*!

*/
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();
    }
}
Пример #20
0
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;
}
Пример #21
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();
    }
Пример #22
0
    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();
    }
Пример #23
0
/*!

 */
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" );
}
Пример #24
0
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;
}
Пример #25
0
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();
}
Пример #26
0
	//=================================================================
	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;
	}
Пример #27
0
/*!

 */
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();
}
Пример #29
0
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;
  }
}
Пример #30
0
/*!

 */
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;
    }
}