37 #include "ompl/base/spaces/RealVectorStateSpace.h"
38 #include "ompl/base/spaces/RealVectorStateProjections.h"
39 #include "ompl/util/Exception.h"
40 #include <boost/lexical_cast.hpp>
52 for (
unsigned int i = 0 ; i < dim ; ++i)
58 const unsigned int dim = space_->getDimension();
63 for (
unsigned int i = 0 ; i < dim ; ++i)
65 rng_.uniformReal(std::max(bounds.
low[i], rnear->
values[i] - distance),
66 std::min(bounds.
high[i], rnear->
values[i] + distance));
71 const unsigned int dim = space_->getDimension();
76 for (
unsigned int i = 0 ; i < dim ; ++i)
78 double v = rng_.gaussian(rmean->
values[i], stdDev);
79 if (v < bounds.
low[i])
82 if (v > bounds.
high[i])
95 int p = std::max(2, (
int)ceil(
log((
double)dimension_)));
111 addDimension(minBound, maxBound);
112 setDimensionName(dimension_ - 1, name);
118 stateBytes_ = dimension_ *
sizeof(double);
119 bounds_.low.push_back(minBound);
120 bounds_.high.push_back(maxBound);
121 dimensionNames_.resize(dimension_,
"");
127 if (bounds.
low.size() != dimension_)
128 throw Exception(
"Bounds do not match dimension of state space: expected dimension " +
129 boost::lexical_cast<std::string>(dimension_) +
" but got dimension " +
130 boost::lexical_cast<std::string>(bounds.
low.size()));
149 if (index < dimensionNames_.size())
150 return dimensionNames_[index];
156 std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
157 return it != dimensionIndex_.end() ? (int)it->second : -1;
162 if (index < dimensionNames_.size())
164 dimensionNames_[index] = name;
165 dimensionIndex_[name] = index;
168 throw Exception(
"Cannot set dimension name. Index out of bounds");
174 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
176 double d = bounds_.high[i] - bounds_.low[i];
185 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
187 if (rstate->
values[i] > bounds_.high[i])
188 rstate->
values[i] = bounds_.high[i];
190 if (rstate->
values[i] < bounds_.low[i])
191 rstate->
values[i] = bounds_.low[i];
198 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
199 if (rstate->
values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
200 rstate->
values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
207 memcpy(static_cast<StateType*>(destination)->values,
208 static_cast<const StateType*>(source)->values, stateBytes_);
229 const double *s1 =
static_cast<const StateType*
>(state1)->values;
230 const double *s2 =
static_cast<const StateType*
>(state2)->values;
232 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
234 double diff = (*s1++) - (*s2++);
242 const double *s1 =
static_cast<const StateType*
>(state1)->values;
243 const double *s2 =
static_cast<const StateType*
>(state2)->values;
244 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
246 double diff = (*s1++) - (*s2++);
247 if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
258 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
270 rstate->
values =
new double[dimension_];
283 return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
288 out <<
"RealVectorState [";
292 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
295 if (i + 1 < dimension_)
300 out <<
"NULL" << std::endl;
301 out <<
']' << std::endl;
306 out <<
"Real vector state space '" << getName() <<
"' of dimension " << dimension_ <<
" with bounds: " << std::endl;
308 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
309 out << bounds_.low[i] <<
" ";
312 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
313 out << bounds_.high[i] <<
" ";
316 bool printNames =
false;
317 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
318 if (!dimensionNames_[i].empty())
322 out <<
" and dimension names: ";
323 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
324 out <<
"'" << dimensionNames_[i] <<
"' ";