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/StateManifold.h"
00036 #include "ompl/util/Exception.h"
00037 #include <boost/thread/mutex.hpp>
00038 #include <boost/lexical_cast.hpp>
00039 #include <numeric>
00040 #include <limits>
00041 #include <queue>
00042 #include <cmath>
00043 #include <list>
00044
00045 const std::string ompl::base::StateManifold::DEFAULT_PROJECTION_NAME = "";
00046
00048 namespace ompl
00049 {
00050 namespace base
00051 {
00052 static std::list<StateManifold*> STATE_MANIFOLD_LIST;
00053 static boost::mutex STATE_MANIFOLD_LIST_LOCK;
00054 }
00055 }
00057
00058 ompl::base::StateManifold::StateManifold(void)
00059 {
00060
00061 static boost::mutex lock;
00062 static unsigned int m = 0;
00063
00064 lock.lock();
00065 m++;
00066 lock.unlock();
00067
00068 name_ = "Manifold" + boost::lexical_cast<std::string>(m);
00069 boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00070 STATE_MANIFOLD_LIST.push_back(this);
00071
00072 longestValidSegment_ = 0.0;
00073 longestValidSegmentFraction_ = 0.01;
00074 longestValidSegmentCountFactor_ = 1;
00075
00076 type_ = STATE_MANIFOLD_UNKNOWN;
00077
00078 maxExtent_ = std::numeric_limits<double>::infinity();
00079 }
00080
00081 ompl::base::StateManifold::~StateManifold(void)
00082 {
00083 boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00084 STATE_MANIFOLD_LIST.remove(this);
00085 }
00086
00087 const std::string& ompl::base::StateManifold::getName(void) const
00088 {
00089 return name_;
00090 }
00091
00092 void ompl::base::StateManifold::setName(const std::string &name)
00093 {
00094 name_ = name;
00095 }
00096
00097 void ompl::base::StateManifold::registerProjections(void)
00098 {
00099 }
00100
00101 void ompl::base::StateManifold::setup(void)
00102 {
00103 maxExtent_ = getMaximumExtent();
00104 longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00105
00106 if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00107 throw Exception("The longest valid segment for manifold " + getName() + " must be positive");
00108
00109
00110 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00111 registerProjections();
00112 for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00113 if (it->second->userConfigured())
00114 {
00115 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00116 if (o != projections_.end())
00117 if (!o->second->userConfigured())
00118 projections_[it->first] = it->second;
00119 }
00120
00121 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00122 it->second->setup();
00123 }
00124
00125 double* ompl::base::StateManifold::getValueAddressAtIndex(State *state, const unsigned int index) const
00126 {
00127 return NULL;
00128 }
00129
00130 void ompl::base::StateManifold::printState(const State *state, std::ostream &out) const
00131 {
00132 out << "State instance [" << state << ']' << std::endl;
00133 }
00134
00135 void ompl::base::StateManifold::printSettings(std::ostream &out) const
00136 {
00137 out << "StateManifold '" << getName() << "' instance: " << this << std::endl;
00138 printProjections(out);
00139 }
00140
00141 void ompl::base::StateManifold::printProjections(std::ostream &out) const
00142 {
00143 if (projections_.empty())
00144 out << "No registered projections" << std::endl;
00145 else
00146 {
00147 out << "Registered projections:" << std::endl;
00148 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00149 {
00150 out << " - ";
00151 if (it->first == DEFAULT_PROJECTION_NAME)
00152 out << "<default>";
00153 else
00154 out << it->first;
00155 out << std::endl;
00156 it->second->printSettings(out);
00157 }
00158 }
00159 }
00160
00162 namespace ompl
00163 {
00164 namespace base
00165 {
00166 static bool StateManifoldIncludes(const StateManifold *self, const StateManifold *other)
00167 {
00168 std::queue<const StateManifold*> q;
00169 q.push(self);
00170 while (!q.empty())
00171 {
00172 const StateManifold *m = q.front();
00173 q.pop();
00174 if (m->getName() == other->getName())
00175 return true;
00176 if (m->isCompound())
00177 {
00178 unsigned int c = m->as<CompoundStateManifold>()->getSubManifoldCount();
00179 for (unsigned int i = 0 ; i < c ; ++i)
00180 q.push(m->as<CompoundStateManifold>()->getSubManifold(i).get());
00181 }
00182 }
00183 return false;
00184 }
00185
00186 static bool StateManifoldCovers(const StateManifold *self, const StateManifold *other)
00187 {
00188 if (StateManifoldIncludes(self, other))
00189 return true;
00190 else
00191 if (other->isCompound())
00192 {
00193 unsigned int c = other->as<CompoundStateManifold>()->getSubManifoldCount();
00194 for (unsigned int i = 0 ; i < c ; ++i)
00195 if (!StateManifoldCovers(self, other->as<CompoundStateManifold>()->getSubManifold(i).get()))
00196 return false;
00197 return true;
00198 }
00199 return false;
00200 }
00201 }
00202 }
00203
00205
00206 bool ompl::base::StateManifold::covers(const StateManifoldPtr &other) const
00207 {
00208 return StateManifoldCovers(this, other.get());
00209 }
00210
00211 bool ompl::base::StateManifold::includes(const StateManifoldPtr &other) const
00212 {
00213 return StateManifoldIncludes(this, other.get());
00214 }
00215
00216 void ompl::base::StateManifold::diagram(std::ostream &out)
00217 {
00218 boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00219 out << "digraph StateManifolds {" << std::endl;
00220 for (std::list<StateManifold*>::iterator it = STATE_MANIFOLD_LIST.begin() ; it != STATE_MANIFOLD_LIST.end(); ++it)
00221 {
00222 out << '"' << (*it)->getName() << '"' << std::endl;
00223 for (std::list<StateManifold*>::iterator jt = STATE_MANIFOLD_LIST.begin() ; jt != STATE_MANIFOLD_LIST.end(); ++jt)
00224 if (it != jt)
00225 {
00226 if ((*it)->isCompound() && (*it)->as<CompoundStateManifold>()->hasSubManifold((*jt)->getName()))
00227 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00228 boost::lexical_cast<std::string>((*it)->as<CompoundStateManifold>()->getSubManifoldWeight((*jt)->getName())) <<
00229 "\"];" << std::endl;
00230 else
00231 if (!StateManifoldIncludes(*it, *jt) && StateManifoldCovers(*it, *jt))
00232 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00233 }
00234 }
00235 out << '}' << std::endl;
00236 }
00237
00238 bool ompl::base::StateManifold::hasDefaultProjection(void) const
00239 {
00240 return hasProjection(DEFAULT_PROJECTION_NAME);
00241 }
00242
00243 bool ompl::base::StateManifold::hasProjection(const std::string &name) const
00244 {
00245 return projections_.find(name) != projections_.end();
00246 }
00247
00248 ompl::base::ProjectionEvaluatorPtr ompl::base::StateManifold::getDefaultProjection(void) const
00249 {
00250 if (hasDefaultProjection())
00251 return getProjection(DEFAULT_PROJECTION_NAME);
00252 else
00253 {
00254 msg_.error("No default projection is set");
00255 return ProjectionEvaluatorPtr();
00256 }
00257 }
00258
00259 ompl::base::ProjectionEvaluatorPtr ompl::base::StateManifold::getProjection(const std::string &name) const
00260 {
00261 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00262 if (it != projections_.end())
00263 return it->second;
00264 else
00265 {
00266 msg_.error("Projection '" + name + "' is not defined");
00267 return ProjectionEvaluatorPtr();
00268 }
00269 }
00270
00271 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateManifold::getRegisteredProjections(void) const
00272 {
00273 return projections_;
00274 }
00275
00276 void ompl::base::StateManifold::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00277 {
00278 registerProjection(DEFAULT_PROJECTION_NAME, projection);
00279 }
00280
00281 void ompl::base::StateManifold::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00282 {
00283 if (projection)
00284 projections_[name] = projection;
00285 else
00286 msg_.error("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00287 }
00288
00289 bool ompl::base::StateManifold::isCompound(void) const
00290 {
00291 return false;
00292 }
00293
00294 void ompl::base::StateManifold::setValidSegmentCountFactor(unsigned int factor)
00295 {
00296 if (factor < 1)
00297 throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00298 longestValidSegmentCountFactor_ = factor;
00299 }
00300
00301 void ompl::base::StateManifold::setLongestValidSegmentFraction(double segmentFraction)
00302 {
00303 if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00304 throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00305 longestValidSegmentFraction_ = segmentFraction;
00306 }
00307
00308 unsigned int ompl::base::StateManifold::getValidSegmentCountFactor(void) const
00309 {
00310 return longestValidSegmentCountFactor_;
00311 }
00312
00313 double ompl::base::StateManifold::getLongestValidSegmentFraction(void) const
00314 {
00315 return longestValidSegmentFraction_;
00316 }
00317
00318 unsigned int ompl::base::StateManifold::validSegmentCount(const State *state1, const State *state2) const
00319 {
00320 return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00321 }
00322
00323 ompl::base::CompoundStateManifold::CompoundStateManifold(void) : StateManifold(), componentCount_(0), locked_(false)
00324 {
00325 setName("Compound" + getName());
00326 }
00327
00328 ompl::base::CompoundStateManifold::CompoundStateManifold(const std::vector<StateManifoldPtr> &components,
00329 const std::vector<double> &weights) : StateManifold(), componentCount_(0), locked_(false)
00330 {
00331 if (components.size() != weights.size())
00332 throw Exception("Number of component manifolds and weights are not the same");
00333 setName("Compound" + getName());
00334 for (unsigned int i = 0 ; i < components.size() ; ++i)
00335 addSubManifold(components[i], weights[i]);
00336 }
00337
00338 void ompl::base::CompoundStateManifold::addSubManifold(const StateManifoldPtr &component, double weight)
00339 {
00340 if (locked_)
00341 throw Exception("This manifold is locked. No further components can be added");
00342 if (weight < 0.0)
00343 throw Exception("Submanifold weight cannot be negative");
00344 components_.push_back(component);
00345 weights_.push_back(weight);
00346 componentCount_ = components_.size();
00347 }
00348
00349 bool ompl::base::CompoundStateManifold::isCompound(void) const
00350 {
00351 return true;
00352 }
00353
00354 unsigned int ompl::base::CompoundStateManifold::getSubManifoldCount(void) const
00355 {
00356 return componentCount_;
00357 }
00358
00359 const ompl::base::StateManifoldPtr& ompl::base::CompoundStateManifold::getSubManifold(const unsigned int index) const
00360 {
00361 if (componentCount_ > index)
00362 return components_[index];
00363 else
00364 throw Exception("Submanifold index does not exist");
00365 }
00366
00367 bool ompl::base::CompoundStateManifold::hasSubManifold(const std::string &name) const
00368 {
00369 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00370 if (components_[i]->getName() == name)
00371 return true;
00372 return false;
00373 }
00374
00375 unsigned int ompl::base::CompoundStateManifold::getSubManifoldIndex(const std::string& name) const
00376 {
00377 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00378 if (components_[i]->getName() == name)
00379 return i;
00380 throw Exception("Submanifold " + name + " does not exist");
00381 }
00382
00383 const ompl::base::StateManifoldPtr& ompl::base::CompoundStateManifold::getSubManifold(const std::string& name) const
00384 {
00385 return components_[getSubManifoldIndex(name)];
00386 }
00387
00388 double ompl::base::CompoundStateManifold::getSubManifoldWeight(const unsigned int index) const
00389 {
00390 if (componentCount_ > index)
00391 return weights_[index];
00392 else
00393 throw Exception("Submanifold index does not exist");
00394 }
00395
00396 double ompl::base::CompoundStateManifold::getSubManifoldWeight(const std::string &name) const
00397 {
00398 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00399 if (components_[i]->getName() == name)
00400 return weights_[i];
00401 throw Exception("Submanifold " + name + " does not exist");
00402 }
00403
00404 void ompl::base::CompoundStateManifold::setSubManifoldWeight(const unsigned int index, double weight)
00405 {
00406 if (weight < 0.0)
00407 throw Exception("Submanifold weight cannot be negative");
00408 if (componentCount_ > index)
00409 weights_[index] = weight;
00410 else
00411 throw Exception("Submanifold index does not exist");
00412 }
00413
00414 void ompl::base::CompoundStateManifold::setSubManifoldWeight(const std::string &name, double weight)
00415 {
00416 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00417 if (components_[i]->getName() == name)
00418 {
00419 setSubManifoldWeight(i, weight);
00420 return;
00421 }
00422 throw Exception("Submanifold " + name + " does not exist");
00423 }
00424
00425 const std::vector<ompl::base::StateManifoldPtr>& ompl::base::CompoundStateManifold::getSubManifolds(void) const
00426 {
00427 return components_;
00428 }
00429
00430 const std::vector<double>& ompl::base::CompoundStateManifold::getSubManifoldWeights(void) const
00431 {
00432 return weights_;
00433 }
00434
00435 unsigned int ompl::base::CompoundStateManifold::getDimension(void) const
00436 {
00437 unsigned int dim = 0;
00438 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00439 dim += components_[i]->getDimension();
00440 return dim;
00441 }
00442
00443 double ompl::base::CompoundStateManifold::getMaximumExtent(void) const
00444 {
00445 double e = 0.0;
00446 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00447 e += weights_[i] * components_[i]->getMaximumExtent();
00448 return e;
00449 }
00450
00451 void ompl::base::CompoundStateManifold::enforceBounds(State *state) const
00452 {
00453 CompoundState *cstate = static_cast<CompoundState*>(state);
00454 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00455 components_[i]->enforceBounds(cstate->components[i]);
00456 }
00457
00458 bool ompl::base::CompoundStateManifold::satisfiesBounds(const State *state) const
00459 {
00460 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00461 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00462 if (!components_[i]->satisfiesBounds(cstate->components[i]))
00463 return false;
00464 return true;
00465 }
00466
00467 void ompl::base::CompoundStateManifold::copyState(State *destination, const State *source) const
00468 {
00469 CompoundState *cdest = static_cast<CompoundState*>(destination);
00470 const CompoundState *csrc = static_cast<const CompoundState*>(source);
00471 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00472 components_[i]->copyState(cdest->components[i], csrc->components[i]);
00473 }
00474
00475 double ompl::base::CompoundStateManifold::distance(const State *state1, const State *state2) const
00476 {
00477 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00478 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00479 double dist = 0.0;
00480 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00481 dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
00482 return dist;
00483 }
00484
00485 void ompl::base::CompoundStateManifold::setLongestValidSegmentFraction(double segmentFraction)
00486 {
00487 StateManifold::setLongestValidSegmentFraction(segmentFraction);
00488 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00489 components_[i]->setLongestValidSegmentFraction(segmentFraction);
00490 }
00491
00492 unsigned int ompl::base::CompoundStateManifold::validSegmentCount(const State *state1, const State *state2) const
00493 {
00494 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00495 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00496 unsigned int sc = 0;
00497 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00498 {
00499 unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
00500 if (sci > sc)
00501 sc = sci;
00502 }
00503 return sc;
00504 }
00505
00506 bool ompl::base::CompoundStateManifold::equalStates(const State *state1, const State *state2) const
00507 {
00508 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00509 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00510 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00511 if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
00512 return false;
00513 return true;
00514 }
00515
00516 void ompl::base::CompoundStateManifold::interpolate(const State *from, const State *to, const double t, State *state) const
00517 {
00518 const CompoundState *cfrom = static_cast<const CompoundState*>(from);
00519 const CompoundState *cto = static_cast<const CompoundState*>(to);
00520 CompoundState *cstate = static_cast<CompoundState*>(state);
00521 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00522 components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
00523 }
00524
00525 ompl::base::ManifoldStateSamplerPtr ompl::base::CompoundStateManifold::allocStateSampler(void) const
00526 {
00527 double totalWeight = std::accumulate(weights_.begin(), weights_.end(), 0.0);
00528 if (totalWeight < std::numeric_limits<double>::epsilon())
00529 totalWeight = 1.0;
00530 CompoundManifoldStateSampler *ss = new CompoundManifoldStateSampler(this);
00531 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00532 ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / totalWeight);
00533 return ManifoldStateSamplerPtr(ss);
00534 }
00535
00536 ompl::base::State* ompl::base::CompoundStateManifold::allocState(void) const
00537 {
00538 CompoundState *state = new CompoundState();
00539 allocStateComponents(state);
00540 return static_cast<State*>(state);
00541 }
00542
00543 void ompl::base::CompoundStateManifold::allocStateComponents(CompoundState *state) const
00544 {
00545 state->components = new State*[componentCount_];
00546 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00547 state->components[i] = components_[i]->allocState();
00548 }
00549
00550 void ompl::base::CompoundStateManifold::freeState(State *state) const
00551 {
00552 CompoundState *cstate = static_cast<CompoundState*>(state);
00553 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00554 components_[i]->freeState(cstate->components[i]);
00555 delete[] cstate->components;
00556 delete cstate;
00557 }
00558
00559 void ompl::base::CompoundStateManifold::lock(void)
00560 {
00561 locked_ = true;
00562 }
00563
00564 bool ompl::base::CompoundStateManifold::isLocked(void) const
00565 {
00566 return locked_;
00567 }
00568
00569 double* ompl::base::CompoundStateManifold::getValueAddressAtIndex(State *state, const unsigned int index) const
00570 {
00571 CompoundState *cstate = static_cast<CompoundState*>(state);
00572 unsigned int idx = 0;
00573
00574 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00575 for (unsigned int j = 0 ; j <= index ; ++j)
00576 {
00577 double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
00578 if (va)
00579 {
00580 if (idx == index)
00581 return va;
00582 else
00583 idx++;
00584 }
00585 else
00586 break;
00587 }
00588 return NULL;
00589 }
00590
00591 void ompl::base::CompoundStateManifold::printState(const State *state, std::ostream &out) const
00592 {
00593 out << "Compound state [" << std::endl;
00594 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00595 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00596 components_[i]->printState(cstate->components[i], out);
00597 out << "]" << std::endl;
00598 }
00599
00600 void ompl::base::CompoundStateManifold::printSettings(std::ostream &out) const
00601 {
00602 out << "Compound state manifold '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
00603 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00604 {
00605 components_[i]->printSettings(out);
00606 out << " of weight " << weights_[i] << std::endl;
00607 }
00608 out << "]" << std::endl;
00609 printProjections(out);
00610 }
00611
00612 void ompl::base::CompoundStateManifold::setup(void)
00613 {
00614 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00615 components_[i]->setup();
00616
00617 StateManifold::setup();
00618 }
00619
00620 namespace ompl
00621 {
00622 namespace base
00623 {
00624
00626
00627 static const int NO_DATA_COPIED = 0;
00628 static const int SOME_DATA_COPIED = 1;
00629 static const int ALL_DATA_COPIED = 2;
00630
00632
00633
00634 int copyStateData(const StateManifoldPtr &destM, State *dest, const StateManifoldPtr &sourceM, const State *source)
00635 {
00636
00637 if (destM->getName() == sourceM->getName())
00638 {
00639 if (dest != source)
00640 destM->copyState(dest, source);
00641 return ALL_DATA_COPIED;
00642 }
00643
00644 int result = NO_DATA_COPIED;
00645
00646
00647 if (destM->isCompound())
00648 {
00649 const CompoundStateManifold *compoundDestM = destM->as<CompoundStateManifold>();
00650 CompoundState *compoundDest = dest->as<CompoundState>();
00651
00652
00653 for (unsigned int i = 0 ; i < compoundDestM->getSubManifoldCount() ; ++i)
00654 if (compoundDestM->getSubManifold(i)->getName() == sourceM->getName())
00655 {
00656 if (compoundDest->components[i] != source)
00657 compoundDestM->getSubManifold(i)->copyState(compoundDest->components[i], source);
00658 return ALL_DATA_COPIED;
00659 }
00660
00661
00662
00663 for (unsigned int i = 0 ; i < compoundDestM->getSubManifoldCount() ; ++i)
00664 {
00665 int res = copyStateData(compoundDestM->getSubManifold(i), compoundDest->components[i], sourceM, source);
00666
00667 if (res != NO_DATA_COPIED)
00668 result = SOME_DATA_COPIED;
00669
00670
00671 if (res == ALL_DATA_COPIED)
00672 return ALL_DATA_COPIED;
00673 }
00674 }
00675
00676
00677
00678 if (sourceM->isCompound())
00679 {
00680 const CompoundStateManifold *compoundSourceM = sourceM->as<CompoundStateManifold>();
00681 const CompoundState *compoundSource = source->as<CompoundState>();
00682
00683 unsigned int copiedComponents = 0;
00684
00685
00686 for (unsigned int i = 0 ; i < compoundSourceM->getSubManifoldCount() ; ++i)
00687 {
00688 int res = copyStateData(destM, dest, compoundSourceM->getSubManifold(i), compoundSource->components[i]);
00689 if (res == ALL_DATA_COPIED)
00690 copiedComponents++;
00691 if (res)
00692 result = SOME_DATA_COPIED;
00693 }
00694
00695
00696 if (copiedComponents == compoundSourceM->getSubManifoldCount())
00697 result = ALL_DATA_COPIED;
00698 }
00699
00700 return result;
00701 }
00702
00704 inline bool StateManifoldHasContent(const StateManifoldPtr &m)
00705 {
00706 if (!m)
00707 return false;
00708 if (m->getDimension() == 0 && m->getType() == STATE_MANIFOLD_UNKNOWN && m->isCompound())
00709 {
00710 const unsigned int nc = m->as<CompoundStateManifold>()->getSubManifoldCount();
00711 for (unsigned int i = 0 ; i < nc ; ++i)
00712 if (StateManifoldHasContent(m->as<CompoundStateManifold>()->getSubManifold(i)))
00713 return true;
00714 return false;
00715 }
00716 return true;
00717 }
00719
00720 StateManifoldPtr operator+(const StateManifoldPtr &a, const StateManifoldPtr &b)
00721 {
00722 if (!StateManifoldHasContent(a) && StateManifoldHasContent(b))
00723 return b;
00724
00725 if (!StateManifoldHasContent(b) && StateManifoldHasContent(a))
00726 return a;
00727
00728 std::vector<StateManifoldPtr> components;
00729 std::vector<double> weights;
00730
00731 bool change = false;
00732 if (a)
00733 {
00734 bool used = false;
00735 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00736 if (!csm_a->isLocked())
00737 {
00738 used = true;
00739 for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00740 {
00741 components.push_back(csm_a->getSubManifold(i));
00742 weights.push_back(csm_a->getSubManifoldWeight(i));
00743 }
00744 }
00745
00746 if (!used)
00747 {
00748 components.push_back(a);
00749 weights.push_back(1.0);
00750 }
00751 }
00752 if (b)
00753 {
00754 bool used = false;
00755 unsigned int size = components.size();
00756
00757 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00758 if (!csm_b->isLocked())
00759 {
00760 used = true;
00761 for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00762 {
00763 bool ok = true;
00764 for (unsigned int j = 0 ; j < size ; ++j)
00765 if (components[j]->getName() == csm_b->getSubManifold(i)->getName())
00766 {
00767 ok = false;
00768 break;
00769 }
00770 if (ok)
00771 {
00772 components.push_back(csm_b->getSubManifold(i));
00773 weights.push_back(csm_b->getSubManifoldWeight(i));
00774 change = true;
00775 }
00776 }
00777 if (components.size() == csm_b->getSubManifoldCount())
00778 return b;
00779 }
00780
00781 if (!used)
00782 {
00783 bool ok = true;
00784 for (unsigned int j = 0 ; j < size ; ++j)
00785 if (components[j]->getName() == b->getName())
00786 {
00787 ok = false;
00788 break;
00789 }
00790 if (ok)
00791 {
00792 components.push_back(b);
00793 weights.push_back(1.0);
00794 change = true;
00795 }
00796 }
00797 }
00798
00799 if (!change && a)
00800 return a;
00801
00802 if (components.size() == 1)
00803 return components[0];
00804
00805 return StateManifoldPtr(new CompoundStateManifold(components, weights));
00806 }
00807
00808 StateManifoldPtr operator-(const StateManifoldPtr &a, const StateManifoldPtr &b)
00809 {
00810 std::vector<StateManifoldPtr> components_a;
00811 std::vector<double> weights_a;
00812 std::vector<StateManifoldPtr> components_b;
00813
00814 if (a)
00815 {
00816 bool used = false;
00817 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00818 if (!csm_a->isLocked())
00819 {
00820 used = true;
00821 for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00822 {
00823 components_a.push_back(csm_a->getSubManifold(i));
00824 weights_a.push_back(csm_a->getSubManifoldWeight(i));
00825 }
00826 }
00827
00828 if (!used)
00829 {
00830 components_a.push_back(a);
00831 weights_a.push_back(1.0);
00832 }
00833 }
00834
00835 if (b)
00836 {
00837 bool used = false;
00838 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00839 if (!csm_b->isLocked())
00840 {
00841 used = true;
00842 for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00843 components_b.push_back(csm_b->getSubManifold(i));
00844 }
00845 if (!used)
00846 components_b.push_back(b);
00847 }
00848
00849 bool change = false;
00850 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
00851 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
00852 if (components_a[j]->getName() == components_b[i]->getName())
00853 {
00854 components_a.erase(components_a.begin() + j);
00855 weights_a.erase(weights_a.begin() + j);
00856 change = true;
00857 break;
00858 }
00859
00860 if (!change && a)
00861 return a;
00862
00863 if (components_a.size() == 1)
00864 return components_a[0];
00865
00866 return StateManifoldPtr(new CompoundStateManifold(components_a, weights_a));
00867 }
00868
00869 StateManifoldPtr operator-(const StateManifoldPtr &a, const std::string &name)
00870 {
00871 std::vector<StateManifoldPtr> components;
00872 std::vector<double> weights;
00873
00874 bool change = false;
00875 if (a)
00876 {
00877 bool used = false;
00878 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00879 if (!csm_a->isLocked())
00880 {
00881 used = true;
00882 for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00883 {
00884 if (csm_a->getSubManifold(i)->getName() == name)
00885 {
00886 change = true;
00887 continue;
00888 }
00889 components.push_back(csm_a->getSubManifold(i));
00890 weights.push_back(csm_a->getSubManifoldWeight(i));
00891 }
00892 }
00893
00894 if (!used)
00895 {
00896 if (a->getName() != name)
00897 {
00898 components.push_back(a);
00899 weights.push_back(1.0);
00900 }
00901 else
00902 change = true;
00903 }
00904 }
00905
00906 if (!change && a)
00907 return a;
00908
00909 if (components.size() == 1)
00910 return components[0];
00911
00912 return StateManifoldPtr(new CompoundStateManifold(components, weights));
00913 }
00914
00915 StateManifoldPtr operator*(const StateManifoldPtr &a, const StateManifoldPtr &b)
00916 {
00917 std::vector<StateManifoldPtr> components_a;
00918 std::vector<double> weights_a;
00919 std::vector<StateManifoldPtr> components_b;
00920 std::vector<double> weights_b;
00921
00922 if (a)
00923 {
00924 bool used = false;
00925 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00926 if (!csm_a->isLocked())
00927 {
00928 used = true;
00929 for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00930 {
00931 components_a.push_back(csm_a->getSubManifold(i));
00932 weights_a.push_back(csm_a->getSubManifoldWeight(i));
00933 }
00934 }
00935
00936 if (!used)
00937 {
00938 components_a.push_back(a);
00939 weights_a.push_back(1.0);
00940 }
00941 }
00942
00943 if (b)
00944 {
00945 bool used = false;
00946 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00947 if (!csm_b->isLocked())
00948 {
00949 used = true;
00950 for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00951 {
00952 components_b.push_back(csm_b->getSubManifold(i));
00953 weights_b.push_back(csm_b->getSubManifoldWeight(i));
00954 }
00955 }
00956
00957 if (!used)
00958 {
00959 components_b.push_back(b);
00960 weights_b.push_back(1.0);
00961 }
00962 }
00963
00964 std::vector<StateManifoldPtr> components;
00965 std::vector<double> weights;
00966
00967 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
00968 {
00969 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
00970 if (components_a[j]->getName() == components_b[i]->getName())
00971 {
00972 components.push_back(components_b[i]);
00973 weights.push_back(std::max(weights_a[j], weights_b[i]));
00974 break;
00975 }
00976 }
00977
00978 if (a && components.size() == components_a.size())
00979 return a;
00980
00981 if (b && components.size() == components_b.size())
00982 return b;
00983
00984 if (components.size() == 1)
00985 return components[0];
00986
00987 return StateManifoldPtr(new CompoundStateManifold(components, weights));
00988 }
00989
00990 }
00991 }