00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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;
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
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
00197
00198
00199
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
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
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
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);
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
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())
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
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
01195 if (destS->isCompound())
01196 {
01197 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
01198 CompoundState *compoundDest = dest->as<CompoundState>();
01199
01200
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
01210
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
01219 if (res == ALL_DATA_COPIED)
01220 return ALL_DATA_COPIED;
01221 }
01222 }
01223
01224
01225
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
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
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 }