/*
read level & print its map
*/
void PrintLevel( BCINT episode, BCINT mission)
{
ReadLevelData( episode, mission);
/* define map center */
MapCenterX = (MapMinX + MapMaxX) / 2;
MapCenterY = (MapMinY + MapMaxY) / 2;
/* initialize, scale & draw the page */
InitPage( episode, mission, ( UserLvlNm != NULL ? UserLvlNm : LevelName ));
InitScale( MapMaxX - MapMinX, MapMaxY - MapMinY,
( UserLvlNm != NULL ? UserLvlNm : LevelName ));
if (FlagAutoZoom && (FlagName || FlagLegend))
{
BCINT n;
for (n = 0; n < NumVertexes; n++)
/* rough & fine check for covering name or legend areas */
if (Vertexes[ n].y > MapCenterY)
if (CheckScale( Vertexes[ n].x, Vertexes[ n].y))
break; /* for */
if (n < NumVertexes)
AdjustScale( MapMaxX - MapMinX, MapMaxY - MapMinY);
}
PrintMap();
TermPage();
/* clean up & free space */
ForgetLevelData();
}
void TestInsert()
{
map<string, int> wordCount;
//示例插入方式之一，插入pair元素，其余与set类似
wordCount.insert(pair<string, int>("hello", 1));
wordCount.insert(pair<string, int>("world", 3));
wordCount.insert(pair<string, int>("aaa", 7));
wordCount.insert(pair<string, int>("bbb", 1));
PrintMap(wordCount); //遍历map的函数
}
int main(int argc, char const *argv[])
{
PrintMap(sokobanMap, 12,12);
int X = 3, Y = 5;
char userInput;
for (int userStay;;)
{
scanf("%c",&userInput);
getchar();
userStay = UserMove(sokobanMap, X, Y, userInput);
{
if (userStay == MOVE_SUCCESS)
{
system("cls");
PrintMap(sokobanMap, 12,12);
printf("go on\n");
continue;
}
if (userStay == MOVE_FAIL)
{
printf("wrong input\n");
continue;
}
if (userStay == USER_WIN)
{
printf("you win\n");
break;
}
if (userStay == USER_STEP_TO_BOUNDARY)
{
printf("where are you going?\n");
continue;
}
if (userStay == USER_STEP_TO_WALL)
{
printf("are you hurt?\n");
continue;
}
}
}
return 0;
}
void CompareDirectories(TDirectory* dir0, TDirectory* dir1=0, TDirectory* dir2=0)
{
std::map<std::string, std::vector<data_point> > results;
if (dir0!=0) results[dir0->GetName()] = ComputeAverageValueForHists(dir0);
if (dir1!=0) results[dir1->GetName()] = ComputeAverageValueForHists(dir1);
if (dir2!=0) results[dir2->GetName()] = ComputeAverageValueForHists(dir2);
PrintMap(results);
}
void TestMap() {
::printf("TestMap\n");
Map_t map;
map.Insert(1, 101);
map.Insert(1, 1001);
map.Insert(-2, -2);
map.Insert(252, 252);
map.Insert(33, 33);
map.Insert(3342, 3342);
map.Insert(-9, -9);
PrintMap(map);
auto iter = map.Find(-2);
map.Erase(iter);
map.Erase(3342);
map.Insert(44, 44);
map.Insert(-65, -65);
PrintMap(map);
}
// print notes and maps for a given location
void OPObjectNode::PrintLocationNotes(DialectLocation* location,
opSectionStream& stream) {
if (!location) return;
// iterate over all registered types, print them in order
// we should print the default visibility before and after each item
if (location->GetId() == DialectLocation::body)
PrintVisibility(vismode_default, stream);
DialectLocationBase::OrderedNoteList& OrderedNotes =
location->GetOrderedNotes();
DialectLocationBase::OrderedNoteList::iterator i = OrderedNotes.begin();
DialectLocationBase::OrderedNoteList::iterator end = OrderedNotes.end();
while (i != end) {
DialectNoteBase* item = *i;
bool bPrintVisibility = true;
if (DialectNote* noteitem = item->ToNote()) {
// print a note
PrintNote(*location, *noteitem, stream);
bPrintVisibility = !noteitem->GetNoteDefinition()->IsBodyEmpty();
} else if (DialectMap* mapitem = item->ToMap()) {
// print a map
PrintMap(*location, *mapitem, stream);
bPrintVisibility = !mapitem->IsAllNotesEmpty();
}
if (bPrintVisibility) {
if (location->GetId() == DialectLocation::body)
PrintVisibility(vismode_default, stream);
}
++i;
}
}
int process(int argc, char* argv[]) {
v8::V8::InitializeICU();
v8::V8::InitializeExternalStartupData(argv[0]);
v8::Platform* platform = v8::platform::CreateDefaultPlatform();
v8::V8::InitializePlatform(platform);
v8::V8::Initialize();
map<string, string> options;
string file;
ParseOptions(argc, argv, &options, &file);
if (file.empty()) {
fprintf(stderr, "No script was specified.\n");
return 1;
}
ArrayBufferAllocator array_buffer_allocator;
Isolate::CreateParams create_params;
create_params.array_buffer_allocator = &array_buffer_allocator;
Isolate* isolate = Isolate::New(create_params);
Isolate::Scope isolate_scope(isolate);
HandleScope scope(isolate);
Local<String> source;
if (!Read_File(isolate, file.c_str()).ToLocal(&source)) {
fprintf(stderr, "Error reading '%s'.\n", file.c_str());
return 1;
}
JsHttpRequestProcessor processor(isolate, source);
map<string, string> output;
if (!processor.Initialize(&options, &output)) {
fprintf(stderr, "Error initializing processor.\n");
return 1;
}
if (!ProcessEntries(platform, &processor, kSampleSize, kSampleRequests))
return 1;
PrintMap(&output);
return 0;
}
/*
* MenuCommand: A menu item has been selected.
*/
void MenuCommand(HWND hwnd, int id, HWND hwndCtl, UINT codeNotify)
{
// Set last action time to now, to prevent keypress from being interpreted as a game action.
KeySetLastNorepeatTime();
// See if module wants to handle menu selection
if (ModuleEvent(EVENT_MENUITEM, id) == False)
return;
/* Handle menu selections */
switch (id)
{
case ID_GAME_EXIT:
if (connection != CON_NONE && !AreYouSure(hInst, hMain, NO_BUTTON, IDS_LOGOFF))
break;
MainQuit(hwnd);
break;
case ID_GAME_PRINTMAP:
if (state == STATE_GAME)
PrintMap(FALSE);
break;
case ID_GAME_CONNECT:
OfflineConnect();
break;
case ID_GAME_DISCONNECT:
if (!AreYouSure(hInst, hMain, NO_BUTTON, IDS_LOGOFF))
break;
Logoff();
MainSetState(STATE_OFFLINE);
break;
case ID_GAME_SETTINGS:
if (DialogBox(hInst, MAKEINTRESOURCE(IDD_SETTINGS), hMain, PreferencesDialogProc) == IDOK)
{
ModuleEvent(EVENT_CONFIGCHANGED);
MenuDisplaySettings(hMain);
}
break;
case ID_CONFIGMENU:
ConfigMenuLaunch();
break;
case ID_GAME_PASSWORD:
PerformAction(A_CHANGEPASSWORD, NULL);
break;
case ID_OPTIONS_TIMEOUT:
UserSetTimeout();
break;
case ID_OPTIONS_MUSIC:
config.play_music = !config.play_music;
CheckMenuItem(menu, ID_OPTIONS_MUSIC, config.play_music ? MF_CHECKED : MF_UNCHECKED);
UserToggleMusic(config.play_music);
break;
case ID_OPTIONS_SOUND:
config.play_sound = !config.play_sound;
CheckMenuItem(menu, ID_OPTIONS_SOUND, config.play_sound ? MF_CHECKED : MF_UNCHECKED);
if (!config.play_sound)
SoundStopAll();
break;
case ID_OPTIONS_SAVENOW:
SaveSettings();
break;
case ID_OPTIONS_SAVEEXIT:
config.save_settings = !config.save_settings;
CheckMenuItem(menu, ID_OPTIONS_SAVEEXIT, config.save_settings ? MF_CHECKED : MF_UNCHECKED);
break;
case ID_OPTIONS_AREA:
// due to issues with certain D3D drivers, this no longer immediately updates the config
// it now sets a temporary variable that will update the config on shutdown
// this means a shutdown and restart are necessary for window size changes
MessageBox(hMain, "You must shutdown and restart Meridian 59 for these changes to take effect",
"Direct3D", MB_OK);
// config.large_area = !config.large_area;
gLargeArea = !gLargeArea;
CheckMenuItem(menu, ID_OPTIONS_AREA, gLargeArea ? MF_CHECKED : MF_UNCHECKED);
/* if (state == STATE_GAME)
// Send ourselves a resize message
ResizeAll();
RedrawAll();*/
break;
case ID_OPTIONS_FONT_MAP_TITLE: UserSelectFont(FONT_MAP_TITLE); break;
case ID_OPTIONS_FONT_MAP_LABEL: UserSelectFont(FONT_MAP_LABEL); break;
case ID_OPTIONS_FONT_MAP_TEXT: UserSelectFont(FONT_MAP_TEXT); break;
case ID_FONT_GAMETEXT:
UserSelectFont(FONT_EDIT);
break;
case ID_FONT_LIST:
UserSelectFont(FONT_LIST);
break;
case ID_FONT_MAIL:
UserSelectFont(FONT_MAIL);
break;
case ID_FONT_TITLES:
UserSelectFont(FONT_TITLES);
break;
case ID_FONT_STATISTICS:
UserSelectFont(FONT_STATS);
break;
case ID_FONT_INPUT:
UserSelectFont(FONT_INPUT);
break;
case ID_FONT_LABELS:
UserSelectFont(FONT_LABELS);
break;
case ID_FONT_DEFAULTS:
FontsRestoreDefaults();
break;
case ID_COLOR_MAIN:
UserSelectColors(COLOR_FGD, COLOR_BGD);
break;
case ID_COLOR_LIST:
UserSelectColors(COLOR_LISTFGD, COLOR_LISTBGD);
break;
case ID_COLOR_LISTSEL:
UserSelectColors(COLOR_LISTSELFGD, COLOR_LISTSELBGD);
break;
case ID_COLOR_MAGIC:
UserSelectColor(COLOR_ITEM_MAGIC_FG);
break;
case ID_COLOR_HIGHLIGHT:
UserSelectColor(COLOR_HIGHLITE);
break;
case ID_COLOR_MAIL:
UserSelectColors(COLOR_MAILFGD, COLOR_MAILBGD);
break;
case ID_COLOR_TEXT:
UserSelectColors(COLOR_MAINEDITFGD, COLOR_MAINEDITBGD);
break;
case ID_COLOR_EDIT:
UserSelectColors(COLOR_EDITFGD, COLOR_EDITBGD);
break;
case ID_COLOR_SYSMSG:
UserSelectColor(COLOR_SYSMSGFGD);
case ID_COLOR_QUESTHEADER:
UserSelectColor(COLOR_QUEST_HEADER);
break;
break;
case ID_COLOR_STATS:
UserSelectColors(COLOR_STATSFGD, COLOR_STATSBGD);
break;
case ID_COLOR_BAR1:
UserSelectColor(COLOR_BAR1);
break;
case ID_COLOR_BAR2:
UserSelectColor(COLOR_BAR2);
break;
case ID_COLOR_BAR3:
UserSelectColor(COLOR_BAR3);
break;
case ID_COLOR_BAR4:
UserSelectColor(COLOR_BAR4);
break;
case ID_COLOR_INVNUM:
UserSelectColors(COLOR_INVNUMFGD, COLOR_INVNUMBGD);
break;
case ID_COLOR_DEFAULTS:
ColorsRestoreDefaults();
break;
case ID_HELP_CONTENTS:
StartHelp();
break;
case ID_HOMEPAGE:
WebLaunchBrowser(GetString(hInst, IDS_HOMEPAGEURL));
break;
case ID_FORUM:
WebLaunchBrowser(GetString(hInst, IDS_FORUMURL));
break;
case ID_WIKI:
WebLaunchBrowser(GetString(hInst, IDS_WIKIURL));
break;
case ID_HELP_ABOUT:
DialogBox(hInst, MAKEINTRESOURCE(IDD_ABOUT), hMain, AboutDialogProc);
break;
}
}
int main ( int argc, char *argv[] )
{
Rows = atoi(argv[1]);
Cols = atoi(argv[1]);
CellWd = 3.2808399*3000/Cols; /* Cell width (E-W) in feet. */ //1 m = 3.2808 ft
CellHt = 3.2808399*3000/Cols; /* Cell height (N-S) in feet. */ //1 m = 3.2808 ft
/* NOTE 2: Change these to set uniform burning conditions. */
size_t Model = 1; /* NFFL 1 */
double WindSpd = atof(argv[2]); /* m/s */
double WindDir = atof(argv[3]); /* degrees clockwise from north */
double M1 = atof(argv[4]); /* 1-hr dead fuel moisture */
double M10 = 0; /* 10-hr dead fuel moisture */
double M100 = 0; /* 100-hr dead fuel moisture */
double Mherb = 0; /* Live herbaceous fuel moisture */
double Mwood = 0; /* Live woody fuel moisture */
double slp_tmp, asp_tmp; //slope and aspect temporary values
char buffer[100]; //buffer when usedm fgets skips lines
/* neighbor's address*/ //N NE E SE S SW W NW a b c d e f g h
static int nCol[16] = { 0, 1, 1, 1, 0, -1, -1, -1, -1, 1, -2, 2, -2, 2, -1, 1};
static int nRow[16] = { -1, -1, 0, 1, 1, 1, 0, -1, -2, -2, -1, -1, 1, 1, 2, 2};
static int nTimes = 0; /* counter for number of time steps */
FuelCatalogPtr catalog; /* fuel catalog handle */
double nDist[16]; /* distance to each neighbor */
double nAzm[16]; /* compass azimuth to each neighbor (0=N) */
double timeNow; /* current time (minutes) */
double timeNext; /* time of next cell ignition (minutes) */
int row, col, cell; /* row, col, and index of current cell */
int nrow, ncol, ncell; /* row, col, and index of neighbor cell */
int n, cells; /* neighbor index, total number of map cells */
size_t modelNumber; /* fuel model number at current cell */
double moisture[6]; /* fuel moisture content at current cell */
double fpm; /* spread rate in direction of neighbor */
double minutes; /* time to spread from cell to neighbor */
double ignTime; /* time neighbor is ignited by current cell */
int atEdge; /* flag indicating fire has reached edge */
size_t *fuelMap; /* ptr to fuel model map */
double *ignMap; /* ptr to ignition time map (minutes) */
double *flMap; /* ptr to flame length map (feet) */
double *slpMap; /* ptr to slope map (rise/reach) */
double *aspMap; /* ptr to aspect map (degrees from north) */
double *wspdMap; /* ptr to wind speed map (ft/min) */
double *wdirMap; /* ptr to wind direction map (deg from north) */
double *m1Map; /* ptr to 1-hr dead fuel moisture map */
double *m10Map; /* ptr to 10-hr dead fuel moisture map */
double *m100Map; /* ptr to 100-hr dead fuel moisture map */
double *mherbMap; /* ptr to live herbaceous fuel moisture map */
double *mwoodMap; /* ptr to live stem fuel moisture map */
FILE *slope_file, *aspect_file;
printf("Running fireSim with Rows:%d, U:%lf, Dir:%lf\n", Rows, WindSpd, WindDir);
/* NOTE 3: allocate all the maps. */
cells = Rows * Cols;
if ( (ignMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (flMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (slpMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (aspMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (wspdMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (wdirMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (m1Map = (double *) calloc(cells, sizeof(double))) == NULL
|| (m10Map = (double *) calloc(cells, sizeof(double))) == NULL
|| (m100Map = (double *) calloc(cells, sizeof(double))) == NULL
|| (mherbMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (mwoodMap = (double *) calloc(cells, sizeof(double))) == NULL
|| (fuelMap = (size_t *) calloc(cells, sizeof(size_t))) == NULL )
{
fprintf(stderr, "Unable to allocate maps with %d cols and %d rows.\n",
Cols, Rows);
return (1);
}
/* NOTE 4: initialize all the maps -- modify them as you please. */
if ( (slope_file = fopen(argv[5],"r")) == NULL ){
printf("Unable to open output map \"%s\".\n", argv[5]);
return (FIRE_STATUS_ERROR);
}
if ( (aspect_file = fopen(argv[6],"r")) == NULL ){
printf("Unable to open output map \"%s\".\n", argv[6]);
return (FIRE_STATUS_ERROR);
}
/*for (n = 0; n < 6; n++){
fgets(buffer, 100, slope_file);
fgets(buffer, 100, aspect_file);
}
*/
for ( cell=0; cell<cells; cell++ )
{
fscanf(aspect_file, "%lf", &asp_tmp);
fscanf(slope_file, "%lf", &slp_tmp);
slpMap[cell] = slp_tmp/100;
//Slope in firelib is a fraction
asp_tmp = (asp_tmp - 90 < 0) ? //while in Grass is percentage rise/reach.
asp_tmp - 90 + 360 : asp_tmp - 90 ; //Aspect in firelib is N=0 and clockwise
aspMap[cell] = 360 - asp_tmp; //while aspect in Grass is E=0 counter-clockwise
fuelMap[cell] = Model;
wspdMap[cell] = 196.850393701 * WindSpd; /* convert m/s into ft/min */
wdirMap[cell] = WindDir;
m1Map[cell] = M1;
m10Map[cell] = M10;
m100Map[cell] = M100;
mherbMap[cell] = Mherb;
mwoodMap[cell] = Mwood;
ignMap[cell] = INFINITY;
flMap[cell] = 0.;
}
/* NOTE 5: set an ignition time & pattern (this ignites the middle cell). */
cell = floor(Cols/2) + Cols*floor(Rows/2);
ignMap[cell] = 0.0;
/* NOTE 6: create a standard fuel model catalog and a flame length table. */
////////////////////////////////
//Create fuel catalog
//Create 13 + 0 (no fuel model) standard NFFL models and creates space for
//aditional custom model
catalog = Fire_FuelCatalogCreateStandard("Standard", 14);
//Create aditional custom model based on NFFL1
//Only the PARTICLE LOAD is customized at the moment
if ( Fire_FuelModelCreate (
catalog, //FuelCatalogData instance
14, //fuel model number
"CUSTOM", //Name
"Custom Fuel model", //longer description
0.197, //bed depth (ft)
Fuel_Mext(catalog, 1), //moisture of extinction (dl)
Fuel_SpreadAdjustment(catalog, 1), //spread adjustment factor (dl)
1) != FIRE_STATUS_OK ) //maximum number of particles
{
fprintf(stderr, "%s\n", FuelCat_Error(catalog));
Fire_FuelCatalogDestroy(catalog);
return (NULL);
}
//Add a particle to the custom model nº 14
if ( Fire_FuelParticleAdd (
catalog, // FuelCatalogData instance pointer
14, //Custom fuel model id
Fuel_Type(catalog,1,0),
0.23, // Custom particle load (lbs/ft2)
3500, // surface-area-to-volume ratio (ft2/ft3)
Fuel_Density(catalog,1,0), //density (lbs/ft3)
Fuel_Heat(catalog,1,0), //heat of combustion (btus/lb)
Fuel_SiTotal(catalog,1,0), //total silica content (lb/lb)
Fuel_SiEffective(catalog,1,0)) //effective silica content (lb/lb)
!= FIRE_STATUS_OK )
{
fprintf(stderr, "%s\n", FuelCat_Error(catalog));
Fire_FuelCatalogDestroy(catalog);
return (NULL);
}
Fire_FlameLengthTable(catalog, 500, 0.1);
/* Calculate distance across cell to each neighbor and its azimuth. */
for ( n=0; n < 16; n++ ) {
nDist[n] = sqrt ( nCol[n] * CellWd * nCol[n] * CellWd
+ nRow[n] * CellHt * nRow[n] * CellHt );
if (n < 8)
nAzm[n] = n * 45.;
else {
nAzm[n] = atanf( (nCol[n] * CellWd) / (nRow[n] * CellHt) );
if ( nCol[n] > 0 && nRow[n] < 0) //1st quadrant
nAzm[n] = RadToDeg( fabs( nAzm[n] ) );
if ( nCol[n] > 0 && nRow[n] > 0) //2st quadrant
nAzm[n] = 180. - RadToDeg( nAzm[n] ) ;
if ( nCol[n] < 0 && nRow[n] > 0) //3st quadrant
nAzm[n] = RadToDeg( fabs( nAzm[n] ) )+ 180.;
if ( nCol[n] < 0 && nRow[n] < 0) //4st quadrant
nAzm[n] = 360. - RadToDeg( fabs( nAzm[n] ));
}
}
/* NOTE 7: find the earliest (starting) ignition time. */
for ( timeNext=INFINITY, cell=0; cell<cells; cell++ )
{
if ( ignMap[cell] < timeNext )
timeNext = ignMap[cell];
}
/* NOTE 8: loop until no more cells can ignite or fire reaches an edge. */
atEdge = 0;
//while ( timeNext < INFINITY && ! atEdge )
while ( timeNext < INFINITY)
{
timeNow = timeNext;
timeNext = INFINITY;
nTimes++;
/* NOTE 9: examine each ignited cell in the fuel array. */
for ( cell=0, row=0; row<Rows; row++ )
{
for ( col=0; col<Cols; col++, cell++ )
{
/* Skip this cell if it has not ignited. */
if ( ignMap[cell] > timeNow )
{
/* NOTE 12: first check if it is the next cell to ignite. */
if ( ignMap[cell] < timeNext )
timeNext = ignMap[cell];
continue;
}
/* NOTE 10: flag if the fire has reached the array edge. */
if ( row==0 || row==Rows-1 || col==0 || col==Cols-1 )
atEdge = 1;
/* NOTE 11: determine basic fire behavior within this cell. */
modelNumber = fuelMap[cell];
moisture[0] = m1Map[cell];
moisture[1] = m10Map[cell];
moisture[2] = m100Map[cell];
moisture[3] = m100Map[cell];
moisture[4] = mherbMap[cell];
moisture[5] = mwoodMap[cell];
Fire_SpreadNoWindNoSlope(catalog, modelNumber, moisture);
Fire_SpreadWindSlopeMax(catalog, modelNumber, wspdMap[cell],
wdirMap[cell], slpMap[cell], aspMap[cell]);
/* NOTE 12: examine each unignited neighbor. */
for ( n=0; n<16; n++ )
{
/* First find the neighbor's location. */
nrow = row + nRow[n];
ncol = col + nCol[n];
if ( nrow<0 || nrow>=Rows || ncol<0 || ncol>=Cols )
continue;
ncell = ncol + nrow*Cols;
/* Skip this neighbor if it is already ignited. */
if ( ignMap[ncell] <= timeNow )
continue;
/* Determine time to spread to this neighbor. */
Fire_SpreadAtAzimuth(catalog, modelNumber, nAzm[n], FIRE_NONE);
if ( (fpm = Fuel_SpreadAny(catalog, modelNumber)) > Smidgen)
{
minutes = nDist[n] / fpm;
/* Assign neighbor the earliest ignition time. */
if ( (ignTime = timeNow + minutes) < ignMap[ncell] )
{
ignMap[ncell] = ignTime;
Fire_FlameScorch(catalog, modelNumber, FIRE_FLAME);
flMap[ncell] = Fuel_FlameLength(catalog,modelNumber);
}
/* Keep track of next cell ignition time. */
if ( ignTime < timeNext )
timeNext = ignTime;
}
} /* next neighbor n */
} /* next source col */
} /* next source row */
} /* next time */
printf("There were %d time steps ending at %3.2f minutes (%3.2f hours).\n",
nTimes, timeNow, timeNow/60.);
/* NOTE 13: save the ignition & flame length maps. */
PrintMap(aspMap,"aspect.Map");
PrintMap(slpMap,"slope.Map");
PrintMap(ignMap, "ign.Map");
PrintMap(flMap, "flame.Map");
return (0);
}
//
// The main function for performing SLAM at the low level. The first argument will return
// whether there is still information to be processed by SLAM (set to 1). The second and third
// arguments return the corrected odometry for the time steps, and the corresponding list of
// observations. This can be used for the higher level SLAM process when using hierarchical SLAM.
//
void LowSlam(TPath **path, TSenseLog **obs)
{
int cnt;
int i, j, overflow = 0;
char name[32];
TPath *tempPath;
TSenseLog *tempObs;
TAncestor *lineage;
// Initialize the worldMap
LowInitializeWorldMap();
// Initialize the ancestry and particles
cleanID = ID_NUMBER - 2; // ID_NUMBER-1 is being used as the root of the ancestry tree.
// Initialize all of our unused ancestor particles to look unused.
for (i = 0; i < ID_NUMBER; i++) {
availableID[i] = i;
l_particleID[i].generation = -1;
l_particleID[i].numChildren = 0;
l_particleID[i].ID = -1;
l_particleID[i].parent = NULL;
l_particleID[i].mapEntries = NULL;
l_particleID[i].path = NULL;
l_particleID[i].seen = 0;
l_particleID[i].total = 0;
l_particleID[i].size = 0;
}
// Initialize the root of our ancestry tree.
l_particleID[ID_NUMBER-1].generation = 0;
l_particleID[ID_NUMBER-1].numChildren = 1;
l_particleID[ID_NUMBER-1].size = 0;
l_particleID[ID_NUMBER-1].total = 0;
l_particleID[ID_NUMBER-1].ID = ID_NUMBER-1;
l_particleID[ID_NUMBER-1].parent = NULL;
l_particleID[ID_NUMBER-1].mapEntries = NULL;
// Create all of our starting particles at the center of the map.
for (i = 0; i < PARTICLE_NUMBER; i++) {
l_particle[i].ancestryNode = &(l_particleID[ID_NUMBER-1]);
l_particle[i].x = MAP_WIDTH / 2;
l_particle[i].y = MAP_HEIGHT / 2;
l_particle[i].theta = 0.001;
l_particle[i].probability = 0;
children[i] = 0;
}
// We really only use the first particle, since they are all essentially the same.
l_particle[0].probability = 1;
l_cur_particles_used = 1;
children[0] = SAMPLE_NUMBER;
// We don't need to initialize the savedParticles, since Localization will create them for us, and they first are used in
// UpdateAncestry, which is called after Localization. This statement isn't necessary, then, but serves as a sort of placeholder
// when reading the code.
cur_saved_particles_used = 0;
overflow = 1;
for (i=0; i < START_ITERATION; i++)
ReadLog(readFile, logfile_index, sense);
curGeneration = 0;
// Add the first thing that you see to the worldMap at the center. This gives us something to localize off of.
ReadLog(readFile, logfile_index, sense);
AddToWorldModel(sense, 0);
curGeneration = 1;
// Make a record of what the first odometry readings were, so that we can compute relative movement across time steps.
lastX = odometry.x;
lastY = odometry.y;
lastTheta = odometry.theta;
// Get our observation log started.
(*obs) = (TSenseLog *)malloc(sizeof(TSenseLog));
for (i=0; i < SENSE_NUMBER; i++) {
(*obs)->sense[i].distance = sense[i].distance;
(*obs)->sense[i].theta = sense[i].theta;
}
(*obs)->next = NULL;
cnt = 0;
while (curGeneration < LEARN_DURATION) {
// Collect information from the data log. If either reading returns 1, we've run out of log data, and
// we need to stop now.
if (ReadLog(readFile, logfile_index, sense) == 1)
overflow = 0;
else
overflow = 1;
// We don't necessarily want to use every last reading that comes in. This allows us to make certain that the
// robot has moved at least a minimal amount (in terms of meters and radians) before we try to localize and update.
if ((sqrt(SQUARE(odometry.x - lastX) + SQUARE(odometry.y - lastY)) < 0.10) && (fabs(odometry.theta - lastTheta) < 0.04))
overflow = 0;
if (overflow > 0) {
overflow--;
// Wipe the slate clean
LowInitializeFlags();
// Apply the localization procedure, which will give us the N best particles
Localize(sense);
// Add these maintained particles to the FamilyTree, so that ancestry can be determined, and then prune dead lineages
UpdateAncestry(sense, l_particleID);
// Update the observation log (used only by hierarchical SLAM)
tempObs = (*obs);
while (tempObs->next != NULL)
tempObs = tempObs->next;
tempObs->next = (TSenseLog *)malloc(sizeof(TSenseLog));
if (tempObs->next == NULL) fprintf(stderr, "Malloc failed in making a new observation!\n");
for (i=0; i < SENSE_NUMBER; i++) {
tempObs->next->sense[i].distance = sense[i].distance;
tempObs->next->sense[i].theta = sense[i].theta;
}
tempObs->next->next = NULL;
curGeneration++;
// Remember these odometry readings for next time. This is what lets us know the incremental motion.
lastX = odometry.x;
lastY = odometry.y;
lastTheta = odometry.theta;
}
}
// Find the most likely particle. Return its path
j = 0;
for (i=0; i < l_cur_particles_used; i++)
if (l_particle[i].probability > l_particle[j].probability)
j = i;
(*path) = NULL;
i = 0;
lineage = l_particle[j].ancestryNode;
while ((lineage != NULL) && (lineage->ID != ID_NUMBER-1)) {
tempPath = lineage->path;
i++;
while (tempPath->next != NULL) {
i++;
tempPath = tempPath->next;
}
tempPath->next = (*path);
(*path) = lineage->path;
lineage->path = NULL;
lineage = lineage->parent;
}
// Print out the map.
sprintf(name, "map");
j = 0;
for (i = 0; i < l_cur_particles_used; i++)
if (l_particle[i].probability > l_particle[j].probability)
j = i;
PrintMap(name, l_particle[j].ancestryNode, FALSE, -1, -1, -1);
sprintf(name, "rm map.ppm");
system(name);
// Clean up the memory being used.
DisposeAncestry(l_particleID);
LowDestroyMap();
}