Bool state_hashed( State S )
{
int i, sum, index;
StateHashEntry *h;
sum = state_sum( S );
index = sum & STATE_HASH_BITS;
h = lstate_hash_entry[index];
for ( i = 0; i < lnum_state_hash_entry[index]; i++ ) {
if ( h->sum != sum ) {
h = h->next;
continue;
}
if ( same_state( &(h->S), &S ) ) {
return TRUE;
}
h = h->next;
}
return FALSE;
}
Bool ehc_state_hashed( State *S )
{
int i, sum, index;
EhcHashEntry *h;
sum = state_sum( S );
index = sum & EHC_HASH_BITS;
h = lehc_hash_entry[index];
for ( i = 0; i < lnum_ehc_hash_entry[index]; i++ ) {
if ( h->sum != sum ) {
h = h->next;
continue;
}
if ( same_state( &(h->ehc_node->S), S ) ) {
return TRUE;
}
h = h->next;
}
return FALSE;
}
void hash_ehc_node( EhcNode *n )
{
int i, sum, index;
EhcHashEntry *h, *prev = NULL;
sum = state_sum( &(n->S) );
index = sum & EHC_HASH_BITS;
h = lehc_hash_entry[index];
if ( !h ) {
h = new_EhcHashEntry();
h->sum = sum;
h->ehc_node = n;
lehc_hash_entry[index] = h;
lnum_ehc_hash_entry[index]++;
if ( !lchanged_ehc_entry[index] ) {
lchanged_ehc_entrys[lnum_changed_ehc_entrys++] = index;
lchanged_ehc_entry[index] = TRUE;
}
return;
}
i = 0;
while ( h ) {
if ( i == lnum_ehc_hash_entry[index] ) {
break;
}
i++;
prev = h;
h = h->next;
}
if ( h ) {
/* current list end is still in allocated list of hash entrys
*/
h->sum = sum;
h->ehc_node = n;
lnum_ehc_hash_entry[index]++;
if ( !lchanged_ehc_entry[index] ) {
lchanged_ehc_entrys[lnum_changed_ehc_entrys++] = index;
lchanged_ehc_entry[index] = TRUE;
}
return;
}
/* allocated list ended; connect a new hash entry to it.
*/
h = new_EhcHashEntry();
h->sum = sum;
h->ehc_node = n;
prev->next = h;
lnum_ehc_hash_entry[index]++;
if ( !lchanged_ehc_entry[index] ) {
lchanged_ehc_entrys[lnum_changed_ehc_entrys++] = index;
lchanged_ehc_entry[index] = TRUE;
}
return;
}
/* apply the observation update to the belief state
* limiting the application to <radius> distance to edge <e>
*/
void state_observation_update (dijk_graph_t *dg, dijk_edge_t *e, int radius, navlcm_feature_list_t *f, GQueue *path, double *variance)
{
if (!e || !e->start)
return;
// convert to dual tree
GNode *tr = dijk_to_tree (e->start, radius);
// apply observation update to the tree
g_node_traverse (tr, G_PRE_ORDER, G_TRAVERSE_ALL, -1, state_observation_cb, f);
// compute variance across tree
*variance = .0;
g_node_traverse (tr, G_PRE_ORDER, G_TRAVERSE_ALL, -1, state_compute_variance_cb, variance);
*variance /= radius;
// destroy tree
g_node_destroy (tr);
// set to zero for all nodes out of the tree
for (GList *iter=g_queue_peek_head_link(dg->nodes);iter;iter=iter->next) {
dijk_node_t *nd = (dijk_node_t*)iter->data;
if (nd->timestamp != f->utime) {
nd->pdf0 = .0;
nd->pdf1 = .0;
}
}
// hack: set to zero for all nodes out of the path
for (GList *iter=g_queue_peek_head_link(dg->nodes);iter;iter=iter->next) {
dijk_node_t *nd = (dijk_node_t*)iter->data;
if (!path) continue;
gboolean on_path = FALSE;
for (GList *eiter=g_queue_peek_head_link (path);eiter;eiter=eiter->next) {
dijk_edge_t *e = (dijk_edge_t*)eiter->data;
if (e->start == nd || e->end == nd) {
on_path = TRUE;
}
}
if (!on_path) {
// nd->pdf0 = .0;
// nd->pdf1 = .0;
}
}
// normalize
double t = state_sum (dg);
if (t > 1E-6) {
for (GList *iter=g_queue_peek_head_link(dg->nodes);iter;iter=iter->next) {
dijk_node_t *nd = (dijk_node_t*)iter->data;
nd->pdf1 /= t;
}
}
}
Bool do_greedy_heuristic_improvement( void )
{
State S, S_;
int h, h_;
source_to_dest( &S, ginitial_state );
h = get_1P_and_AH( S );
if ( h == INFINITY ) {
printf("\n\nGoals can't be reached -- problem proved unsolvable!\n\n");
exit( 1 );
}
source_to_dest( &(gplan_states[0]), ginitial_state );
gplan_states_sum[0] = state_sum( ginitial_state );
printf("\n\nCueing down from goal distance: %4d", h);
while ( h != 0 ) {
if ( !search_H_for_better_state( S, h, &S_, &h_ ) ) {
/* H space got empty, switch to complete space
*/
if ( gcmd_line.display_info ) {
printf("\n\nH search space got empty !");
printf("\nswitching to complete search space now.\n");
fflush( stdout );
}
get_1P_and_AH( S );
reset_hash_entrys();
if ( !search_A_for_better_state( S, h, &S_, &h_ ) ) {
if ( gcmd_line.display_info ) {
printf("\n\nsearch space got empty !");
printf("\neither - problem is not invertible or");
printf("\n - problem is unsolvable\n\n");
}
return FALSE;
}
}
source_to_dest( &S, S_ );
h = h_;
printf("\n %4d", h);
}
return TRUE;
}
/*add the state S into hash table*/
PlanHashEntry *hash_plan_state( State *S, int step ) {
int sum, index;
PlanHashEntry *h, *tmp;
sum = state_sum( S );
index = sum & PLAN_HASH_BITS;
for ( h = lplan_hash_entry[index]; h; h = h->next ) {
if ( h->sum != sum ) continue;
if ( same_state( S, &(h->S) ) ) break;
}
/*found the same state in the hash table*/
if ( h ) {
if ( h->step != -1 ) {
printf("\n\n[error]:reencountering a state that is already in plan! debug me\n\n");
exit( 1 );
}
h->step = step;
return h;
}
/* not found the same state in hash table*/
/* h points to the last one*/
/* jovi: comments by jovi, seems it is not necessary
* This is for addding a nes state
*/
for ( h = lplan_hash_entry[index]; h && h->next; h = h->next ) ;
tmp = new_PlanHashEntry();
tmp->sum = sum;
copy_source_to_dest( &(tmp->S), S );
tmp->step = step;
if ( h ) {
h->next = tmp;
} else {
lplan_hash_entry[index] = tmp;
}
return tmp;
}
PlanHashEntry *plan_state_hashed(State *S) {
int sum, index;
PlanHashEntry *h;
sum = state_sum(S);
index = sum & PLAN_HASH_BITS;
for (h = lplan_hash_entry[index]; h; h = h->next) {
if (h->sum != sum)
continue;
if (same_state(S, &(h->S)))
break;
}
if (h && h->step != -1) {
return h;
}
return NULL;
}
PlanHashEntry *hash_plan_state( State *S, int step )
{
int sum, index;
PlanHashEntry *h, *tmp;
sum = state_sum( S );
index = sum & PLAN_HASH_BITS;
for ( h = lplan_hash_entry[index]; h; h = h->next ) {
if ( h->sum != sum ) continue;
if ( same_state( S, &(h->S) ) ) break;
}
if ( h ) {
if ( h->step != -1 ) {
printf("\n\nreencountering a state that is already in plan! debug me\n\n");
exit( 1 );
}
h->step = step;
return h;
}
for ( h = lplan_hash_entry[index]; h && h->next; h = h->next );
tmp = new_PlanHashEntry();
tmp->sum = sum;
copy_source_to_dest( &(tmp->S), S );
tmp->step = step;
if ( h ) {
h->next = tmp;
} else {
lplan_hash_entry[index] = tmp;
}
return tmp;
}
void extract_plan_fragment( void )
{
int i, seen;
int ops[MAX_PLAN_LENGTH], num_ops;
num_ops = 0;
for ( i = lspace_start; i != -1; i = lsearch_space[i].father ) {
if ( num_ops == MAX_PLAN_LENGTH ) {
printf("\nincrease MAX_PLAN_LENGTH! currently %d\n\n",
MAX_PLAN_LENGTH);
exit( 1 );
}
ops[num_ops++] = lsearch_space[i].op;
}
for ( i = num_ops - 1; i > -1; i-- ) {
/* SCHNELLERE METHODE, hashing oder UBTree verwenden !
*/
if ( gnum_plan_ops == MAX_PLAN_LENGTH ) {
printf("\nincrease MAX_PLAN_LENGTH! currently %d\n\n",
MAX_PLAN_LENGTH);
exit( 1 );
}
result_to_dest( &(gplan_states[gnum_plan_ops + 1]),
gplan_states[gnum_plan_ops], ops[i] );
gplan_states_sum[gnum_plan_ops + 1] = state_sum( gplan_states[gnum_plan_ops + 1] );
if ( (seen = state_seen( gnum_plan_ops + 1)) != -1 ) {
gnum_plan_ops = seen;
printf("\ngoing back to %d", seen);
continue;
}
gplan_ops[gnum_plan_ops++] = ops[i];
}
}
void hash_state( State S )
{
static Bool first_call = TRUE;
int i, sum, index;
StateHashEntry *h, *prev = NULL;
if ( first_call ) {
for ( i = 0; i < STATE_HASH_SIZE; i++ ) {
lstate_hash_entry[i] = NULL;
lnum_state_hash_entry[i] = 0;
lchanged_entry[i] = FALSE;
}
lnum_changed_entrys = 0;
first_call = FALSE;
}
sum = state_sum( S );
index = sum & STATE_HASH_BITS;
h = lstate_hash_entry[index];
if ( !h ) {
h = new_StateHashEntry();
source_to_dest( &(h->S), S );
h->sum = sum;
lstate_hash_entry[index] = h;
lnum_state_hash_entry[index]++;
if ( !lchanged_entry[index] ) {
lchanged_entrys[lnum_changed_entrys++] = index;
lchanged_entry[index] = TRUE;
}
return;
}
i = 0;
while ( h ) {
if ( i == lnum_state_hash_entry[index] ) {
break;
}
i++;
prev = h;
h = h->next;
}
if ( h ) {
/* current list end is still in allocated list of hash entrys
*/
source_to_dest( &(h->S), S );
h->sum = sum;
lnum_state_hash_entry[index]++;
if ( !lchanged_entry[index] ) {
lchanged_entrys[lnum_changed_entrys++] = index;
lchanged_entry[index] = TRUE;
}
return;
}
/* allocated list ended; connect a new hash entry to it.
*/
h = new_StateHashEntry();
source_to_dest( &(h->S), S );
h->sum = sum;
prev->next = h;
lnum_state_hash_entry[index]++;
if ( !lchanged_entry[index] ) {
lchanged_entrys[lnum_changed_entrys++] = index;
lchanged_entry[index] = TRUE;
}
return;
}
