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

# /Users/fpy/titan-1-5-MBARI/Mers/Planning/Reactive/CCA/rpStatePtrs.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 // rpStatePtrs class
    00014 //
    00015 //=============================================================================
    00016
    00017 #include <cstdio>
    00018 #include <cassert>
    00019
    00020 #include "rpStatePtrs.h"
    00021 #include "rpState.h"
    00022 //------------------------------------------------------------------------------
    00023 // Constructors, Copy Constructors, & Destructors
    00024 //------------------------------------------------------------------------------
    00025 rpStatePtrs::rpStatePtrs() {
    00026         _rpStatePtr_array       = NULL;
    00027         _size                           = 0;
    00028 }
    00029 rpStatePtrs::rpStatePtrs(sizeType numb) {
    00030         _size                           = numb;
    00031         _rpStatePtr_array       = new const rpState *[size()];
    00032         for (sizeType i = 0; i < size(); i++) {
    00033                 _rpStatePtr_array[i] = NULL;
    00034         }
    00035 }
    00036 rpStatePtrs::rpStatePtrs(const rpStatePtrs &other) {
    00037         if (this != &other) {
    00038                 copy(other);
    00039         }
    00040 }
    00041 rpStatePtrs::~rpStatePtrs() {
    00042         destroy();
    00043 }
    00044 //------------------------------------------------------------------------------
    00045 // Public Function Members
    00046 //------------------------------------------------------------------------------
    00047 const unsigned int rpStatePtrs::size() const {
    00048         return _size;
    00049 }
    00050 bool rpStatePtrs::empty() const {
    00051         return (_size == 0);
    00052 }
    00053 //------------------------------------------------------------------------------
    00054 void rpStatePtrs::sort() {
    00055         if (size() > 0) {
    00056                 quicksort(0, size() - 1);
    00057         }
    00058 }
    00059 void rpStatePtrs::clear() {
    00060         for (sizeType i = 0; i < size(); i++) {
    00061                 _rpStatePtr_array[i] = NULL;
    00062         }
    00063 }
    00064 //------------------------------------------------------------------------------
    00065 // Public Operators
    00066 //------------------------------------------------------------------------------
    00067 const rpState *&rpStatePtrs::operator [](const unsigned int index_) {
    00068         if (index_ < size()) {
    00069                 return _rpStatePtr_array[index_];
    00070         } else {
    00071                 return _rpStatePtr_array[0];
    00072         }
    00073 }
    00074 const rpState *const &rpStatePtrs::operator [](const unsigned int index_) const {
    00075         if (index_ < size()) {
    00076                 return _rpStatePtr_array[index_];
    00077         } else {
    00078                 return _rpStatePtr_array[0];
    00079         }
    00080 }
    00081 const rpStatePtrs &rpStatePtrs::operator =(const rpStatePtrs &other) {
    00082         if (this != &other) {
    00083                 destroy();
    00084                 copy(other);
    00085         }
    00086         return (*this);
    00087 }
    00088 const bool rpStatePtrs::operator ==(const rpStatePtrs &other) const {
    00089         if (_size == other._size) {
    00090                 for (sizeType i = 0; i < size(); i++) {
    00091                         if (_rpStatePtr_array[i] != other._rpStatePtr_array[i]) return false;
    00092                 }
    00093                 return true;
    00094         }
    00095         return false;
    00096 }
    00097 const bool rpStatePtrs::operator !=(const rpStatePtrs &other) const {
    00098         return !(*this == other);
    00099 }
    00100 std::ostream &operator <<(std::ostream &_ostream, const rpStatePtrs &_rpStatePtrs) {
    00101         _ostream << "rpStatePtrs:" << std::endl;
    00102         rpStatePtrs::sizeType i = 0;
    00103         while (i < _rpStatePtrs.size()) {
    00104                 _ostream << *_rpStatePtrs[i];
    00105                 i++;
    00106                 if (i < _rpStatePtrs.size()) {
    00107                         _ostream << std::endl;
    00108                 }
    00109         }
    00110         return _ostream;
    00111 }
    00112 //------------------------------------------------------------------------------
    00113 // Private Function Members
    00114 //------------------------------------------------------------------------------
    00115 void rpStatePtrs::swap(const rpState *&rpStatePtr_1,
    00116                                            const rpState *&rpStatePtr_2) {
    00117         const rpState *temp_rpStatePtr  = rpStatePtr_1;
    00118         rpStatePtr_1                                    = rpStatePtr_2;
    00119         rpStatePtr_2                                    = temp_rpStatePtr;
    00120 }
    00121 unsigned int rpStatePtrs::quicksort_partition(unsigned int low,
    00122                                                                                           unsigned int hi) {
    00123         assert(low < hi);
    00124         unsigned int left       = low + 1;
    00125         unsigned int right      = hi;
    00126         while (left <= right) {
    00127                 if (*_rpStatePtr_array[left] <= *_rpStatePtr_array[low]) {
    00128                         left++;
    00129                 } else if (*_rpStatePtr_array[right] > *_rpStatePtr_array[low]) {
    00130                         right--;
    00131                 } else {
    00132                         swap(_rpStatePtr_array[left], _rpStatePtr_array[right]);
    00133                         left++;
    00134                         right--;
    00135                 }
    00136         }
    00137         swap(_rpStatePtr_array[low], _rpStatePtr_array[left - 1]);
    00138         return left - 1;
    00139 }
    00140 void rpStatePtrs::quicksort(unsigned int low, unsigned int hi) {
    00141         if (low >= hi) {
    00142                 return;
    00143         } else {
    00144                 unsigned int mid = quicksort_partition(low, hi);
    00145     if (mid > 0) {
    00146                 quicksort(low, mid - 1);
    00147     }
    00148                 quicksort(mid + 1, hi);
    00149         }
    00150 }
    00151 //------------------------------------------------------------------------------
    00152 void rpStatePtrs::destroy() {
    00153         clear();
    00154         delete [] _rpStatePtr_array;
    00155         _size = 0;
    00156 }
    00157 void rpStatePtrs::copy(const rpStatePtrs &other) {
    00158         _size                                   = other._size;
    00159         _rpStatePtr_array               = new const rpState *[size()];
    00160         for (sizeType i = 0; i < size(); i++) {
    00161                 _rpStatePtr_array[i] = other[i];
    00162         }
    00163 }