All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 
00048 const std::string ompl::base::StateSpace::DEFAULT_PROJECTION_NAME = "";
00049 
00051 namespace ompl
00052 {
00053     namespace base
00054     {
00055         struct AllocatedSpaces
00056         {
00057             std::list<StateSpace*> list_;
00058             boost::mutex           lock_;
00059         };
00060 
00061         static AllocatedSpaces& getAllocatedSpaces(void)
00062         {
00063             static AllocatedSpaces as;
00064             return as;
00065         }
00066     }
00067 }
00069 
00070 ompl::base::StateSpace::StateSpace(void)
00071 {
00072     // autocompute a unique name
00073     static boost::mutex lock;
00074     static unsigned int m = 0;
00075 
00076     lock.lock();
00077     m++;
00078     lock.unlock();
00079 
00080     name_ = "Space" + boost::lexical_cast<std::string>(m);
00081     msg_.setPrefix(name_);
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", msg_,
00092                                  boost::bind(&StateSpace::setLongestValidSegmentFraction, this, _1),
00093                                  boost::bind(&StateSpace::getLongestValidSegmentFraction, this));
00094 
00095     params_.declareParam<unsigned int>("valid_segment_count_factor", msg_,
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, std::vector<StateSpace::ValueLocation> &locationsArray,
00129                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap, StateSpace::ValueLocation loc)
00130         {
00131             State *test = s->allocState();
00132             if (s->getValueAddressAtIndex(test, 0) != NULL)
00133             {
00134                 loc.index = 0;
00135                 loc.space = s;
00136                 locationsMap[s->getName()] = loc;
00137                 // if the space is compound, we will find this value again in the first subspace
00138                 if (!s->isCompound())
00139                 {
00140                     if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00141                     {
00142                         const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
00143                         if (!name.empty())
00144                             locationsMap[name] = loc;
00145                     }
00146                     locationsArray.push_back(loc);
00147                     while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
00148                     {
00149                         if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00150                         {
00151                             const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
00152                             if (!name.empty())
00153                                 locationsMap[name] = loc;
00154                         }
00155                         locationsArray.push_back(loc);
00156                     }
00157                 }
00158             }
00159             s->freeState(test);
00160 
00161             if (s->isCompound())
00162                 for (unsigned int i = 0 ; i < s->as<base::CompoundStateSpace>()->getSubSpaceCount() ; ++i)
00163                 {
00164                     loc.chain.push_back(i);
00165                     computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubSpace(i).get(), locationsArray, locationsMap, loc);
00166                     loc.chain.pop_back();
00167                 }
00168         }
00169 
00170         void computeLocationsHelper(const StateSpace *s, std::vector<StateSpace::ValueLocation> &locationsArray,
00171                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap)
00172         {
00173             locationsArray.clear();
00174             locationsMap.clear();
00175             computeLocationsHelper(s, locationsArray, locationsMap, StateSpace::ValueLocation());
00176         }
00177     }
00178 }
00180 
00181 const std::string& ompl::base::StateSpace::getName(void) const
00182 {
00183     return name_;
00184 }
00185 
00186 void ompl::base::StateSpace::setName(const std::string &name)
00187 {
00188     name_ = name;
00189     msg_.setPrefix(name_);
00190 
00191     // we don't want to call this function during the state space construction because calls to virtual functions are made,
00192     // so we check if any values were previously inserted as value locations;
00193     // if none were, then we either have none (so no need to call this function again)
00194     // or setup() was not yet called
00195     if (!valueLocationsInOrder_.empty())
00196         computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00197 }
00198 
00199 void ompl::base::StateSpace::computeLocations(void)
00200 {
00201     computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00202 }
00203 
00204 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
00205 {
00206     signature.clear();
00207     computeStateSpaceSignatureHelper(this, signature);
00208     signature.insert(signature.begin(), signature.size());
00209 }
00210 
00211 void ompl::base::StateSpace::registerProjections(void)
00212 {
00213 }
00214 
00215 void ompl::base::StateSpace::setup(void)
00216 {
00217     maxExtent_ = getMaximumExtent();
00218     longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00219 
00220     if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00221         throw Exception("The longest valid segment for state space " + getName() + " must be positive");
00222 
00223     computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00224 
00225     // make sure we don't overwrite projections that have been configured by the user
00226     std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00227     registerProjections();
00228     for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00229         if (it->second->userConfigured())
00230         {
00231             std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00232             if (o != projections_.end())
00233                 if (!o->second->userConfigured())
00234                     projections_[it->first] = it->second;
00235         }
00236 
00237     // remove previously set parameters for projections
00238     std::vector<std::string> pnames;
00239     params_.getParamNames(pnames);
00240     for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
00241         if (it->substr(0, 11) == "projection.")
00242             params_.remove(*it);
00243 
00244     // setup projections and add their parameters
00245     for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00246     {
00247         it->second->setup();
00248         if (it->first == DEFAULT_PROJECTION_NAME)
00249             params_.include(it->second->params(), "projection");
00250         else
00251             params_.include(it->second->params(), "projection." + it->first);
00252     }
00253 }
00254 
00255 double* ompl::base::StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00256 {
00257     return NULL;
00258 }
00259 
00260 const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
00261 {
00262     double *val = getValueAddressAtIndex(const_cast<State*>(state), index); // this const-cast does not hurt, since the state is not modified
00263     return val;
00264 }
00265 
00266 const std::vector<ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocations(void) const
00267 {
00268     return valueLocationsInOrder_;
00269 }
00270 
00271 const std::map<std::string, ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocationsByName(void) const
00272 {
00273     return valueLocationsByName_;
00274 }
00275 
00276 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
00277 {
00278     reals.resize(valueLocationsInOrder_.size());
00279     for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
00280         reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
00281 }
00282 
00283 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
00284 {
00285     assert(reals.size() == valueLocationsInOrder_.size());
00286     for (std::size_t i = 0 ; i < reals.size() ; ++i)
00287         *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
00288 }
00289 
00290 double* ompl::base::StateSpace::getValueAddressAtLocation(State *state, const ValueLocation &loc) const
00291 {
00292     std::size_t index = 0;
00293     while (loc.chain.size() > index)
00294         state = state->as<CompoundState>()->components[loc.chain[index++]];
00295     return loc.space->getValueAddressAtIndex(state, loc.index);
00296 }
00297 
00298 const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
00299 {
00300     std::size_t index = 0;
00301     while (loc.chain.size() > index)
00302         state = state->as<CompoundState>()->components[loc.chain[index++]];
00303     return loc.space->getValueAddressAtIndex(state, loc.index);
00304 }
00305 
00306 double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
00307 {
00308     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00309     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00310 }
00311 
00312 const double* ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
00313 {
00314     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00315     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00316 }
00317 
00318 unsigned int ompl::base::StateSpace::getSerializationLength(void) const
00319 {
00320     return 0;
00321 }
00322 
00323 void ompl::base::StateSpace::serialize(void *serialization, const State *state) const
00324 {
00325 }
00326 
00327 void ompl::base::StateSpace::deserialize(State *state, const void *serialization) const
00328 {
00329 }
00330 
00331 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
00332 {
00333     out << "State instance [" << state << ']' << std::endl;
00334 }
00335 
00336 void ompl::base::StateSpace::printSettings(std::ostream &out) const
00337 {
00338     out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
00339     printProjections(out);
00340 }
00341 
00342 void ompl::base::StateSpace::printProjections(std::ostream &out) const
00343 {
00344     if (projections_.empty())
00345         out << "No registered projections" << std::endl;
00346     else
00347     {
00348         out << "Registered projections:" << std::endl;
00349         for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00350         {
00351             out << "  - ";
00352             if (it->first == DEFAULT_PROJECTION_NAME)
00353                 out << "<default>";
00354             else
00355                 out << it->first;
00356             out << std::endl;
00357             it->second->printSettings(out);
00358         }
00359     }
00360 }
00361 
00363 namespace ompl
00364 {
00365     namespace base
00366     {
00367         static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
00368         {
00369             std::queue<const StateSpace*> q;
00370             q.push(self);
00371             while (!q.empty())
00372             {
00373                 const StateSpace *m = q.front();
00374                 q.pop();
00375                 if (m->getName() == other->getName())
00376                     return true;
00377                 if (m->isCompound())
00378                 {
00379                     unsigned int c = m->as<CompoundStateSpace>()->getSubSpaceCount();
00380                     for (unsigned int i = 0 ; i < c ; ++i)
00381                         q.push(m->as<CompoundStateSpace>()->getSubSpace(i).get());
00382                 }
00383             }
00384             return false;
00385         }
00386 
00387         static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
00388         {
00389             if (StateSpaceIncludes(self, other))
00390                 return true;
00391             else
00392                 if (other->isCompound())
00393                 {
00394                     unsigned int c = other->as<CompoundStateSpace>()->getSubSpaceCount();
00395                     for (unsigned int i = 0 ; i < c ; ++i)
00396                         if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubSpace(i).get()))
00397                             return false;
00398                     return true;
00399                 }
00400             return false;
00401         }
00402     }
00403 }
00404 
00406 
00407 bool ompl::base::StateSpace::covers(const StateSpacePtr &other) const
00408 {
00409     return StateSpaceCovers(this, other.get());
00410 }
00411 
00412 bool ompl::base::StateSpace::includes(const StateSpacePtr &other) const
00413 {
00414     return StateSpaceIncludes(this, other.get());
00415 }
00416 
00417 void ompl::base::StateSpace::List(std::ostream &out)
00418 {
00419     AllocatedSpaces &as = getAllocatedSpaces();
00420     boost::mutex::scoped_lock smLock(as.lock_);
00421     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00422         out << "@ " << *it << ": " << (*it)->getName() << std::endl;
00423 }
00424 
00425 void ompl::base::StateSpace::Diagram(std::ostream &out)
00426 {
00427     AllocatedSpaces &as = getAllocatedSpaces();
00428     boost::mutex::scoped_lock smLock(as.lock_);
00429     out << "digraph StateSpaces {" << std::endl;
00430     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00431     {
00432         out << '"' << (*it)->getName() << '"' << std::endl;
00433         for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
00434             if (it != jt)
00435             {
00436                 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubSpace((*jt)->getName()))
00437                     out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00438                         boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubSpaceWeight((*jt)->getName())) <<
00439                         "\"];" << std::endl;
00440                 else
00441                     if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
00442                         out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00443             }
00444     }
00445     out << '}' << std::endl;
00446 }
00447 
00448 void ompl::base::StateSpace::sanityChecks(void) const
00449 {
00450     sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), ~0);
00451 }
00452 
00453 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
00454 {
00455     // Test that distances are always positive
00456     {
00457         double maxExt = getMaximumExtent();
00458 
00459         State *s1 = allocState();
00460         State *s2 = allocState();
00461         StateSamplerPtr ss = allocStateSampler();
00462 
00463         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00464         {
00465             ss->sampleUniform(s1);
00466             if (flags & STATESPACE_DISTANCE_TO_SELF && distance(s1, s1) > eps)
00467                 throw Exception("Distance from a state to itself should be 0");
00468             if (flags & STATESPACE_EQUAL_TO_SELF && !equalStates(s1, s1))
00469                 throw Exception("A state should be equal to itself");
00470             ss->sampleUniform(s2);
00471             if (!equalStates(s1, s2))
00472             {
00473                 double d12 = distance(s1, s2);
00474                 if (flags & STATESPACE_DISTANCE_DIFFERENT_STATES && d12 < zero)
00475                     throw Exception("Distance between different states should be above 0");
00476                 double d21 = distance(s2, s1);
00477                 if (flags & STATESPACE_DISTANCE_SYMMETRIC && fabs(d12 - d21) > eps)
00478                     throw Exception("The distance function should be symmetric (A->B=" +
00479                                     boost::lexical_cast<std::string>(d12) + ", B->A=" +
00480                                     boost::lexical_cast<std::string>(d21) + ", difference is " +
00481                                     boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
00482                 if (flags & STATESPACE_DISTANCE_BOUND)
00483                     if (d12 > maxExt + zero)
00484                         throw Exception("The distance function should not report values larger than the maximum extent ("+
00485                                         boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
00486             }
00487         }
00488 
00489         freeState(s1);
00490         freeState(s2);
00491     }
00492 
00493 
00494     // Test that interpolation works as expected and also test triangle inequality
00495     if (!isDiscrete() && !isHybrid())
00496     {
00497         State *s1 = allocState();
00498         State *s2 = allocState();
00499         State *s3 = allocState();
00500         StateSamplerPtr ss = allocStateSampler();
00501 
00502         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00503         {
00504             ss->sampleUniform(s1);
00505             ss->sampleUniform(s2);
00506             ss->sampleUniform(s3);
00507 
00508             interpolate(s1, s2, 0.0, s3);
00509             if (flags & STATESPACE_INTERPOLATION && distance(s1, s3) > eps)
00510                 throw Exception("Interpolation from a state at time 0 should be not change the original state");
00511 
00512             interpolate(s1, s2, 1.0, s3);
00513             if (flags & STATESPACE_INTERPOLATION && distance(s2, s3) > eps)
00514                 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
00515 
00516             interpolate(s1, s2, 0.5, s3);
00517             double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
00518             if (flags & STATESPACE_TRIANGLE_INEQUALITY && fabs(diff) > eps)
00519                 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
00520                                 boost::lexical_cast<std::string>(diff) + " difference)");
00521 
00522             interpolate(s3, s2, 0.5, s3);
00523             interpolate(s1, s2, 0.75, s2);
00524 
00525             if (flags & STATESPACE_INTERPOLATION && distance(s2, s3) > eps)
00526                 throw Exception("Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
00527         }
00528         freeState(s1);
00529         freeState(s2);
00530         freeState(s3);
00531     }
00532 }
00533 
00534 bool ompl::base::StateSpace::hasDefaultProjection(void) const
00535 {
00536     return hasProjection(DEFAULT_PROJECTION_NAME);
00537 }
00538 
00539 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
00540 {
00541     return projections_.find(name) != projections_.end();
00542 }
00543 
00544 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection(void) const
00545 {
00546     if (hasDefaultProjection())
00547         return getProjection(DEFAULT_PROJECTION_NAME);
00548     else
00549     {
00550         msg_.error("No default projection is set. Perhaps setup() needs to be called");
00551         return ProjectionEvaluatorPtr();
00552     }
00553 }
00554 
00555 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
00556 {
00557     std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00558     if (it != projections_.end())
00559         return it->second;
00560     else
00561     {
00562         msg_.error("Projection '" + name + "' is not defined");
00563         return ProjectionEvaluatorPtr();
00564     }
00565 }
00566 
00567 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections(void) const
00568 {
00569     return projections_;
00570 }
00571 
00572 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00573 {
00574     registerProjection(DEFAULT_PROJECTION_NAME, projection);
00575 }
00576 
00577 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00578 {
00579     if (projection)
00580         projections_[name] = projection;
00581     else
00582         msg_.error("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00583 }
00584 
00585 bool ompl::base::StateSpace::isCompound(void) const
00586 {
00587     return false;
00588 }
00589 
00590 bool ompl::base::StateSpace::isDiscrete(void) const
00591 {
00592     return false;
00593 }
00594 
00595 bool ompl::base::StateSpace::isHybrid(void) const
00596 {
00597     return false;
00598 }
00599 
00600 void ompl::base::StateSpace::setStateSamplerAllocator(const StateSamplerAllocator &ssa)
00601 {
00602     ssa_ = ssa;
00603 }
00604 
00605 void ompl::base::StateSpace::clearStateSamplerAllocator(void)
00606 {
00607     ssa_ = StateSamplerAllocator();
00608 }
00609 
00610 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocStateSampler(void) const
00611 {
00612     if (ssa_)
00613         return ssa_(this);
00614     else
00615         return allocDefaultStateSampler();
00616 }
00617 
00618 void ompl::base::StateSpace::setValidSegmentCountFactor(unsigned int factor)
00619 {
00620     if (factor < 1)
00621         throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00622     longestValidSegmentCountFactor_ = factor;
00623 }
00624 
00625 void ompl::base::StateSpace::setLongestValidSegmentFraction(double segmentFraction)
00626 {
00627     if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00628         throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00629     longestValidSegmentFraction_ = segmentFraction;
00630 }
00631 
00632 unsigned int ompl::base::StateSpace::getValidSegmentCountFactor(void) const
00633 {
00634     return longestValidSegmentCountFactor_;
00635 }
00636 
00637 double ompl::base::StateSpace::getLongestValidSegmentFraction(void) const
00638 {
00639     return longestValidSegmentFraction_;
00640 }
00641 
00642 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
00643 {
00644     return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00645 }
00646 
00647 ompl::base::CompoundStateSpace::CompoundStateSpace(void) : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00648 {
00649     setName("Compound" + getName());
00650 }
00651 
00652 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
00653                                                    const std::vector<double> &weights) :
00654     StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00655 {
00656     if (components.size() != weights.size())
00657         throw Exception("Number of component spaces and weights are not the same");
00658     setName("Compound" + getName());
00659     for (unsigned int i = 0 ; i < components.size() ; ++i)
00660         addSubSpace(components[i], weights[i]);
00661 }
00662 
00663 void ompl::base::CompoundStateSpace::addSubSpace(const StateSpacePtr &component, double weight)
00664 {
00665     if (locked_)
00666         throw Exception("This state space is locked. No further components can be added");
00667     if (weight < 0.0)
00668         throw Exception("Subspace weight cannot be negative");
00669     components_.push_back(component);
00670     weights_.push_back(weight);
00671     weightSum_ += weight;
00672     componentCount_ = components_.size();
00673 }
00674 
00675 bool ompl::base::CompoundStateSpace::isCompound(void) const
00676 {
00677     return true;
00678 }
00679 
00680 bool ompl::base::CompoundStateSpace::isHybrid(void) const
00681 {
00682     bool c = false;
00683     bool d = false;
00684     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00685     {
00686         if (components_[i]->isHybrid())
00687             return true;
00688         if (components_[i]->isDiscrete())
00689             d = true;
00690         else
00691             c = true;
00692     }
00693     return c && d;
00694 }
00695 
00696 unsigned int ompl::base::CompoundStateSpace::getSubSpaceCount(void) const
00697 {
00698     return componentCount_;
00699 }
00700 
00701 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const unsigned int index) const
00702 {
00703     if (componentCount_ > index)
00704         return components_[index];
00705     else
00706         throw Exception("Subspace index does not exist");
00707 }
00708 
00709 bool ompl::base::CompoundStateSpace::hasSubSpace(const std::string &name) const
00710 {
00711     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00712         if (components_[i]->getName() == name)
00713             return true;
00714     return false;
00715 }
00716 
00717 unsigned int ompl::base::CompoundStateSpace::getSubSpaceIndex(const std::string& name) const
00718 {
00719     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00720         if (components_[i]->getName() == name)
00721             return i;
00722     throw Exception("Subspace " + name + " does not exist");
00723 }
00724 
00725 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const std::string& name) const
00726 {
00727     return components_[getSubSpaceIndex(name)];
00728 }
00729 
00730 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const unsigned int index) const
00731 {
00732     if (componentCount_ > index)
00733         return weights_[index];
00734     else
00735         throw Exception("Subspace index does not exist");
00736 }
00737 
00738 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const std::string &name) const
00739 {
00740     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00741         if (components_[i]->getName() == name)
00742             return weights_[i];
00743     throw Exception("Subspace " + name + " does not exist");
00744 }
00745 
00746 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const unsigned int index, double weight)
00747 {
00748     if (weight < 0.0)
00749         throw Exception("Subspace weight cannot be negative");
00750     if (componentCount_ > index)
00751     {
00752         weightSum_ += weight - weights_[index];
00753         weights_[index] = weight;
00754     }
00755     else
00756         throw Exception("Subspace index does not exist");
00757 }
00758 
00759 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const std::string &name, double weight)
00760 {
00761     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00762         if (components_[i]->getName() == name)
00763         {
00764             setSubSpaceWeight(i, weight);
00765             return;
00766         }
00767     throw Exception("Subspace " + name + " does not exist");
00768 }
00769 
00770 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubSpaces(void) const
00771 {
00772     return components_;
00773 }
00774 
00775 const std::vector<double>& ompl::base::CompoundStateSpace::getSubSpaceWeights(void) const
00776 {
00777     return weights_;
00778 }
00779 
00780 unsigned int ompl::base::CompoundStateSpace::getDimension(void) const
00781 {
00782     unsigned int dim = 0;
00783     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00784         dim += components_[i]->getDimension();
00785     return dim;
00786 }
00787 
00788 double ompl::base::CompoundStateSpace::getMaximumExtent(void) const
00789 {
00790     double e = 0.0;
00791     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00792         if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
00793             e += weights_[i] * components_[i]->getMaximumExtent();
00794     return e;
00795 }
00796 
00797 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
00798 {
00799     CompoundState *cstate = static_cast<CompoundState*>(state);
00800     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00801         components_[i]->enforceBounds(cstate->components[i]);
00802 }
00803 
00804 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
00805 {
00806     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00807     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00808         if (!components_[i]->satisfiesBounds(cstate->components[i]))
00809             return false;
00810     return true;
00811 }
00812 
00813 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
00814 {
00815     CompoundState      *cdest = static_cast<CompoundState*>(destination);
00816     const CompoundState *csrc = static_cast<const CompoundState*>(source);
00817     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00818         components_[i]->copyState(cdest->components[i], csrc->components[i]);
00819 }
00820 
00821 unsigned int ompl::base::CompoundStateSpace::getSerializationLength(void) const
00822 {
00823     unsigned int l = 0;
00824     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00825         l += components_[i]->getSerializationLength();
00826     return l;
00827 }
00828 
00829 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
00830 {
00831     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00832     unsigned int l = 0;
00833     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00834     {
00835         components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->components[i]);
00836         l += components_[i]->getSerializationLength();
00837     }
00838 }
00839 
00840 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
00841 {
00842     CompoundState *cstate = static_cast<CompoundState*>(state);
00843     unsigned int l = 0;
00844     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00845     {
00846         components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char*>(serialization) + l);
00847         l += components_[i]->getSerializationLength();
00848     }
00849 }
00850 
00851 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
00852 {
00853     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00854     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00855     double dist = 0.0;
00856     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00857         dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
00858     return dist;
00859 }
00860 
00861 void ompl::base::CompoundStateSpace::setLongestValidSegmentFraction(double segmentFraction)
00862 {
00863     StateSpace::setLongestValidSegmentFraction(segmentFraction);
00864     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00865         components_[i]->setLongestValidSegmentFraction(segmentFraction);
00866 }
00867 
00868 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
00869 {
00870     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00871     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00872     unsigned int sc = 0;
00873     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00874     {
00875         unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
00876         if (sci > sc)
00877             sc = sci;
00878     }
00879     return sc;
00880 }
00881 
00882 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
00883 {
00884     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00885     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00886     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00887         if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
00888             return false;
00889     return true;
00890 }
00891 
00892 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00893 {
00894     const CompoundState *cfrom  = static_cast<const CompoundState*>(from);
00895     const CompoundState *cto    = static_cast<const CompoundState*>(to);
00896     CompoundState       *cstate = static_cast<CompoundState*>(state);
00897     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00898         components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
00899 }
00900 
00901 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocDefaultStateSampler(void) const
00902 {
00903     CompoundStateSampler *ss = new CompoundStateSampler(this);
00904     if (weightSum_ < std::numeric_limits<double>::epsilon())
00905         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00906             ss->addSampler(components_[i]->allocStateSampler(), 1.0);
00907     else
00908         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00909             ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
00910     return StateSamplerPtr(ss);
00911 }
00912 
00913 ompl::base::State* ompl::base::CompoundStateSpace::allocState(void) const
00914 {
00915     CompoundState *state = new CompoundState();
00916     allocStateComponents(state);
00917     return static_cast<State*>(state);
00918 }
00919 
00920 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
00921 {
00922     state->components = new State*[componentCount_];
00923     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00924         state->components[i] = components_[i]->allocState();
00925 }
00926 
00927 void ompl::base::CompoundStateSpace::freeState(State *state) const
00928 {
00929     CompoundState *cstate = static_cast<CompoundState*>(state);
00930     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00931         components_[i]->freeState(cstate->components[i]);
00932     delete[] cstate->components;
00933     delete cstate;
00934 }
00935 
00936 void ompl::base::CompoundStateSpace::lock(void)
00937 {
00938     locked_ = true;
00939 }
00940 
00941 bool ompl::base::CompoundStateSpace::isLocked(void) const
00942 {
00943     return locked_;
00944 }
00945 
00946 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00947 {
00948     CompoundState *cstate = static_cast<CompoundState*>(state);
00949     unsigned int idx = 0;
00950 
00951     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00952         for (unsigned int j = 0 ; j <= index ; ++j)
00953         {
00954             double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
00955             if (va)
00956             {
00957                 if (idx == index)
00958                     return va;
00959                 else
00960                     idx++;
00961             }
00962             else
00963                 break;
00964         }
00965     return NULL;
00966 }
00967 
00968 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
00969 {
00970     out << "Compound state [" << std::endl;
00971     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00972     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00973         components_[i]->printState(cstate->components[i], out);
00974     out << "]" << std::endl;
00975 }
00976 
00977 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
00978 {
00979     out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
00980     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00981     {
00982         components_[i]->printSettings(out);
00983         out << " of weight " << weights_[i] << std::endl;
00984     }
00985     out << "]" << std::endl;
00986     printProjections(out);
00987 }
00988 
00989 void ompl::base::CompoundStateSpace::setup(void)
00990 {
00991     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00992         components_[i]->setup();
00993 
00994     StateSpace::setup();
00995 }
00996 
00997 void ompl::base::CompoundStateSpace::computeLocations(void)
00998 {
00999     StateSpace::computeLocations();
01000     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01001         components_[i]->computeLocations();
01002 }
01003 
01004 namespace ompl
01005 {
01006     namespace base
01007     {
01008 
01010 
01011         static const int NO_DATA_COPIED   = 0;
01012         static const int SOME_DATA_COPIED = 1;
01013         static const int ALL_DATA_COPIED  = 2;
01014 
01016 
01017         // return one of the constants defined above
01018         int copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
01019         {
01020             // if states correspond to the same space, simply do copy
01021             if (destS->getName() == sourceS->getName())
01022             {
01023                 if (dest != source)
01024                     destS->copyState(dest, source);
01025                 return ALL_DATA_COPIED;
01026             }
01027 
01028             int result = NO_DATA_COPIED;
01029 
01030             // if "to" state is compound
01031             if (destS->isCompound())
01032             {
01033                 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
01034                 CompoundState *compoundDest = dest->as<CompoundState>();
01035 
01036                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01037                 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
01038                     if (compoundDestS->getSubSpace(i)->getName() == sourceS->getName())
01039                     {
01040                         if (compoundDest->components[i] != source)
01041                             compoundDestS->getSubSpace(i)->copyState(compoundDest->components[i], source);
01042                         return ALL_DATA_COPIED;
01043                     }
01044 
01045                 // it could be there are further levels of compound spaces where the data can be set
01046                 // so we call this function recursively
01047                 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
01048                 {
01049                     int res = copyStateData(compoundDestS->getSubSpace(i), compoundDest->components[i], sourceS, source);
01050 
01051                     if (res != NO_DATA_COPIED)
01052                         result = SOME_DATA_COPIED;
01053 
01054                     // if all data was copied, we stop
01055                     if (res == ALL_DATA_COPIED)
01056                         return ALL_DATA_COPIED;
01057                 }
01058             }
01059 
01060             // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
01061             // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
01062             if (sourceS->isCompound())
01063             {
01064                 const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
01065                 const CompoundState *compoundSource = source->as<CompoundState>();
01066 
01067                 unsigned int copiedComponents = 0;
01068 
01069                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01070                 for (unsigned int i = 0 ; i < compoundSourceS->getSubSpaceCount() ; ++i)
01071                 {
01072                     int res = copyStateData(destS, dest, compoundSourceS->getSubSpace(i), compoundSource->components[i]);
01073                     if (res == ALL_DATA_COPIED)
01074                         copiedComponents++;
01075                     if (res)
01076                         result = SOME_DATA_COPIED;
01077                 }
01078 
01079                 // if each individual component got copied, then the entire data in "from" got copied
01080                 if (copiedComponents == compoundSourceS->getSubSpaceCount())
01081                     result = ALL_DATA_COPIED;
01082             }
01083 
01084             return result;
01085         }
01086 
01088         inline bool StateSpaceHasContent(const StateSpacePtr &m)
01089         {
01090             if (!m)
01091                 return false;
01092             if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
01093             {
01094                 const unsigned int nc = m->as<CompoundStateSpace>()->getSubSpaceCount();
01095                 for (unsigned int i = 0 ; i < nc ; ++i)
01096                     if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubSpace(i)))
01097                         return true;
01098                 return false;
01099             }
01100             return true;
01101         }
01103 
01104         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
01105         {
01106             if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
01107                 return b;
01108 
01109             if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
01110                 return a;
01111 
01112             std::vector<StateSpacePtr> components;
01113             std::vector<double>           weights;
01114 
01115             bool change = false;
01116             if (a)
01117             {
01118                 bool used = false;
01119                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01120                     if (!csm_a->isLocked())
01121                     {
01122                         used = true;
01123                         for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01124                         {
01125                             components.push_back(csm_a->getSubSpace(i));
01126                             weights.push_back(csm_a->getSubSpaceWeight(i));
01127                         }
01128                     }
01129 
01130                 if (!used)
01131                 {
01132                     components.push_back(a);
01133                     weights.push_back(1.0);
01134                 }
01135             }
01136             if (b)
01137             {
01138                 bool used = false;
01139                 unsigned int size = components.size();
01140 
01141                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01142                     if (!csm_b->isLocked())
01143                     {
01144                         used = true;
01145                         for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01146                         {
01147                             bool ok = true;
01148                             for (unsigned int j = 0 ; j < size ; ++j)
01149                                 if (components[j]->getName() == csm_b->getSubSpace(i)->getName())
01150                                 {
01151                                     ok = false;
01152                                     break;
01153                                 }
01154                             if (ok)
01155                             {
01156                                 components.push_back(csm_b->getSubSpace(i));
01157                                 weights.push_back(csm_b->getSubSpaceWeight(i));
01158                                 change = true;
01159                             }
01160                         }
01161                         if (components.size() == csm_b->getSubSpaceCount())
01162                             return b;
01163                     }
01164 
01165                 if (!used)
01166                 {
01167                     bool ok = true;
01168                     for (unsigned int j = 0 ; j < size ; ++j)
01169                         if (components[j]->getName() == b->getName())
01170                         {
01171                             ok = false;
01172                             break;
01173                         }
01174                     if (ok)
01175                     {
01176                         components.push_back(b);
01177                         weights.push_back(1.0);
01178                         change = true;
01179                     }
01180                 }
01181             }
01182 
01183             if (!change && a)
01184                 return a;
01185 
01186             if (components.size() == 1)
01187                 return components[0];
01188 
01189             return StateSpacePtr(new CompoundStateSpace(components, weights));
01190         }
01191 
01192         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
01193         {
01194             std::vector<StateSpacePtr> components_a;
01195             std::vector<double>           weights_a;
01196             std::vector<StateSpacePtr> components_b;
01197 
01198             if (a)
01199             {
01200                 bool used = false;
01201                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01202                     if (!csm_a->isLocked())
01203                     {
01204                         used = true;
01205                         for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01206                         {
01207                             components_a.push_back(csm_a->getSubSpace(i));
01208                             weights_a.push_back(csm_a->getSubSpaceWeight(i));
01209                         }
01210                     }
01211 
01212                 if (!used)
01213                 {
01214                     components_a.push_back(a);
01215                     weights_a.push_back(1.0);
01216                 }
01217             }
01218 
01219             if (b)
01220             {
01221                 bool used = false;
01222                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01223                     if (!csm_b->isLocked())
01224                     {
01225                         used = true;
01226                         for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01227                             components_b.push_back(csm_b->getSubSpace(i));
01228                     }
01229                 if (!used)
01230                     components_b.push_back(b);
01231             }
01232 
01233             bool change = false;
01234             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01235                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01236                     if (components_a[j]->getName() == components_b[i]->getName())
01237                     {
01238                         components_a.erase(components_a.begin() + j);
01239                         weights_a.erase(weights_a.begin() + j);
01240                         change = true;
01241                         break;
01242                     }
01243 
01244             if (!change && a)
01245                 return a;
01246 
01247             if (components_a.size() == 1)
01248                 return components_a[0];
01249 
01250             return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
01251         }
01252 
01253         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
01254         {
01255             std::vector<StateSpacePtr> components;
01256             std::vector<double>           weights;
01257 
01258             bool change = false;
01259             if (a)
01260             {
01261                 bool used = false;
01262                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01263                     if (!csm_a->isLocked())
01264                     {
01265                         used = true;
01266                         for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01267                         {
01268                             if (csm_a->getSubSpace(i)->getName() == name)
01269                             {
01270                                 change = true;
01271                                 continue;
01272                             }
01273                             components.push_back(csm_a->getSubSpace(i));
01274                             weights.push_back(csm_a->getSubSpaceWeight(i));
01275                         }
01276                     }
01277 
01278                 if (!used)
01279                 {
01280                     if (a->getName() != name)
01281                     {
01282                         components.push_back(a);
01283                         weights.push_back(1.0);
01284                     }
01285                     else
01286                         change = true;
01287                 }
01288             }
01289 
01290             if (!change && a)
01291                 return a;
01292 
01293             if (components.size() == 1)
01294                 return components[0];
01295 
01296             return StateSpacePtr(new CompoundStateSpace(components, weights));
01297         }
01298 
01299         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
01300         {
01301             std::vector<StateSpacePtr> components_a;
01302             std::vector<double>           weights_a;
01303             std::vector<StateSpacePtr> components_b;
01304             std::vector<double>           weights_b;
01305 
01306             if (a)
01307             {
01308                 bool used = false;
01309                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01310                     if (!csm_a->isLocked())
01311                     {
01312                         used = true;
01313                         for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01314                         {
01315                             components_a.push_back(csm_a->getSubSpace(i));
01316                             weights_a.push_back(csm_a->getSubSpaceWeight(i));
01317                         }
01318                     }
01319 
01320                 if (!used)
01321                 {
01322                     components_a.push_back(a);
01323                     weights_a.push_back(1.0);
01324                 }
01325             }
01326 
01327             if (b)
01328             {
01329                 bool used = false;
01330                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01331                     if (!csm_b->isLocked())
01332                     {
01333                         used = true;
01334                         for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01335                         {
01336                             components_b.push_back(csm_b->getSubSpace(i));
01337                             weights_b.push_back(csm_b->getSubSpaceWeight(i));
01338                         }
01339                     }
01340 
01341                 if (!used)
01342                 {
01343                     components_b.push_back(b);
01344                     weights_b.push_back(1.0);
01345                 }
01346             }
01347 
01348             std::vector<StateSpacePtr> components;
01349             std::vector<double>           weights;
01350 
01351             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01352             {
01353                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01354                     if (components_a[j]->getName() == components_b[i]->getName())
01355                     {
01356                         components.push_back(components_b[i]);
01357                         weights.push_back(std::max(weights_a[j], weights_b[i]));
01358                         break;
01359                     }
01360             }
01361 
01362             if (a && components.size() == components_a.size())
01363                 return a;
01364 
01365             if (b && components.size() == components_b.size())
01366                 return b;
01367 
01368             if (components.size() == 1)
01369                 return components[0];
01370 
01371             return StateSpacePtr(new CompoundStateSpace(components, weights));
01372         }
01373     }
01374 }