<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml; charset=UTF-8"/>
<title>OMPL: src/ompl/base/src/StateSpace.cpp Source File</title>
<meta name="author" content="Ioan A. Șucan, Mark Moll, Lydia E. Kavraki">
<link rel="stylesheet" href="../css/screen.css" type="text/css" media="screen, projection">
<link rel="stylesheet" href="../css/print.css" type="text/css" media="print">
<!--[if lt IE 7]>
<script type="text/javascript" src="../js/jquery/jquery.js"></script>
<script type="text/javascript" src="../js/jquery/jquery.dropdown.js"></script>
<![endif]-->
<script type="text/javaScript" src="search/search.js"></script>
<script type="text/javascript">
var _gaq = _gaq || [];
_gaq.push(['_setAccount', 'UA-9156598-2']);
_gaq.push(['_trackPageview']);
(function() {
var ga = document.createElement('script'); ga.type = 'text/javascript'; ga.async = true;
ga.src = ('https:' == document.location.protocol ? 'https://ssl' : 'http://www') + '.google-analytics.com/ga.js';
var s = document.getElementsByTagName('script')[0]; s.parentNode.insertBefore(ga, s);
})();
</script>
</head>
<body onload='searchBox.OnSelectItem(0);'>
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search API');
--></script>
<div class="navigation" id="top">
<div class="tabs" id="ompltitle">
<ul class="tablist">
<li>The Open Motion Planning Library</li>
<li id="searchli">
<div id="MSearchBox" class="MSearchBoxInactive">
<span class="left">
<img id="MSearchSelect" src="search/mag_sel.png"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
alt=""/>
<input type="text" id="MSearchField" value="Search API" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
</span><span class="right">
<a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
</span>
</div>
</li>
</ul>
</div>
<ul id="nav" class="dropdown">
<li class="first"><a href="index.html">Home</a></li>
<li><a href="download.html">Download</a></li>
<li><a href="documentation.html">Documentation</a></li>
<li><span class="dir">Code API</span>
<ul>
<li><a href="api_overview.html">API Overview</a></li>
<li><a href="namespaces.html">Namespaces</a></li>
<li><a href="annotated.html">Classes</a></li>
<li><a href="files.html">Files</a></li>
<li><a href="dirs.html">Directories</a></li>
</ul>
</li>
<li><span class="dir">Community</span>
<ul>
<li><a href="developers.html">Developers</a></li>
<li><a href="thirdparty.html">Contributions</a></li>
<li><a href="education.html">Education</a></li>
<li><a href="gallery.html">Gallery</a></li>
</ul>
</li>
<li><span class="dir">About</span>
<ul>
<li><a href="license.html">License</a></li>
<li><a href="citations.html">Citations</a></li>
<li><a href="acknowledgements.html">Acknowledgments</a></li>
<li><a href="contact.html">Contact Us</a></li>
</ul>
</li>
</ul>
</div>
<!--- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark"> </span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark"> </span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark"> </span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark"> </span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark"> </span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark"> </span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark"> </span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark"> </span>Enumerator</a></div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="" frameborder="0"name="MSearchResults" id="MSearchResults"></iframe>
</div>
<div class="container">
<div class="span-22 push-2 first last">
<div>
<!-- Generated by Doxygen 1.7.4 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div id="nav-path" class="navpath">
<ul>
<li class="navelem"><a class="el" href="dir_f5421e52a658cd938113ed6044324834.html">src</a> </li>
<li class="navelem"><a class="el" href="dir_ae92c2ff78847f0cb49b545f9089bbbc.html">ompl</a> </li>
<li class="navelem"><a class="el" href="dir_40bc83a902349ad67ef9d1cb49964511.html">base</a> </li>
<li class="navelem"><a class="el" href="dir_638c121d1114340bf3e84a5e9d67e9f2.html">src</a> </li>
</ul>
</div>
</div>
<div class="header">
<div class="headertitle">
<div class="title">StateSpace.cpp</div> </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*********************************************************************</span>
<a name="l00002"></a>00002 <span class="comment">* Software License Agreement (BSD License)</span>
<a name="l00003"></a>00003 <span class="comment">*</span>
<a name="l00004"></a>00004 <span class="comment">* Copyright (c) 2010, Rice University</span>
<a name="l00005"></a>00005 <span class="comment">* All rights reserved.</span>
<a name="l00006"></a>00006 <span class="comment">*</span>
<a name="l00007"></a>00007 <span class="comment">* Redistribution and use in source and binary forms, with or without</span>
<a name="l00008"></a>00008 <span class="comment">* modification, are permitted provided that the following conditions</span>
<a name="l00009"></a>00009 <span class="comment">* are met:</span>
<a name="l00010"></a>00010 <span class="comment">*</span>
<a name="l00011"></a>00011 <span class="comment">* * Redistributions of source code must retain the above copyright</span>
<a name="l00012"></a>00012 <span class="comment">* notice, this list of conditions and the following disclaimer.</span>
<a name="l00013"></a>00013 <span class="comment">* * Redistributions in binary form must reproduce the above</span>
<a name="l00014"></a>00014 <span class="comment">* copyright notice, this list of conditions and the following</span>
<a name="l00015"></a>00015 <span class="comment">* disclaimer in the documentation and/or other materials provided</span>
<a name="l00016"></a>00016 <span class="comment">* with the distribution.</span>
<a name="l00017"></a>00017 <span class="comment">* * Neither the name of the Rice University nor the names of its</span>
<a name="l00018"></a>00018 <span class="comment">* contributors may be used to endorse or promote products derived</span>
<a name="l00019"></a>00019 <span class="comment">* from this software without specific prior written permission.</span>
<a name="l00020"></a>00020 <span class="comment">*</span>
<a name="l00021"></a>00021 <span class="comment">* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS</span>
<a name="l00022"></a>00022 <span class="comment">* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT</span>
<a name="l00023"></a>00023 <span class="comment">* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS</span>
<a name="l00024"></a>00024 <span class="comment">* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE</span>
<a name="l00025"></a>00025 <span class="comment">* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,</span>
<a name="l00026"></a>00026 <span class="comment">* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,</span>
<a name="l00027"></a>00027 <span class="comment">* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span>
<a name="l00028"></a>00028 <span class="comment">* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER</span>
<a name="l00029"></a>00029 <span class="comment">* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT</span>
<a name="l00030"></a>00030 <span class="comment">* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN</span>
<a name="l00031"></a>00031 <span class="comment">* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE</span>
<a name="l00032"></a>00032 <span class="comment">* POSSIBILITY OF SUCH DAMAGE.</span>
<a name="l00033"></a>00033 <span class="comment">*********************************************************************/</span>
<a name="l00034"></a>00034
<a name="l00035"></a>00035 <span class="preprocessor">#include "ompl/base/StateSpace.h"</span>
<a name="l00036"></a>00036 <span class="preprocessor">#include "ompl/util/Exception.h"</span>
<a name="l00037"></a>00037 <span class="preprocessor">#include "ompl/tools/config/MagicConstants.h"</span>
<a name="l00038"></a>00038 <span class="preprocessor">#include <boost/thread/mutex.hpp></span>
<a name="l00039"></a>00039 <span class="preprocessor">#include <boost/lexical_cast.hpp></span>
<a name="l00040"></a>00040 <span class="preprocessor">#include <numeric></span>
<a name="l00041"></a>00041 <span class="preprocessor">#include <limits></span>
<a name="l00042"></a>00042 <span class="preprocessor">#include <queue></span>
<a name="l00043"></a>00043 <span class="preprocessor">#include <cmath></span>
<a name="l00044"></a>00044 <span class="preprocessor">#include <list></span>
<a name="l00045"></a>00045
<a name="l00046"></a>00046 <span class="keyword">const</span> std::string <a class="code" href="classompl_1_1base_1_1StateSpace.html#a45e65cf86a2c5cb1e4394dd8aafb4d3b" title="The name used for the default projection.">ompl::base::StateSpace::DEFAULT_PROJECTION_NAME</a> = <span class="stringliteral">""</span>;
<a name="l00047"></a>00047
<a name="l00049"></a>00049 <span class="keyword">namespace </span>ompl
<a name="l00050"></a>00050 {
<a name="l00051"></a>00051 <span class="keyword">namespace </span>base
<a name="l00052"></a>00052 {
<a name="l00053"></a>00053 <span class="keyword">static</span> std::list<StateSpace*> STATE_SPACE_LIST;
<a name="l00054"></a>00054 <span class="keyword">static</span> boost::mutex STATE_SPACE_LIST_LOCK;
<a name="l00055"></a>00055 }
<a name="l00056"></a>00056 }
<a name="l00058"></a>00058
<a name="l00059"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a59b001dad7dc63d37f63c78c04f1232d">00059</a> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a59b001dad7dc63d37f63c78c04f1232d" title="Constructor. Assigns a unique name to the space.">ompl::base::StateSpace::StateSpace</a>(<span class="keywordtype">void</span>)
<a name="l00060"></a>00060 {
<a name="l00061"></a>00061 <span class="comment">// autocompute a unique name</span>
<a name="l00062"></a>00062 <span class="keyword">static</span> boost::mutex lock;
<a name="l00063"></a>00063 <span class="keyword">static</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> m = 0;
<a name="l00064"></a>00064
<a name="l00065"></a>00065 lock.lock();
<a name="l00066"></a>00066 m++;
<a name="l00067"></a>00067 lock.unlock();
<a name="l00068"></a>00068
<a name="l00069"></a>00069 name_ = <span class="stringliteral">"Space"</span> + boost::lexical_cast<std::string>(m);
<a name="l00070"></a>00070 <a class="code" href="classompl_1_1base_1_1StateSpace.html#aaad59b5cd0da08195dd066485e6fa946" title="Interface used for console output.">msg_</a>.<a class="code" href="classompl_1_1msg_1_1Interface.html#a4dd5a3d384aa0cc95d86d258dea0e773" title="Set the text that will appear in front of every message forwarded by this Interface instance...">setPrefix</a>(name_);
<a name="l00071"></a>00071
<a name="l00072"></a>00072 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
<a name="l00073"></a>00073 STATE_SPACE_LIST.push_back(<span class="keyword">this</span>);
<a name="l00074"></a>00074
<a name="l00075"></a>00075 <a class="code" href="classompl_1_1base_1_1StateSpace.html#a8967afde4dc99333aef63eb85554bf81" title="The longest valid segment at the time setup() was called.">longestValidSegment_</a> = 0.0;
<a name="l00076"></a>00076 <a class="code" href="classompl_1_1base_1_1StateSpace.html#aeaba86765fbfc6686312e50934edfd70" title="The fraction of the longest valid segment.">longestValidSegmentFraction_</a> = 0.01; <span class="comment">// 1%</span>
<a name="l00077"></a>00077 <a class="code" href="classompl_1_1base_1_1StateSpace.html#aa3fbdc24d4e07283890eb68f49bc1467" title="The factor to multiply the value returned by validSegmentCount()">longestValidSegmentCountFactor_</a> = 1;
<a name="l00078"></a>00078
<a name="l00079"></a>00079 <a class="code" href="classompl_1_1base_1_1StateSpace.html#a68556edd816136a66a0d81544baef08c" title="A type assigned for this state space.">type_</a> = <a class="code" href="namespaceompl_1_1base.html#a056b022e14fe04a75f81789947353920a4247bd6859b13c6482caed7f9dd9996d" title="Unset type; this is the default type.">STATE_SPACE_UNKNOWN</a>;
<a name="l00080"></a>00080
<a name="l00081"></a>00081 <a class="code" href="classompl_1_1base_1_1StateSpace.html#af139d63d7b1067a70cb6e744e85fef96" title="The extent of this space at the time setup() was called.">maxExtent_</a> = std::numeric_limits<double>::infinity();
<a name="l00082"></a>00082 }
<a name="l00083"></a>00083
<a name="l00084"></a>00084 ompl::base::StateSpace::~StateSpace(<span class="keywordtype">void</span>)
<a name="l00085"></a>00085 {
<a name="l00086"></a>00086 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
<a name="l00087"></a>00087 STATE_SPACE_LIST.remove(<span class="keyword">this</span>);
<a name="l00088"></a>00088 }
<a name="l00089"></a>00089
<a name="l00090"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c">00090</a> <span class="keyword">const</span> std::string& <a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c" title="Get the name of the state space.">ompl::base::StateSpace::getName</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00091"></a>00091 <span class="keyword"></span>{
<a name="l00092"></a>00092 <span class="keywordflow">return</span> name_;
<a name="l00093"></a>00093 }
<a name="l00094"></a>00094
<a name="l00095"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#ad12cc022ef531dcb563f6d3d13b847ad">00095</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad12cc022ef531dcb563f6d3d13b847ad" title="Set the name of the state space.">ompl::base::StateSpace::setName</a>(<span class="keyword">const</span> std::string &name)
<a name="l00096"></a>00096 {
<a name="l00097"></a>00097 name_ = name;
<a name="l00098"></a>00098 msg_.setPrefix(name_);
<a name="l00099"></a>00099 }
<a name="l00100"></a>00100
<a name="l00101"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a18363a441aa3ec273b5a32bb9f9f21f3">00101</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a18363a441aa3ec273b5a32bb9f9f21f3" title="Register the projections for this state space. Usually, this is at least the default projection...">ompl::base::StateSpace::registerProjections</a>(<span class="keywordtype">void</span>)
<a name="l00102"></a>00102 {
<a name="l00103"></a>00103 }
<a name="l00104"></a>00104
<a name="l00105"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a62881b1915544da2e2843008da5f1109">00105</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a62881b1915544da2e2843008da5f1109" title="Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them. It is safe to call this function multiple times.">ompl::base::StateSpace::setup</a>(<span class="keywordtype">void</span>)
<a name="l00106"></a>00106 {
<a name="l00107"></a>00107 maxExtent_ = getMaximumExtent();
<a name="l00108"></a>00108 longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
<a name="l00109"></a>00109
<a name="l00110"></a>00110 <span class="keywordflow">if</span> (longestValidSegment_ < std::numeric_limits<double>::epsilon())
<a name="l00111"></a>00111 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"The longest valid segment for state space "</span> + getName() + <span class="stringliteral">" must be positive"</span>);
<a name="l00112"></a>00112
<a name="l00113"></a>00113 <span class="comment">// make sure we don't overwrite projections that have been configured by the user</span>
<a name="l00114"></a>00114 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
<a name="l00115"></a>00115 registerProjections();
<a name="l00116"></a>00116 <span class="keywordflow">for</span> (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
<a name="l00117"></a>00117 <span class="keywordflow">if</span> (it->second->userConfigured())
<a name="l00118"></a>00118 {
<a name="l00119"></a>00119 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
<a name="l00120"></a>00120 <span class="keywordflow">if</span> (o != projections_.end())
<a name="l00121"></a>00121 <span class="keywordflow">if</span> (!o->second->userConfigured())
<a name="l00122"></a>00122 projections_[it->first] = it->second;
<a name="l00123"></a>00123 }
<a name="l00124"></a>00124
<a name="l00125"></a>00125 <span class="keywordflow">for</span> (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
<a name="l00126"></a>00126 it->second->setup();
<a name="l00127"></a>00127 }
<a name="l00128"></a>00128
<a name="l00129"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a8584afb9165d2aa7408fec81c18659f4">00129</a> <span class="keywordtype">double</span>* <a class="code" href="classompl_1_1base_1_1StateSpace.html#a8584afb9165d2aa7408fec81c18659f4" title="Many states contain a number of double values. This function provides a means to get the memory addre...">ompl::base::StateSpace::getValueAddressAtIndex</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state, <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index)<span class="keyword"> const</span>
<a name="l00130"></a>00130 <span class="keyword"></span>{
<a name="l00131"></a>00131 <span class="keywordflow">return</span> NULL;
<a name="l00132"></a>00132 }
<a name="l00133"></a>00133
<a name="l00134"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a5f92fa826285aaa7adff4c1fd07a4023">00134</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a5f92fa826285aaa7adff4c1fd07a4023" title="Print a state to a stream.">ompl::base::StateSpace::printState</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state, std::ostream &out)<span class="keyword"> const</span>
<a name="l00135"></a>00135 <span class="keyword"></span>{
<a name="l00136"></a>00136 out << <span class="stringliteral">"State instance ["</span> << state << <span class="charliteral">']'</span> << std::endl;
<a name="l00137"></a>00137 }
<a name="l00138"></a>00138
<a name="l00139"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a0d78e50cceae2e0a432fea171a431324">00139</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a0d78e50cceae2e0a432fea171a431324" title="Print the settings for this state space to a stream.">ompl::base::StateSpace::printSettings</a>(std::ostream &out)<span class="keyword"> const</span>
<a name="l00140"></a>00140 <span class="keyword"></span>{
<a name="l00141"></a>00141 out << <span class="stringliteral">"StateSpace '"</span> << getName() << <span class="stringliteral">"' instance: "</span> << <span class="keyword">this</span> << std::endl;
<a name="l00142"></a>00142 printProjections(out);
<a name="l00143"></a>00143 }
<a name="l00144"></a>00144
<a name="l00145"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a988d0d31d8cf99ca55e83d83be778918">00145</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a988d0d31d8cf99ca55e83d83be778918" title="Print the list of registered projections. This function is also called by printSettings()">ompl::base::StateSpace::printProjections</a>(std::ostream &out)<span class="keyword"> const</span>
<a name="l00146"></a>00146 <span class="keyword"></span>{
<a name="l00147"></a>00147 <span class="keywordflow">if</span> (projections_.empty())
<a name="l00148"></a>00148 out << <span class="stringliteral">"No registered projections"</span> << std::endl;
<a name="l00149"></a>00149 <span class="keywordflow">else</span>
<a name="l00150"></a>00150 {
<a name="l00151"></a>00151 out << <span class="stringliteral">"Registered projections:"</span> << std::endl;
<a name="l00152"></a>00152 <span class="keywordflow">for</span> (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
<a name="l00153"></a>00153 {
<a name="l00154"></a>00154 out << <span class="stringliteral">" - "</span>;
<a name="l00155"></a>00155 <span class="keywordflow">if</span> (it->first == DEFAULT_PROJECTION_NAME)
<a name="l00156"></a>00156 out << <span class="stringliteral">"<default>"</span>;
<a name="l00157"></a>00157 <span class="keywordflow">else</span>
<a name="l00158"></a>00158 out << it->first;
<a name="l00159"></a>00159 out << std::endl;
<a name="l00160"></a>00160 it->second->printSettings(out);
<a name="l00161"></a>00161 }
<a name="l00162"></a>00162 }
<a name="l00163"></a>00163 }
<a name="l00164"></a>00164
<a name="l00166"></a>00166 <span class="keyword">namespace </span>ompl
<a name="l00167"></a>00167 {
<a name="l00168"></a>00168 <span class="keyword">namespace </span>base
<a name="l00169"></a>00169 {
<a name="l00170"></a>00170 <span class="keyword">static</span> <span class="keywordtype">bool</span> StateSpaceIncludes(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">StateSpace</a> *<span class="keyword">self</span>, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">StateSpace</a> *other)
<a name="l00171"></a>00171 {
<a name="l00172"></a>00172 std::queue<const StateSpace*> q;
<a name="l00173"></a>00173 q.push(<span class="keyword">self</span>);
<a name="l00174"></a>00174 <span class="keywordflow">while</span> (!q.empty())
<a name="l00175"></a>00175 {
<a name="l00176"></a>00176 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">StateSpace</a> *m = q.front();
<a name="l00177"></a>00177 q.pop();
<a name="l00178"></a>00178 <span class="keywordflow">if</span> (m-><a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c" title="Get the name of the state space.">getName</a>() == other-><a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c" title="Get the name of the state space.">getName</a>())
<a name="l00179"></a>00179 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00180"></a>00180 <span class="keywordflow">if</span> (m-><a class="code" href="classompl_1_1base_1_1StateSpace.html#af2876ac053f907e94f5b6d0df9719f1b" title="Check if the state space is compound.">isCompound</a>())
<a name="l00181"></a>00181 {
<a name="l00182"></a>00182 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> c = m-><a class="code" href="classompl_1_1base_1_1StateSpace.html#af1d70611d231dd7d463cea6220d3b01a" title="Cast this instance to a desired type.">as</a><<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>()->getSubSpaceCount();
<a name="l00183"></a>00183 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < c ; ++i)
<a name="l00184"></a>00184 q.push(m-><a class="code" href="classompl_1_1base_1_1StateSpace.html#af1d70611d231dd7d463cea6220d3b01a" title="Cast this instance to a desired type.">as</a><<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>()-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">getSubSpace</a>(i).get());
<a name="l00185"></a>00185 }
<a name="l00186"></a>00186 }
<a name="l00187"></a>00187 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00188"></a>00188 }
<a name="l00189"></a>00189
<a name="l00190"></a>00190 <span class="keyword">static</span> <span class="keywordtype">bool</span> StateSpaceCovers(<span class="keyword">const</span> StateSpace *<span class="keyword">self</span>, <span class="keyword">const</span> StateSpace *other)
<a name="l00191"></a>00191 {
<a name="l00192"></a>00192 <span class="keywordflow">if</span> (StateSpaceIncludes(<span class="keyword">self</span>, other))
<a name="l00193"></a>00193 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00194"></a>00194 <span class="keywordflow">else</span>
<a name="l00195"></a>00195 <span class="keywordflow">if</span> (other->isCompound())
<a name="l00196"></a>00196 {
<a name="l00197"></a>00197 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> c = other->as<CompoundStateSpace>()->getSubSpaceCount();
<a name="l00198"></a>00198 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < c ; ++i)
<a name="l00199"></a>00199 <span class="keywordflow">if</span> (!StateSpaceCovers(<span class="keyword">self</span>, other->as<CompoundStateSpace>()->getSubSpace(i).get()))
<a name="l00200"></a>00200 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00201"></a>00201 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00202"></a>00202 }
<a name="l00203"></a>00203 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00204"></a>00204 }
<a name="l00205"></a>00205 }
<a name="l00206"></a>00206 }
<a name="l00207"></a>00207
<a name="l00209"></a>00209
<a name="l00210"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a41a2cdf29ab85faa2b76cca36c23229d">00210</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a41a2cdf29ab85faa2b76cca36c23229d" title="Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this o...">ompl::base::StateSpace::covers</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &other)<span class="keyword"> const</span>
<a name="l00211"></a>00211 <span class="keyword"></span>{
<a name="l00212"></a>00212 <span class="keywordflow">return</span> StateSpaceCovers(<span class="keyword">this</span>, other.get());
<a name="l00213"></a>00213 }
<a name="l00214"></a>00214
<a name="l00215"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#af294ec92dcc0e35793570d039d61d019">00215</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#af294ec92dcc0e35793570d039d61d019" title="Return true if other is a space included (perhaps equal, perhaps a subspace) in this one...">ompl::base::StateSpace::includes</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &other)<span class="keyword"> const</span>
<a name="l00216"></a>00216 <span class="keyword"></span>{
<a name="l00217"></a>00217 <span class="keywordflow">return</span> StateSpaceIncludes(<span class="keyword">this</span>, other.get());
<a name="l00218"></a>00218 }
<a name="l00219"></a>00219
<a name="l00220"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a925ed4f0e97f137b4c2128afe9f36b2f">00220</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a925ed4f0e97f137b4c2128afe9f36b2f" title="Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...">ompl::base::StateSpace::Diagram</a>(std::ostream &out)
<a name="l00221"></a>00221 {
<a name="l00222"></a>00222 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
<a name="l00223"></a>00223 out << <span class="stringliteral">"digraph StateSpaces {"</span> << std::endl;
<a name="l00224"></a>00224 <span class="keywordflow">for</span> (std::list<StateSpace*>::iterator it = STATE_SPACE_LIST.begin() ; it != STATE_SPACE_LIST.end(); ++it)
<a name="l00225"></a>00225 {
<a name="l00226"></a>00226 out << <span class="charliteral">'"'</span> << (*it)->getName() << <span class="charliteral">'"'</span> << std::endl;
<a name="l00227"></a>00227 <span class="keywordflow">for</span> (std::list<StateSpace*>::iterator jt = STATE_SPACE_LIST.begin() ; jt != STATE_SPACE_LIST.end(); ++jt)
<a name="l00228"></a>00228 <span class="keywordflow">if</span> (it != jt)
<a name="l00229"></a>00229 {
<a name="l00230"></a>00230 <span class="keywordflow">if</span> ((*it)->isCompound() && (*it)->as<<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>()->hasSubSpace((*jt)->getName()))
<a name="l00231"></a>00231 out << <span class="charliteral">'"'</span> << (*it)->getName() << <span class="stringliteral">"\" -> \""</span> << (*jt)->getName() << <span class="stringliteral">"\" [label=\""</span> <<
<a name="l00232"></a>00232 boost::lexical_cast<std::string>((*it)->as<<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>()->getSubSpaceWeight((*jt)->getName())) <<
<a name="l00233"></a>00233 <span class="stringliteral">"\"];"</span> << std::endl;
<a name="l00234"></a>00234 <span class="keywordflow">else</span>
<a name="l00235"></a>00235 <span class="keywordflow">if</span> (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
<a name="l00236"></a>00236 out << <span class="charliteral">'"'</span> << (*it)->getName() << <span class="stringliteral">"\" -> \""</span> << (*jt)->getName() << <span class="stringliteral">"\" [style=dashed];"</span> << std::endl;
<a name="l00237"></a>00237 }
<a name="l00238"></a>00238 }
<a name="l00239"></a>00239 out << <span class="charliteral">'}'</span> << std::endl;
<a name="l00240"></a>00240 }
<a name="l00241"></a>00241
<a name="l00242"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a8e7d1bb109cf5fccbd537722ca5e6761">00242</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a8e7d1bb109cf5fccbd537722ca5e6761" title="Perform sanity checks for this state space. Throws an exception if failures are found.">ompl::base::StateSpace::sanityChecks</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00243"></a>00243 <span class="keyword"></span>{
<a name="l00244"></a>00244 <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">double</span> EPS = std::numeric_limits<float>::epsilon(); <span class="comment">// we want to allow for reduced accuracy in computation</span>
<a name="l00245"></a>00245 <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">double</span> ZERO = std::numeric_limits<double>::epsilon();
<a name="l00246"></a>00246
<a name="l00247"></a>00247 <span class="comment">// Test that distances are always positive</span>
<a name="l00248"></a>00248 {
<a name="l00249"></a>00249 <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *s1 = allocState();
<a name="l00250"></a>00250 <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *s2 = allocState();
<a name="l00251"></a>00251 <a class="code" href="classompl_1_1base_1_1StateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::StateSampler.">StateSamplerPtr</a> ss = allocStateSampler();
<a name="l00252"></a>00252
<a name="l00253"></a>00253 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < <a class="code" href="namespaceompl_1_1magic.html#ad96353c64e790206f36a2fb9b2c7d646" title="When multiple states need to be generated as part of the computation of various information (usually ...">magic::TEST_STATE_COUNT</a> ; ++i)
<a name="l00254"></a>00254 {
<a name="l00255"></a>00255 ss->sampleUniform(s1);
<a name="l00256"></a>00256 <span class="keywordflow">if</span> (distance(s1, s1) > EPS)
<a name="l00257"></a>00257 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Distance from a state to itself should be 0"</span>);
<a name="l00258"></a>00258 ss->sampleUniform(s2);
<a name="l00259"></a>00259 <span class="keywordflow">if</span> (!equalStates(s1, s2))
<a name="l00260"></a>00260 {
<a name="l00261"></a>00261 <span class="keywordtype">double</span> d12 = distance(s1, s2);
<a name="l00262"></a>00262 <span class="keywordflow">if</span> (d12 < ZERO)
<a name="l00263"></a>00263 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Distance between different states should be above 0"</span>);
<a name="l00264"></a>00264 <span class="keywordtype">double</span> d21 = distance(s2, s1);
<a name="l00265"></a>00265 <span class="keywordflow">if</span> (fabs(d12 - d21) > EPS)
<a name="l00266"></a>00266 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"The distance function should be symmetric"</span>);
<a name="l00267"></a>00267 }
<a name="l00268"></a>00268 }
<a name="l00269"></a>00269
<a name="l00270"></a>00270 freeState(s1);
<a name="l00271"></a>00271 freeState(s2);
<a name="l00272"></a>00272 }
<a name="l00273"></a>00273
<a name="l00274"></a>00274
<a name="l00275"></a>00275 <span class="comment">// Test that interpolation works as expected and also test triangle inequality</span>
<a name="l00276"></a>00276 <span class="keywordflow">if</span> (!isDiscrete() && !isHybrid())
<a name="l00277"></a>00277 {
<a name="l00278"></a>00278 <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *s1 = allocState();
<a name="l00279"></a>00279 <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *s2 = allocState();
<a name="l00280"></a>00280 <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *s3 = allocState();
<a name="l00281"></a>00281 <a class="code" href="classompl_1_1base_1_1StateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::StateSampler.">StateSamplerPtr</a> ss = allocStateSampler();
<a name="l00282"></a>00282
<a name="l00283"></a>00283 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < <a class="code" href="namespaceompl_1_1magic.html#ad96353c64e790206f36a2fb9b2c7d646" title="When multiple states need to be generated as part of the computation of various information (usually ...">magic::TEST_STATE_COUNT</a> ; ++i)
<a name="l00284"></a>00284 {
<a name="l00285"></a>00285 ss->sampleUniform(s1);
<a name="l00286"></a>00286 ss->sampleUniform(s2);
<a name="l00287"></a>00287 ss->sampleUniform(s3);
<a name="l00288"></a>00288
<a name="l00289"></a>00289 interpolate(s1, s2, 0.0, s3);
<a name="l00290"></a>00290 <span class="keywordflow">if</span> (distance(s1, s3) > EPS)
<a name="l00291"></a>00291 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Interpolation from a state at time 0 should be not change the original state"</span>);
<a name="l00292"></a>00292
<a name="l00293"></a>00293 interpolate(s1, s2, 1.0, s3);
<a name="l00294"></a>00294 <span class="keywordflow">if</span> (distance(s2, s3) > EPS)
<a name="l00295"></a>00295 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Interpolation to a state at time 1 should be the same as the final state"</span>);
<a name="l00296"></a>00296
<a name="l00297"></a>00297 interpolate(s1, s2, 0.5, s3);
<a name="l00298"></a>00298 <span class="keywordtype">double</span> diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
<a name="l00299"></a>00299 <span class="keywordflow">if</span> (fabs(diff) > EPS)
<a name="l00300"></a>00300 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality ("</span> +
<a name="l00301"></a>00301 boost::lexical_cast<std::string>(diff) + <span class="stringliteral">" difference)"</span>);
<a name="l00302"></a>00302
<a name="l00303"></a>00303 interpolate(s3, s2, 0.5, s3);
<a name="l00304"></a>00304 interpolate(s1, s2, 0.75, s2);
<a name="l00305"></a>00305
<a name="l00306"></a>00306 <span class="keywordflow">if</span> (distance(s2, s3) > EPS)
<a name="l00307"></a>00307 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Continued interpolation does not work as expected"</span>);
<a name="l00308"></a>00308 }
<a name="l00309"></a>00309 freeState(s1);
<a name="l00310"></a>00310 freeState(s2);
<a name="l00311"></a>00311 freeState(s3);
<a name="l00312"></a>00312 }
<a name="l00313"></a>00313 }
<a name="l00314"></a>00314
<a name="l00315"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a72ae78109f391727694c9f582a746e1b">00315</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a72ae78109f391727694c9f582a746e1b" title="Check if a default projection is available.">ompl::base::StateSpace::hasDefaultProjection</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00316"></a>00316 <span class="keyword"></span>{
<a name="l00317"></a>00317 <span class="keywordflow">return</span> hasProjection(DEFAULT_PROJECTION_NAME);
<a name="l00318"></a>00318 }
<a name="l00319"></a>00319
<a name="l00320"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#aade8b8097e7c5defa159797a80c2ebf2">00320</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#aade8b8097e7c5defa159797a80c2ebf2" title="Check if a projection with a specified name is available.">ompl::base::StateSpace::hasProjection</a>(<span class="keyword">const</span> std::string &name)<span class="keyword"> const</span>
<a name="l00321"></a>00321 <span class="keyword"></span>{
<a name="l00322"></a>00322 <span class="keywordflow">return</span> projections_.find(name) != projections_.end();
<a name="l00323"></a>00323 }
<a name="l00324"></a>00324
<a name="l00325"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#adae69d06a2060e4e555e9e42453ca97d">00325</a> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ompl::base::ProjectionEvaluatorPtr</a> <a class="code" href="classompl_1_1base_1_1StateSpace.html#adae69d06a2060e4e555e9e42453ca97d" title="Get the default projection.">ompl::base::StateSpace::getDefaultProjection</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00326"></a>00326 <span class="keyword"></span>{
<a name="l00327"></a>00327 <span class="keywordflow">if</span> (hasDefaultProjection())
<a name="l00328"></a>00328 <span class="keywordflow">return</span> getProjection(DEFAULT_PROJECTION_NAME);
<a name="l00329"></a>00329 <span class="keywordflow">else</span>
<a name="l00330"></a>00330 {
<a name="l00331"></a>00331 msg_.error(<span class="stringliteral">"No default projection is set. Perhaps setup() needs to be called"</span>);
<a name="l00332"></a>00332 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a>();
<a name="l00333"></a>00333 }
<a name="l00334"></a>00334 }
<a name="l00335"></a>00335
<a name="l00336"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a05e75a507f6e8e538d1542bc9e31b256">00336</a> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ompl::base::ProjectionEvaluatorPtr</a> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a05e75a507f6e8e538d1542bc9e31b256" title="Get the projection registered under a specific name.">ompl::base::StateSpace::getProjection</a>(<span class="keyword">const</span> std::string &name)<span class="keyword"> const</span>
<a name="l00337"></a>00337 <span class="keyword"></span>{
<a name="l00338"></a>00338 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
<a name="l00339"></a>00339 <span class="keywordflow">if</span> (it != projections_.end())
<a name="l00340"></a>00340 <span class="keywordflow">return</span> it->second;
<a name="l00341"></a>00341 <span class="keywordflow">else</span>
<a name="l00342"></a>00342 {
<a name="l00343"></a>00343 msg_.error(<span class="stringliteral">"Projection '"</span> + name + <span class="stringliteral">"' is not defined"</span>);
<a name="l00344"></a>00344 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a>();
<a name="l00345"></a>00345 }
<a name="l00346"></a>00346 }
<a name="l00347"></a>00347
<a name="l00348"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#ad5b424bc0e9b65552442104372639a64">00348</a> <span class="keyword">const</span> std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad5b424bc0e9b65552442104372639a64" title="Get all the registered projections.">ompl::base::StateSpace::getRegisteredProjections</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00349"></a>00349 <span class="keyword"></span>{
<a name="l00350"></a>00350 <span class="keywordflow">return</span> projections_;
<a name="l00351"></a>00351 }
<a name="l00352"></a>00352
<a name="l00353"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#ac79f6342fb2d85f1c2c87bfff812fbbf">00353</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#ac79f6342fb2d85f1c2c87bfff812fbbf" title="Register the default projection for this state space.">ompl::base::StateSpace::registerDefaultProjection</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a> &projection)
<a name="l00354"></a>00354 {
<a name="l00355"></a>00355 registerProjection(DEFAULT_PROJECTION_NAME, projection);
<a name="l00356"></a>00356 }
<a name="l00357"></a>00357
<a name="l00358"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a309f2cdc5412ff16d31b87fec92b3595">00358</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a309f2cdc5412ff16d31b87fec92b3595" title="Register a projection for this state space under a specified name.">ompl::base::StateSpace::registerProjection</a>(<span class="keyword">const</span> std::string &name, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a> &projection)
<a name="l00359"></a>00359 {
<a name="l00360"></a>00360 <span class="keywordflow">if</span> (projection)
<a name="l00361"></a>00361 projections_[name] = projection;
<a name="l00362"></a>00362 <span class="keywordflow">else</span>
<a name="l00363"></a>00363 msg_.error(<span class="stringliteral">"Attempting to register invalid projection under name '%s'. Ignoring."</span>, name.c_str());
<a name="l00364"></a>00364 }
<a name="l00365"></a>00365
<a name="l00366"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#af2876ac053f907e94f5b6d0df9719f1b">00366</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#af2876ac053f907e94f5b6d0df9719f1b" title="Check if the state space is compound.">ompl::base::StateSpace::isCompound</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00367"></a>00367 <span class="keyword"></span>{
<a name="l00368"></a>00368 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00369"></a>00369 }
<a name="l00370"></a>00370
<a name="l00371"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a0ba7529960eb53ab38ce532236277baf">00371</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a0ba7529960eb53ab38ce532236277baf" title="Check if the set of states is discrete.">ompl::base::StateSpace::isDiscrete</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00372"></a>00372 <span class="keyword"></span>{
<a name="l00373"></a>00373 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00374"></a>00374 }
<a name="l00375"></a>00375
<a name="l00376"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a28d46d3774a6632735df7889f91fc257">00376</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a28d46d3774a6632735df7889f91fc257" title="Check if this is a hybrid state space (i.e., both discrete and continuous components exist)...">ompl::base::StateSpace::isHybrid</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00377"></a>00377 <span class="keyword"></span>{
<a name="l00378"></a>00378 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00379"></a>00379 }
<a name="l00380"></a>00380
<a name="l00381"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a9babd71497fc6a6f82fe97f699531ceb">00381</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a9babd71497fc6a6f82fe97f699531ceb" title="Set factor to be the value to multiply the return value of validSegmentCount(). By default...">ompl::base::StateSpace::setValidSegmentCountFactor</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> factor)
<a name="l00382"></a>00382 {
<a name="l00383"></a>00383 <span class="keywordflow">if</span> (factor < 1)
<a name="l00384"></a>00384 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"The multiplicative factor for the valid segment count between two states must be strictly positive"</span>);
<a name="l00385"></a>00385 longestValidSegmentCountFactor_ = factor;
<a name="l00386"></a>00386 }
<a name="l00387"></a>00387
<a name="l00388"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#ad8e6450ad306d272df39751aa7e690b8">00388</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad8e6450ad306d272df39751aa7e690b8" title="When performing discrete validation of motions, the length of the longest segment that does not requi...">ompl::base::StateSpace::setLongestValidSegmentFraction</a>(<span class="keywordtype">double</span> segmentFraction)
<a name="l00389"></a>00389 {
<a name="l00390"></a>00390 <span class="keywordflow">if</span> (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
<a name="l00391"></a>00391 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"The fraction of the extent must be larger than 0 and less than 1"</span>);
<a name="l00392"></a>00392 longestValidSegmentFraction_ = segmentFraction;
<a name="l00393"></a>00393 }
<a name="l00394"></a>00394
<a name="l00395"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#ad310457600b4bad5b43cc28d3a835f41">00395</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad310457600b4bad5b43cc28d3a835f41" title="Get the value used to multiply the return value of validSegmentCount().">ompl::base::StateSpace::getValidSegmentCountFactor</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00396"></a>00396 <span class="keyword"></span>{
<a name="l00397"></a>00397 <span class="keywordflow">return</span> longestValidSegmentCountFactor_;
<a name="l00398"></a>00398 }
<a name="l00399"></a>00399
<a name="l00400"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#af5a789c55cf60407f1f015fc11ec7f57">00400</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#af5a789c55cf60407f1f015fc11ec7f57" title="When performing discrete validation of motions, the length of the longest segment that does not requi...">ompl::base::StateSpace::getLongestValidSegmentFraction</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00401"></a>00401 <span class="keyword"></span>{
<a name="l00402"></a>00402 <span class="keywordflow">return</span> longestValidSegmentFraction_;
<a name="l00403"></a>00403 }
<a name="l00404"></a>00404
<a name="l00405"></a><a class="code" href="classompl_1_1base_1_1StateSpace.html#a17443b992677f5043d3fc38e9288b9a7">00405</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a17443b992677f5043d3fc38e9288b9a7" title="Count how many segments of the "longest valid length" fit on the motion from state1 to state2...">ompl::base::StateSpace::validSegmentCount</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state1, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state2)<span class="keyword"> const</span>
<a name="l00406"></a>00406 <span class="keyword"></span>{
<a name="l00407"></a>00407 <span class="keywordflow">return</span> longestValidSegmentCountFactor_ * (<span class="keywordtype">unsigned</span> int)ceil(distance(state1, state2) / longestValidSegment_);
<a name="l00408"></a>00408 }
<a name="l00409"></a>00409
<a name="l00410"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a43fcd7596d5bd743c05a9142e7405bb9">00410</a> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a43fcd7596d5bd743c05a9142e7405bb9" title="Construct an empty compound state space.">ompl::base::CompoundStateSpace::CompoundStateSpace</a>(<span class="keywordtype">void</span>) : <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">StateSpace</a>(), componentCount_(0), locked_(false)
<a name="l00411"></a>00411 {
<a name="l00412"></a>00412 <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad12cc022ef531dcb563f6d3d13b847ad" title="Set the name of the state space.">setName</a>(<span class="stringliteral">"Compound"</span> + <a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c" title="Get the name of the state space.">getName</a>());
<a name="l00413"></a>00413 }
<a name="l00414"></a>00414
<a name="l00415"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a813ec9cca9b410654350c8125892c5b2">00415</a> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a43fcd7596d5bd743c05a9142e7405bb9" title="Construct an empty compound state space.">ompl::base::CompoundStateSpace::CompoundStateSpace</a>(<span class="keyword">const</span> std::vector<StateSpacePtr> &components,
<a name="l00416"></a>00416 <span class="keyword">const</span> std::vector<double> &weights) : <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">StateSpace</a>(), componentCount_(0), locked_(false)
<a name="l00417"></a>00417 {
<a name="l00418"></a>00418 <span class="keywordflow">if</span> (components.size() != weights.size())
<a name="l00419"></a>00419 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Number of component spaces and weights are not the same"</span>);
<a name="l00420"></a>00420 <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad12cc022ef531dcb563f6d3d13b847ad" title="Set the name of the state space.">setName</a>(<span class="stringliteral">"Compound"</span> + <a class="code" href="classompl_1_1base_1_1StateSpace.html#a90015676df178b4406a8ab8399f0624c" title="Get the name of the state space.">getName</a>());
<a name="l00421"></a>00421 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < components.size() ; ++i)
<a name="l00422"></a>00422 <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aba4aafa2e39b134bf55c5a2ba17d29af" title="Adds a new state space as part of the compound state space. For computing distances within the compou...">addSubSpace</a>(components[i], weights[i]);
<a name="l00423"></a>00423 }
<a name="l00424"></a>00424
<a name="l00425"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aba4aafa2e39b134bf55c5a2ba17d29af">00425</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aba4aafa2e39b134bf55c5a2ba17d29af" title="Adds a new state space as part of the compound state space. For computing distances within the compou...">ompl::base::CompoundStateSpace::addSubSpace</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &component, <span class="keywordtype">double</span> weight)
<a name="l00426"></a>00426 {
<a name="l00427"></a>00427 <span class="keywordflow">if</span> (locked_)
<a name="l00428"></a>00428 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"This state space is locked. No further components can be added"</span>);
<a name="l00429"></a>00429 <span class="keywordflow">if</span> (weight < 0.0)
<a name="l00430"></a>00430 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace weight cannot be negative"</span>);
<a name="l00431"></a>00431 components_.push_back(component);
<a name="l00432"></a>00432 weights_.push_back(weight);
<a name="l00433"></a>00433 componentCount_ = components_.size();
<a name="l00434"></a>00434 }
<a name="l00435"></a>00435
<a name="l00436"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a2b2134589bd8224a23c02602b8440ee2">00436</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a2b2134589bd8224a23c02602b8440ee2" title="Check if the state space is compound.">ompl::base::CompoundStateSpace::isCompound</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00437"></a>00437 <span class="keyword"></span>{
<a name="l00438"></a>00438 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00439"></a>00439 }
<a name="l00440"></a>00440
<a name="l00441"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aabe662bef58276a2576610ec986c3c64">00441</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aabe662bef58276a2576610ec986c3c64" title="Check if this is a hybrid state space (i.e., both discrete and continuous components exist)...">ompl::base::CompoundStateSpace::isHybrid</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00442"></a>00442 <span class="keyword"></span>{
<a name="l00443"></a>00443 <span class="keywordtype">bool</span> c = <span class="keyword">false</span>;
<a name="l00444"></a>00444 <span class="keywordtype">bool</span> d = <span class="keyword">false</span>;
<a name="l00445"></a>00445 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00446"></a>00446 {
<a name="l00447"></a>00447 <span class="keywordflow">if</span> (components_[i]->isHybrid())
<a name="l00448"></a>00448 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00449"></a>00449 <span class="keywordflow">if</span> (components_[i]->isDiscrete())
<a name="l00450"></a>00450 d = <span class="keyword">true</span>;
<a name="l00451"></a>00451 <span class="keywordflow">else</span>
<a name="l00452"></a>00452 c = <span class="keyword">true</span>;
<a name="l00453"></a>00453 }
<a name="l00454"></a>00454 <span class="keywordflow">return</span> c && d;
<a name="l00455"></a>00455 }
<a name="l00456"></a>00456
<a name="l00457"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502">00457</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502" title="Get the number of state spaces that make up the compound state space.">ompl::base::CompoundStateSpace::getSubSpaceCount</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00458"></a>00458 <span class="keyword"></span>{
<a name="l00459"></a>00459 <span class="keywordflow">return</span> componentCount_;
<a name="l00460"></a>00460 }
<a name="l00461"></a>00461
<a name="l00462"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258">00462</a> <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">ompl::base::StateSpacePtr</a>& <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">ompl::base::CompoundStateSpace::getSubSpace</a>(<span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index)<span class="keyword"> const</span>
<a name="l00463"></a>00463 <span class="keyword"></span>{
<a name="l00464"></a>00464 <span class="keywordflow">if</span> (componentCount_ > index)
<a name="l00465"></a>00465 <span class="keywordflow">return</span> components_[index];
<a name="l00466"></a>00466 <span class="keywordflow">else</span>
<a name="l00467"></a>00467 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace index does not exist"</span>);
<a name="l00468"></a>00468 }
<a name="l00469"></a>00469
<a name="l00470"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a549962d6cc1b693d477a03be14c40e20">00470</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a549962d6cc1b693d477a03be14c40e20" title="Check if a specific subspace is contained in this state space.">ompl::base::CompoundStateSpace::hasSubSpace</a>(<span class="keyword">const</span> std::string &name)<span class="keyword"> const</span>
<a name="l00471"></a>00471 <span class="keyword"></span>{
<a name="l00472"></a>00472 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00473"></a>00473 <span class="keywordflow">if</span> (components_[i]->getName() == name)
<a name="l00474"></a>00474 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00475"></a>00475 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00476"></a>00476 }
<a name="l00477"></a>00477
<a name="l00478"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a70b32e56e81f9a115bfa21d9a457a219">00478</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a70b32e56e81f9a115bfa21d9a457a219" title="Get the index of a specific subspace from the compound state space.">ompl::base::CompoundStateSpace::getSubSpaceIndex</a>(<span class="keyword">const</span> std::string& name)<span class="keyword"> const</span>
<a name="l00479"></a>00479 <span class="keyword"></span>{
<a name="l00480"></a>00480 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00481"></a>00481 <span class="keywordflow">if</span> (components_[i]->getName() == name)
<a name="l00482"></a>00482 <span class="keywordflow">return</span> i;
<a name="l00483"></a>00483 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace "</span> + name + <span class="stringliteral">" does not exist"</span>);
<a name="l00484"></a>00484 }
<a name="l00485"></a>00485
<a name="l00486"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ab963001ee41246e2519a82af61c3abe5">00486</a> <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">ompl::base::StateSpacePtr</a>& <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">ompl::base::CompoundStateSpace::getSubSpace</a>(<span class="keyword">const</span> std::string& name)<span class="keyword"> const</span>
<a name="l00487"></a>00487 <span class="keyword"></span>{
<a name="l00488"></a>00488 <span class="keywordflow">return</span> components_[getSubSpaceIndex(name)];
<a name="l00489"></a>00489 }
<a name="l00490"></a>00490
<a name="l00491"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ad4ba7cf6b02982be877b184ac605cef4">00491</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ad4ba7cf6b02982be877b184ac605cef4" title="Get the weight of a subspace from the compound state space (used in distance computation)">ompl::base::CompoundStateSpace::getSubSpaceWeight</a>(<span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index)<span class="keyword"> const</span>
<a name="l00492"></a>00492 <span class="keyword"></span>{
<a name="l00493"></a>00493 <span class="keywordflow">if</span> (componentCount_ > index)
<a name="l00494"></a>00494 <span class="keywordflow">return</span> weights_[index];
<a name="l00495"></a>00495 <span class="keywordflow">else</span>
<a name="l00496"></a>00496 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace index does not exist"</span>);
<a name="l00497"></a>00497 }
<a name="l00498"></a>00498
<a name="l00499"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a90fd8617431ddcc30a8c4597d3719a19">00499</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ad4ba7cf6b02982be877b184ac605cef4" title="Get the weight of a subspace from the compound state space (used in distance computation)">ompl::base::CompoundStateSpace::getSubSpaceWeight</a>(<span class="keyword">const</span> std::string &name)<span class="keyword"> const</span>
<a name="l00500"></a>00500 <span class="keyword"></span>{
<a name="l00501"></a>00501 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00502"></a>00502 <span class="keywordflow">if</span> (components_[i]->getName() == name)
<a name="l00503"></a>00503 <span class="keywordflow">return</span> weights_[i];
<a name="l00504"></a>00504 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace "</span> + name + <span class="stringliteral">" does not exist"</span>);
<a name="l00505"></a>00505 }
<a name="l00506"></a>00506
<a name="l00507"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a9a0b91b7687d5c920da4e7e22dde78ad">00507</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a9a0b91b7687d5c920da4e7e22dde78ad" title="Set the weight of a subspace in the compound state space (used in distance computation)">ompl::base::CompoundStateSpace::setSubSpaceWeight</a>(<span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index, <span class="keywordtype">double</span> weight)
<a name="l00508"></a>00508 {
<a name="l00509"></a>00509 <span class="keywordflow">if</span> (weight < 0.0)
<a name="l00510"></a>00510 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace weight cannot be negative"</span>);
<a name="l00511"></a>00511 <span class="keywordflow">if</span> (componentCount_ > index)
<a name="l00512"></a>00512 weights_[index] = weight;
<a name="l00513"></a>00513 <span class="keywordflow">else</span>
<a name="l00514"></a>00514 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace index does not exist"</span>);
<a name="l00515"></a>00515 }
<a name="l00516"></a>00516
<a name="l00517"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a20ad33526273208fa9e2a4524d40ddb3">00517</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a9a0b91b7687d5c920da4e7e22dde78ad" title="Set the weight of a subspace in the compound state space (used in distance computation)">ompl::base::CompoundStateSpace::setSubSpaceWeight</a>(<span class="keyword">const</span> std::string &name, <span class="keywordtype">double</span> weight)
<a name="l00518"></a>00518 {
<a name="l00519"></a>00519 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00520"></a>00520 <span class="keywordflow">if</span> (components_[i]->getName() == name)
<a name="l00521"></a>00521 {
<a name="l00522"></a>00522 setSubSpaceWeight(i, weight);
<a name="l00523"></a>00523 <span class="keywordflow">return</span>;
<a name="l00524"></a>00524 }
<a name="l00525"></a>00525 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Subspace "</span> + name + <span class="stringliteral">" does not exist"</span>);
<a name="l00526"></a>00526 }
<a name="l00527"></a>00527
<a name="l00528"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a68e5c2ffbeb22a2b637dec2489dd09e0">00528</a> <span class="keyword">const</span> std::vector<ompl::base::StateSpacePtr>& <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a68e5c2ffbeb22a2b637dec2489dd09e0" title="Get the list of components.">ompl::base::CompoundStateSpace::getSubSpaces</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00529"></a>00529 <span class="keyword"></span>{
<a name="l00530"></a>00530 <span class="keywordflow">return</span> components_;
<a name="l00531"></a>00531 }
<a name="l00532"></a>00532
<a name="l00533"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a609e33e2d2d29aa0c8b7c4a7404b3103">00533</a> <span class="keyword">const</span> std::vector<double>& <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a609e33e2d2d29aa0c8b7c4a7404b3103" title="Get the list of component weights.">ompl::base::CompoundStateSpace::getSubSpaceWeights</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00534"></a>00534 <span class="keyword"></span>{
<a name="l00535"></a>00535 <span class="keywordflow">return</span> weights_;
<a name="l00536"></a>00536 }
<a name="l00537"></a>00537
<a name="l00538"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a40684ad7c0a264f4061b088cb414e162">00538</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a40684ad7c0a264f4061b088cb414e162" title="Get the dimension of the space (not the dimension of the surrounding ambient space)">ompl::base::CompoundStateSpace::getDimension</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00539"></a>00539 <span class="keyword"></span>{
<a name="l00540"></a>00540 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> dim = 0;
<a name="l00541"></a>00541 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00542"></a>00542 dim += components_[i]->getDimension();
<a name="l00543"></a>00543 <span class="keywordflow">return</span> dim;
<a name="l00544"></a>00544 }
<a name="l00545"></a>00545
<a name="l00546"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a70ff28c17797dc5aa6e56f25034410c7">00546</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a70ff28c17797dc5aa6e56f25034410c7" title="Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...">ompl::base::CompoundStateSpace::getMaximumExtent</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00547"></a>00547 <span class="keyword"></span>{
<a name="l00548"></a>00548 <span class="keywordtype">double</span> e = 0.0;
<a name="l00549"></a>00549 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00550"></a>00550 e += weights_[i] * components_[i]->getMaximumExtent();
<a name="l00551"></a>00551 <span class="keywordflow">return</span> e;
<a name="l00552"></a>00552 }
<a name="l00553"></a>00553
<a name="l00554"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a894235af9908df797d88f0df6e81b8d1">00554</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a894235af9908df797d88f0df6e81b8d1" title="Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...">ompl::base::CompoundStateSpace::enforceBounds</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state)<span class="keyword"> const</span>
<a name="l00555"></a>00555 <span class="keyword"></span>{
<a name="l00556"></a>00556 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00557"></a>00557 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00558"></a>00558 components_[i]->enforceBounds(cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00559"></a>00559 }
<a name="l00560"></a>00560
<a name="l00561"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a9774b49bc4ac4bcb6aed4f6a99454385">00561</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a9774b49bc4ac4bcb6aed4f6a99454385" title="Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...">ompl::base::CompoundStateSpace::satisfiesBounds</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state)<span class="keyword"> const</span>
<a name="l00562"></a>00562 <span class="keyword"></span>{
<a name="l00563"></a>00563 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00564"></a>00564 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00565"></a>00565 <span class="keywordflow">if</span> (!components_[i]->satisfiesBounds(cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]))
<a name="l00566"></a>00566 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00567"></a>00567 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00568"></a>00568 }
<a name="l00569"></a>00569
<a name="l00570"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a4dbc38743832260896d42d89e0b3feec">00570</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a4dbc38743832260896d42d89e0b3feec" title="Copy a state to another. The memory of source and destination should NOT overlap.">ompl::base::CompoundStateSpace::copyState</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *destination, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *source)<span class="keyword"> const</span>
<a name="l00571"></a>00571 <span class="keyword"></span>{
<a name="l00572"></a>00572 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cdest = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(destination);
<a name="l00573"></a>00573 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *csrc = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(source);
<a name="l00574"></a>00574 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00575"></a>00575 components_[i]->copyState(cdest-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], csrc-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00576"></a>00576 }
<a name="l00577"></a>00577
<a name="l00578"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a3f3a7c201f2deb0e8f4c5d3be02300eb">00578</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a3f3a7c201f2deb0e8f4c5d3be02300eb" title="Computes distance to between two states. This function satisfies the properties of a metric and its r...">ompl::base::CompoundStateSpace::distance</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state1, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state2)<span class="keyword"> const</span>
<a name="l00579"></a>00579 <span class="keyword"></span>{
<a name="l00580"></a>00580 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate1 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state1);
<a name="l00581"></a>00581 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate2 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state2);
<a name="l00582"></a>00582 <span class="keywordtype">double</span> dist = 0.0;
<a name="l00583"></a>00583 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00584"></a>00584 dist += weights_[i] * components_[i]->distance(cstate1-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], cstate2-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00585"></a>00585 <span class="keywordflow">return</span> dist;
<a name="l00586"></a>00586 }
<a name="l00587"></a>00587
<a name="l00588"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ac07e99d9d6749436e4e3549023ae96fe">00588</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ac07e99d9d6749436e4e3549023ae96fe" title="When performing discrete validation of motions, the length of the longest segment that does not requi...">ompl::base::CompoundStateSpace::setLongestValidSegmentFraction</a>(<span class="keywordtype">double</span> segmentFraction)
<a name="l00589"></a>00589 {
<a name="l00590"></a>00590 <a class="code" href="classompl_1_1base_1_1StateSpace.html#ad8e6450ad306d272df39751aa7e690b8" title="When performing discrete validation of motions, the length of the longest segment that does not requi...">StateSpace::setLongestValidSegmentFraction</a>(segmentFraction);
<a name="l00591"></a>00591 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00592"></a>00592 components_[i]->setLongestValidSegmentFraction(segmentFraction);
<a name="l00593"></a>00593 }
<a name="l00594"></a>00594
<a name="l00595"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#adc9e674f7301a307b1a547d9548b542f">00595</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#adc9e674f7301a307b1a547d9548b542f" title="Count how many segments of the "longest valid length" fit on the motion from state1 to state2...">ompl::base::CompoundStateSpace::validSegmentCount</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state1, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state2)<span class="keyword"> const</span>
<a name="l00596"></a>00596 <span class="keyword"></span>{
<a name="l00597"></a>00597 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate1 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state1);
<a name="l00598"></a>00598 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate2 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state2);
<a name="l00599"></a>00599 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> sc = 0;
<a name="l00600"></a>00600 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00601"></a>00601 {
<a name="l00602"></a>00602 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> sci = components_[i]->validSegmentCount(cstate1-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], cstate2-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00603"></a>00603 <span class="keywordflow">if</span> (sci > sc)
<a name="l00604"></a>00604 sc = sci;
<a name="l00605"></a>00605 }
<a name="l00606"></a>00606 <span class="keywordflow">return</span> sc;
<a name="l00607"></a>00607 }
<a name="l00608"></a>00608
<a name="l00609"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a382c3d7ac348ae7ea9ef6f8a4284d8f9">00609</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a382c3d7ac348ae7ea9ef6f8a4284d8f9" title="Checks whether two states are equal.">ompl::base::CompoundStateSpace::equalStates</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state1, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state2)<span class="keyword"> const</span>
<a name="l00610"></a>00610 <span class="keyword"></span>{
<a name="l00611"></a>00611 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate1 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state1);
<a name="l00612"></a>00612 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate2 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state2);
<a name="l00613"></a>00613 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00614"></a>00614 <span class="keywordflow">if</span> (!components_[i]->equalStates(cstate1-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], cstate2-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]))
<a name="l00615"></a>00615 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00616"></a>00616 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00617"></a>00617 }
<a name="l00618"></a>00618
<a name="l00619"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a734abdd06f902b42adf134f52359a3f9">00619</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a734abdd06f902b42adf134f52359a3f9" title="Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...">ompl::base::CompoundStateSpace::interpolate</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *from, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *to, <span class="keyword">const</span> <span class="keywordtype">double</span> t, <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state)<span class="keyword"> const</span>
<a name="l00620"></a>00620 <span class="keyword"></span>{
<a name="l00621"></a>00621 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cfrom = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(from);
<a name="l00622"></a>00622 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cto = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(to);
<a name="l00623"></a>00623 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00624"></a>00624 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00625"></a>00625 components_[i]->interpolate(cfrom-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], cto-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], t, cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00626"></a>00626 }
<a name="l00627"></a>00627
<a name="l00628"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#acb3b812d5dc49953993f1f08a2cdd6c8">00628</a> <a class="code" href="classompl_1_1base_1_1StateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::StateSampler.">ompl::base::StateSamplerPtr</a> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#acb3b812d5dc49953993f1f08a2cdd6c8" title="Allocate an instance of a uniform state sampler for this space.">ompl::base::CompoundStateSpace::allocStateSampler</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00629"></a>00629 <span class="keyword"></span>{
<a name="l00630"></a>00630 <span class="keywordtype">double</span> totalWeight = std::accumulate(weights_.begin(), weights_.end(), 0.0);
<a name="l00631"></a>00631 <span class="keywordflow">if</span> (totalWeight < std::numeric_limits<double>::epsilon())
<a name="l00632"></a>00632 totalWeight = 1.0;
<a name="l00633"></a>00633 <a class="code" href="classompl_1_1base_1_1CompoundStateSampler.html" title="Definition of a compound state sampler. This is useful to construct samplers for compound states...">CompoundStateSampler</a> *ss = <span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSampler.html" title="Definition of a compound state sampler. This is useful to construct samplers for compound states...">CompoundStateSampler</a>(<span class="keyword">this</span>);
<a name="l00634"></a>00634 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00635"></a>00635 ss-><a class="code" href="classompl_1_1base_1_1CompoundStateSampler.html#a1c3640b72b4eecbae07f1699e5690042" title="Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...">addSampler</a>(components_[i]->allocStateSampler(), weights_[i] / totalWeight);
<a name="l00636"></a>00636 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1StateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::StateSampler.">StateSamplerPtr</a>(ss);
<a name="l00637"></a>00637 }
<a name="l00638"></a>00638
<a name="l00639"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a2b70c7adffb92a2a23c50a2c0937010e">00639</a> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ompl::base::State</a>* <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a2b70c7adffb92a2a23c50a2c0937010e" title="Allocate a state that can store a point in the described space.">ompl::base::CompoundStateSpace::allocState</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00640"></a>00640 <span class="keyword"></span>{
<a name="l00641"></a>00641 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *state = <span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>();
<a name="l00642"></a>00642 allocStateComponents(state);
<a name="l00643"></a>00643 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a>*<span class="keyword">></span>(state);
<a name="l00644"></a>00644 }
<a name="l00645"></a>00645
<a name="l00646"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a1486c4680ec6e01c579fba2b3614d137">00646</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a1486c4680ec6e01c579fba2b3614d137" title="Allocate the state components. Called by allocState(). Usually called by derived state spaces...">ompl::base::CompoundStateSpace::allocStateComponents</a>(<a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *state)<span class="keyword"> const</span>
<a name="l00647"></a>00647 <span class="keyword"></span>{
<a name="l00648"></a>00648 state-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a> = <span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a>*[componentCount_];
<a name="l00649"></a>00649 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00650"></a>00650 state-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i] = components_[i]->allocState();
<a name="l00651"></a>00651 }
<a name="l00652"></a>00652
<a name="l00653"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a891f9acb430b0486043efd4be95776bd">00653</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a891f9acb430b0486043efd4be95776bd" title="Free the memory of the allocated state.">ompl::base::CompoundStateSpace::freeState</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state)<span class="keyword"> const</span>
<a name="l00654"></a>00654 <span class="keyword"></span>{
<a name="l00655"></a>00655 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00656"></a>00656 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00657"></a>00657 components_[i]->freeState(cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i]);
<a name="l00658"></a>00658 <span class="keyword">delete</span>[] cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>;
<a name="l00659"></a>00659 <span class="keyword">delete</span> cstate;
<a name="l00660"></a>00660 }
<a name="l00661"></a>00661
<a name="l00662"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a7aca5d3cfc2ae6bc5b2543a88ef5cb9c">00662</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a7aca5d3cfc2ae6bc5b2543a88ef5cb9c" title="Lock this state space. This means no further spaces can be added as components. This function can be ...">ompl::base::CompoundStateSpace::lock</a>(<span class="keywordtype">void</span>)
<a name="l00663"></a>00663 {
<a name="l00664"></a>00664 locked_ = <span class="keyword">true</span>;
<a name="l00665"></a>00665 }
<a name="l00666"></a>00666
<a name="l00667"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a1515cf17375f4985c3567b4bd00afadd">00667</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a1515cf17375f4985c3567b4bd00afadd" title="Return true if the state space is locked. A value of true means that no further spaces can be added a...">ompl::base::CompoundStateSpace::isLocked</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00668"></a>00668 <span class="keyword"></span>{
<a name="l00669"></a>00669 <span class="keywordflow">return</span> locked_;
<a name="l00670"></a>00670 }
<a name="l00671"></a>00671
<a name="l00672"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a88d40189fcde811ba620ebc96af33901">00672</a> <span class="keywordtype">double</span>* <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a88d40189fcde811ba620ebc96af33901" title="Many states contain a number of double values. This function provides a means to get the memory addre...">ompl::base::CompoundStateSpace::getValueAddressAtIndex</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state, <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index)<span class="keyword"> const</span>
<a name="l00673"></a>00673 <span class="keyword"></span>{
<a name="l00674"></a>00674 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00675"></a>00675 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> idx = 0;
<a name="l00676"></a>00676
<a name="l00677"></a>00677 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00678"></a>00678 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j <= index ; ++j)
<a name="l00679"></a>00679 {
<a name="l00680"></a>00680 <span class="keywordtype">double</span> *va = components_[i]->getValueAddressAtIndex(cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], j);
<a name="l00681"></a>00681 <span class="keywordflow">if</span> (va)
<a name="l00682"></a>00682 {
<a name="l00683"></a>00683 <span class="keywordflow">if</span> (idx == index)
<a name="l00684"></a>00684 <span class="keywordflow">return</span> va;
<a name="l00685"></a>00685 <span class="keywordflow">else</span>
<a name="l00686"></a>00686 idx++;
<a name="l00687"></a>00687 }
<a name="l00688"></a>00688 <span class="keywordflow">else</span>
<a name="l00689"></a>00689 <span class="keywordflow">break</span>;
<a name="l00690"></a>00690 }
<a name="l00691"></a>00691 <span class="keywordflow">return</span> NULL;
<a name="l00692"></a>00692 }
<a name="l00693"></a>00693
<a name="l00694"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aba02f1cf2c9b0f885fb7331ef2c4a8f6">00694</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aba02f1cf2c9b0f885fb7331ef2c4a8f6" title="Print a state to a stream.">ompl::base::CompoundStateSpace::printState</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state, std::ostream &out)<span class="keyword"> const</span>
<a name="l00695"></a>00695 <span class="keyword"></span>{
<a name="l00696"></a>00696 out << <span class="stringliteral">"Compound state ["</span> << std::endl;
<a name="l00697"></a>00697 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *cstate = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>*<span class="keyword">></span>(state);
<a name="l00698"></a>00698 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00699"></a>00699 components_[i]->printState(cstate-><a class="code" href="classompl_1_1base_1_1CompoundState.html#acbfb1f1ff3d5eb2315f47c13fc79a86e" title="The components that make up a compound state.">components</a>[i], out);
<a name="l00700"></a>00700 out << <span class="stringliteral">"]"</span> << std::endl;
<a name="l00701"></a>00701 }
<a name="l00702"></a>00702
<a name="l00703"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ac58df4b2dafa03e935e260c2f9ba5fc9">00703</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#ac58df4b2dafa03e935e260c2f9ba5fc9" title="Print the settings for this state space to a stream.">ompl::base::CompoundStateSpace::printSettings</a>(std::ostream &out)<span class="keyword"> const</span>
<a name="l00704"></a>00704 <span class="keyword"></span>{
<a name="l00705"></a>00705 out << <span class="stringliteral">"Compound state space '"</span> << getName() << <span class="stringliteral">"' of dimension "</span> << getDimension() << (isLocked() ? <span class="stringliteral">" (locked)"</span> : <span class="stringliteral">""</span>) << <span class="stringliteral">" ["</span> << std::endl;
<a name="l00706"></a>00706 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00707"></a>00707 {
<a name="l00708"></a>00708 components_[i]->printSettings(out);
<a name="l00709"></a>00709 out << <span class="stringliteral">" of weight "</span> << weights_[i] << std::endl;
<a name="l00710"></a>00710 }
<a name="l00711"></a>00711 out << <span class="stringliteral">"]"</span> << std::endl;
<a name="l00712"></a>00712 printProjections(out);
<a name="l00713"></a>00713 }
<a name="l00714"></a>00714
<a name="l00715"></a><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#af1ae2ce741021ce9d6eb40bc288fb702">00715</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#af1ae2ce741021ce9d6eb40bc288fb702" title="Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them. It is safe to call this function multiple times.">ompl::base::CompoundStateSpace::setup</a>(<span class="keywordtype">void</span>)
<a name="l00716"></a>00716 {
<a name="l00717"></a>00717 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < componentCount_ ; ++i)
<a name="l00718"></a>00718 components_[i]->setup();
<a name="l00719"></a>00719
<a name="l00720"></a>00720 <a class="code" href="classompl_1_1base_1_1StateSpace.html#a62881b1915544da2e2843008da5f1109" title="Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them. It is safe to call this function multiple times.">StateSpace::setup</a>();
<a name="l00721"></a>00721 }
<a name="l00722"></a>00722
<a name="l00723"></a>00723 <span class="keyword">namespace </span>ompl
<a name="l00724"></a>00724 {
<a name="l00725"></a>00725 <span class="keyword">namespace </span>base
<a name="l00726"></a>00726 {
<a name="l00727"></a>00727
<a name="l00729"></a>00729
<a name="l00730"></a>00730 <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">int</span> NO_DATA_COPIED = 0;
<a name="l00731"></a>00731 <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">int</span> SOME_DATA_COPIED = 1;
<a name="l00732"></a>00732 <span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">int</span> ALL_DATA_COPIED = 2;
<a name="l00733"></a>00733
<a name="l00735"></a>00735
<a name="l00736"></a>00736 <span class="comment">// return one of the constants defined above</span>
<a name="l00737"></a><a class="code" href="namespaceompl_1_1base.html#a28f47f0634ddee5c73674b5e0fb2d9aa">00737</a> <span class="keywordtype">int</span> <a class="code" href="namespaceompl_1_1base.html#a28f47f0634ddee5c73674b5e0fb2d9aa" title="Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...">copyStateData</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &destS, <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *dest, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &sourceS, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *source)
<a name="l00738"></a>00738 {
<a name="l00739"></a>00739 <span class="comment">// if states correspond to the same space, simply do copy</span>
<a name="l00740"></a>00740 <span class="keywordflow">if</span> (destS->getName() == sourceS->getName())
<a name="l00741"></a>00741 {
<a name="l00742"></a>00742 <span class="keywordflow">if</span> (dest != source)
<a name="l00743"></a>00743 destS->copyState(dest, source);
<a name="l00744"></a>00744 <span class="keywordflow">return</span> ALL_DATA_COPIED;
<a name="l00745"></a>00745 }
<a name="l00746"></a>00746
<a name="l00747"></a>00747 <span class="keywordtype">int</span> result = NO_DATA_COPIED;
<a name="l00748"></a>00748
<a name="l00749"></a>00749 <span class="comment">// if "to" state is compound</span>
<a name="l00750"></a>00750 <span class="keywordflow">if</span> (destS->isCompound())
<a name="l00751"></a>00751 {
<a name="l00752"></a>00752 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *compoundDestS = destS->as<<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>();
<a name="l00753"></a>00753 <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *compoundDest = dest-><a class="code" href="classompl_1_1base_1_1State.html#ae4ed5c095a6b2d4a5c946b91bc65e9cb" title="Cast this instance to a desired type.">as</a><<a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>>();
<a name="l00754"></a>00754
<a name="l00755"></a>00755 <span class="comment">// if there is a subspace in "to" that corresponds to "from", set the data and return</span>
<a name="l00756"></a>00756 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < compoundDestS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502" title="Get the number of state spaces that make up the compound state space.">getSubSpaceCount</a>() ; ++i)
<a name="l00757"></a>00757 <span class="keywordflow">if</span> (compoundDestS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">getSubSpace</a>(i)->getName() == sourceS->getName())
<a name="l00758"></a>00758 {
<a name="l00759"></a>00759 <span class="keywordflow">if</span> (compoundDest->components[i] != source)
<a name="l00760"></a>00760 compoundDestS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">getSubSpace</a>(i)->copyState(compoundDest->components[i], source);
<a name="l00761"></a>00761 <span class="keywordflow">return</span> ALL_DATA_COPIED;
<a name="l00762"></a>00762 }
<a name="l00763"></a>00763
<a name="l00764"></a>00764 <span class="comment">// it could be there are further levels of compound spaces where the data can be set</span>
<a name="l00765"></a>00765 <span class="comment">// so we call this function recursively</span>
<a name="l00766"></a>00766 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < compoundDestS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502" title="Get the number of state spaces that make up the compound state space.">getSubSpaceCount</a>() ; ++i)
<a name="l00767"></a>00767 {
<a name="l00768"></a>00768 <span class="keywordtype">int</span> res = <a class="code" href="namespaceompl_1_1base.html#a28f47f0634ddee5c73674b5e0fb2d9aa" title="Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...">copyStateData</a>(compoundDestS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">getSubSpace</a>(i), compoundDest->components[i], sourceS, source);
<a name="l00769"></a>00769
<a name="l00770"></a>00770 <span class="keywordflow">if</span> (res != NO_DATA_COPIED)
<a name="l00771"></a>00771 result = SOME_DATA_COPIED;
<a name="l00772"></a>00772
<a name="l00773"></a>00773 <span class="comment">// if all data was copied, we stop</span>
<a name="l00774"></a>00774 <span class="keywordflow">if</span> (res == ALL_DATA_COPIED)
<a name="l00775"></a>00775 <span class="keywordflow">return</span> ALL_DATA_COPIED;
<a name="l00776"></a>00776 }
<a name="l00777"></a>00777 }
<a name="l00778"></a>00778
<a name="l00779"></a>00779 <span class="comment">// if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"</span>
<a name="l00780"></a>00780 <span class="comment">// it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed</span>
<a name="l00781"></a>00781 <span class="keywordflow">if</span> (sourceS->isCompound())
<a name="l00782"></a>00782 {
<a name="l00783"></a>00783 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *compoundSourceS = sourceS->as<<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>>();
<a name="l00784"></a>00784 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a> *compoundSource = source-><a class="code" href="classompl_1_1base_1_1State.html#ae4ed5c095a6b2d4a5c946b91bc65e9cb" title="Cast this instance to a desired type.">as</a><<a class="code" href="classompl_1_1base_1_1CompoundState.html" title="Definition of a compound state.">CompoundState</a>>();
<a name="l00785"></a>00785
<a name="l00786"></a>00786 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> copiedComponents = 0;
<a name="l00787"></a>00787
<a name="l00788"></a>00788 <span class="comment">// if there is a subspace in "to" that corresponds to "from", set the data and return</span>
<a name="l00789"></a>00789 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < compoundSourceS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502" title="Get the number of state spaces that make up the compound state space.">getSubSpaceCount</a>() ; ++i)
<a name="l00790"></a>00790 {
<a name="l00791"></a>00791 <span class="keywordtype">int</span> res = <a class="code" href="namespaceompl_1_1base.html#a28f47f0634ddee5c73674b5e0fb2d9aa" title="Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...">copyStateData</a>(destS, dest, compoundSourceS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#a6b39c8d168cc33fde93263e20544f258" title="Get a specific subspace from the compound state space.">getSubSpace</a>(i), compoundSource->components[i]);
<a name="l00792"></a>00792 <span class="keywordflow">if</span> (res == ALL_DATA_COPIED)
<a name="l00793"></a>00793 copiedComponents++;
<a name="l00794"></a>00794 <span class="keywordflow">if</span> (res)
<a name="l00795"></a>00795 result = SOME_DATA_COPIED;
<a name="l00796"></a>00796 }
<a name="l00797"></a>00797
<a name="l00798"></a>00798 <span class="comment">// if each individual component got copied, then the entire data in "from" got copied</span>
<a name="l00799"></a>00799 <span class="keywordflow">if</span> (copiedComponents == compoundSourceS-><a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html#aa721e2f6acea876a8f099d2548ddd502" title="Get the number of state spaces that make up the compound state space.">getSubSpaceCount</a>())
<a name="l00800"></a>00800 result = ALL_DATA_COPIED;
<a name="l00801"></a>00801 }
<a name="l00802"></a>00802
<a name="l00803"></a>00803 <span class="keywordflow">return</span> result;
<a name="l00804"></a>00804 }
<a name="l00805"></a>00805
<a name="l00807"></a>00807 <span class="keyword">inline</span> <span class="keywordtype">bool</span> StateSpaceHasContent(<span class="keyword">const</span> StateSpacePtr &m)
<a name="l00808"></a>00808 {
<a name="l00809"></a>00809 <span class="keywordflow">if</span> (!m)
<a name="l00810"></a>00810 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00811"></a>00811 <span class="keywordflow">if</span> (m->getDimension() == 0 && m->getType() == <a class="code" href="namespaceompl_1_1base.html#a056b022e14fe04a75f81789947353920a4247bd6859b13c6482caed7f9dd9996d" title="Unset type; this is the default type.">STATE_SPACE_UNKNOWN</a> && m->isCompound())
<a name="l00812"></a>00812 {
<a name="l00813"></a>00813 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> nc = m->as<CompoundStateSpace>()->getSubSpaceCount();
<a name="l00814"></a>00814 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < nc ; ++i)
<a name="l00815"></a>00815 <span class="keywordflow">if</span> (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubSpace(i)))
<a name="l00816"></a>00816 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00817"></a>00817 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00818"></a>00818 }
<a name="l00819"></a>00819 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00820"></a>00820 }
<a name="l00822"></a>00822
<a name="l00823"></a><a class="code" href="group__stateAndSpaceOperators.html#ga1a2a068c3ba88614e32425cf4c493261">00823</a> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> <a class="code" href="group__stateAndSpaceOperators.html#ga1a2a068c3ba88614e32425cf4c493261" title="Construct a compound state space from two existing state spaces. The components of this compound spac...">operator+</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &a, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &b)
<a name="l00824"></a>00824 {
<a name="l00825"></a>00825 <span class="keywordflow">if</span> (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
<a name="l00826"></a>00826 <span class="keywordflow">return</span> b;
<a name="l00827"></a>00827
<a name="l00828"></a>00828 <span class="keywordflow">if</span> (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
<a name="l00829"></a>00829 <span class="keywordflow">return</span> a;
<a name="l00830"></a>00830
<a name="l00831"></a>00831 std::vector<StateSpacePtr> components;
<a name="l00832"></a>00832 std::vector<double> weights;
<a name="l00833"></a>00833
<a name="l00834"></a>00834 <span class="keywordtype">bool</span> change = <span class="keyword">false</span>;
<a name="l00835"></a>00835 <span class="keywordflow">if</span> (a)
<a name="l00836"></a>00836 {
<a name="l00837"></a>00837 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l00838"></a>00838 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
<a name="l00839"></a>00839 <span class="keywordflow">if</span> (!csm_a->isLocked())
<a name="l00840"></a>00840 {
<a name="l00841"></a>00841 used = <span class="keyword">true</span>;
<a name="l00842"></a>00842 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
<a name="l00843"></a>00843 {
<a name="l00844"></a>00844 components.push_back(csm_a->getSubSpace(i));
<a name="l00845"></a>00845 weights.push_back(csm_a->getSubSpaceWeight(i));
<a name="l00846"></a>00846 }
<a name="l00847"></a>00847 }
<a name="l00848"></a>00848
<a name="l00849"></a>00849 <span class="keywordflow">if</span> (!used)
<a name="l00850"></a>00850 {
<a name="l00851"></a>00851 components.push_back(a);
<a name="l00852"></a>00852 weights.push_back(1.0);
<a name="l00853"></a>00853 }
<a name="l00854"></a>00854 }
<a name="l00855"></a>00855 <span class="keywordflow">if</span> (b)
<a name="l00856"></a>00856 {
<a name="l00857"></a>00857 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l00858"></a>00858 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> size = components.size();
<a name="l00859"></a>00859
<a name="l00860"></a>00860 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
<a name="l00861"></a>00861 <span class="keywordflow">if</span> (!csm_b->isLocked())
<a name="l00862"></a>00862 {
<a name="l00863"></a>00863 used = <span class="keyword">true</span>;
<a name="l00864"></a>00864 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
<a name="l00865"></a>00865 {
<a name="l00866"></a>00866 <span class="keywordtype">bool</span> ok = <span class="keyword">true</span>;
<a name="l00867"></a>00867 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j < size ; ++j)
<a name="l00868"></a>00868 <span class="keywordflow">if</span> (components[j]->getName() == csm_b->getSubSpace(i)->getName())
<a name="l00869"></a>00869 {
<a name="l00870"></a>00870 ok = <span class="keyword">false</span>;
<a name="l00871"></a>00871 <span class="keywordflow">break</span>;
<a name="l00872"></a>00872 }
<a name="l00873"></a>00873 <span class="keywordflow">if</span> (ok)
<a name="l00874"></a>00874 {
<a name="l00875"></a>00875 components.push_back(csm_b->getSubSpace(i));
<a name="l00876"></a>00876 weights.push_back(csm_b->getSubSpaceWeight(i));
<a name="l00877"></a>00877 change = <span class="keyword">true</span>;
<a name="l00878"></a>00878 }
<a name="l00879"></a>00879 }
<a name="l00880"></a>00880 <span class="keywordflow">if</span> (components.size() == csm_b->getSubSpaceCount())
<a name="l00881"></a>00881 <span class="keywordflow">return</span> b;
<a name="l00882"></a>00882 }
<a name="l00883"></a>00883
<a name="l00884"></a>00884 <span class="keywordflow">if</span> (!used)
<a name="l00885"></a>00885 {
<a name="l00886"></a>00886 <span class="keywordtype">bool</span> ok = <span class="keyword">true</span>;
<a name="l00887"></a>00887 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j < size ; ++j)
<a name="l00888"></a>00888 <span class="keywordflow">if</span> (components[j]->getName() == b->getName())
<a name="l00889"></a>00889 {
<a name="l00890"></a>00890 ok = <span class="keyword">false</span>;
<a name="l00891"></a>00891 <span class="keywordflow">break</span>;
<a name="l00892"></a>00892 }
<a name="l00893"></a>00893 <span class="keywordflow">if</span> (ok)
<a name="l00894"></a>00894 {
<a name="l00895"></a>00895 components.push_back(b);
<a name="l00896"></a>00896 weights.push_back(1.0);
<a name="l00897"></a>00897 change = <span class="keyword">true</span>;
<a name="l00898"></a>00898 }
<a name="l00899"></a>00899 }
<a name="l00900"></a>00900 }
<a name="l00901"></a>00901
<a name="l00902"></a>00902 <span class="keywordflow">if</span> (!change && a)
<a name="l00903"></a>00903 <span class="keywordflow">return</span> a;
<a name="l00904"></a>00904
<a name="l00905"></a>00905 <span class="keywordflow">if</span> (components.size() == 1)
<a name="l00906"></a>00906 <span class="keywordflow">return</span> components[0];
<a name="l00907"></a>00907
<a name="l00908"></a>00908 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>(components, weights));
<a name="l00909"></a>00909 }
<a name="l00910"></a>00910
<a name="l00911"></a><a class="code" href="group__stateAndSpaceOperators.html#ga849c861f1e209144fbab68a95c479eb8">00911</a> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> <a class="code" href="group__stateAndSpaceOperators.html#ga849c861f1e209144fbab68a95c479eb8" title="Construct a compound state space that contains subspaces only from a. If a is compound, b (or the components from b, if b is compound) are removed and the remaining components are returned as a compound state space. If the compound space would end up containing solely one component, that component is returned instead.">operator-</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &a, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &b)
<a name="l00912"></a>00912 {
<a name="l00913"></a>00913 std::vector<StateSpacePtr> components_a;
<a name="l00914"></a>00914 std::vector<double> weights_a;
<a name="l00915"></a>00915 std::vector<StateSpacePtr> components_b;
<a name="l00916"></a>00916
<a name="l00917"></a>00917 <span class="keywordflow">if</span> (a)
<a name="l00918"></a>00918 {
<a name="l00919"></a>00919 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l00920"></a>00920 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
<a name="l00921"></a>00921 <span class="keywordflow">if</span> (!csm_a->isLocked())
<a name="l00922"></a>00922 {
<a name="l00923"></a>00923 used = <span class="keyword">true</span>;
<a name="l00924"></a>00924 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
<a name="l00925"></a>00925 {
<a name="l00926"></a>00926 components_a.push_back(csm_a->getSubSpace(i));
<a name="l00927"></a>00927 weights_a.push_back(csm_a->getSubSpaceWeight(i));
<a name="l00928"></a>00928 }
<a name="l00929"></a>00929 }
<a name="l00930"></a>00930
<a name="l00931"></a>00931 <span class="keywordflow">if</span> (!used)
<a name="l00932"></a>00932 {
<a name="l00933"></a>00933 components_a.push_back(a);
<a name="l00934"></a>00934 weights_a.push_back(1.0);
<a name="l00935"></a>00935 }
<a name="l00936"></a>00936 }
<a name="l00937"></a>00937
<a name="l00938"></a>00938 <span class="keywordflow">if</span> (b)
<a name="l00939"></a>00939 {
<a name="l00940"></a>00940 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l00941"></a>00941 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
<a name="l00942"></a>00942 <span class="keywordflow">if</span> (!csm_b->isLocked())
<a name="l00943"></a>00943 {
<a name="l00944"></a>00944 used = <span class="keyword">true</span>;
<a name="l00945"></a>00945 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
<a name="l00946"></a>00946 components_b.push_back(csm_b->getSubSpace(i));
<a name="l00947"></a>00947 }
<a name="l00948"></a>00948 <span class="keywordflow">if</span> (!used)
<a name="l00949"></a>00949 components_b.push_back(b);
<a name="l00950"></a>00950 }
<a name="l00951"></a>00951
<a name="l00952"></a>00952 <span class="keywordtype">bool</span> change = <span class="keyword">false</span>;
<a name="l00953"></a>00953 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < components_b.size() ; ++i)
<a name="l00954"></a>00954 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j < components_a.size() ; ++j)
<a name="l00955"></a>00955 <span class="keywordflow">if</span> (components_a[j]->getName() == components_b[i]->getName())
<a name="l00956"></a>00956 {
<a name="l00957"></a>00957 components_a.erase(components_a.begin() + j);
<a name="l00958"></a>00958 weights_a.erase(weights_a.begin() + j);
<a name="l00959"></a>00959 change = <span class="keyword">true</span>;
<a name="l00960"></a>00960 <span class="keywordflow">break</span>;
<a name="l00961"></a>00961 }
<a name="l00962"></a>00962
<a name="l00963"></a>00963 <span class="keywordflow">if</span> (!change && a)
<a name="l00964"></a>00964 <span class="keywordflow">return</span> a;
<a name="l00965"></a>00965
<a name="l00966"></a>00966 <span class="keywordflow">if</span> (components_a.size() == 1)
<a name="l00967"></a>00967 <span class="keywordflow">return</span> components_a[0];
<a name="l00968"></a>00968
<a name="l00969"></a>00969 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>(components_a, weights_a));
<a name="l00970"></a>00970 }
<a name="l00971"></a>00971
<a name="l00972"></a><a class="code" href="group__stateAndSpaceOperators.html#gae1b5c9d6ea57481723cf8a9683a4eabc">00972</a> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> <a class="code" href="group__stateAndSpaceOperators.html#ga849c861f1e209144fbab68a95c479eb8" title="Construct a compound state space that contains subspaces only from a. If a is compound, b (or the components from b, if b is compound) are removed and the remaining components are returned as a compound state space. If the compound space would end up containing solely one component, that component is returned instead.">operator-</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &a, <span class="keyword">const</span> std::string &name)
<a name="l00973"></a>00973 {
<a name="l00974"></a>00974 std::vector<StateSpacePtr> components;
<a name="l00975"></a>00975 std::vector<double> weights;
<a name="l00976"></a>00976
<a name="l00977"></a>00977 <span class="keywordtype">bool</span> change = <span class="keyword">false</span>;
<a name="l00978"></a>00978 <span class="keywordflow">if</span> (a)
<a name="l00979"></a>00979 {
<a name="l00980"></a>00980 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l00981"></a>00981 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
<a name="l00982"></a>00982 <span class="keywordflow">if</span> (!csm_a->isLocked())
<a name="l00983"></a>00983 {
<a name="l00984"></a>00984 used = <span class="keyword">true</span>;
<a name="l00985"></a>00985 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
<a name="l00986"></a>00986 {
<a name="l00987"></a>00987 <span class="keywordflow">if</span> (csm_a->getSubSpace(i)->getName() == name)
<a name="l00988"></a>00988 {
<a name="l00989"></a>00989 change = <span class="keyword">true</span>;
<a name="l00990"></a>00990 <span class="keywordflow">continue</span>;
<a name="l00991"></a>00991 }
<a name="l00992"></a>00992 components.push_back(csm_a->getSubSpace(i));
<a name="l00993"></a>00993 weights.push_back(csm_a->getSubSpaceWeight(i));
<a name="l00994"></a>00994 }
<a name="l00995"></a>00995 }
<a name="l00996"></a>00996
<a name="l00997"></a>00997 <span class="keywordflow">if</span> (!used)
<a name="l00998"></a>00998 {
<a name="l00999"></a>00999 <span class="keywordflow">if</span> (a->getName() != name)
<a name="l01000"></a>01000 {
<a name="l01001"></a>01001 components.push_back(a);
<a name="l01002"></a>01002 weights.push_back(1.0);
<a name="l01003"></a>01003 }
<a name="l01004"></a>01004 <span class="keywordflow">else</span>
<a name="l01005"></a>01005 change = <span class="keyword">true</span>;
<a name="l01006"></a>01006 }
<a name="l01007"></a>01007 }
<a name="l01008"></a>01008
<a name="l01009"></a>01009 <span class="keywordflow">if</span> (!change && a)
<a name="l01010"></a>01010 <span class="keywordflow">return</span> a;
<a name="l01011"></a>01011
<a name="l01012"></a>01012 <span class="keywordflow">if</span> (components.size() == 1)
<a name="l01013"></a>01013 <span class="keywordflow">return</span> components[0];
<a name="l01014"></a>01014
<a name="l01015"></a>01015 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>(components, weights));
<a name="l01016"></a>01016 }
<a name="l01017"></a>01017
<a name="l01018"></a><a class="code" href="group__stateAndSpaceOperators.html#ga16229a85742bb392d45ea9fdc49d7434">01018</a> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> <a class="code" href="group__stateAndSpaceOperators.html#ga16229a85742bb392d45ea9fdc49d7434" title="Construct a compound state space that contains subspaces that are in both a and b.">operator*</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &a, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a> &b)
<a name="l01019"></a>01019 {
<a name="l01020"></a>01020 std::vector<StateSpacePtr> components_a;
<a name="l01021"></a>01021 std::vector<double> weights_a;
<a name="l01022"></a>01022 std::vector<StateSpacePtr> components_b;
<a name="l01023"></a>01023 std::vector<double> weights_b;
<a name="l01024"></a>01024
<a name="l01025"></a>01025 <span class="keywordflow">if</span> (a)
<a name="l01026"></a>01026 {
<a name="l01027"></a>01027 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l01028"></a>01028 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
<a name="l01029"></a>01029 <span class="keywordflow">if</span> (!csm_a->isLocked())
<a name="l01030"></a>01030 {
<a name="l01031"></a>01031 used = <span class="keyword">true</span>;
<a name="l01032"></a>01032 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
<a name="l01033"></a>01033 {
<a name="l01034"></a>01034 components_a.push_back(csm_a->getSubSpace(i));
<a name="l01035"></a>01035 weights_a.push_back(csm_a->getSubSpaceWeight(i));
<a name="l01036"></a>01036 }
<a name="l01037"></a>01037 }
<a name="l01038"></a>01038
<a name="l01039"></a>01039 <span class="keywordflow">if</span> (!used)
<a name="l01040"></a>01040 {
<a name="l01041"></a>01041 components_a.push_back(a);
<a name="l01042"></a>01042 weights_a.push_back(1.0);
<a name="l01043"></a>01043 }
<a name="l01044"></a>01044 }
<a name="l01045"></a>01045
<a name="l01046"></a>01046 <span class="keywordflow">if</span> (b)
<a name="l01047"></a>01047 {
<a name="l01048"></a>01048 <span class="keywordtype">bool</span> used = <span class="keyword">false</span>;
<a name="l01049"></a>01049 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a> *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
<a name="l01050"></a>01050 <span class="keywordflow">if</span> (!csm_b->isLocked())
<a name="l01051"></a>01051 {
<a name="l01052"></a>01052 used = <span class="keyword">true</span>;
<a name="l01053"></a>01053 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
<a name="l01054"></a>01054 {
<a name="l01055"></a>01055 components_b.push_back(csm_b->getSubSpace(i));
<a name="l01056"></a>01056 weights_b.push_back(csm_b->getSubSpaceWeight(i));
<a name="l01057"></a>01057 }
<a name="l01058"></a>01058 }
<a name="l01059"></a>01059
<a name="l01060"></a>01060 <span class="keywordflow">if</span> (!used)
<a name="l01061"></a>01061 {
<a name="l01062"></a>01062 components_b.push_back(b);
<a name="l01063"></a>01063 weights_b.push_back(1.0);
<a name="l01064"></a>01064 }
<a name="l01065"></a>01065 }
<a name="l01066"></a>01066
<a name="l01067"></a>01067 std::vector<StateSpacePtr> components;
<a name="l01068"></a>01068 std::vector<double> weights;
<a name="l01069"></a>01069
<a name="l01070"></a>01070 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < components_b.size() ; ++i)
<a name="l01071"></a>01071 {
<a name="l01072"></a>01072 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j < components_a.size() ; ++j)
<a name="l01073"></a>01073 <span class="keywordflow">if</span> (components_a[j]->getName() == components_b[i]->getName())
<a name="l01074"></a>01074 {
<a name="l01075"></a>01075 components.push_back(components_b[i]);
<a name="l01076"></a>01076 weights.push_back(std::max(weights_a[j], weights_b[i]));
<a name="l01077"></a>01077 <span class="keywordflow">break</span>;
<a name="l01078"></a>01078 }
<a name="l01079"></a>01079 }
<a name="l01080"></a>01080
<a name="l01081"></a>01081 <span class="keywordflow">if</span> (a && components.size() == components_a.size())
<a name="l01082"></a>01082 <span class="keywordflow">return</span> a;
<a name="l01083"></a>01083
<a name="l01084"></a>01084 <span class="keywordflow">if</span> (b && components.size() == components_b.size())
<a name="l01085"></a>01085 <span class="keywordflow">return</span> b;
<a name="l01086"></a>01086
<a name="l01087"></a>01087 <span class="keywordflow">if</span> (components.size() == 1)
<a name="l01088"></a>01088 <span class="keywordflow">return</span> components[0];
<a name="l01089"></a>01089
<a name="l01090"></a>01090 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">StateSpacePtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1CompoundStateSpace.html" title="A space to allow the composition of state spaces.">CompoundStateSpace</a>(components, weights));
<a name="l01091"></a>01091 }
<a name="l01092"></a>01092
<a name="l01093"></a>01093 }
<a name="l01094"></a>01094 }
</pre></div></div>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark"> </span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark"> </span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark"> </span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark"> </span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark"> </span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark"> </span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark"> </span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark"> </span>Enumerator</a></div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
</div>
<div class="footer span-22 push-2 last">
<a href="http://www.kavrakilab.org">Physical and Biological Computing Group</a> •
<a href="http://www.cs.rice.edu">Department of Computer Science</a> •
<a href="http://www.rice.edu">Rice University</a><br>
<div class="gray">Generated on Sun Oct 9 2011 23:04:39 by <a href="http://www.doxygen.org/index.html">doxygen</a> 1.7.4</div>
</div>
</div>
</body>
</html>
distrib
>
Fedora
>
14
>
x86_64
>
media
>
updates
>
by-pkgid
>
e7618febbb9cbed15bb79e326774c050
>
files
>
271
