- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/rpAutomaton.cpp

    00001 //=============================================================================
    00002 //
    00003 // Copyright (c) 2001           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Author:                      Seung H. Chung
    00010 // Organization:        Model-based Embedded and Robotic Systems Group
    00011 //                                      Massachusetts Institute of Technology
    00012 //
    00013 // rpAutomaton class
    00014 //
    00015 //=============================================================================
    00016
    00017 #include <cstdio>
    00018 #include <cassert>
    00019
    00020 #include "rpCCAutomata.h"
    00021 #include "rpAutomaton.h"
    00022 #include "rpTransitions.h"
    00023 #include "rpState.h"
    00024 #include "rpStatePtrs.h"
    00025
    00027
    00028 using Mers::Models::CCA::CCAAutomaton;
    00029 using Mers::Models::CCA::CCAStates;
    00030 using Mers::Models::CCA::CCAState;
    00031 using Mers::Models::CCA::CCATransitions;
    00032 using Mers::Models::CCA::CCAAssignments;
    00033 using Mers::Models::CCA::CCAModel;
    00034 using Mers::Models::CCA::CCAVariable;
    00035 using Mers::Models::CCA::CCAValue;
    00036 using Mers::Utils::Lang::String;
    00037
    00039 // Constructors, Copy Constructors, & Destructors
    00041
    00042 rpAutomaton::rpAutomaton() {
    00043         _automaton_p            = NULL;
    00044         _rpCCAutomata_p         = NULL;
    00045         _rpStates                       = rpStates();
    00046         _current_rpState_p      = NULL;
    00047         _goal_rpState_p         = NULL;
    00048         _rpTransitions          = rpTransitions();
    00049         _rpPolicies                     = rpPolicies();
    00050 }
    00051 rpAutomaton::rpAutomaton(const CCAAutomaton &_automaton) {
    00052         _automaton_p            = &_automaton;
    00053         _rpCCAutomata_p         = NULL;
    00054         _current_rpState_p      = NULL;
    00055         _goal_rpState_p         = NULL;
    00056         if (_automaton.states_p() != NULL) {
    00057                 _rpStates               = rpStates(_automaton.states_p()->getLength());
    00058                 _rpPolicies             = rpPolicies(_automaton.states_p()->getLength());
    00059                 CCAStates::const_iterator
    00060                         states_const_it = _automaton.states_p()->begin();
    00061                 unsigned int i;
    00062                 // Insert each state into rpStates with the corresponding i.
    00063                 // And set each rpState in rpStates the rpAutomaton it belongs to.
    00064                 for(i = 0; i < _rpStates.size(); i++, states_const_it++) {
    00065                         _rpStates[i] = rpState(*states_const_it, i);
    00066                         _rpStates[i].set_rpAutomaton_p(*this);
    00067                 }
    00068                 // Insert transitions into rpTransitions.
    00069                 if (_automaton.transitions_p() != NULL) {
    00070                         CCATransitions::const_iterator  transitions_const_it =
    00071                                 _automaton.transitions_p()->begin();
    00072                         rpStatePtrs                             _rpStateConditions;
    00073                         CCAAssignments                  _rpControlConditions(false);
    00074                         //unsigned int                  numb_of_state_conditions = 0;
    00075                         //clause::const_iterator        clause_const_it;
    00076                         _rpTransitions  = rpTransitions(_automaton.transitions_p()->getLength());
    00077                         // Insert each transition into rpTransitions
    00078                         for(i = 0; i < _rpTransitions.size();
    00079                         i++, transitions_const_it++) {
    00080                                 // Create rpTransition and insert it into rpTransitoins.
    00081                                 // Record where it is inserted.
    00082                                 _rpTransitions[i] =
    00083                                         rpTransition(*transitions_const_it,
    00084                                         *_rpStates.rpState_p(
    00085                                                 *transitions_const_it->from_state_p()->value_p()),
    00086                                         *_rpStates.rpState_p(
    00087                                                 *transitions_const_it->to_state_p()->value_p()));
    00088                                 // Insert the new rpTransition_p into rpStates' in and out
    00089                                 // rpTransitionPtrs.
    00090                                 _rpTransitions[i].to_rpState_p()->
    00091                                         insert_in_rpTransition(_rpTransitions[i]);
    00092                                 _rpTransitions[i].from_rpState_p()->
    00093                                         insert_out_rpTransition(_rpTransitions[i]);
    00094                         }
    00095                 } else {
    00096                         _rpTransitions  = rpTransitions();
    00097                 }
    00098         } else {
    00099                 _rpStates               = rpStates();
    00100                 _rpTransitions  = rpTransitions();
    00101                 _rpPolicies             = rpPolicies();
    00102         }
    00103 }
    00104 rpAutomaton::rpAutomaton(const rpAutomaton &other) {
    00105         copy(other);
    00106 }
    00107 rpAutomaton::~rpAutomaton() {
    00108         destroy();
    00109 }
    00110 //------------------------------------------------------------------------------
    00111 // Public Function Members
    00112 //------------------------------------------------------------------------------
    00113 const CCAModel *const rpAutomaton::model_p() const {
    00114         if (_automaton_p != NULL) {
    00115                 return _automaton_p->model_p();
    00116         } else {
    00117                 return NULL;
    00118         }
    00119 }
    00120 const rpCCAutomata *const rpAutomaton::rpCCAutomata_p() const {
    00121         return _rpCCAutomata_p;
    00122 }
    00123 //------------------------------------------------------------------------------
    00124 const String rpAutomaton::name() const {
    00125         if (_automaton_p != NULL) {
    00126                 return _automaton_p->name();
    00127         } else {
    00128                 return "";
    00129         }
    00130 }
    00131 const CCAVariable *const rpAutomaton::variable_p() const {
    00132         if (_automaton_p != NULL) {
    00133                 return _automaton_p->variable_p();
    00134         } else {
    00135                 return NULL;
    00136         }
    00137 }
    00138 const rpStates *const rpAutomaton::rpStates_p() const {
    00139         return &_rpStates;
    00140 }
    00141 const rpState *const rpAutomaton::rpStates_p(const unsigned int index_) const {
    00142         return &_rpStates[index_];
    00143 }
    00144 const rpState *const rpAutomaton::rpState_p(const CCAValue &_value) const {
    00145         return _rpStates.rpState_p(_value);
    00146 }
    00147 const rpState *const rpAutomaton::current_rpState_p() const {
    00148         return _current_rpState_p;
    00149 }
    00150 const rpState *const rpAutomaton::goal_rpState_p() const {
    00151         return _goal_rpState_p;
    00152 }
    00153 const rpTransitions *const rpAutomaton::rpTransitions_p() const {
    00154         return &_rpTransitions;
    00155 }
    00156 const rpPolicies *const rpAutomaton::rpPolicies_p() const {
    00157         return &_rpPolicies;
    00158 }
    00159 const rpTransition *const
    00160 rpAutomaton::rpTransition_p(const rpState &goal_rpState) const {
    00161         return _rpPolicies.rpPolicy(*_current_rpState_p, goal_rpState);
    00162 }
    00163 const unsigned int rpAutomaton::topological_numb() const {
    00164         return _automaton_p->topological_numb();
    00165 }
    00166 //------------------------------------------------------------------------------
    00167 void rpAutomaton::set_current_rpState(const rpState &_rpState) {
    00168         _current_rpState_p = &_rpState;
    00169 }
    00170 void rpAutomaton::set_goal_rpState(const rpState &_rpState) {
    00171         _goal_rpState_p = &_rpState;
    00172 }
    00173 void rpAutomaton::set_rpCCAutomata_p(const rpCCAutomata &_rpCCAutomata) {
    00174         _rpCCAutomata_p = &_rpCCAutomata;
    00175 }
    00176 /*void rpAutomaton::insert_rpTransition(const rpTransition &_rpTransition) {
    00177         if (_rpTransition.rpAutomaton_p() == this) {
    00178                 _rpTransitions.insert_rpTransition(_rpTransition);
    00179                 _rpStates.insert_in_rpTransition(
    00180                         *_rpTransition.to_rpState_p(), _rpTransition);
    00181                 _rpStates.insert_out_rpTransition(
    00182                         *_rpTransition.from_rpState_p(), _rpTransition);
    00183         }
    00184 }*/
    00185 void rpAutomaton::reset_rpTransition_conditions() {
    00186         for (unsigned int i = 0; i < _rpTransitions.size(); i++) {
    00187                 _rpTransitions[i].reset_conditions();
    00188         }
    00189 }
    00190 const bool rpAutomaton::update_status(const bool predecessor_status_changed,
    00191                                                                           const bool initializing) {
    00192         // First update the status of the transitions.
    00193         // Note: the transitions status needs to be updated only if the predecessor
    00194         // status has changed. Since the status is updated from the cause to
    00195         // effect, if the predecessor's status has not changed, then the transition
    00196         // status should not change. This is so due to the fact that the transition
    00197         // conditions should be on only itself and predecessors.
    00198         bool transition_status_changed  = false;
    00199         bool changed                                    = false;
    00200         bool state_status_changed               = false;
    00201         unsigned int i;
    00202         if (predecessor_status_changed || initializing) {
    00203                 for (i = 0; i < _rpTransitions.size(); i++) {
    00204                         changed = false;
    00205                         changed = _rpTransitions[i].update_status();
    00206                         transition_status_changed = changed || transition_status_changed;
    00207                 }
    00208         }
    00209         // Check if the current state status requires updating.
    00210         bool current_state_status_update_required = false;
    00211         if (_current_rpState_p != NULL) {
    00212                 if (_current_rpState_p->reachability() == rpState::NEVER ||
    00213                         _current_rpState_p->reachability() == rpState::UNKNOWN) {
    00214                         current_state_status_update_required = true;
    00215                 }
    00216         }
    00217         // Update state stutus if necessary.
    00218         if ((initializing || transition_status_changed ||
    00219                 current_state_status_update_required) &&
    00220                 _current_rpState_p != NULL) {
    00221                 // First clear the reachability of each state by setting the
    00222                 // reachability of each state to UNKNOWN.
    00223                 for (i = 0; i < _rpStates.size(); i++) {
    00224                         _rpStates[i].set_reachability(rpState::UNKNOWN);
    00225                 }
    00226                 compute_reachability();
    00227                 // mark unreachable state; do it in compute reachabiliyt();
    00228                 state_status_changed = true;
    00229         }
    00230         return (transition_status_changed || state_status_changed);
    00231 }
    00232 void rpAutomaton::clear() {
    00233         _automaton_p            = NULL;
    00234         _rpCCAutomata_p         = NULL;
    00235         _rpStates.clear();
    00236         _current_rpState_p      = NULL;
    00237         _goal_rpState_p         = NULL;
    00238         _rpTransitions.clear();
    00239         _rpPolicies.clear();
    00240 }
    00241 //------------------------------------------------------------------------------
    00242 // Public Operators
    00243 //------------------------------------------------------------------------------
    00244 const rpAutomaton &rpAutomaton::operator =(const rpAutomaton &other) {
    00245         if (this != &other) {
    00246                 destroy();
    00247                 copy(other);
    00248         }
    00249         return *this;
    00250 }
    00251 const bool rpAutomaton::operator ==(const rpAutomaton &other) const {
    00252         if (_rpStates           == other._rpStates              &&
    00253                 _rpTransitions  == other._rpTransitions) {
    00254                 return true;
    00255         } else {
    00256                 return false;
    00257         }
    00258 }
    00259 const bool rpAutomaton::operator !=(const rpAutomaton &other) const {
    00260         return !(*this == other);
    00261 }
    00262 const bool rpAutomaton::operator >(const rpAutomaton &other) const {
    00263         if (topological_numb() > other.topological_numb()) {
    00264                 return true;
    00265         } else {
    00266                 return false;
    00267         }
    00268 }
    00269 const bool rpAutomaton::operator <(const rpAutomaton &other) const {
    00270         if (topological_numb() < other.topological_numb()) {
    00271                 return true;
    00272         } else {
    00273                 return false;
    00274         }
    00275 }
    00276 const bool rpAutomaton::operator >=(const rpAutomaton &other) const {
    00277         if (topological_numb() >= other.topological_numb()) {
    00278                 return true;
    00279         } else {
    00280                 return false;
    00281         }
    00282 }
    00283 const bool rpAutomaton::operator <=(const rpAutomaton &other) const {
    00284         if (topological_numb() <= other.topological_numb()) {
    00285                 return true;
    00286         } else {
    00287                 return false;
    00288         }
    00289 }
    00290 std::ostream &
    00291 operator <<(std::ostream &_ostream, const rpAutomaton &_rpAutomaton) {
    00292         _ostream << "Automaton: "       << _rpAutomaton.name()                  << std::endl;
    00293         _ostream << "---------"                                                                         << std::endl;
    00294         _ostream << *_rpAutomaton.rpStates_p()                                          << std::endl;
    00295         _ostream << "Transitions: "                                                                     << std::endl;
    00296         _ostream << *_rpAutomaton.rpTransitions_p()                                     << std::endl;
    00297         _ostream << "Policies: "                                                                        << std::endl;
    00298         _ostream << *_rpAutomaton.rpPolicies_p();
    00299         return _ostream;
    00300 }
    00301 //------------------------------------------------------------------------------
    00302 // Private Function Members
    00303 //------------------------------------------------------------------------------
    00304 /* Much optimization can be done here. For example, compute policies for
    00305    multpile goals at once, etc. */
    00306 void rpAutomaton::update_policies() {
    00307         unsigned int                    i, j;
    00308         const rpTransition *    rpTransition_p_;
    00309         rpStatePtrList                  rpStatePtr_queue;
    00310         _rpPolicies.clear();
    00311         for (i = 0; i < _rpStates.size(); i++) {
    00312                 if (_rpStates[i].reachability() != rpState::NEVER) {
    00313                         rpStatePtr_queue.push_back(&_rpStates[i]);
    00314                         while (!rpStatePtr_queue.empty()) {
    00315                                 for (j = 0;
    00316                                 j < rpStatePtr_queue.front()->in_rpTransitionPtrs_p()->size();
    00317                                 j++) {
    00318                                         rpTransition_p_ =
    00319                                                 rpStatePtr_queue.front()->in_rpTransitionPtrs(j);
    00320                                         if (rpTransition_p_->current_status() ==
    00321                                                         rpTransition::ALLOWED &&
    00322                                                 rpTransition_p_->from_rpState_p() !=
    00323                                                         &_rpStates[i] &&
    00324                                                 rpTransition_p_->from_rpState_p()->reachability() !=
    00325                                                         rpState::NEVER &&
    00326                                                 _rpPolicies.rpPolicy(*rpTransition_p_->from_rpState_p(),
    00327                                                                                          _rpStates[i]) == NULL) {
    00328                                                 _rpPolicies.set_rpPolicy(
    00329                                                         *rpTransition_p_->from_rpState_p(), _rpStates[i],
    00330                                                         *rpTransition_p_);
    00331                                                 rpStatePtr_queue.push_back(
    00332                                                         rpTransition_p_->from_rpState_p());
    00333                                         }
    00334                                 }
    00335                                 rpStatePtr_queue.pop_front();
    00336                         }
    00337                 }
    00338         }
    00339 }
    00340 void rpAutomaton::compute_reachability() {
    00341         unsigned int i;
    00342         // First, compute the strongly connected components starting from the
    00343         // current state.
    00344         compute_SCC();
    00345         // -------------------------------------------------------------------------
    00346         // Changes to make: consider marking all paths that lead to the first
    00347         //                                      nominal state. This will make the initially reachable
    00348         //                                      states complete.  Currently only a single path
    00349         //                                      to a nominal state is recorded. Expanding this to
    00350         //                                      all path marking requires the ability to keep track of
    00351         //                                      predecessor list instead of a single predecessor.
    00352         // Second, compute first reachable nominal assignments by searching
    00353         // depth-first.
    00354         rpStatePtrs _nearest_nominal_rpStatePtrs =
    00355                 nearest_nominal_rpStatePtrs(_rpStates[_current_rpState_p->index()]);
    00356         //--------------------------------------------------------------------------
    00357         // Now that all first nominal states have been found, mark reachability for
    00358         // each state.
    00359
    00360         // If the nearest_nominal_rpStatePtrList is empty, there are no nominal
    00361         // states that can be reached from the current state. Only the strongly
    00362         // connected component of the current state should be always reachable.
    00363         // The rest are never reachable.
    00364         if (_nearest_nominal_rpStatePtrs.empty()) {
    00365                 set_SCC_always_reachable(_rpStates[_current_rpState_p->index()]);
    00366         // Else, if only one nominal state is in nearest_nominal_rpStatePtrList,
    00367         // the strongly connected component of the nominal state is always
    00368         // reachable, the path from the current state to this nominal state is
    00369         // initially reachable, and the rest are never reachable.
    00370         } else if (_nearest_nominal_rpStatePtrs.size() == 1) {
    00371                 set_SCC_always_reachable(
    00372                         _rpStates[_nearest_nominal_rpStatePtrs[0]->index()]);
    00373                 set_predecessors_initially_reachable(
    00374                         *_nearest_nominal_rpStatePtrs[0]);
    00375         // Otherwise multiple nearest nominal states exist.
    00376         } else {
    00377                 bool                    all_in_same_SCC = true;
    00378                 // If all are in the same strongly connected component, set the
    00379                 // strongly connected component as always reachable, set predecessors
    00380                 // as initially reachable, and rest never reachable.
    00381                 for (i = 0; i < _nearest_nominal_rpStatePtrs.size(); i++) {
    00382                         if (_nearest_nominal_rpStatePtrs[i]->SCC_root_p() !=
    00383                                 _nearest_nominal_rpStatePtrs[0]->SCC_root_p()) {
    00384                                 all_in_same_SCC = false;
    00385                                 break;
    00386                         }
    00387                 }
    00388                 // If all nearest nominal states are in the same strongly connnected
    00389                 // component, all in the strongly connected component are always
    00390                 // reachable, all predecessors of the nearest nominal states are
    00391                 // intially reachable, and the rest are unreachable.
    00392                 if (all_in_same_SCC) {
    00393                         // Set the strongly connected component of nearest nominal states
    00394                         // always reachable.
    00395                         set_SCC_always_reachable(
    00396                                 _rpStates[_nearest_nominal_rpStatePtrs[0]->index()]);
    00397                         // Loop through the nearest nominal states and set the predecessors
    00398                         // initially reachable.
    00399                         for (i = 0; i < _nearest_nominal_rpStatePtrs.size(); i++) {
    00400                                 set_predecessors_initially_reachable(
    00401                                         *_nearest_nominal_rpStatePtrs[i]);
    00402                         }
    00403                 // Otherwise, the nearest nominal states are in more than one stongly
    00404                 // connected components.
    00405                 } else {
    00406                         // Look for a single strongly connected component from which all
    00407                         // other strongly connected components can be reached.
    00408                         const rpState *root_rpState_p =
    00409                                 root_rpState_p_of_rpStatePtrs(_nearest_nominal_rpStatePtrs);
    00410                         // If there is no root then only mark the strongly connected
    00411                         // component of the current state always reachable and the rest
    00412                         // never reachable.
    00413                         if (root_rpState_p == NULL) {
    00414                                 set_SCC_always_reachable(
    00415                                         _rpStates[_current_rpState_p->index()]);
    00416                         // Since there is a single root from which all other strongly
    00417                         // connected components are reachable, set all states with the same
    00418                         // SCC_root_p as the root always reachable, all predecessors to
    00419                         // the root initially reachable, and the rest unreachable.
    00420                         } else {
    00421                                 set_SCC_always_reachable(_rpStates[root_rpState_p->index()]);
    00422                                 for (i = 0; i < _nearest_nominal_rpStatePtrs.size(); i++) {
    00423                                         if (_nearest_nominal_rpStatePtrs[i]->SCC_root_p() ==
    00424                                                 root_rpState_p->SCC_root_p()) {
    00425                                                 set_predecessors_initially_reachable(
    00426                                                         *_nearest_nominal_rpStatePtrs[i]);
    00427                                         }
    00428                                 }
    00429                         }
    00430                 }
    00431         }
    00432         // Finally, set all unknown reachability states never reachable.
    00433         for (i = 0; i < _rpStates.size(); i++) {
    00434                 if (_rpStates[i].reachability() == rpState::UNKNOWN) {
    00435                         _rpStates[i].set_reachability(rpState::NEVER);
    00436                 }
    00437         }
    00438         // Remove all predecessors that were assigned during SCC computation;
    00439         reset_predecessors_set_in_SCC_computation();
    00440 }
    00441 const rpState *
    00442 rpAutomaton::root_rpState_p_of_rpStatePtrs(rpStatePtrs &_rpStatePtrs) {
    00443         unsigned int i, j;
    00444         bool all_rpStatePtrs_marked;
    00445         // Sort _rpStatePtrs (i.e. in increasing order of depth-first number).
    00446         if (_rpStatePtrs.size() > 0) {
    00447                 _rpStatePtrs.sort();
    00448                 for (i = 0; i < _rpStatePtrs.size(); i++) {
    00449                         if (i == 0 ||
    00450                                 _rpStatePtrs[i]->SCC_root_p() !=
    00451                                 _rpStatePtrs[i-1]->SCC_root_p()) {
    00452                                 all_rpStatePtrs_marked = true;
    00453                                 mark_rpStates_depth_first(_rpStates[_rpStatePtrs[i]->index()]);
    00454                                 for (j = 0; j <_rpStatePtrs.size(); i++) {
    00455                                         if (!_rpStatePtrs[j]->marked()) {
    00456                                                 all_rpStatePtrs_marked = false;
    00457                                                 break;
    00458                                         }
    00459                                 }
    00460                                 unmark_rpStates_depth_first(
    00461                                         _rpStates[_rpStatePtrs[i]->index()]);
    00462                                 if (all_rpStatePtrs_marked) return _rpStatePtrs[i];
    00463                         }
    00464                 }
    00465                 return NULL;
    00466         } else {
    00467                 return NULL;
    00468         }
    00469 }
    00470 void rpAutomaton::mark_rpStates_depth_first(rpState &_rpState) {
    00471         // If _rpState is not marked, _rpState must be the root of the
    00472         // search tree. Mark it so that if the root is found on it's branches,
    00473         // it will be ignored.
    00474         if (!_rpState.marked()) {
    00475                 _rpState.mark();
    00476         }
    00477         for (unsigned int i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00478                 // Search only along the "allowed" trasition path and only if it is
    00479                 // not marked (i.e. the state hasn't been visited)
    00480                 if (_rpState.out_rpTransitionPtrs(i)->current_status() ==
    00481                         rpTransition::ALLOWED &&
    00482                         !_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->marked()) {
    00483                         // Mark it
    00484                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->mark();
    00485                         mark_rpStates_depth_first(
    00486                                 *_rpState.out_rpTransitionPtrs(i)->to_rpState_p());
    00487                 }
    00488         }
    00489 }
    00490 void rpAutomaton::unmark_rpStates_depth_first(rpState &_rpState) {
    00491         // If _rpState is marked, _rpState must be the root of the
    00492         // search tree. Unmark it so that if the root is found on it's branches,
    00493         // it will be ignored.
    00494         if (_rpState.marked()) {
    00495                 _rpState.unmark();
    00496         }
    00497         for (unsigned int i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00498                 // Search only along the "allowed" trasition path and only if it is
    00499                 // marked (i.e. the state hasn't been visited)
    00500                 if (_rpState.out_rpTransitionPtrs(i)->current_status() ==
    00501                         rpTransition::ALLOWED &&
    00502                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->marked()) {
    00503                         // Unmark it
    00504                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->mark();
    00505                         unmark_rpStates_depth_first(
    00506                                 *_rpState.out_rpTransitionPtrs(i)->to_rpState_p());
    00507                 }
    00508         }
    00509 }
    00510 const rpStatePtrs
    00511 rpAutomaton::nearest_nominal_rpStatePtrs(rpState &_rpState) const {
    00512         rpStatePtrList _nearest_nominal_rpStatePtrList;
    00513         depth_first_search_nearest_nominal_rpStatePtrs(
    00514                 _rpState, _nearest_nominal_rpStatePtrList);
    00515         rpStatePtrs             _nearest_nominal_rpStatePtrs(
    00516                                                 _nearest_nominal_rpStatePtrList.size());
    00517         for (unsigned int i = 0; i < _nearest_nominal_rpStatePtrs.size(); i++) {
    00518                 _nearest_nominal_rpStatePtrs[i] =
    00519                         _nearest_nominal_rpStatePtrList.front();
    00520                 _nearest_nominal_rpStatePtrList.pop_front();
    00521         }
    00522         return _nearest_nominal_rpStatePtrs;
    00523 }
    00524 // Search depth-first recursively all nominal states nearest to the _rpState.
    00525 void rpAutomaton::depth_first_search_nearest_nominal_rpStatePtrs(
    00526         rpState &_rpState, rpStatePtrList &nearest_nominal_rpStatePtrList) const {
    00527         // If the predecessor of _rpState is NULL, _rpState must be the root of the
    00528         // search tree. Mark it's predecessor as itself so that if the root is found
    00529         // on it's branches, it will be ignored.
    00530         if (_rpState.predecessor_p() == NULL) {
    00531                 _rpState.set_predecessor(_rpState);
    00532                 // Check if the root itself is a nominal state.
    00533                 if (_rpState.type() == CCAState::NOMINAL) {
    00534                                 nearest_nominal_rpStatePtrList.push_front(&_rpState);
    00535                                 return;
    00536                         }
    00537         }
    00538         for (unsigned int i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00539                 // Search only along the "allowed" trasition path and only if the
    00540                 // predecessor is NUll (i.e. the state hasn't been visited)
    00541                 if (_rpState.out_rpTransitionPtrs(i)->current_status() ==
    00542                         rpTransition::ALLOWED &&
    00543                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->predecessor_p() ==
    00544                         NULL) {
    00545                         // Mark the predecessor.
    00546                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00547                                 set_predecessor(_rpState);
    00548                         // Push into the search stack.
    00549                         if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->type() ==
    00550                                 CCAState::NOMINAL) {
    00551                                         nearest_nominal_rpStatePtrList.push_front(
    00552                                                 _rpState.out_rpTransitionPtrs(i)->to_rpState_p());
    00553                         } else {
    00554                                 depth_first_search_nearest_nominal_rpStatePtrs(
    00555                                         *_rpState.out_rpTransitionPtrs(i)->to_rpState_p(),
    00556                                         nearest_nominal_rpStatePtrList);
    00557                         }
    00558                 }
    00559         }
    00560 }
    00561 // Search depth-first recursively to set all states with the same SCC_root_p as
    00562 // _rpState always reachable.
    00563 void rpAutomaton::set_SCC_always_reachable(rpState &_rpState) {
    00564         // If _rpState's reachability is unknwon, start by setting _rpState always
    00565         // reachable.
    00566         if (_rpState.reachability() == rpState::UNKNOWN) {
    00567                 _rpState.set_reachability(rpState::ALWAYS);
    00568         }
    00569         unsigned int i;
    00570         for (i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00571                 // Set always reachable if the reachability is unknown and if
    00572                 // it is in the same strongly connected components.
    00573                 if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->reachability() ==
    00574                         rpState::UNKNOWN &&
    00575                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->SCC_root_p() ==
    00576                         _rpState.SCC_root_p()) {
    00577                         // Set always reachable.
    00578                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->set_reachability(
    00579                                 rpState::ALWAYS);
    00580                         // Recursively set the child states with same SCC_root_p always
    00581                         // reachable.
    00582                         set_SCC_always_reachable(*_rpState.out_rpTransitionPtrs(i)->
    00583                                                                          to_rpState_p());
    00584                 }
    00585         }
    00586 }
    00587 /*void rpAutomaton::set_SCC_always_reachable(rpState &_rpState) {
    00588         rpStatePtrList search_rpStatePtrList;
    00589         rpState *               parent_rpState_p;
    00590         rpState *               child_rpState_p;
    00591         unsigned int    i;
    00592         // Start from current state.
    00593         _rpStates[_rpState.index()].set_reachability(rpState::ALWAYS);
    00594         search_rpStatePtrList.push_front(&_rpStates[_rpState.index()]);
    00595         while (!search_rpStatePtrList.empty()) {
    00596                 parent_rpState_p = search_rpStatePtrList.front();
    00597                 search_rpStatePtrList.pop_front();
    00598                 // Search all outgoing edges.
    00599                 for (i = 0;     i < parent_rpState_p->out_rpTransitionPtrs_p()->size();
    00600                 i++) {
    00601                         child_rpState_p = parent_rpState_p->out_rpTransitionPtrs(i)->
    00602                                                           to_rpState_p();
    00603                         // Set always reachable if the reachability is unknown and if
    00604                         // it is in the same strongly connected components.
    00605                         if (child_rpState_p->reachability() == rpState::UNKNOWN &&
    00606                                 child_rpState_p->SCC_root_p() ==
    00607                                 parent_rpState_p->SCC_root_p()) {
    00608                                 // Set always reachable.
    00609                                 child_rpState_p->set_reachability(rpState::ALWAYS);
    00610                                 // Push into the search stack to search its outgoing edges.
    00611                                 search_rpStatePtrList.push_front(child_rpState_p);
    00612                         }
    00613                 }
    00614         }
    00615 }*/
    00616 // Changes to make: Should change it such that it can handle multiple
    00617 //                                      predecessors for a single state.
    00618 void
    00619 rpAutomaton::set_predecessors_initially_reachable(const rpState &_rpState) {
    00620         const rpState *predecessor_p = _rpState.predecessor_p();
    00621         while (predecessor_p                    != NULL &&
    00622 //                 predecessor_p->type()        == state::UNKNOWN) {
    00623                    predecessor_p->reachability()        == rpState::UNKNOWN) {
    00624                 _rpStates[predecessor_p->index()].set_reachability(rpState::INITIALLY);
    00625                 predecessor_p = predecessor_p->predecessor_p();
    00626         }
    00627 }
    00628 void rpAutomaton::compute_SCC() {
    00629         unsigned int    count = 1;
    00630         rpStatePtrList _rpStatePtrList;
    00631         unsigned int i;
    00632         for (i = 0; i < _rpStates.size(); i++) {
    00633                 _rpStates[i].set_depth_first_numb(0);
    00634                 _rpStates[i].set_lowlink_numb(0);
    00635                 _rpStates[i].reset_SCC_root();
    00636         }
    00637         SCC_search(_rpStates[_current_rpState_p->index()], _rpStatePtrList, count);
    00638 }
    00639 void rpAutomaton::SCC_search(rpState &_rpState,
    00640                                                          rpStatePtrList &_rpStatePtrList,
    00641                                                          unsigned int &count) {
    00642         unsigned int    i;
    00643         // Set the current vertex depth-first and lowlink number to current count.
    00644         _rpState.set_depth_first_numb(count);
    00645         _rpState.set_lowlink_numb(count);
    00646         // Increment count.
    00647         count++;
    00648         // Push current vertex into the stack.
    00649         _rpStatePtrList.push_front(&_rpState);
    00650         // Loop on each vertex connected by an outgoing edge from the current
    00651         // vertex.
    00652         for (i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00653                 // Only consider the allowed transitions
    00654                 if (_rpState.out_rpTransitionPtrs(i)->current_status() ==
    00655                         rpTransition::ALLOWED) {
    00656                         // If the vertex has not been visited then do the following.
    00657                         // Note: depth-first number = 0 implies not visited yet.
    00658                         if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00659                                 depth_first_numb() == 0) {
    00660                                 // Recurively search on the other vertices.
    00661                                 SCC_search(*_rpState.out_rpTransitionPtrs(i)->to_rpState_p(),
    00662                                                    _rpStatePtrList,
    00663                                                    count);
    00664                                 // Set current vertex lowlink number to the lower of it's
    00665                                 // lowlink and other's lowlink number.
    00666                                 if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00667                                         lowlink_numb() < _rpState.lowlink_numb()) {
    00668                                         _rpState.set_lowlink_numb(_rpState.out_rpTransitionPtrs(i)->
    00669                                                 to_rpState_p()->lowlink_numb());
    00670                                 }
    00671                         // This other vertex has been visited already.
    00672                         } else if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00673                                 depth_first_numb() < _rpState.depth_first_numb() &&
    00674                                 _rpStatePtrList.is_in_list(&_rpState)) {
    00675                                 if (_rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00676                                         depth_first_numb() < _rpState.lowlink_numb()) {
    00677                                         _rpState.set_lowlink_numb(_rpState.out_rpTransitionPtrs(i)->
    00678                                                                                           to_rpState_p()->
    00679                                                                                           depth_first_numb());
    00680                                 }
    00681                         }
    00682                 }
    00683         }
    00684         if (_rpState.lowlink_numb() == _rpState.depth_first_numb()) {
    00685                 do {
    00686                         i = _rpStatePtrList.front()->index();
    00687                         _rpStatePtrList.front()->set_SCC_root(_rpState);
    00688                         _rpStatePtrList.pop_front();
    00689                 } while (i != _rpState.index());
    00690         }
    00691 }
    00692 void rpAutomaton::reset_predecessors_set_in_SCC_computation() {
    00693         depth_first_reset_predecessors(_rpStates[_current_rpState_p->index()]);
    00694 }
    00695 void rpAutomaton::depth_first_reset_predecessors(rpState &_rpState) {
    00696         // If _rpState's predecessor is not nULL, _rpState must be the root of the
    00697         // search tree. Set predecessor NULL so that if the root is found on its
    00698         // branches, it will be ignored.
    00699         if (_rpState.predecessor_p() != NULL) {
    00700                 _rpState.reset_predecessor();
    00701         }
    00702         for (unsigned int i = 0; i < _rpState.out_rpTransitionPtrs_p()->size(); i++) {
    00703                 // Search only along the "allowed" trasition path and only if its
    00704                 // predecessor is not NULL (i.e. the state hasn't been visited)
    00705                 if (_rpState.out_rpTransitionPtrs(i)->current_status() ==
    00706                         rpTransition::ALLOWED &&
    00707                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->predecessor_p() !=
    00708                         NULL) {
    00709                         // reset the predecessor
    00710                         _rpState.out_rpTransitionPtrs(i)->to_rpState_p()->
    00711                                 reset_predecessor();
    00712                         depth_first_reset_predecessors(
    00713                                 *_rpState.out_rpTransitionPtrs(i)->to_rpState_p());
    00714                 }
    00715         }
    00716 }
    00717
    00718 //------------------------------------------------------------------------------
    00719 void rpAutomaton::destroy() {
    00720         _automaton_p            = NULL;
    00721         _rpCCAutomata_p         = NULL;
    00722         _rpStates.clear();
    00723         _current_rpState_p      = NULL;
    00724         _goal_rpState_p         = NULL;
    00725         _rpTransitions.clear();
    00726         _rpPolicies.clear();
    00727 }
    00728 void rpAutomaton::copy(const rpAutomaton &other) {
    00729         _rpCCAutomata_p         = other._rpCCAutomata_p;
    00730         _automaton_p            = other._automaton_p;
    00731         _rpStates                       = rpStates(other._rpStates);
    00732         _current_rpState_p      = other._current_rpState_p;
    00733         _goal_rpState_p         = other._goal_rpState_p;
    00734         _rpTransitions          = rpTransitions(other._rpTransitions);
    00735         _rpPolicies                     = rpPolicies(other._rpPolicies);
    00736         unsigned int i, j;
    00737         for (i = 0; i < _rpStates.size(); i++) {
    00738                 if (other._rpStates[i].SCC_root_p() != NULL) {
    00739                         _rpStates[i].set_SCC_root(
    00740                                 *rpState_p(*other._rpStates[i].SCC_root_p()->value_p()));
    00741                 }
    00742                 if (other._rpStates[i].predecessor_p() != NULL) {
    00743                         _rpStates[i].set_predecessor(
    00744                                 *rpState_p(*other._rpStates[i].predecessor_p()->value_p()));
    00745                 }
    00746         }
    00747         for (i = 0; i < _rpTransitions.size(); i++) {
    00748                 if (_rpTransitions[i].from_rpState_p() != NULL) {
    00749                         _rpTransitions[i].set_from_rpState_p(
    00750                                 *_rpStates.rpState_p(
    00751                                 *other._rpTransitions[i].from_rpState_p()->value_p()));
    00752                 }
    00753                 if (_rpTransitions[i].to_rpState_p() != NULL) {
    00754                         _rpTransitions[i].set_to_rpState_p(
    00755                                 *_rpStates.rpState_p(
    00756                                 *other._rpTransitions[i].to_rpState_p()->value_p()));
    00757                 }
    00758         }
    00759         for (i = 0; i < _rpStates.size(); i++) {
    00760                 _rpStates[i].clear_in_rpTransitionPtrs();
    00761                 for (j = 0; j < other._rpStates[i].in_rpTransitionPtrs_p()->size();
    00762                 j++) {
    00763                         if (other._rpStates[i].in_rpTransitionPtrs(j) != NULL) {
    00764                                 _rpStates[i].insert_in_rpTransition(
    00765                                         *_rpTransitions.rpTransition_p(*other._rpStates[i].
    00766                                         in_rpTransitionPtrs(j)->transition_p()));
    00767                         }
    00768                 }
    00769                 _rpStates[i].clear_out_rpTransitionPtrs();
    00770                 for (j = 0; j < other._rpStates[i].out_rpTransitionPtrs_p()->size();
    00771                 j++) {
    00772                         if (other._rpStates[i].out_rpTransitionPtrs(j) != NULL) {
    00773                                 _rpStates[i].insert_out_rpTransition(
    00774                                         *_rpTransitions.rpTransition_p(*other._rpStates[i].
    00775                                         out_rpTransitionPtrs(j)->transition_p()));
    00776                         }
    00777                 }
    00778         }
    00779         for (i = 0; i < _rpStates.size(); i++) {
    00780                 _rpStates[i].set_rpAutomaton_p(*this);
    00781         }
    00782         for (i = 0; i < _rpPolicies.size(); i++) {
    00783                 if (other._rpPolicies[i] == NULL) {
    00784                         _rpPolicies[i] = NULL;
    00785                 } else {
    00786                         _rpPolicies[i] = _rpTransitions.rpTransition_p(
    00787                                                          *other._rpPolicies[i]->transition_p());
    00788                 }
    00789         }
    00790 }
    00791 //------------------------------------------------------------------------------
    00792 // Constructors, Copy Constructors, & Destructors
    00793 //------------------------------------------------------------------------------
    00794 rpAutomaton::rpStatePtrList::rpStatePtrList() {
    00795         _head_node_p                                    = new node();
    00796         _head_node_p->_previous_node_p  = _head_node_p;
    00797         _head_node_p->_next_node_p              = _head_node_p;
    00798 }
    00799 rpAutomaton::rpStatePtrList::~rpStatePtrList() {
    00800         destroy();
    00801         delete _head_node_p;
    00802 }
    00803 //------------------------------------------------------------------------------
    00804 // node Class
    00805 //------------------------------------------------------------------------------
    00806 rpAutomaton::rpStatePtrList::node::node(rpState *new_rpState_p,
    00807                                                                                 node *previous_node_p,
    00808                                                                             node *next_node_p) {
    00809         _rpState_p                      = new_rpState_p;
    00810         _previous_node_p        = previous_node_p;
    00811         _next_node_p            = next_node_p;
    00812 }
    00813 //------------------------------------------------------------------------------
    00814 // Public Function Members
    00815 //------------------------------------------------------------------------------
    00816 const unsigned int rpAutomaton::rpStatePtrList::size() const {
    00817         unsigned int i = 0;
    00818         for (node *temp_node_p = _head_node_p;
    00819         temp_node_p->_next_node_p != _head_node_p;
    00820         temp_node_p = temp_node_p->_next_node_p) {
    00821                 i++;
    00822         }
    00823         return i;
    00824 }
    00825 const bool rpAutomaton::rpStatePtrList::empty() const {
    00826         if (_head_node_p->_next_node_p == _head_node_p) {
    00827                 return true;
    00828         } else {
    00829                 return false;
    00830         }
    00831 }
    00832 rpState *rpAutomaton::rpStatePtrList::front() {
    00833         if (!empty()) {
    00834                 return _head_node_p->_next_node_p->_rpState_p;
    00835         } else {
    00836                 return NULL;
    00837         }
    00838 }
    00839 rpState *const rpAutomaton::rpStatePtrList::front() const {
    00840         if (!empty()) {
    00841                 return _head_node_p->_next_node_p->_rpState_p;
    00842         } else {
    00843                 return NULL;
    00844         }
    00845 }
    00846 rpState *rpAutomaton::rpStatePtrList::back() {
    00847         if (!empty()) {
    00848                 return _head_node_p->_previous_node_p->_rpState_p;
    00849         } else {
    00850                 return NULL;
    00851         }
    00852 }
    00853 rpState *const rpAutomaton::rpStatePtrList::back() const {
    00854         if (!empty()) {
    00855                 return _head_node_p->_previous_node_p->_rpState_p;
    00856         } else {
    00857                 return NULL;
    00858         }
    00859 }
    00860 const bool
    00861 rpAutomaton::rpStatePtrList::is_in_list(const rpState *const _rpState_p) {
    00862         for (node *temp_node_p = _head_node_p;
    00863         temp_node_p->_next_node_p != _head_node_p;
    00864         temp_node_p = temp_node_p->_next_node_p) {
    00865                 if (temp_node_p->_rpState_p == _rpState_p) return true;
    00866         }
    00867         return false;
    00868 }
    00869 //------------------------------------------------------------------------------
    00870 void rpAutomaton::rpStatePtrList::push_front(rpState *const _rpState_p) {
    00871         _head_node_p->_next_node_p = new node(_rpState_p,
    00872                                                                                   _head_node_p,
    00873                                                                                   _head_node_p->_next_node_p);
    00874         _head_node_p->_next_node_p->_next_node_p->_previous_node_p =
    00875                 _head_node_p->_next_node_p;
    00876 }
    00877 void rpAutomaton::rpStatePtrList::push_back(rpState *const _rpState_p) {
    00878         _head_node_p->_previous_node_p = new node(_rpState_p,
    00879                                                                                           _head_node_p->_previous_node_p,
    00880                                                                                           _head_node_p);
    00881         _head_node_p->_previous_node_p->_previous_node_p->_next_node_p =
    00882                 _head_node_p->_previous_node_p;
    00883 }
    00884 void rpAutomaton::rpStatePtrList::pop_front() {
    00885         _head_node_p->_next_node_p = _head_node_p->_next_node_p->_next_node_p;
    00886         delete _head_node_p->_next_node_p->_previous_node_p;
    00887         _head_node_p->_next_node_p->_previous_node_p = _head_node_p;
    00888 }
    00889 void rpAutomaton::rpStatePtrList::pop_back() {
    00890         _head_node_p->_previous_node_p =
    00891                 _head_node_p->_previous_node_p->_previous_node_p;
    00892         delete _head_node_p->_previous_node_p->_next_node_p;
    00893         _head_node_p->_previous_node_p->_next_node_p = _head_node_p;
    00894 }
    00895
    00896 void rpAutomaton::rpStatePtrList::clear() {
    00897         while (_head_node_p->_next_node_p != _head_node_p) {
    00898                 pop_front();
    00899         }
    00900 }
    00901 //------------------------------------------------------------------------------
    00902 // Private Function Members
    00903 //------------------------------------------------------------------------------
    00904 void rpAutomaton::rpStatePtrList::destroy() {
    00905         clear();
    00906 }