<!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/spaces/src/RealVectorStateSpace.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_7f116b207a3e42501f874f0585585053.html">spaces</a> </li>
<li class="navelem"><a class="el" href="dir_8bacea68f46bad3796bf3f4dc8ff506f.html">src</a> </li>
</ul>
</div>
</div>
<div class="header">
<div class="headertitle">
<div class="title">RealVectorStateSpace.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="comment">/* Author: Ioan Sucan */</span>
<a name="l00036"></a>00036
<a name="l00037"></a>00037 <span class="preprocessor">#include "ompl/base/spaces/RealVectorStateSpace.h"</span>
<a name="l00038"></a>00038 <span class="preprocessor">#include "ompl/base/spaces/RealVectorStateProjections.h"</span>
<a name="l00039"></a>00039 <span class="preprocessor">#include "ompl/util/Exception.h"</span>
<a name="l00040"></a>00040 <span class="preprocessor">#include <boost/lexical_cast.hpp></span>
<a name="l00041"></a>00041 <span class="preprocessor">#include <algorithm></span>
<a name="l00042"></a>00042 <span class="preprocessor">#include <cstring></span>
<a name="l00043"></a>00043 <span class="preprocessor">#include <limits></span>
<a name="l00044"></a>00044 <span class="preprocessor">#include <cmath></span>
<a name="l00045"></a>00045
<a name="l00046"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a7c7a1af5acf4d03c33dd5ec6a4ab50b4">00046</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a7c7a1af5acf4d03c33dd5ec6a4ab50b4" title="Sample a state.">ompl::base::RealVectorStateSampler::sampleUniform</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *state)
<a name="l00047"></a>00047 {
<a name="l00048"></a>00048 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> dim = <a class="code" href="classompl_1_1base_1_1StateSampler.html#ade7594512889b577995b3479c71f3b6d" title="The state space this sampler samples.">space_</a>-><a class="code" href="classompl_1_1base_1_1StateSpace.html#a4d929613e0cf9dcced2fbe2e88cb72ab" title="Get the dimension of the space (not the dimension of the surrounding ambient space)">getDimension</a>();
<a name="l00049"></a>00049 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">RealVectorBounds</a> &bounds = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html" title="A state space representing Rn. The distance function is the L2 norm.">RealVectorStateSpace</a>*<span class="keyword">></span>(<a class="code" href="classompl_1_1base_1_1StateSampler.html#ade7594512889b577995b3479c71f3b6d" title="The state space this sampler samples.">space_</a>)->getBounds();
<a name="l00050"></a>00050
<a name="l00051"></a>00051 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a>*<span class="keyword">></span>(state);
<a name="l00052"></a>00052 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dim ; ++i)
<a name="l00053"></a>00053 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] = <a class="code" href="classompl_1_1base_1_1StateSampler.html#ad8b2f106c42eaa4788bb6914cd5371fb" title="An instance of a random number generator.">rng_</a>.<a class="code" href="classompl_1_1RNG.html#a691ddf8d21630c69d4ba3f8b1f8c449f" title="Generate a random real within given bounds: [lower_bound, upper_bound)">uniformReal</a>(bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>[i], bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a06827561804a768d6ee86a20a664153c" title="Upper bound.">high</a>[i]);
<a name="l00054"></a>00054 }
<a name="l00055"></a>00055
<a name="l00056"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a04d589c5cb197f5cfa0626557491fac8">00056</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a04d589c5cb197f5cfa0626557491fac8" title="Sample a state near another, within specified distance.">ompl::base::RealVectorStateSampler::sampleUniformNear</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 class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *near, <span class="keyword">const</span> <span class="keywordtype">double</span> distance)
<a name="l00057"></a>00057 {
<a name="l00058"></a>00058 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> dim = space_->getDimension();
<a name="l00059"></a>00059 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">RealVectorBounds</a> &bounds = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html" title="A state space representing Rn. The distance function is the L2 norm.">RealVectorStateSpace</a>*<span class="keyword">></span>(space_)->getBounds();
<a name="l00060"></a>00060
<a name="l00061"></a>00061 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a>*<span class="keyword">></span>(state);
<a name="l00062"></a>00062 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a> *rnear = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a>*<span class="keyword">></span>(near);
<a name="l00063"></a>00063 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dim ; ++i)
<a name="l00064"></a>00064 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] =
<a name="l00065"></a>00065 rng_.uniformReal(std::max(bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>[i], rnear-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] - distance),
<a name="l00066"></a>00066 std::min(bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a06827561804a768d6ee86a20a664153c" title="Upper bound.">high</a>[i], rnear-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] + distance));
<a name="l00067"></a>00067 }
<a name="l00068"></a>00068
<a name="l00069"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a403799674d976ff4a1ae0a817b6c1b73">00069</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html#a403799674d976ff4a1ae0a817b6c1b73" title="Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev)...">ompl::base::RealVectorStateSampler::sampleGaussian</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 class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">State</a> *mean, <span class="keyword">const</span> <span class="keywordtype">double</span> stdDev)
<a name="l00070"></a>00070 {
<a name="l00071"></a>00071 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> dim = space_->getDimension();
<a name="l00072"></a>00072 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">RealVectorBounds</a> &bounds = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html" title="A state space representing Rn. The distance function is the L2 norm.">RealVectorStateSpace</a>*<span class="keyword">></span>(space_)->getBounds();
<a name="l00073"></a>00073
<a name="l00074"></a>00074 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a>*<span class="keyword">></span>(state);
<a name="l00075"></a>00075 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a> *rmean = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">RealVectorStateSpace::StateType</a>*<span class="keyword">></span>(mean);
<a name="l00076"></a>00076 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dim ; ++i)
<a name="l00077"></a>00077 {
<a name="l00078"></a>00078 <span class="keywordtype">double</span> v = rng_.gaussian(rmean-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i], stdDev);
<a name="l00079"></a>00079 <span class="keywordflow">if</span> (v < bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>[i])
<a name="l00080"></a>00080 v = bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>[i];
<a name="l00081"></a>00081 <span class="keywordflow">else</span>
<a name="l00082"></a>00082 <span class="keywordflow">if</span> (v > bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a06827561804a768d6ee86a20a664153c" title="Upper bound.">high</a>[i])
<a name="l00083"></a>00083 v = bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a06827561804a768d6ee86a20a664153c" title="Upper bound.">high</a>[i];
<a name="l00084"></a>00084 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] = v;
<a name="l00085"></a>00085 }
<a name="l00086"></a>00086 }
<a name="l00087"></a>00087
<a name="l00088"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae9578af74fb1deb62f2ac655e26bd89b">00088</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae9578af74fb1deb62f2ac655e26bd89b" title="Register the projections for this state space. Usually, this is at least the default projection...">ompl::base::RealVectorStateSpace::registerProjections</a>(<span class="keywordtype">void</span>)
<a name="l00089"></a>00089 {
<a name="l00090"></a>00090 <span class="comment">// compute a default random projection</span>
<a name="l00091"></a>00091 <span class="keywordflow">if</span> (dimension_ > 0)
<a name="l00092"></a>00092 {
<a name="l00093"></a>00093 <span class="keywordflow">if</span> (dimension_ > 2)
<a name="l00094"></a>00094 {
<a name="l00095"></a>00095 <span class="keywordtype">int</span> p = std::max(2, (<span class="keywordtype">int</span>)ceil(log((<span class="keywordtype">double</span>)dimension_)));
<a name="l00096"></a>00096 registerDefaultProjection(<a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1RealVectorRandomLinearProjectionEvaluator.html" title="Definition for a class computing a random linear projections.">RealVectorRandomLinearProjectionEvaluator</a>(<span class="keyword">this</span>, p)));
<a name="l00097"></a>00097 }
<a name="l00098"></a>00098 <span class="keywordflow">else</span>
<a name="l00099"></a>00099 registerDefaultProjection(<a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ProjectionEvaluatorPtr</a>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1RealVectorIdentityProjectionEvaluator.html" title="Define the identity projection.">RealVectorIdentityProjectionEvaluator</a>(<span class="keyword">this</span>)));
<a name="l00100"></a>00100 }
<a name="l00101"></a>00101 }
<a name="l00102"></a>00102
<a name="l00103"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a9b72767f319e1744a9860649dcb4950a">00103</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a9b72767f319e1744a9860649dcb4950a" 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::RealVectorStateSpace::setup</a>(<span class="keywordtype">void</span>)
<a name="l00104"></a>00104 {
<a name="l00105"></a>00105 bounds_.check();
<a name="l00106"></a>00106 <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="l00107"></a>00107 }
<a name="l00108"></a>00108
<a name="l00109"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a03b7187e42e78e2874db062dc88a1bd0">00109</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2893436790633b216ed31123b3cf1634" title="Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...">ompl::base::RealVectorStateSpace::addDimension</a>(<span class="keyword">const</span> std::string &name, <span class="keywordtype">double</span> minBound, <span class="keywordtype">double</span> maxBound)
<a name="l00110"></a>00110 {
<a name="l00111"></a>00111 addDimension(minBound, maxBound);
<a name="l00112"></a>00112 setDimensionName(dimension_ - 1, name);
<a name="l00113"></a>00113 }
<a name="l00114"></a>00114
<a name="l00115"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2893436790633b216ed31123b3cf1634">00115</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2893436790633b216ed31123b3cf1634" title="Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...">ompl::base::RealVectorStateSpace::addDimension</a>(<span class="keywordtype">double</span> minBound, <span class="keywordtype">double</span> maxBound)
<a name="l00116"></a>00116 {
<a name="l00117"></a>00117 dimension_++;
<a name="l00118"></a>00118 stateBytes_ = dimension_ * <span class="keyword">sizeof</span>(double);
<a name="l00119"></a>00119 bounds_.low.push_back(minBound);
<a name="l00120"></a>00120 bounds_.high.push_back(maxBound);
<a name="l00121"></a>00121 dimensionNames_.resize(dimension_, <span class="stringliteral">""</span>);
<a name="l00122"></a>00122 }
<a name="l00123"></a>00123
<a name="l00124"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ab53c0ef2a517e34f4c387785dd3f8956">00124</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ab53c0ef2a517e34f4c387785dd3f8956" title="Set the bounds of this state space. This defines the range of the space in which sampling is performe...">ompl::base::RealVectorStateSpace::setBounds</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">RealVectorBounds</a> &bounds)
<a name="l00125"></a>00125 {
<a name="l00126"></a>00126 bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a233cea07153725208456d27459fc8f92" title="Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...">check</a>();
<a name="l00127"></a>00127 <span class="keywordflow">if</span> (bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>.size() != dimension_)
<a name="l00128"></a>00128 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Bounds do not match dimension of state space: expected dimension "</span> +
<a name="l00129"></a>00129 boost::lexical_cast<std::string>(dimension_) + <span class="stringliteral">" but got dimension "</span> +
<a name="l00130"></a>00130 boost::lexical_cast<std::string>(bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a36ebebb6a9e116093581afbd07425baa" title="Lower bound.">low</a>.size()));
<a name="l00131"></a>00131 bounds_ = bounds;
<a name="l00132"></a>00132 }
<a name="l00133"></a>00133
<a name="l00134"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a592f658c4d3c546d96342146d18b5d13">00134</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ab53c0ef2a517e34f4c387785dd3f8956" title="Set the bounds of this state space. This defines the range of the space in which sampling is performe...">ompl::base::RealVectorStateSpace::setBounds</a>(<span class="keywordtype">double</span> low, <span class="keywordtype">double</span> high)
<a name="l00135"></a>00135 {
<a name="l00136"></a>00136 <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">RealVectorBounds</a> bounds(dimension_);
<a name="l00137"></a>00137 bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a6f63c8d793a056e2160e5d9d85c27a58" title="Set the lower bound in each dimension to a specific value.">setLow</a>(low);
<a name="l00138"></a>00138 bounds.<a class="code" href="classompl_1_1base_1_1RealVectorBounds.html#a756253c602ddc6bd299dc9c870fadfee" title="Set the upper bound in each dimension to a specific value.">setHigh</a>(high);
<a name="l00139"></a>00139 setBounds(bounds);
<a name="l00140"></a>00140 }
<a name="l00141"></a>00141
<a name="l00142"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a415bba6e6653b33fdd531871e0199cb8">00142</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a415bba6e6653b33fdd531871e0199cb8" title="Get the dimension of the space (not the dimension of the surrounding ambient space)">ompl::base::RealVectorStateSpace::getDimension</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00143"></a>00143 <span class="keyword"></span>{
<a name="l00144"></a>00144 <span class="keywordflow">return</span> dimension_;
<a name="l00145"></a>00145 }
<a name="l00146"></a>00146
<a name="l00147"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae5112c5d1bfbe7d69094b71ead4f84ca">00147</a> <span class="keyword">const</span> std::string& <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae5112c5d1bfbe7d69094b71ead4f84ca" title="Each dimension can optionally have a name associated to it. If it does, this function returns that na...">ompl::base::RealVectorStateSpace::getDimensionName</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index)<span class="keyword"> const</span>
<a name="l00148"></a>00148 <span class="keyword"></span>{
<a name="l00149"></a>00149 <span class="keywordflow">if</span> (index < dimensionNames_.size())
<a name="l00150"></a>00150 <span class="keywordflow">return</span> dimensionNames_[index];
<a name="l00151"></a>00151 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Index out of bounds"</span>);
<a name="l00152"></a>00152 }
<a name="l00153"></a>00153
<a name="l00154"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a3b43a37b02d3164f55581c4cda94fa00">00154</a> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a3b43a37b02d3164f55581c4cda94fa00" title="Get the index of a specific dimension, by name. Return -1 if name is not found.">ompl::base::RealVectorStateSpace::getDimensionIndex</a>(<span class="keyword">const</span> std::string &name)<span class="keyword"> const</span>
<a name="l00155"></a>00155 <span class="keyword"></span>{
<a name="l00156"></a>00156 std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
<a name="l00157"></a>00157 <span class="keywordflow">return</span> it != dimensionIndex_.end() ? it->second : -1;
<a name="l00158"></a>00158 }
<a name="l00159"></a>00159
<a name="l00160"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2d387a4a02fafea56315ddd449703847">00160</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2d387a4a02fafea56315ddd449703847" title="Set the name of a dimension.">ompl::base::RealVectorStateSpace::setDimensionName</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> index, <span class="keyword">const</span> std::string &name)
<a name="l00161"></a>00161 {
<a name="l00162"></a>00162 <span class="keywordflow">if</span> (index < dimensionNames_.size())
<a name="l00163"></a>00163 {
<a name="l00164"></a>00164 dimensionNames_[index] = name;
<a name="l00165"></a>00165 dimensionIndex_[name] = index;
<a name="l00166"></a>00166 }
<a name="l00167"></a>00167 <span class="keywordflow">else</span>
<a name="l00168"></a>00168 <span class="keywordflow">throw</span> <a class="code" href="classompl_1_1Exception.html" title="The exception type for ompl.">Exception</a>(<span class="stringliteral">"Cannot set dimension name. Index out of bounds"</span>);
<a name="l00169"></a>00169 }
<a name="l00170"></a>00170
<a name="l00171"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#af5a3758aabe1a915a64c99d7c93d692e">00171</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#af5a3758aabe1a915a64c99d7c93d692e" title="Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...">ompl::base::RealVectorStateSpace::getMaximumExtent</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00172"></a>00172 <span class="keyword"></span>{
<a name="l00173"></a>00173 <span class="keywordtype">double</span> e = 0.0;
<a name="l00174"></a>00174 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00175"></a>00175 {
<a name="l00176"></a>00176 <span class="keywordtype">double</span> d = bounds_.high[i] - bounds_.low[i];
<a name="l00177"></a>00177 e += d*d;
<a name="l00178"></a>00178 }
<a name="l00179"></a>00179 <span class="keywordflow">return</span> sqrt(e);
<a name="l00180"></a>00180 }
<a name="l00181"></a>00181
<a name="l00182"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae473cc4d0feaefb5e568578cd927d9b9">00182</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae473cc4d0feaefb5e568578cd927d9b9" title="Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...">ompl::base::RealVectorStateSpace::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="l00183"></a>00183 <span class="keyword"></span>{
<a name="l00184"></a>00184 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state);
<a name="l00185"></a>00185 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00186"></a>00186 {
<a name="l00187"></a>00187 <span class="keywordflow">if</span> (rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] > bounds_.high[i])
<a name="l00188"></a>00188 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] = bounds_.high[i];
<a name="l00189"></a>00189 <span class="keywordflow">else</span>
<a name="l00190"></a>00190 <span class="keywordflow">if</span> (rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] < bounds_.low[i])
<a name="l00191"></a>00191 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] = bounds_.low[i];
<a name="l00192"></a>00192 }
<a name="l00193"></a>00193 }
<a name="l00194"></a>00194
<a name="l00195"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a8ea04b460df8e8cb638ced218fef71e0">00195</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a8ea04b460df8e8cb638ced218fef71e0" title="Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...">ompl::base::RealVectorStateSpace::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="l00196"></a>00196 <span class="keyword"></span>{
<a name="l00197"></a>00197 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state);
<a name="l00198"></a>00198 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00199"></a>00199 <span class="keywordflow">if</span> (rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
<a name="l00200"></a>00200 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
<a name="l00201"></a>00201 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00202"></a>00202 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00203"></a>00203 }
<a name="l00204"></a>00204
<a name="l00205"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2304bbddd850d5b10eaa70142547b67d">00205</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2304bbddd850d5b10eaa70142547b67d" title="Copy a state to another. The memory of source and destination should NOT overlap.">ompl::base::RealVectorStateSpace::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="l00206"></a>00206 <span class="keyword"></span>{
<a name="l00207"></a>00207 memcpy(static_cast<StateType*>(destination)->values,
<a name="l00208"></a>00208 static_cast<const StateType*>(source)->values, stateBytes_);
<a name="l00209"></a>00209 }
<a name="l00210"></a>00210
<a name="l00211"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae9dba965480f0b4771455c79aae8a195">00211</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#ae9dba965480f0b4771455c79aae8a195" title="Computes distance to between two states. This function satisfies the properties of a metric and its r...">ompl::base::RealVectorStateSpace::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="l00212"></a>00212 <span class="keyword"></span>{
<a name="l00213"></a>00213 <span class="keywordtype">double</span> dist = 0.0;
<a name="l00214"></a>00214 <span class="keyword">const</span> <span class="keywordtype">double</span> *s1 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state1)->values;
<a name="l00215"></a>00215 <span class="keyword">const</span> <span class="keywordtype">double</span> *s2 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state2)->values;
<a name="l00216"></a>00216
<a name="l00217"></a>00217 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00218"></a>00218 {
<a name="l00219"></a>00219 <span class="keywordtype">double</span> diff = (*s1++) - (*s2++);
<a name="l00220"></a>00220 dist += diff * diff;
<a name="l00221"></a>00221 }
<a name="l00222"></a>00222 <span class="keywordflow">return</span> sqrt(dist);
<a name="l00223"></a>00223 }
<a name="l00224"></a>00224
<a name="l00225"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a7de4ee53afed57ec78012c764758274e">00225</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a7de4ee53afed57ec78012c764758274e" title="Checks whether two states are equal.">ompl::base::RealVectorStateSpace::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="l00226"></a>00226 <span class="keyword"></span>{
<a name="l00227"></a>00227 <span class="keyword">const</span> <span class="keywordtype">double</span> *s1 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state1)->values;
<a name="l00228"></a>00228 <span class="keyword">const</span> <span class="keywordtype">double</span> *s2 = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state2)->values;
<a name="l00229"></a>00229 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00230"></a>00230 {
<a name="l00231"></a>00231 <span class="keywordtype">double</span> diff = (*s1++) - (*s2++);
<a name="l00232"></a>00232 <span class="keywordflow">if</span> (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
<a name="l00233"></a>00233 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00234"></a>00234 }
<a name="l00235"></a>00235 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00236"></a>00236 }
<a name="l00237"></a>00237
<a name="l00238"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a5135607e2fedf624b07d5546f8fec2c5">00238</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a5135607e2fedf624b07d5546f8fec2c5" title="Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...">ompl::base::RealVectorStateSpace::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="l00239"></a>00239 <span class="keyword"></span>{
<a name="l00240"></a>00240 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rfrom = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(from);
<a name="l00241"></a>00241 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rto = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(to);
<a name="l00242"></a>00242 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state);
<a name="l00243"></a>00243 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00244"></a>00244 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] = rfrom-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] + (rto-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i] - rfrom-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i]) * t;
<a name="l00245"></a>00245 }
<a name="l00246"></a>00246
<a name="l00247"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a741fcd3240ca1a34c80f0ea9f141e2e8">00247</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_1RealVectorStateSpace.html#a741fcd3240ca1a34c80f0ea9f141e2e8" title="Allocate an instance of a uniform state sampler for this space.">ompl::base::RealVectorStateSpace::allocStateSampler</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00248"></a>00248 <span class="keyword"></span>{
<a name="l00249"></a>00249 <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>(<span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSampler.html" title="State sampler for the Rn state space.">RealVectorStateSampler</a>(<span class="keyword">this</span>));
<a name="l00250"></a>00250 }
<a name="l00251"></a>00251
<a name="l00252"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a9d460e47e6636871542bdc4f4bc017e3">00252</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_1RealVectorStateSpace.html#a9d460e47e6636871542bdc4f4bc017e3" title="Allocate a state that can store a point in the described space.">ompl::base::RealVectorStateSpace::allocState</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00253"></a>00253 <span class="keyword"></span>{
<a name="l00254"></a>00254 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">new</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>();
<a name="l00255"></a>00255 rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a> = <span class="keyword">new</span> <span class="keywordtype">double</span>[dimension_];
<a name="l00256"></a>00256 <span class="keywordflow">return</span> rstate;
<a name="l00257"></a>00257 }
<a name="l00258"></a>00258
<a name="l00259"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#af2ed57519f3da6e87cd96d3c300336cb">00259</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#af2ed57519f3da6e87cd96d3c300336cb" title="Free the memory of the allocated state.">ompl::base::RealVectorStateSpace::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="l00260"></a>00260 <span class="keyword"></span>{
<a name="l00261"></a>00261 <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">static_cast<</span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state);
<a name="l00262"></a>00262 <span class="keyword">delete</span>[] rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>;
<a name="l00263"></a>00263 <span class="keyword">delete</span> rstate;
<a name="l00264"></a>00264 }
<a name="l00265"></a>00265
<a name="l00266"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#aa263419b2d378049b3315286413d5a2f">00266</a> <span class="keywordtype">double</span>* <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#aa263419b2d378049b3315286413d5a2f" title="Many states contain a number of double values. This function provides a means to get the memory addre...">ompl::base::RealVectorStateSpace::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="l00267"></a>00267 <span class="keyword"></span>{
<a name="l00268"></a>00268 <span class="keywordflow">return</span> index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
<a name="l00269"></a>00269 }
<a name="l00270"></a>00270
<a name="l00271"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2a3d613e6aef0a811f89bfae0ac3f181">00271</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a2a3d613e6aef0a811f89bfae0ac3f181" title="Print a state to a stream.">ompl::base::RealVectorStateSpace::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="l00272"></a>00272 <span class="keyword"></span>{
<a name="l00273"></a>00273 out << <span class="stringliteral">"RealVectorState ["</span>;
<a name="l00274"></a>00274 <span class="keywordflow">if</span> (state)
<a name="l00275"></a>00275 {
<a name="l00276"></a>00276 <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a> *rstate = <span class="keyword">static_cast<</span><span class="keyword">const </span><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html" title="The definition of a state in Rn">StateType</a>*<span class="keyword">></span>(state);
<a name="l00277"></a>00277 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00278"></a>00278 {
<a name="l00279"></a>00279 out << rstate-><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace_1_1StateType.html#a916575eb76687da3b47a2d9699128498" title="The value of the actual vector in Rn">values</a>[i];
<a name="l00280"></a>00280 <span class="keywordflow">if</span> (i + 1 < dimension_)
<a name="l00281"></a>00281 out << <span class="charliteral">' '</span>;
<a name="l00282"></a>00282 }
<a name="l00283"></a>00283 }
<a name="l00284"></a>00284 <span class="keywordflow">else</span>
<a name="l00285"></a>00285 out << <span class="stringliteral">"NULL"</span> << std::endl;
<a name="l00286"></a>00286 out << <span class="charliteral">']'</span> << std::endl;
<a name="l00287"></a>00287 }
<a name="l00288"></a>00288
<a name="l00289"></a><a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a7297b3664373e5dbe9dea943f9a49bc4">00289</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1RealVectorStateSpace.html#a7297b3664373e5dbe9dea943f9a49bc4" title="Print the settings for this state space to a stream.">ompl::base::RealVectorStateSpace::printSettings</a>(std::ostream &out)<span class="keyword"> const</span>
<a name="l00290"></a>00290 <span class="keyword"></span>{
<a name="l00291"></a>00291 out << <span class="stringliteral">"Real vector state space '"</span> << getName() << <span class="stringliteral">"' of dimension "</span> << dimension_ << <span class="stringliteral">" with bounds: "</span> << std::endl;
<a name="l00292"></a>00292 out << <span class="stringliteral">" - min: "</span>;
<a name="l00293"></a>00293 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00294"></a>00294 out << bounds_.low[i] << <span class="stringliteral">" "</span>;
<a name="l00295"></a>00295 out << std::endl;
<a name="l00296"></a>00296 out << <span class="stringliteral">" - max: "</span>;
<a name="l00297"></a>00297 for (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00298"></a>00298 out << bounds_.high[i] << <span class="stringliteral">" "</span>;
<a name="l00299"></a>00299 out << std::endl;
<a name="l00300"></a>00300
<a name="l00301"></a>00301 <span class="keywordtype">bool</span> printNames = <span class="keyword">false</span>;
<a name="l00302"></a>00302 for (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00303"></a>00303 <span class="keywordflow">if</span> (!dimensionNames_[i].empty())
<a name="l00304"></a>00304 printNames = <span class="keyword">true</span>;
<a name="l00305"></a>00305 <span class="keywordflow">if</span> (printNames)
<a name="l00306"></a>00306 {
<a name="l00307"></a>00307 out << <span class="stringliteral">" and dimension names: "</span>;
<a name="l00308"></a>00308 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i < dimension_ ; ++i)
<a name="l00309"></a>00309 out << <span class="stringliteral">"'"</span> << dimensionNames_[i] << <span class="stringliteral">"' "</span>;
<a name="l00310"></a>00310 out << std::endl;
<a name="l00311"></a>00311 }
<a name="l00312"></a>00312 }
</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
>
239
