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

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/rpCCAutomata.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 // rpCCAutomata class
    00014 //
    00015 //=============================================================================
    00016
    00017 #include <cstdio>
    00018 #include <cassert>
    00019
    00020 #include "rpCCAutomata.h"
    00021 #include "rpAutomaton.h"
    00022 #include "rpState.h"
    00023 #include "rpTransitions.h"
    00024 #include "rpTransition.h"
    00025
    00027
    00028 using Mers::Models::CCA::CCAAutomata;
    00029 using Mers::Models::CCA::CCAVariable;
    00030 using Mers::Models::CCA::CCAAssignment;
    00031
    00033 // Constructors, Copy Constructors, & Destructors
    00035 rpCCAutomata::rpCCAutomata() {
    00036         _rpAutomaton_array      = NULL;
    00037         _size                           = 0;
    00038         _initialized            = false;
    00039 }
    00040 rpCCAutomata::rpCCAutomata(const CCAAutomata &_ccAutomata) {
    00041         unsigned int i;
    00042         _ccAutomata_p           = &_ccAutomata;
    00043         _size                           = _ccAutomata.getLength();
    00044         _rpAutomaton_array      = new rpAutomaton[_size];
    00045         CCAAutomata::const_iterator     ccAutomata_const_it = _ccAutomata.begin();
    00046         // Insert each automaton into rpCCAutomata. Transition guard information
    00047         // cannot be inserted until all rpCCAutomata have been initialized.
    00048         for (i = 0; i < _size; i++, ccAutomata_const_it++) {
    00049                 _rpAutomaton_array[i] = rpAutomaton(*ccAutomata_const_it);
    00050                 _rpAutomaton_array[i].set_rpCCAutomata_p(*this);
    00051         }
    00052         sort();
    00053         // Now that all rpCCAutomata have been initialized, Transition guard can be
    00054         // inserted.
    00055         for (i = 0; i < _size; i++) {
    00056                 _rpAutomaton_array[i].reset_rpTransition_conditions();
    00057         }
    00058         _initialized            = false;
    00059 }
    00060 rpCCAutomata::rpCCAutomata(const rpCCAutomata &other) {
    00061         if (this != &other) {
    00062                 copy(other);
    00063         }
    00064 }
    00065 rpCCAutomata::~rpCCAutomata() {
    00066         destroy();
    00067 }
    00068 //------------------------------------------------------------------------------
    00069 // Public Function Members
    00070 //------------------------------------------------------------------------------
    00071 bool rpCCAutomata::empty() const {
    00072         return (_size == 0);
    00073 }
    00074 void rpCCAutomata::clear() {
    00075         for (unsigned int index_ = 0; index_ < _size; index_++) {
    00076                 _rpAutomaton_array[index_].clear();
    00077         }
    00078 }
    00079 unsigned int rpCCAutomata::size() const {
    00080         return _size;
    00081 }
    00082 const rpAutomaton *
    00083 rpCCAutomata::rpAutomaton_p(const CCAVariable &_variable) const {
    00084         for (unsigned int index_ = 0; index_ < _size; index_++) {
    00085                 if (_rpAutomaton_array[index_].variable_p() == &_variable) {
    00086                         return &_rpAutomaton_array[index_];
    00087                 }
    00088         }
    00089         return NULL;
    00090 }
    00091 const rpState *
    00092 rpCCAutomata::rpState_p(const CCAAssignment &_assignment) const {
    00093         if (rpAutomaton_p(*_assignment.getVariable()) != NULL) {
    00094                 return  rpAutomaton_p(*_assignment.getVariable())->
    00095                         rpState_p(*_assignment.getValue());
    00096         } else {
    00097                 return NULL;
    00098         }
    00099 }
    00100 //------------------------------------------------------------------------------
    00101 // Public Operators
    00102 //------------------------------------------------------------------------------
    00103 rpAutomaton &rpCCAutomata::operator [](unsigned int index_) {
    00104         return _rpAutomaton_array[index_];
    00105 }
    00106 const rpAutomaton &rpCCAutomata::operator [](unsigned int index_) const {
    00107         return _rpAutomaton_array[index_];
    00108 }
    00109 const rpCCAutomata &rpCCAutomata::operator =(const rpCCAutomata &other) {
    00110         if (this != &other) {
    00111                 destroy();
    00112                 copy(other);
    00113         }
    00114         return (*this);
    00115 }
    00116 bool rpCCAutomata::operator ==(const rpCCAutomata &other) const {
    00117         if (_size == other._size) {
    00118                 for (unsigned int i = 0; i < _size; i++) {
    00119                         if (_rpAutomaton_array[i] != other._rpAutomaton_array[i]) {
    00120                                 return false;
    00121                         }
    00122                 }
    00123                 return true;
    00124         }
    00125         return false;
    00126 }
    00127 bool rpCCAutomata::operator !=(const rpCCAutomata &other) const {
    00128         return !(*this == other);
    00129 }
    00130 std::ostream &operator <<(std::ostream &_ostream,
    00131                                                   const rpCCAutomata &_rpCCAutomata) {
    00132         _ostream << "rpCCAutomata:" << std::endl;
    00133         for (unsigned int i = 0; i < _rpCCAutomata.size(); i++) {
    00134                 _ostream << _rpCCAutomata[i] << std::endl;
    00135         }
    00136         return _ostream;
    00137 }
    00138 //------------------------------------------------------------------------------
    00139 // Private Function Members
    00140 //------------------------------------------------------------------------------
    00141 void rpCCAutomata::sort() {
    00142         if (size() > 0) {
    00143                 quicksort(0, size() - 1);
    00144         }
    00145 }
    00146 void rpCCAutomata::swap(rpAutomaton &rpAutomaton_1,
    00147                                                 rpAutomaton &rpAutomaton_2) {
    00148         rpAutomaton temp_rpAutomaton    = rpAutomaton_1;
    00149         rpAutomaton_1                                   = rpAutomaton_2;
    00150         rpAutomaton_2                                   = temp_rpAutomaton;
    00151 }
    00152 unsigned int rpCCAutomata::quicksort_partition(unsigned int low,
    00153                                                                                            unsigned int high) {
    00154         assert(low < high);
    00155         unsigned int left       = low + 1;
    00156         unsigned int right      = high;
    00157         while (left <= right) {
    00158                 if (_rpAutomaton_array[left] <= _rpAutomaton_array[low]) {
    00159                         left++;
    00160                 } else if (_rpAutomaton_array[right] > _rpAutomaton_array[low]) {
    00161                         right--;
    00162                 } else {
    00163                         swap(_rpAutomaton_array[left], _rpAutomaton_array[right]);
    00164                         left++;
    00165                         right--;
    00166                 }
    00167         }
    00168         swap(_rpAutomaton_array[low], _rpAutomaton_array[left - 1]);
    00169         return left - 1;
    00170 }
    00171 void rpCCAutomata::quicksort(unsigned int low, unsigned int high) {
    00172         if (low >= high) {
    00173                 return;
    00174         } else {
    00175                 unsigned int mid = quicksort_partition(low, high);
    00176     if (mid > 0) {
    00177                 quicksort(low, mid - 1);
    00178     }
    00179                 quicksort(mid + 1, high);
    00180         }
    00181 }
    00182 void rpCCAutomata::destroy() {
    00183         clear();
    00184         delete [] _rpAutomaton_array;
    00185         _size = 0;
    00186 }
    00187 void rpCCAutomata::copy(const rpCCAutomata &other) {
    00188         _size                           = other.size();
    00189         _rpAutomaton_array      = new rpAutomaton[_size];
    00190         for (unsigned int i = 0; i < _size; i++) {
    00191                 _rpAutomaton_array[i] = other[i];
    00192         }
    00193 }