//
// When we add a new entry to workingArray, there is a chance that we will run into a dead entry.
// If so, we will need to delete the dead entry, by copying the last entry in the array onto its
// location. We then need to recursively add the entry (that we just copied onto that spot) to
// the workingArray
//
static void AddToWorkingArray(int i, TMapStarter *node, short int workingArray[])
{
int j, source, last;
TEntryList *entries;
// Keep an eye out for dead entries. They will be made apparent when two entries both have the same ID.
if (workingArray[node->array[i].ID] == -1)
workingArray[node->array[i].ID] = i;
else {
// The node we are currently looking at is the dead one.
if (node->array[i].distance < node->array[ workingArray[node->array[i].ID] ].distance) {
// Otherwise, remove the source, then remove the entry. Follow with a recursive call.
j = i;
if (node->array[i].parentGen >= 0)
node->array[ workingArray[node->array[i].ID] ].parentGen = node->array[i].parentGen;
}
// The previously entered entry is outdated. Replace it with this newer one.
else {
j = workingArray[node->array[i].ID];
workingArray[node->array[i].ID] = i;
if (node->array[j].parentGen >= 0)
node->array[i].parentGen = node->array[j].parentGen;
}
// The node identified as "j" is dead. Remove its entry from the list of altered squares in the ancestor tree.
l_particleID[node->array[j].ID].total--;
entries = l_particleID[node->array[j].ID].mapEntries;
source = node->array[j].source;
last = l_particleID[node->array[j].ID].total;
if (last != source) {
entries[source].x = entries[last].x;
entries[source].y = entries[last].y;
entries[source].node = entries[last].node;
// Somewhat confusing- we just removed an entry from the list of altered squares maintained by an ancestor particle (entries)
// This means moving an entry from the end of that list to the spot which was vacated (entries[l_particleID[node->array[j].ID].total])
// Therefore, the entry in the map corresponding to that last entry needs to point to the new entry.
lowMap[ entries[source].x ][ entries[source].y ]->array[ entries[source].node ].source = source;
}
// Now remove the node itself
node->total--;
node->dead--;
if (j != node->total) {
node->array[j].parentGen = node->array[node->total].parentGen;
node->array[j].distance = node->array[node->total].distance;
node->array[j].source = node->array[node->total].source;
node->array[j].hits = node->array[node->total].hits;
node->array[j].ID = node->array[node->total].ID;
// We just moved the last entry in the list to position j. Update it's source entry in the ancestry tree to reflect its new position
l_particleID[ node->array[j].ID ].mapEntries[ node->array[j].source ].node = j;
// If the entry we just moved was in workingArray, we need to correct workingArray.
// Also, we know that since it has been entered already, we don't need to enter it again
if (workingArray[node->array[j].ID] == node->total)
workingArray[node->array[j].ID] = j;
else if (i != node->total)
// Final step- add this newly copied node to the working array (we don't want it skipped over)
AddToWorkingArray(j, node, workingArray);
}
}
}
//
// This function is called whenever a grid square in the global map (which has at least one
// observation associated with it) is accessed for the first time in an iteration. This
// function then creates an entry in the observationArray for this location. This
// effectively expands the local map by one grid square, and allows any future accesses to
// this grid square to be completed in constant time. This function itself can take O(P) time.
//
carmen_inline void LowBuildObservation(int x, int y, char usage)
{
TAncestor *lineage;
PAncestor stack[PARTICLE_NUMBER];
short int workingArray[ID_NUMBER+1];
int i, here, topStack;
char flag = 0;
// The size of the observationArray is not large enough- we throw out an error
// message and stop the program
if (observationID >= AREA)
fprintf(stderr, "aRoll over!\n");
// Grab a slot in the observationArray
flagMap[x][y] = observationID;
obsX[observationID] = x;
obsY[observationID] = y;
observationID++;
here = flagMap[x][y];
// Initialize the slot and the ancestor particles
for (i=0; i < ID_NUMBER; i++) {
observationArray[here][i] = -1;
workingArray[i] = -1;
l_particleID[i].seen = 0;
}
// Fill in the particle entries of the array that made direct observations
// to this grid square
for (i=0; i < lowMap[x][y]->total; i++)
AddToWorkingArray(i, lowMap[x][y], workingArray);
// A trick to speed up code when localizing. If an observation has no hits,
// then it has a 0% chance of stopping the laser, regardless of any other
// info. Marking that specially will remove an extra memory call, which
// would almost certainly be a cache miss. Also, if all entries are "empty",
// then maybe we can skip the whole access to the observationArray entirely.
if (usage) {
flag = 1;
for (i=0; i < lowMap[x][y]->total; i++)
if (lowMap[x][y]->array[i].hits > 0)
flag = 0;
else
workingArray[lowMap[x][y]->array[i].ID] = -2;
}
// Fill in the holes in the observation array, by using the value of their parents
for (i=0; i < l_cur_particles_used; i++) {
lineage = l_particle[i].ancestryNode;
topStack = 0;
// Eventually we will either get to an ancestor that we have already seen,
// or we will hit the top of the tree (and thus its parent is NULL)
// We never have to play with the root of the observation tree, because it has no parent
while ((lineage != NULL) && (lineage->seen == 0)) {
// put this ancestor on the stack to look at later
stack[topStack] = lineage;
topStack++;
// Note that we already have seen this ancestor, for later lineage searches
lineage->seen = 1;
lineage = lineage->parent; // Advance to this ancestor's parent
}
// Now trapse back down the stack, filling in each ancestor's info if need be
while (topStack > 0) {
topStack--;
lineage = stack[topStack];
// Try to fill in the holes of UNKNOWN. If the parent is also UNKNOWN, we know by construction
// that all of the other ancestors are also UNKNOWN, and thus the designation is correct
if ((workingArray[lineage->ID] == -1) && (lineage->parent != NULL)) {
workingArray[lineage->ID] = workingArray[lineage->parent->ID];
// If any particle still has an "unobserved" value for this square, we can't use our cheat to
// speed up code.
if (workingArray[lineage->ID] == -1)
flag = 0;
}
}
}
// If we are only localizing right now (usage) and all particles agree that this grid square
// is empty (flag), then any access to this grid square doesn't even have to go as far as the
// observation array- a glance at the flagMap can indicate that the desity is 0, regardless of
// which particle is making the access.
if ((usage) && (flag))
flagMap[x][y] = -2;
else
for (i=0; i < ID_NUMBER; i++)
observationArray[here][i] = workingArray[i];
}
inline void HighBuildObservation(int x, int y, char usage)
{
TAncestor *lineage;
PAncestor stack[H_PARTICLE_NUMBER];
short int workingArray[H_ID_NUMBER+1];
int i, here, topStack;
char flag;
if (observationID >= AREA)
fprintf(stderr, "cRoll over!\n");
// Grab a slot
flagMap[x][y] = observationID;
obsX[observationID] = x;
obsY[observationID] = y;
observationID++;
// Display ownership of this slot
here = flagMap[x][y];
// Initialize the slot and the ancestor particles
for (i=0; i < H_ID_NUMBER; i++)
observationArray[here][i] = -1;
for (i=0; i < H_ID_NUMBER; i++) {
workingArray[i] = -1;
h_particleID[i].seen = 0;
}
if (usage) {
flag = 1;
for (i=0; i < highMap[x][y]->total; i++)
if (highMap[x][y]->array[i].hits > 0)
flag = 0;
}
// Fill in the areas of the array that made direct observations
for (i=0; i < highMap[x][y]->total; i++)
AddToWorkingArray(i, highMap[x][y], workingArray);
// Fill in the holes in the observation array, by using the value of their parents
for (i=0; i < h_cur_particles_used; i++) {
lineage = h_particle[i].ancestryNode;
topStack = 0;
// Eventually we will either get to an ancestor that we have already seen,
// or we will hit the top of the tree (and thus its parent is NULL)
// We never have to play with the root of the observation tree, because it has no parent
while ((lineage != NULL) && (lineage->seen == 0)) {
// put this ancestor on the stack to look at later
stack[topStack] = lineage;
topStack++;
// Note that we already have seen this ancestor, for later lineage searches
lineage->seen = 1;
lineage = lineage->parent; // Advance to this ancestor's parent
}
// Now trapse back down the stack, filling in each ancestor's info if need be
while (topStack > 0) {
topStack--;
lineage = stack[topStack];
// Try to fill in the holes of UNKNOWN. If the parent is also UNKNOWN, we know by construction
// that all of the other ancestors are also UNKNOWN, and thus the designation is correct
if ((workingArray[lineage->ID] == -1) && (lineage->parent != NULL))
workingArray[lineage->ID] = workingArray[lineage->parent->ID];
if (workingArray[lineage->ID] == -1)
flag = 0;
}
}
if ((usage) && (flag))
flagMap[x][y] = -2;
else
for (i=0; i < H_ID_NUMBER; i++)
observationArray[here][i] = workingArray[i];
}
