All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/base/src/StateSpace.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "ompl/base/StateSpace.h"
00036 #include "ompl/util/Exception.h"
00037 #include "ompl/tools/config/MagicConstants.h"
00038 #include "ompl/base/spaces/RealVectorStateSpace.h"
00039 #include <boost/thread/mutex.hpp>
00040 #include <boost/lexical_cast.hpp>
00041 #include <boost/bind.hpp>
00042 #include <numeric>
00043 #include <limits>
00044 #include <queue>
00045 #include <cmath>
00046 #include <list>
00047 #include <set>
00048 
00049 const std::string ompl::base::StateSpace::DEFAULT_PROJECTION_NAME = "";
00050 
00052 namespace ompl
00053 {
00054     namespace base
00055     {
00056         struct AllocatedSpaces
00057         {
00058             std::list<StateSpace*> list_;
00059             boost::mutex           lock_;
00060         };
00061 
00062         static AllocatedSpaces& getAllocatedSpaces(void)
00063         {
00064             static AllocatedSpaces as;
00065             return as;
00066         }
00067     }
00068 }
00070 
00071 ompl::base::StateSpace::StateSpace(void)
00072 {
00073     // autocompute a unique name
00074     static boost::mutex lock;
00075     static unsigned int m = 0;
00076 
00077     lock.lock();
00078     m++;
00079     lock.unlock();
00080 
00081     name_ = "Space" + boost::lexical_cast<std::string>(m);
00082 
00083     longestValidSegment_ = 0.0;
00084     longestValidSegmentFraction_ = 0.01; // 1%
00085     longestValidSegmentCountFactor_ = 1;
00086 
00087     type_ = STATE_SPACE_UNKNOWN;
00088 
00089     maxExtent_ = std::numeric_limits<double>::infinity();
00090 
00091     params_.declareParam<double>("longest_valid_segment_fraction",
00092                                  boost::bind(&StateSpace::setLongestValidSegmentFraction, this, _1),
00093                                  boost::bind(&StateSpace::getLongestValidSegmentFraction, this));
00094 
00095     params_.declareParam<unsigned int>("valid_segment_count_factor",
00096                                        boost::bind(&StateSpace::setValidSegmentCountFactor, this, _1),
00097                                        boost::bind(&StateSpace::getValidSegmentCountFactor, this));
00098     AllocatedSpaces &as = getAllocatedSpaces();
00099     boost::mutex::scoped_lock smLock(as.lock_);
00100     as.list_.push_back(this);
00101 }
00102 
00103 ompl::base::StateSpace::~StateSpace(void)
00104 {
00105     AllocatedSpaces &as = getAllocatedSpaces();
00106     boost::mutex::scoped_lock smLock(as.lock_);
00107     as.list_.remove(this);
00108 }
00109 
00111 namespace ompl
00112 {
00113     namespace base
00114     {
00115         static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
00116         {
00117             signature.push_back(space->getType());
00118             signature.push_back(space->getDimension());
00119 
00120             if (space->isCompound())
00121             {
00122                 unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
00123                 for (unsigned int i = 0 ; i < c ; ++i)
00124                     computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
00125             }
00126         }
00127 
00128         void computeLocationsHelper(const StateSpace *s,
00129                                     std::map<std::string, StateSpace::SubstateLocation> &substateMap,
00130                                     std::vector<StateSpace::ValueLocation> &locationsArray,
00131                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap, StateSpace::ValueLocation loc)
00132         {
00133             loc.stateLocation.space = s;
00134             substateMap[s->getName()] = loc.stateLocation;
00135             State *test = s->allocState();
00136             if (s->getValueAddressAtIndex(test, 0) != NULL)
00137             {
00138                 loc.index = 0;
00139                 locationsMap[s->getName()] = loc;
00140                 // if the space is compound, we will find this value again in the first subspace
00141                 if (!s->isCompound())
00142                 {
00143                     if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00144                     {
00145                         const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
00146                         if (!name.empty())
00147                             locationsMap[name] = loc;
00148                     }
00149                     locationsArray.push_back(loc);
00150                     while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
00151                     {
00152                         if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00153                         {
00154                             const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
00155                             if (!name.empty())
00156                                 locationsMap[name] = loc;
00157                         }
00158                         locationsArray.push_back(loc);
00159                     }
00160                 }
00161             }
00162             s->freeState(test);
00163 
00164             if (s->isCompound())
00165                 for (unsigned int i = 0 ; i < s->as<base::CompoundStateSpace>()->getSubspaceCount() ; ++i)
00166                 {
00167                     loc.stateLocation.chain.push_back(i);
00168                     computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap, locationsArray, locationsMap, loc);
00169                     loc.stateLocation.chain.pop_back();
00170                 }
00171         }
00172 
00173         void computeLocationsHelper(const StateSpace *s,
00174                                     std::map<std::string, StateSpace::SubstateLocation> &substateMap,
00175                                     std::vector<StateSpace::ValueLocation> &locationsArray,
00176                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap)
00177         {
00178             substateMap.clear();
00179             locationsArray.clear();
00180             locationsMap.clear();
00181             computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
00182         }
00183     }
00184 }
00186 
00187 const std::string& ompl::base::StateSpace::getName(void) const
00188 {
00189     return name_;
00190 }
00191 
00192 void ompl::base::StateSpace::setName(const std::string &name)
00193 {
00194     name_ = name;
00195 
00196     // we don't want to call this function during the state space construction because calls to virtual functions are made,
00197     // so we check if any values were previously inserted as value locations;
00198     // if none were, then we either have none (so no need to call this function again)
00199     // or setup() was not yet called
00200     if (!valueLocationsInOrder_.empty())
00201       computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00202 }
00203 
00204 void ompl::base::StateSpace::computeLocations(void)
00205 {
00206     computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00207 }
00208 
00209 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
00210 {
00211     signature.clear();
00212     computeStateSpaceSignatureHelper(this, signature);
00213     signature.insert(signature.begin(), signature.size());
00214 }
00215 
00216 void ompl::base::StateSpace::registerProjections(void)
00217 {
00218 }
00219 
00220 void ompl::base::StateSpace::setup(void)
00221 {
00222     maxExtent_ = getMaximumExtent();
00223     longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00224 
00225     if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00226         throw Exception("The longest valid segment for state space " + getName() + " must be positive");
00227 
00228     computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00229 
00230     // make sure we don't overwrite projections that have been configured by the user
00231     std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00232     registerProjections();
00233     for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00234         if (it->second->userConfigured())
00235         {
00236             std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00237             if (o != projections_.end())
00238                 if (!o->second->userConfigured())
00239                     projections_[it->first] = it->second;
00240         }
00241 
00242     // remove previously set parameters for projections
00243     std::vector<std::string> pnames;
00244     params_.getParamNames(pnames);
00245     for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
00246         if (it->substr(0, 11) == "projection.")
00247             params_.remove(*it);
00248 
00249     // setup projections and add their parameters
00250     for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00251     {
00252         it->second->setup();
00253         if (it->first == DEFAULT_PROJECTION_NAME)
00254             params_.include(it->second->params(), "projection");
00255         else
00256             params_.include(it->second->params(), "projection." + it->first);
00257     }
00258 }
00259 
00260 const std::map<std::string, ompl::base::StateSpace::SubstateLocation>& ompl::base::StateSpace::getSubstateLocationsByName(void) const
00261 {
00262     return substateLocationsByName_;
00263 }
00264 
00265 ompl::base::State* ompl::base::StateSpace::getSubstateAtLocation(State *state, const SubstateLocation &loc) const
00266 {
00267     std::size_t index = 0;
00268     while (loc.chain.size() > index)
00269         state = state->as<CompoundState>()->components[loc.chain[index++]];
00270     return state;
00271 }
00272 
00273 const ompl::base::State* ompl::base::StateSpace::getSubstateAtLocation(const State *state, const SubstateLocation &loc) const
00274 {    
00275     std::size_t index = 0;
00276     while (loc.chain.size() > index)
00277         state = state->as<CompoundState>()->components[loc.chain[index++]];
00278     return state;
00279 }
00280 
00281 double* ompl::base::StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00282 {
00283     return NULL;
00284 }
00285 
00286 const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
00287 {
00288     double *val = getValueAddressAtIndex(const_cast<State*>(state), index); // this const-cast does not hurt, since the state is not modified
00289     return val;
00290 }
00291 
00292 const std::vector<ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocations(void) const
00293 {
00294     return valueLocationsInOrder_;
00295 }
00296 
00297 const std::map<std::string, ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocationsByName(void) const
00298 {
00299     return valueLocationsByName_;
00300 }
00301 
00302 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
00303 {
00304     reals.resize(valueLocationsInOrder_.size());
00305     for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
00306         reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
00307 }
00308 
00309 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
00310 {
00311     assert(reals.size() == valueLocationsInOrder_.size());
00312     for (std::size_t i = 0 ; i < reals.size() ; ++i)
00313         *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
00314 }
00315 
00316 double* ompl::base::StateSpace::getValueAddressAtLocation(State *state, const ValueLocation &loc) const
00317 {
00318     std::size_t index = 0;
00319     while (loc.stateLocation.chain.size() > index)
00320         state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
00321     return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
00322 }
00323 
00324 const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
00325 {
00326     std::size_t index = 0;
00327     while (loc.stateLocation.chain.size() > index)
00328         state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
00329     return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
00330 }
00331 
00332 double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
00333 {
00334     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00335     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00336 }
00337 
00338 const double* ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
00339 {
00340     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00341     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00342 }
00343 
00344 unsigned int ompl::base::StateSpace::getSerializationLength(void) const
00345 {
00346     return 0;
00347 }
00348 
00349 void ompl::base::StateSpace::serialize(void *serialization, const State *state) const
00350 {
00351 }
00352 
00353 void ompl::base::StateSpace::deserialize(State *state, const void *serialization) const
00354 {
00355 }
00356 
00357 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
00358 {
00359     out << "State instance [" << state << ']' << std::endl;
00360 }
00361 
00362 void ompl::base::StateSpace::printSettings(std::ostream &out) const
00363 {
00364     out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
00365     printProjections(out);
00366 }
00367 
00368 void ompl::base::StateSpace::printProjections(std::ostream &out) const
00369 {
00370     if (projections_.empty())
00371         out << "No registered projections" << std::endl;
00372     else
00373     {
00374         out << "Registered projections:" << std::endl;
00375         for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00376         {
00377             out << "  - ";
00378             if (it->first == DEFAULT_PROJECTION_NAME)
00379                 out << "<default>";
00380             else
00381                 out << it->first;
00382             out << std::endl;
00383             it->second->printSettings(out);
00384         }
00385     }
00386 }
00387 
00389 namespace ompl
00390 {
00391     namespace base
00392     {
00393         static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
00394         {
00395             std::queue<const StateSpace*> q;
00396             q.push(self);
00397             while (!q.empty())
00398             {
00399                 const StateSpace *m = q.front();
00400                 q.pop();
00401                 if (m->getName() == other->getName())
00402                     return true;
00403                 if (m->isCompound())
00404                 {
00405                     unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00406                     for (unsigned int i = 0 ; i < c ; ++i)
00407                         q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
00408                 }
00409             }
00410             return false;
00411         }
00412 
00413         static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
00414         {
00415             if (StateSpaceIncludes(self, other))
00416                 return true;
00417             else
00418                 if (other->isCompound())
00419                 {
00420                     unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
00421                     for (unsigned int i = 0 ; i < c ; ++i)
00422                         if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
00423                             return false;
00424                     return true;
00425                 }
00426             return false;
00427         }
00428 
00429         struct CompareSubstateLocation
00430         {
00431             bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
00432             {
00433               if (a.space->getDimension() != b.space->getDimension())
00434                   return a.space->getDimension() > b.space->getDimension();
00435               return a.space->getName() > b.space->getName();
00436             }
00437         };
00438     
00439     }
00440 }
00441 
00443 
00444 bool ompl::base::StateSpace::covers(const StateSpacePtr &other) const
00445 {
00446     return StateSpaceCovers(this, other.get());
00447 }
00448 
00449 bool ompl::base::StateSpace::includes(const StateSpacePtr &other) const
00450 {
00451     return StateSpaceIncludes(this, other.get());
00452 }
00453 
00454 bool ompl::base::StateSpace::covers(const StateSpace *other) const
00455 {
00456     return StateSpaceCovers(this, other);
00457 }
00458 
00459 bool ompl::base::StateSpace::includes(const StateSpace *other) const
00460 {
00461     return StateSpaceIncludes(this, other);
00462 }
00463 
00464 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
00465 {
00466     getCommonSubspaces(other.get(), subspaces);
00467 }
00468 
00469 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
00470 {
00471     std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
00472     const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
00473     for (std::map<std::string, StateSpace::SubstateLocation>::const_iterator it = substateLocationsByName_.begin() ; it != substateLocationsByName_.end() ; ++it)
00474     {
00475         if (S.find(it->first) != S.end())
00476             intersection.insert(it->second);
00477     }
00478 
00479     bool found = true;
00480     while (found)
00481     {
00482       found = false;      
00483       for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
00484           for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator jt = intersection.begin() ; jt != intersection.end() ; ++jt)
00485               if (it != jt)
00486                   if (StateSpaceCovers(it->space, jt->space))
00487                   {
00488                       intersection.erase(jt);
00489                       found = true;
00490                       break;
00491                   }
00492     }
00493     subspaces.clear();
00494     for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
00495         subspaces.push_back(it->space->getName());
00496 }
00497 
00498 void ompl::base::StateSpace::List(std::ostream &out)
00499 {
00500     AllocatedSpaces &as = getAllocatedSpaces();
00501     boost::mutex::scoped_lock smLock(as.lock_);
00502     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00503         out << "@ " << *it << ": " << (*it)->getName() << std::endl;
00504 }
00505 
00506 void ompl::base::StateSpace::list(std::ostream &out) const
00507 {
00508     std::queue<const StateSpace*> q;
00509     q.push(this);
00510     while (!q.empty())
00511     {
00512         const StateSpace *m = q.front();
00513         q.pop();
00514         out << "@ " << m << ": " << m->getName() << std::endl;
00515         if (m->isCompound())
00516         {
00517             unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00518             for (unsigned int i = 0 ; i < c ; ++i)
00519                 q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
00520         }
00521     }
00522 }
00523 
00524 void ompl::base::StateSpace::diagram(std::ostream &out) const
00525 {
00526     out << "digraph StateSpace {" << std::endl;
00527     out << '"' << getName() << '"' << std::endl;
00528 
00529     std::queue<const StateSpace*> q;
00530     q.push(this);
00531     while (!q.empty())
00532     {
00533         const StateSpace *m = q.front();
00534         q.pop();
00535         if (m->isCompound())
00536         {
00537             unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00538             for (unsigned int i = 0 ; i < c ; ++i)
00539             {
00540                 const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
00541                 q.push(s);
00542                 out << '"' << m->getName() << "\" -> \"" << s->getName() << "\" [label=\"" <<
00543                     boost::lexical_cast<std::string>(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << "\"];" << std::endl;
00544             }
00545         }
00546     }
00547 
00548     out << '}' << std::endl;
00549 }
00550 
00551 void ompl::base::StateSpace::Diagram(std::ostream &out)
00552 {
00553     AllocatedSpaces &as = getAllocatedSpaces();
00554     boost::mutex::scoped_lock smLock(as.lock_);
00555     out << "digraph StateSpaces {" << std::endl;
00556     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00557     {
00558         out << '"' << (*it)->getName() << '"' << std::endl;
00559         for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
00560             if (it != jt)
00561             {
00562                 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
00563                     out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00564                         boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
00565                         "\"];" << std::endl;
00566                 else
00567                     if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
00568                         out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00569             }
00570     }
00571     out << '}' << std::endl;
00572 }
00573 
00574 void ompl::base::StateSpace::sanityChecks(void) const
00575 {
00576     sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), ~0);
00577 }
00578 
00579 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
00580 {
00581     {
00582         double maxExt = getMaximumExtent();
00583 
00584         State *s1 = allocState();
00585         State *s2 = allocState();
00586         StateSamplerPtr ss = allocStateSampler();
00587         char *serialization = NULL;
00588         if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
00589             serialization = new char[getSerializationLength()];
00590         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00591         {
00592             ss->sampleUniform(s1);
00593             if (distance(s1, s1) > eps)
00594                 throw Exception("Distance from a state to itself should be 0");
00595             if (!equalStates(s1, s1))
00596                 throw Exception("A state should be equal to itself");
00597             if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
00598                 throw Exception("Sampled states should be within bounds");
00599             copyState(s2, s1);
00600             if (!equalStates(s1, s2))
00601                 throw Exception("Copy of a state is not the same as the original state. copyState() may not work correctly.");
00602             if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
00603             {
00604                 enforceBounds(s1);
00605                 if  (!equalStates(s1, s2))
00606                     throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
00607             }
00608             if (flags & STATESPACE_SERIALIZATION)
00609             {
00610                 ss->sampleUniform(s2);
00611                 serialize(serialization, s1);
00612                 deserialize(s2, serialization);
00613                 if  (!equalStates(s1, s2))
00614                     throw Exception("Serialization/deserialization operations do not seem to work as expected.");
00615             }
00616             ss->sampleUniform(s2);
00617             if (!equalStates(s1, s2))
00618             {
00619                 double d12 = distance(s1, s2);
00620                 if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
00621                     throw Exception("Distance between different states should be above 0");
00622                 double d21 = distance(s2, s1);
00623                 if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
00624                     throw Exception("The distance function should be symmetric (A->B=" +
00625                                     boost::lexical_cast<std::string>(d12) + ", B->A=" +
00626                                     boost::lexical_cast<std::string>(d21) + ", difference is " +
00627                                     boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
00628                 if (flags & STATESPACE_DISTANCE_BOUND)
00629                     if (d12 > maxExt + zero)
00630                         throw Exception("The distance function should not report values larger than the maximum extent ("+
00631                                         boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
00632             }
00633         }
00634         if (serialization)
00635             delete[] serialization;
00636         freeState(s1);
00637         freeState(s2);
00638     }
00639 
00640 
00641     // Test that interpolation works as expected and also test triangle inequality
00642     if (!isDiscrete() && !isHybrid())
00643     {
00644         State *s1 = allocState();
00645         State *s2 = allocState();
00646         State *s3 = allocState();
00647         StateSamplerPtr ss = allocStateSampler();
00648 
00649         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00650         {
00651             ss->sampleUniform(s1);
00652             ss->sampleUniform(s2);
00653             ss->sampleUniform(s3);
00654 
00655             interpolate(s1, s2, 0.0, s3);
00656             if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
00657                 throw Exception("Interpolation from a state at time 0 should be not change the original state");
00658 
00659             interpolate(s1, s2, 1.0, s3);
00660             if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
00661                 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
00662 
00663             interpolate(s1, s2, 0.5, s3);
00664             double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
00665             if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
00666                 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
00667                                 boost::lexical_cast<std::string>(diff) + " difference)");
00668 
00669             interpolate(s3, s2, 0.5, s3);
00670             interpolate(s1, s2, 0.75, s2);
00671 
00672             if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
00673                 throw Exception("Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
00674         }
00675         freeState(s1);
00676         freeState(s2);
00677         freeState(s3);
00678     }
00679 }
00680 
00681 bool ompl::base::StateSpace::hasDefaultProjection(void) const
00682 {
00683     return hasProjection(DEFAULT_PROJECTION_NAME);
00684 }
00685 
00686 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
00687 {
00688     return projections_.find(name) != projections_.end();
00689 }
00690 
00691 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection(void) const
00692 {
00693     if (hasDefaultProjection())
00694         return getProjection(DEFAULT_PROJECTION_NAME);
00695     else
00696     {
00697         logError("No default projection is set. Perhaps setup() needs to be called");
00698         return ProjectionEvaluatorPtr();
00699     }
00700 }
00701 
00702 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
00703 {
00704     std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00705     if (it != projections_.end())
00706         return it->second;
00707     else
00708     {
00709         logError("Projection '%s' is not defined", name.c_str());
00710         return ProjectionEvaluatorPtr();
00711     }
00712 }
00713 
00714 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections(void) const
00715 {
00716     return projections_;
00717 }
00718 
00719 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00720 {
00721     registerProjection(DEFAULT_PROJECTION_NAME, projection);
00722 }
00723 
00724 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00725 {
00726     if (projection)
00727         projections_[name] = projection;
00728     else
00729         logError("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00730 }
00731 
00732 bool ompl::base::StateSpace::isCompound(void) const
00733 {
00734     return false;
00735 }
00736 
00737 bool ompl::base::StateSpace::isDiscrete(void) const
00738 {
00739     return false;
00740 }
00741 
00742 bool ompl::base::StateSpace::isHybrid(void) const
00743 {
00744     return false;
00745 }
00746 
00747 void ompl::base::StateSpace::setStateSamplerAllocator(const StateSamplerAllocator &ssa)
00748 {
00749     ssa_ = ssa;
00750 }
00751 
00752 void ompl::base::StateSpace::clearStateSamplerAllocator(void)
00753 {
00754     ssa_ = StateSamplerAllocator();
00755 }
00756 
00757 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocStateSampler(void) const
00758 {
00759     if (ssa_)
00760         return ssa_(this);
00761     else
00762         return allocDefaultStateSampler();
00763 }
00764 
00765 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpacePtr &subspace) const
00766 {
00767     return allocSubspaceStateSampler(subspace.get());
00768 }
00769 
00770 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
00771 {
00772     if (subspace->getName() == getName())
00773         return allocStateSampler();
00774     return StateSamplerPtr(new SubspaceStateSampler(this, subspace, 1.0));
00775 }
00776 
00777 void ompl::base::StateSpace::setValidSegmentCountFactor(unsigned int factor)
00778 {
00779     if (factor < 1)
00780         throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00781     longestValidSegmentCountFactor_ = factor;
00782 }
00783 
00784 void ompl::base::StateSpace::setLongestValidSegmentFraction(double segmentFraction)
00785 {
00786     if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00787         throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00788     longestValidSegmentFraction_ = segmentFraction;
00789 }
00790 
00791 unsigned int ompl::base::StateSpace::getValidSegmentCountFactor(void) const
00792 {
00793     return longestValidSegmentCountFactor_;
00794 }
00795 
00796 double ompl::base::StateSpace::getLongestValidSegmentFraction(void) const
00797 {
00798     return longestValidSegmentFraction_;
00799 }
00800 
00801 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
00802 {
00803     return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00804 }
00805 
00806 ompl::base::CompoundStateSpace::CompoundStateSpace(void) : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00807 {
00808     setName("Compound" + getName());
00809 }
00810 
00811 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
00812                                                    const std::vector<double> &weights) :
00813     StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00814 {
00815     if (components.size() != weights.size())
00816         throw Exception("Number of component spaces and weights are not the same");
00817     setName("Compound" + getName());
00818     for (unsigned int i = 0 ; i < components.size() ; ++i)
00819         addSubspace(components[i], weights[i]);
00820 }
00821 
00822 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
00823 {
00824     if (locked_)
00825         throw Exception("This state space is locked. No further components can be added");
00826     if (weight < 0.0)
00827         throw Exception("Subspace weight cannot be negative");
00828     components_.push_back(component);
00829     weights_.push_back(weight);
00830     weightSum_ += weight;
00831     componentCount_ = components_.size();
00832 }
00833 
00834 bool ompl::base::CompoundStateSpace::isCompound(void) const
00835 {
00836     return true;
00837 }
00838 
00839 bool ompl::base::CompoundStateSpace::isHybrid(void) const
00840 {
00841     bool c = false;
00842     bool d = false;
00843     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00844     {
00845         if (components_[i]->isHybrid())
00846             return true;
00847         if (components_[i]->isDiscrete())
00848             d = true;
00849         else
00850             c = true;
00851     }
00852     return c && d;
00853 }
00854 
00855 unsigned int ompl::base::CompoundStateSpace::getSubspaceCount(void) const
00856 {
00857     return componentCount_;
00858 }
00859 
00860 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
00861 {
00862     if (componentCount_ > index)
00863         return components_[index];
00864     else
00865         throw Exception("Subspace index does not exist");
00866 }
00867 
00868 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
00869 {
00870     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00871         if (components_[i]->getName() == name)
00872             return true;
00873     return false;
00874 }
00875 
00876 unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string& name) const
00877 {
00878     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00879         if (components_[i]->getName() == name)
00880             return i;
00881     throw Exception("Subspace " + name + " does not exist");
00882 }
00883 
00884 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubspace(const std::string& name) const
00885 {
00886     return components_[getSubspaceIndex(name)];
00887 }
00888 
00889 double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
00890 {
00891     if (componentCount_ > index)
00892         return weights_[index];
00893     else
00894         throw Exception("Subspace index does not exist");
00895 }
00896 
00897 double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
00898 {
00899     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00900         if (components_[i]->getName() == name)
00901             return weights_[i];
00902     throw Exception("Subspace " + name + " does not exist");
00903 }
00904 
00905 void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
00906 {
00907     if (weight < 0.0)
00908         throw Exception("Subspace weight cannot be negative");
00909     if (componentCount_ > index)
00910     {
00911         weightSum_ += weight - weights_[index];
00912         weights_[index] = weight;
00913     }
00914     else
00915         throw Exception("Subspace index does not exist");
00916 }
00917 
00918 void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
00919 {
00920     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00921         if (components_[i]->getName() == name)
00922         {
00923             setSubspaceWeight(i, weight);
00924             return;
00925         }
00926     throw Exception("Subspace " + name + " does not exist");
00927 }
00928 
00929 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubspaces(void) const
00930 {
00931     return components_;
00932 }
00933 
00934 const std::vector<double>& ompl::base::CompoundStateSpace::getSubspaceWeights(void) const
00935 {
00936     return weights_;
00937 }
00938 
00939 unsigned int ompl::base::CompoundStateSpace::getDimension(void) const
00940 {
00941     unsigned int dim = 0;
00942     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00943         dim += components_[i]->getDimension();
00944     return dim;
00945 }
00946 
00947 double ompl::base::CompoundStateSpace::getMaximumExtent(void) const
00948 {
00949     double e = 0.0;
00950     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00951         if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
00952             e += weights_[i] * components_[i]->getMaximumExtent();
00953     return e;
00954 }
00955 
00956 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
00957 {
00958     CompoundState *cstate = static_cast<CompoundState*>(state);
00959     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00960         components_[i]->enforceBounds(cstate->components[i]);
00961 }
00962 
00963 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
00964 {
00965     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00966     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00967         if (!components_[i]->satisfiesBounds(cstate->components[i]))
00968             return false;
00969     return true;
00970 }
00971 
00972 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
00973 {
00974     CompoundState      *cdest = static_cast<CompoundState*>(destination);
00975     const CompoundState *csrc = static_cast<const CompoundState*>(source);
00976     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00977         components_[i]->copyState(cdest->components[i], csrc->components[i]);
00978 }
00979 
00980 unsigned int ompl::base::CompoundStateSpace::getSerializationLength(void) const
00981 {
00982     unsigned int l = 0;
00983     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00984         l += components_[i]->getSerializationLength();
00985     return l;
00986 }
00987 
00988 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
00989 {
00990     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00991     unsigned int l = 0;
00992     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00993     {
00994         components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->components[i]);
00995         l += components_[i]->getSerializationLength();
00996     }
00997 }
00998 
00999 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
01000 {
01001     CompoundState *cstate = static_cast<CompoundState*>(state);
01002     unsigned int l = 0;
01003     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01004     {
01005         components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char*>(serialization) + l);
01006         l += components_[i]->getSerializationLength();
01007     }
01008 }
01009 
01010 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
01011 {
01012     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01013     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01014     double dist = 0.0;
01015     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01016         dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
01017     return dist;
01018 }
01019 
01020 void ompl::base::CompoundStateSpace::setLongestValidSegmentFraction(double segmentFraction)
01021 {
01022     StateSpace::setLongestValidSegmentFraction(segmentFraction);
01023     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01024         components_[i]->setLongestValidSegmentFraction(segmentFraction);
01025 }
01026 
01027 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
01028 {
01029     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01030     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01031     unsigned int sc = 0;
01032     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01033     {
01034         unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
01035         if (sci > sc)
01036             sc = sci;
01037     }
01038     return sc;
01039 }
01040 
01041 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
01042 {
01043     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01044     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01045     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01046         if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
01047             return false;
01048     return true;
01049 }
01050 
01051 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
01052 {
01053     const CompoundState *cfrom  = static_cast<const CompoundState*>(from);
01054     const CompoundState *cto    = static_cast<const CompoundState*>(to);
01055     CompoundState       *cstate = static_cast<CompoundState*>(state);
01056     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01057         components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
01058 }
01059 
01060 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocDefaultStateSampler(void) const
01061 {
01062     CompoundStateSampler *ss = new CompoundStateSampler(this);
01063     if (weightSum_ < std::numeric_limits<double>::epsilon())
01064         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01065             ss->addSampler(components_[i]->allocStateSampler(), 1.0);
01066     else
01067         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01068             ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
01069     return StateSamplerPtr(ss);
01070 }
01071 
01072 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
01073 {
01074     if (subspace->getName() == getName())
01075         return allocStateSampler();
01076     if (hasSubspace(subspace->getName()))
01077         return StateSamplerPtr(new SubspaceStateSampler(this, subspace, getSubspaceWeight(subspace->getName()) / weightSum_));
01078     return StateSpace::allocSubspaceStateSampler(subspace);
01079 }
01080 
01081 ompl::base::State* ompl::base::CompoundStateSpace::allocState(void) const
01082 {
01083     CompoundState *state = new CompoundState();
01084     allocStateComponents(state);
01085     return static_cast<State*>(state);
01086 }
01087 
01088 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
01089 {
01090     state->components = new State*[componentCount_];
01091     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01092         state->components[i] = components_[i]->allocState();
01093 }
01094 
01095 void ompl::base::CompoundStateSpace::freeState(State *state) const
01096 {
01097     CompoundState *cstate = static_cast<CompoundState*>(state);
01098     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01099         components_[i]->freeState(cstate->components[i]);
01100     delete[] cstate->components;
01101     delete cstate;
01102 }
01103 
01104 void ompl::base::CompoundStateSpace::lock(void)
01105 {
01106     locked_ = true;
01107 }
01108 
01109 bool ompl::base::CompoundStateSpace::isLocked(void) const
01110 {
01111     return locked_;
01112 }
01113 
01114 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
01115 {
01116     CompoundState *cstate = static_cast<CompoundState*>(state);
01117     unsigned int idx = 0;
01118 
01119     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01120         for (unsigned int j = 0 ; j <= index ; ++j)
01121         {
01122             double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
01123             if (va)
01124             {
01125                 if (idx == index)
01126                     return va;
01127                 else
01128                     idx++;
01129             }
01130             else
01131                 break;
01132         }
01133     return NULL;
01134 }
01135 
01136 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
01137 {
01138     out << "Compound state [" << std::endl;
01139     const CompoundState *cstate = static_cast<const CompoundState*>(state);
01140     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01141         components_[i]->printState(cstate->components[i], out);
01142     out << "]" << std::endl;
01143 }
01144 
01145 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
01146 {
01147     out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
01148     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01149     {
01150         components_[i]->printSettings(out);
01151         out << " of weight " << weights_[i] << std::endl;
01152     }
01153     out << "]" << std::endl;
01154     printProjections(out);
01155 }
01156 
01157 void ompl::base::CompoundStateSpace::setup(void)
01158 {
01159     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01160         components_[i]->setup();
01161 
01162     StateSpace::setup();
01163 }
01164 
01165 void ompl::base::CompoundStateSpace::computeLocations(void)
01166 {
01167     StateSpace::computeLocations();
01168     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01169         components_[i]->computeLocations();
01170 }
01171 
01172 namespace ompl
01173 {
01174     namespace base
01175     {
01176 
01177         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
01178         {
01179           return copyStateData(destS.get(), dest, sourceS.get(), source);
01180         }
01181 
01182         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS, const State *source)
01183         {
01184             // if states correspond to the same space, simply do copy
01185             if (destS->getName() == sourceS->getName())
01186             {
01187                 if (dest != source)
01188                     destS->copyState(dest, source);
01189                 return ALL_DATA_COPIED;
01190             }
01191 
01192             AdvancedStateCopyOperation result = NO_DATA_COPIED;
01193 
01194             // if "to" state is compound
01195             if (destS->isCompound())
01196             {
01197                 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
01198                 CompoundState *compoundDest = dest->as<CompoundState>();
01199 
01200                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01201                 for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
01202                     if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
01203                     {
01204                         if (compoundDest->components[i] != source)
01205                             compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
01206                         return ALL_DATA_COPIED;
01207                     }
01208 
01209                 // it could be there are further levels of compound spaces where the data can be set
01210                 // so we call this function recursively
01211                 for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
01212                 {
01213                     AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(), compoundDest->components[i], sourceS, source);
01214 
01215                     if (res != NO_DATA_COPIED)
01216                         result = SOME_DATA_COPIED;
01217 
01218                     // if all data was copied, we stop
01219                     if (res == ALL_DATA_COPIED)
01220                         return ALL_DATA_COPIED;
01221                 }
01222             }
01223 
01224             // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
01225             // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
01226             if (sourceS->isCompound())
01227             {
01228                 const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
01229                 const CompoundState *compoundSource = source->as<CompoundState>();
01230 
01231                 unsigned int copiedComponents = 0;
01232 
01233                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01234                 for (unsigned int i = 0 ; i < compoundSourceS->getSubspaceCount() ; ++i)
01235                 {
01236                     AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(), compoundSource->components[i]);
01237                     if (res == ALL_DATA_COPIED)
01238                         copiedComponents++;
01239                     if (res != NO_DATA_COPIED)
01240                         result = SOME_DATA_COPIED;
01241                 }
01242 
01243                 // if each individual component got copied, then the entire data in "from" got copied
01244                 if (copiedComponents == compoundSourceS->getSubspaceCount())
01245                     result = ALL_DATA_COPIED;
01246             }
01247 
01248             return result;
01249         }
01250     
01251         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
01252                                                  const StateSpacePtr &sourceS, const State *source,
01253                                                  const std::vector<std::string> &subspaces)
01254         {
01255             return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
01256         }   
01257 
01258         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
01259                                                  const StateSpace *sourceS, const State *source,
01260                                                  const std::vector<std::string> &subspaces)
01261         {
01262             std::size_t copyCount = 0;
01263             const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
01264             const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc = sourceS->getSubstateLocationsByName();
01265             for (std::size_t i = 0 ; i < subspaces.size() ; ++i)
01266             {
01267                 std::map<std::string, StateSpace::SubstateLocation>::const_iterator dt = destLoc.find(subspaces[i]);
01268                 if (dt != destLoc.end())
01269                 {
01270                     std::map<std::string, StateSpace::SubstateLocation>::const_iterator st = sourceLoc.find(subspaces[i]);
01271                     if (st != sourceLoc.end())
01272                     {
01273                         dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second), sourceS->getSubstateAtLocation(source, st->second));
01274                         ++copyCount;
01275                     }
01276                 }
01277             }
01278             if (copyCount == subspaces.size())
01279                 return ALL_DATA_COPIED;
01280             if (copyCount > 0)
01281                 return SOME_DATA_COPIED;
01282             return NO_DATA_COPIED;
01283         }
01284     
01286         inline bool StateSpaceHasContent(const StateSpacePtr &m)
01287         {
01288             if (!m)
01289                 return false;
01290             if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
01291             {
01292                 const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
01293                 for (unsigned int i = 0 ; i < nc ; ++i)
01294                     if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
01295                         return true;
01296                 return false;
01297             }
01298             return true;
01299         }
01301 
01302         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
01303         {
01304             if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
01305                 return b;
01306 
01307             if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
01308                 return a;
01309 
01310             std::vector<StateSpacePtr> components;
01311             std::vector<double>           weights;
01312 
01313             bool change = false;
01314             if (a)
01315             {
01316                 bool used = false;
01317                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01318                     if (!csm_a->isLocked())
01319                     {
01320                         used = true;
01321                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01322                         {
01323                             components.push_back(csm_a->getSubspace(i));
01324                             weights.push_back(csm_a->getSubspaceWeight(i));
01325                         }
01326                     }
01327 
01328                 if (!used)
01329                 {
01330                     components.push_back(a);
01331                     weights.push_back(1.0);
01332                 }
01333             }
01334             if (b)
01335             {
01336                 bool used = false;
01337                 unsigned int size = components.size();
01338 
01339                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01340                     if (!csm_b->isLocked())
01341                     {
01342                         used = true;
01343                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01344                         {
01345                             bool ok = true;
01346                             for (unsigned int j = 0 ; j < size ; ++j)
01347                                 if (components[j]->getName() == csm_b->getSubspace(i)->getName())
01348                                 {
01349                                     ok = false;
01350                                     break;
01351                                 }
01352                             if (ok)
01353                             {
01354                                 components.push_back(csm_b->getSubspace(i));
01355                                 weights.push_back(csm_b->getSubspaceWeight(i));
01356                                 change = true;
01357                             }
01358                         }
01359                         if (components.size() == csm_b->getSubspaceCount())
01360                             return b;
01361                     }
01362 
01363                 if (!used)
01364                 {
01365                     bool ok = true;
01366                     for (unsigned int j = 0 ; j < size ; ++j)
01367                         if (components[j]->getName() == b->getName())
01368                         {
01369                             ok = false;
01370                             break;
01371                         }
01372                     if (ok)
01373                     {
01374                         components.push_back(b);
01375                         weights.push_back(1.0);
01376                         change = true;
01377                     }
01378                 }
01379             }
01380 
01381             if (!change && a)
01382                 return a;
01383 
01384             if (components.size() == 1)
01385                 return components[0];
01386 
01387             return StateSpacePtr(new CompoundStateSpace(components, weights));
01388         }
01389 
01390         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
01391         {
01392             std::vector<StateSpacePtr> components_a;
01393             std::vector<double>           weights_a;
01394             std::vector<StateSpacePtr> components_b;
01395 
01396             if (a)
01397             {
01398                 bool used = false;
01399                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01400                     if (!csm_a->isLocked())
01401                     {
01402                         used = true;
01403                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01404                         {
01405                             components_a.push_back(csm_a->getSubspace(i));
01406                             weights_a.push_back(csm_a->getSubspaceWeight(i));
01407                         }
01408                     }
01409 
01410                 if (!used)
01411                 {
01412                     components_a.push_back(a);
01413                     weights_a.push_back(1.0);
01414                 }
01415             }
01416 
01417             if (b)
01418             {
01419                 bool used = false;
01420                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01421                     if (!csm_b->isLocked())
01422                     {
01423                         used = true;
01424                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01425                             components_b.push_back(csm_b->getSubspace(i));
01426                     }
01427                 if (!used)
01428                     components_b.push_back(b);
01429             }
01430 
01431             bool change = false;
01432             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01433                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01434                     if (components_a[j]->getName() == components_b[i]->getName())
01435                     {
01436                         components_a.erase(components_a.begin() + j);
01437                         weights_a.erase(weights_a.begin() + j);
01438                         change = true;
01439                         break;
01440                     }
01441 
01442             if (!change && a)
01443                 return a;
01444 
01445             if (components_a.size() == 1)
01446                 return components_a[0];
01447 
01448             return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
01449         }
01450 
01451         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
01452         {
01453             std::vector<StateSpacePtr> components;
01454             std::vector<double>           weights;
01455 
01456             bool change = false;
01457             if (a)
01458             {
01459                 bool used = false;
01460                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01461                     if (!csm_a->isLocked())
01462                     {
01463                         used = true;
01464                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01465                         {
01466                             if (csm_a->getSubspace(i)->getName() == name)
01467                             {
01468                                 change = true;
01469                                 continue;
01470                             }
01471                             components.push_back(csm_a->getSubspace(i));
01472                             weights.push_back(csm_a->getSubspaceWeight(i));
01473                         }
01474                     }
01475 
01476                 if (!used)
01477                 {
01478                     if (a->getName() != name)
01479                     {
01480                         components.push_back(a);
01481                         weights.push_back(1.0);
01482                     }
01483                     else
01484                         change = true;
01485                 }
01486             }
01487 
01488             if (!change && a)
01489                 return a;
01490 
01491             if (components.size() == 1)
01492                 return components[0];
01493 
01494             return StateSpacePtr(new CompoundStateSpace(components, weights));
01495         }
01496 
01497         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
01498         {
01499             std::vector<StateSpacePtr> components_a;
01500             std::vector<double>           weights_a;
01501             std::vector<StateSpacePtr> components_b;
01502             std::vector<double>           weights_b;
01503 
01504             if (a)
01505             {
01506                 bool used = false;
01507                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01508                     if (!csm_a->isLocked())
01509                     {
01510                         used = true;
01511                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01512                         {
01513                             components_a.push_back(csm_a->getSubspace(i));
01514                             weights_a.push_back(csm_a->getSubspaceWeight(i));
01515                         }
01516                     }
01517 
01518                 if (!used)
01519                 {
01520                     components_a.push_back(a);
01521                     weights_a.push_back(1.0);
01522                 }
01523             }
01524 
01525             if (b)
01526             {
01527                 bool used = false;
01528                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01529                     if (!csm_b->isLocked())
01530                     {
01531                         used = true;
01532                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01533                         {
01534                             components_b.push_back(csm_b->getSubspace(i));
01535                             weights_b.push_back(csm_b->getSubspaceWeight(i));
01536                         }
01537                     }
01538 
01539                 if (!used)
01540                 {
01541                     components_b.push_back(b);
01542                     weights_b.push_back(1.0);
01543                 }
01544             }
01545 
01546             std::vector<StateSpacePtr> components;
01547             std::vector<double>           weights;
01548 
01549             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01550             {
01551                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01552                     if (components_a[j]->getName() == components_b[i]->getName())
01553                     {
01554                         components.push_back(components_b[i]);
01555                         weights.push_back(std::max(weights_a[j], weights_b[i]));
01556                         break;
01557                     }
01558             }
01559 
01560             if (a && components.size() == components_a.size())
01561                 return a;
01562 
01563             if (b && components.size() == components_b.size())
01564                 return b;
01565 
01566             if (components.size() == 1)
01567                 return components[0];
01568 
01569             return StateSpacePtr(new CompoundStateSpace(components, weights));
01570         }
01571     }
01572 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines