00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/spaces/RealVectorBounds.h"
00038 #include "ompl/util/Exception.h"
00039 #include <algorithm>
00040 #include <limits>
00041
00042 void ompl::base::RealVectorBounds::setLow(double value)
00043 {
00044 std::fill(low.begin(), low.end(), value);
00045 }
00046
00047 void ompl::base::RealVectorBounds::setHigh(double value)
00048 {
00049 std::fill(high.begin(), high.end(), value);
00050 }
00051
00052 void ompl::base::RealVectorBounds::setLow(unsigned int index, double value)
00053 {
00054 low[index] = value;
00055 }
00056
00057 void ompl::base::RealVectorBounds::setHigh(unsigned int index, double value)
00058 {
00059 high[index] = value;
00060 }
00061
00062 std::vector<double> ompl::base::RealVectorBounds::getDifference(void) const
00063 {
00064 unsigned int n = std::min(low.size(), high.size());
00065 std::vector<double> result(n);
00066 for (unsigned int i = 0 ; i < n ; ++i)
00067 result[i] = high[i] - low[i];
00068 return result;
00069 }
00070
00071 double ompl::base::RealVectorBounds::getVolume(void) const
00072 {
00073 double v = 1.0;
00074 unsigned int n = std::min(low.size(), high.size());
00075 for (unsigned int i = 0 ; i < n ; ++i)
00076 {
00077 double d = high[i] - low[i];
00078 v *= d;
00079 }
00080 return v;
00081 }
00082
00083 void ompl::base::RealVectorBounds::check(void) const
00084 {
00085 if (low.size() != high.size())
00086 throw Exception("Lower and upper bounds are not of same dimension");
00087 for (unsigned int i = 0 ; i < low.size() ; ++i)
00088 if (low[i] > high[i])
00089 throw Exception("Bounds for real vector space seem to be incorrect (lower bound must be stricly less than upper bound). Sampling will not be possible");
00090 }