00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "nel/ai/logic/fo_operator.h"
00021
00022 #include <list>
00023 #include <vector>
00024 #include "nel/ai/logic/ai_assert.h"
00025 #include "nel/ai/logic/varset.h"
00026 #include "nel/ai/logic/valueset.h"
00027 #include "nel/ai/logic/fact.h"
00028 #include "nel/ai/logic/factbase.h"
00029 #include "nel/ai/logic/fo_assert.h"
00030
00031 namespace NLAILOGIC
00032 {
00033 using namespace NLAIAGENT;
00034
00035 CFirstOrderOperator::CFirstOrderOperator()
00036 {
00037 _Comment = NULL;
00038 }
00039
00040 CFirstOrderOperator::CFirstOrderOperator(const CFirstOrderOperator &c)
00041 {
00042 if ( c._Comment )
00043 {
00044 _Comment = new char[ strlen( c._Comment ) ];
00045 strcpy( _Comment, c._Comment );
00046 }
00047 else
00048 _Comment = NULL;
00049
00050 std::vector<IBaseAssert *>::const_iterator it_c = c._Concs.begin();
00051 while ( it_c != c._Concs.end() )
00052 {
00053 _Concs.push_back( *it_c );
00054 it_c++;
00055 }
00056
00057 it_c = c._Conds.begin();
00058 while ( it_c != c._Conds.end() )
00059 {
00060 _Conds.push_back( *it_c );
00061 it_c++;
00062 }
00063
00064 std::vector< std::vector<sint32> >::const_iterator it_li = c._PosVarsCond.begin();
00065 while ( it_li != c._PosVarsCond.end() )
00066 {
00067 std::vector<sint32>::const_iterator it_i = (*it_li).begin();
00068 _PosVarsCond.push_back( std::vector<sint32>() );
00069
00070 while ( it_i != (*it_li).end() )
00071 {
00072 _PosVarsCond.back().push_back( *it_i ) ;
00073 it_i++;
00074 }
00075 it_li++;
00076 }
00077
00078 it_li = c._PosVarsConc.begin();
00079 while ( it_li != c._PosVarsConc.end() )
00080 {
00081 std::vector<sint32>::const_iterator it_i = (*it_li).begin();
00082 _PosVarsConc.push_back( std::vector<sint32>() );
00083
00084 while ( it_i != (*it_li).end() )
00085 {
00086 _PosVarsConc.back().push_back( *it_i ) ;
00087 it_i++;
00088 }
00089 it_li++;
00090 }
00091 }
00092
00094 sint32 CFirstOrderOperator::getVarPos(IBaseVar *var)
00095 {
00096 if ( _Vars.size() )
00097 {
00098 for (sint32 i = 0; i < (sint32)_Vars.size() ; i++ )
00099 {
00100 if ( var->getName() == _Vars[ i ]->getName() )
00101 {
00102 return i;
00103 }
00104 }
00105 }
00106 return -1;
00107 }
00108
00109 CFirstOrderOperator::~CFirstOrderOperator()
00110 {
00111 for ( sint32 i = 0; i < (sint32) _Vars.size() ; i++ )
00112 _Vars[ i ]->release();
00113 }
00114
00115 void CFirstOrderOperator::compileFactPattern(CFactPattern *fp, std::vector<IBaseAssert *>&patterns, std::vector<sint32> &pos_Vars)
00116 {
00117
00118 std::vector<IBaseVar *> *vars_pattern = fp->getVars();
00119 if ( vars_pattern )
00120 {
00121 std::vector<IBaseVar *>::iterator it_cond = vars_pattern->begin();
00122 while ( it_cond != vars_pattern->end() )
00123 {
00124 sint32 id_var = getVarPos( *it_cond );
00125 if ( id_var != -1 )
00126 {
00127 pos_Vars.push_back( id_var );
00128 }
00129 else
00130 {
00131 _Vars.push_back( (IBaseVar *)(*it_cond)->clone() );
00132 pos_Vars.push_back( _Vars.size() - 1);
00133 }
00134 it_cond++;
00135 }
00136 }
00137
00138 for ( sint32 i = 0; i < (sint32) vars_pattern->size(); i++ )
00139 {
00140 (*vars_pattern)[i]->release();
00141 }
00142 delete vars_pattern;
00143 }
00144
00145 void CFirstOrderOperator::addPrecondition(CFactPattern *pattern)
00146 {
00147 if ( pattern->getAssert() )
00148 {
00149 std::vector<sint32> pos_Vars;
00150 compileFactPattern( pattern, _Conds, pos_Vars);
00151
00152
00153 _Conds.push_back( pattern->getAssert() );
00154 _PosVarsCond.push_back( pos_Vars );
00155
00156 }
00157 }
00158
00159 void CFirstOrderOperator::addPostcondition(CFactPattern *pattern)
00160 {
00161 if ( pattern->getAssert() )
00162 {
00163 std::vector<sint32> pos_Vars;
00164 compileFactPattern( pattern, _Conds, pos_Vars);
00165
00166 pattern->getAssert()->addInput( this );
00167 _Concs.push_back( pattern->getAssert() );
00168 _PosVarsConc.push_back( pos_Vars );
00169 }
00170 }
00171
00172
00173
00174 void CFirstOrderOperator::getPosListBackward(sint32 no_conc, sint32 no_cond, std::vector<sint32> &cond_pos)
00175 {
00176 std::vector<sint32>::iterator it_conc = _PosVarsConc[ no_conc ].begin();
00177 while ( it_conc != _PosVarsConc[ no_conc ].end() )
00178 {
00179 std::vector<sint32>::iterator it_cond = _PosVarsCond[ no_cond ].begin();
00180 while ( it_cond != _PosVarsCond[ no_cond ].end() )
00181 {
00182 if ( (*it_conc) == (*it_cond) )
00183 {
00184 cond_pos.push_back( *it_cond );
00185 }
00186 it_cond++;
00187 }
00188 it_conc++;
00189 }
00190 }
00191
00192
00193 void CFirstOrderOperator::getPosListForward(sint32 no_cond, sint32 no_conc, std::vector<sint32> &conc_pos)
00194 {
00195 std::vector<sint32>::iterator it_cond = _PosVarsCond[ no_cond ].begin();
00196 while ( it_cond != _PosVarsCond[ no_cond ].end() )
00197 {
00198 std::vector<sint32>::iterator it_conc = _PosVarsConc[ no_conc ].begin();
00199 while ( it_conc != _PosVarsConc[ no_conc ].end() )
00200 {
00201 if ( (*it_cond) == (*it_conc) )
00202 {
00203 conc_pos.push_back( *it_conc );
00204 }
00205 it_conc++;
00206 }
00207 it_cond++;
00208 }
00209 }
00210
00211 void CFirstOrderOperator::getAssertPos(IBaseAssert *a, std::vector<IBaseAssert *> &l, std::vector<sint32> &pos)
00212 {
00213 for (sint32 i = 0; i < (sint32) l.size() ; i++ )
00214 {
00215 if ( (*(l[i])) == a )
00216 pos.push_back(i);
00217 }
00218 }
00219
00220 std::list<CFactPattern *> *CFirstOrderOperator::forward(CFactPattern *fact)
00221 {
00222 return NULL;
00223 }
00224
00225 CValueSet *CFirstOrderOperator::unifyBackward(std::list<CFact *> &facts)
00226 {
00227 CValueSet *unified = new CValueSet( _Vars.size() );
00228 std::list<CFact *>::iterator it_f = facts.begin();
00229 while ( it_f != facts.end() )
00230 {
00231 std::vector<sint32> pos_assert;
00232 getAssertPos( (*it_f)->getAssert(), _Concs, pos_assert );
00233 for (sint32 pos = 0; pos < (sint32) pos_assert.size(); pos++)
00234 {
00235 for ( sint32 ivar = 0; ivar < (sint32) _PosVarsConc[ pos_assert[pos] ].size(); ivar++ )
00236 {
00237 sint32 l_pos = _PosVarsConc[ pos_assert[pos] ][ivar];
00238
00239 IObjectIA *l_val = (*unified)[ l_pos ];
00240 IObjectIA *r_val = (**it_f)[ ivar ];
00241
00242 if ( !l_val )
00243 {
00244 if ( r_val )
00245 {
00246 unified->setValue( l_pos, r_val );
00247 }
00248 }
00249 else
00250 {
00251 if ( r_val && ( l_val != r_val ) )
00252 {
00253 unified->release();
00254 return NULL;
00255 }
00256 }
00257 }
00258 }
00259 it_f++;
00260 }
00261 return unified;
00262 }
00263
00264 CValueSet *CFirstOrderOperator::unifyForward(std::list<CFact *> &facts)
00265 {
00266 CValueSet *unified = new CValueSet( _Vars.size() );
00267 std::list<CFact *>::iterator it_f = facts.begin();
00268 while ( it_f != facts.end() )
00269 {
00270 std::vector<sint32> pos_assert;
00271 getAssertPos( (*it_f)->getAssert(), _Conds, pos_assert );
00272 for (sint32 pos = 0; pos < (sint32) pos_assert.size(); pos++)
00273 {
00274 for ( sint32 ivar = 0; ivar < (sint32) _PosVarsCond[ pos_assert[pos] ].size(); ivar++ )
00275 {
00276 sint32 l_pos = _PosVarsCond[ pos_assert[pos] ][ivar];
00277
00278 IObjectIA *l_val = (*unified)[ l_pos ];
00279 IObjectIA *r_val = (**it_f)[ ivar ];
00280
00281 if ( !l_val )
00282 {
00283 if ( r_val )
00284 {
00285 unified->setValue( l_pos, r_val );
00286 }
00287 }
00288 else
00289 {
00290 if ( r_val && ( l_val != r_val ) )
00291 {
00292 unified->release();
00293 return NULL;
00294 }
00295 }
00296 }
00297 }
00298 it_f++;
00299 }
00300 return unified;
00301 }
00302
00303 CFact *CFirstOrderOperator::buildFromVars(IBaseAssert *assert, std::vector<sint32> &pl, CValueSet *vars)
00304 {
00305 CFact *result = new CFact( assert);
00306 for (sint32 i = 0; i < (sint32) pl.size() ; i++ )
00307 {
00308 sint32 p = pl[i];
00309 result->setValue( i, (*vars)[ pl[i] ] );
00310 }
00311 return result;
00312 }
00313
00314 std::list<CFact *> *CFirstOrderOperator::backward(std::list<CFact *> &facts)
00315 {
00316 CValueSet *unified = unifyBackward( facts );
00317 std::list<CFact *> *result = new std::list<CFact *>;
00318 for (sint32 i = 0; i < (sint32) _Conds.size(); i++ )
00319 {
00320 CFact *tmp = buildFromVars( _Conds[i], _PosVarsCond[i], unified );
00321 result->push_back( tmp );
00322 #ifdef NL_DEBUG
00323 std::string buffer;
00324 tmp->getDebugString(buffer);
00325 #endif
00326 }
00327 unified->release();
00328 return result;
00329 }
00330
00331 std::list<CFact *> *CFirstOrderOperator::forward(std::list<CFact *> &facts)
00332 {
00333 CValueSet *unified = unifyForward( facts );
00334
00335 #ifdef NL_DEBUG
00336 std::string buf;
00337 unified->getDebugString( buf );
00338 #endif
00339
00340 std::list<CFact *> *result = new std::list<CFact *>;
00341 for (sint32 i = 0; i < (sint32) _Concs.size(); i++ )
00342 {
00343 CFact *tmp = buildFromVars( _Concs[i], _PosVarsConc[i], unified );
00344 result->push_back( tmp );
00345
00346 #ifdef NL_DEBUG
00347 std::string buffer;
00348 tmp->getDebugString(buffer);
00349 #endif
00350 }
00351 unified->release();
00352 return result;
00353 }
00354
00355
00356 std::list<CFact *> *CFirstOrderOperator::propagate(std::list<CFact *> &facts)
00357 {
00358 std::list<CFact *> *conflicts = new std::list<CFact *>;
00359 std::list< CValueSet *> liaisons;
00360 CValueSet *empty = new CValueSet( _Vars.size() );
00361 liaisons.push_back( empty );
00362
00363 std::list<CFact *>::iterator it_f = facts.begin();
00364 while ( it_f != facts.end() )
00365 {
00366 std::vector<sint32> pos_asserts;
00367 getAssertPos( (*it_f)->getAssert() , _Conds, pos_asserts);
00368 for (sint32 i = 0; i < (sint32) pos_asserts.size(); i++ )
00369 {
00370 std::list<CValueSet *> *links = propagate( liaisons, *it_f, _PosVarsCond[ pos_asserts[i] ] );
00371 if ( links )
00372 {
00373 while ( links->size() )
00374 {
00375 for (sint32 i = 0; i < (sint32) _Concs.size(); i++ )
00376 {
00377 CFact *r = buildFromVars( _Concs[i], _PosVarsConc[i], links->front() );
00378 #ifdef NL_DEBUG
00379 std::string buf;
00380 r->getDebugString( buf );
00381 #endif
00382
00383 bool found = false;
00384 std::list<CFact *>::iterator it_c = conflicts->begin();
00385 while ( ! found && it_c != conflicts->end() )
00386 {
00387 found = (**it_c) == *r;
00388 it_c++;
00389 }
00390 if ( !found )
00391 {
00392 #ifdef NL_DEBUG
00393 std::string buf;
00394 r->getDebugString( buf );
00395 #endif
00396 conflicts->push_back( r );
00397 }
00398 }
00399 links->front()->release();
00400 links->pop_front();
00401 }
00402 delete links;
00403 }
00404 }
00405 it_f++;
00406 }
00407
00408 while ( liaisons.size() )
00409 {
00410 liaisons.front()->release();
00411 liaisons.pop_front();
00412 }
00413
00414 return conflicts;
00415 }
00416
00417 CValueSet *CFirstOrderOperator::unifyLiaison( const CValueSet *fp, CValueSet *vals, std::vector<sint32> &pos_vals)
00418 {
00419 CValueSet *result;
00420
00421 if ( result = fp->unify( vals, pos_vals ) )
00422 return result;
00423 else
00424 {
00425 delete result;
00426 return NULL;
00427 }
00428 }
00429
00430 std::list<CValueSet *> *CFirstOrderOperator::propagate(std::list<CValueSet *> &liaisons, CValueSet *fact, std::vector<sint32> &pos_vals)
00431 {
00432 std::list<CValueSet *> *conflits = new std::list<CValueSet *>;
00433 std::list<CValueSet *> buf_liaisons;
00434
00435 std::list< CValueSet *>::iterator it_l = liaisons.begin();
00436
00437 while ( it_l != liaisons.end() )
00438 {
00439
00440 CValueSet *l = *it_l;
00441 #ifdef NL_DEBUG
00442 std::string buf;
00443 l->getDebugString( buf );
00444 #endif
00445
00446 CValueSet *result = unifyLiaison( l, fact, pos_vals );
00447 if ( result )
00448 {
00449 #ifdef NL_DEBUG
00450 std::string buf;
00451 result->getDebugString( buf );
00452 #endif
00453
00454 if ( result->undefined() == 0 )
00455 {
00456 conflits->push_back( result );
00457 }
00458 else
00459 buf_liaisons.push_back( result );
00460 }
00461 it_l++;
00462 }
00463
00464 while ( buf_liaisons.size() )
00465 {
00466 liaisons.push_back( buf_liaisons.front() );
00467 buf_liaisons.pop_front();
00468 }
00469
00470 return conflits;
00471 }
00472
00473 std::list<CFact *> *CFirstOrderOperator::test(std::list<CFact *> &facts)
00474 {
00475 std::list<CFact *> *preconds = backward( facts );
00476
00477 std::list<CFact *>::iterator it_f = preconds->begin();
00478 while ( it_f != preconds->end() )
00479 {
00480 ( (CFirstOrderAssert *)(*it_f)->getAssert() )->backward( *it_f );
00481 it_f++;
00482 }
00483 return NULL;
00484 }
00485
00486
00487 const NLAIC::IBasicType *CFirstOrderOperator::clone() const
00488 {
00489 CFirstOrderOperator *clone = new CFirstOrderOperator( *this );
00490 return clone;
00491 }
00492
00493 const NLAIC::IBasicType *CFirstOrderOperator::newInstance() const
00494 {
00495 CFirstOrderOperator *instance = new CFirstOrderOperator;
00496 return instance;
00497 }
00498
00499 void CFirstOrderOperator::save(NLMISC::IStream &os)
00500 {
00501 }
00502
00503 void CFirstOrderOperator::load(NLMISC::IStream &is)
00504 {
00505 }
00506
00507 void CFirstOrderOperator::getDebugString(std::string &txt) const
00508 {
00509 txt += "Operator:\n -Preconditions:\n";
00510 if ( _Comment )
00511 {
00512 txt += _Comment;
00513 txt += "\n";
00514 }
00515
00516 std::vector<IBaseAssert *>::const_iterator it_a = _Conds.begin();
00517 std::vector<std::vector<sint32> >::const_iterator it_p = _PosVarsCond.begin();
00518 while ( it_a != _Conds.end() )
00519 {
00520 std::string buf;
00521 (*it_a)->getDebugString(buf);
00522 txt += " ( ";
00523 txt += buf;
00524 for (sint32 i = 0; i < (sint32) (*it_p).size(); i++ )
00525 {
00526 txt += _Vars[ (*it_p)[i] ]->getName().getString();
00527 txt += " ";
00528 }
00529 txt += ")\n";
00530
00531 it_a++;
00532 it_p++;
00533 }
00534
00535 txt += " -Postconditions:\n";
00536 it_a = _Concs.begin();
00537 it_p = _PosVarsConc.begin();
00538 while ( it_a != _Concs.end() )
00539 {
00540 std::string buf;
00541 (*it_a)->getDebugString(buf);
00542 txt += " ( ";
00543 txt += buf;
00544 for (sint32 i = 0; i < (sint32) (*it_p).size(); i++ )
00545 {
00546 txt += _Vars[ (*it_p)[i] ]->getName().getString();
00547 txt += " ";
00548 }
00549 txt += ")\n";
00550
00551 it_a++;
00552 it_p++;
00553 }
00554 }
00555
00556 bool CFirstOrderOperator::isEqual(const CFirstOrderOperator &a) const
00557 {
00558 return false;
00559 }
00560
00561 const IObjectIA::CProcessResult &CFirstOrderOperator::run()
00562 {
00563 return IObjectIA::ProcessRun;
00564 }
00565
00566 bool CFirstOrderOperator::isEqual(const IBasicObjectIA &a) const
00567 {
00568 return false;
00569 }
00570
00571 bool CFirstOrderOperator::isTrue() const
00572 {
00573 return false;
00574 }
00575
00576 const NLAIC::CIdentType &CFirstOrderOperator::getType() const
00577 {
00578 return IdCFirstOrderOperator;
00579 }
00580
00581 bool CFirstOrderOperator::operator==(CFirstOrderOperator *)
00582 {
00583 return false;
00584 }
00585
00586 float CFirstOrderOperator::truthValue() const
00587 {
00588 return 0.0;
00589 }
00590
00591
00592 bool CFirstOrderOperator::isValid(CFactBase *fb)
00593 {
00594 std::list<CFact *> *facts = new std::list<CFact *>;
00595 for (sint32 i = 0; i < (sint32) _Conds.size(); i++ )
00596 {
00597 std::list<CFact *> *fa = fb->getAssertFacts( _Conds[i] );
00598 while ( fa->size() )
00599 {
00600 facts->push_back( fa->front() );
00601 fa->pop_front();
00602 }
00603 delete fa;
00604 }
00605 std::list<CFact *> *res = propagate( *facts );
00606 bool is_valid = !res->empty();
00607 while ( res->size() )
00608 {
00609 #ifdef NL_DEBUG
00610 std::string buffer;
00611 res->front()->getDebugString( buffer );
00612 #endif
00613 res->front()->release();
00614 res->pop_front();
00615 }
00616 delete res;
00617
00618 while ( facts->size() )
00619 {
00620 facts->front()->release();
00621 facts->pop_front();
00622 }
00623 delete facts;
00624
00625 return is_valid;
00626 }
00627
00628 float CFirstOrderOperator::priority() const
00629 {
00630 return 0.0;
00631 }
00632
00633 void CFirstOrderOperator::success()
00634 {
00635 }
00636
00637 void CFirstOrderOperator::failure()
00638 {
00639 }
00640
00641 void CFirstOrderOperator::success(IBaseOperator *)
00642 {
00643 }
00644
00645 void CFirstOrderOperator::failure(IBaseOperator *)
00646 {
00647 }
00648
00649 }
