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/DiscreteMotionValidator.h"
00038 #include "ompl/util/Exception.h"
00039 #include <queue>
00040
00041 void ompl::base::DiscreteMotionValidator::defaultSettings(void)
00042 {
00043 stateSpace_ = si_->getStateSpace().get();
00044 if (!stateSpace_)
00045 throw Exception("No state space for motion validator");
00046 }
00047
00048 bool ompl::base::DiscreteMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00049 {
00050
00051
00052 bool result = true;
00053 int nd = stateSpace_->validSegmentCount(s1, s2);
00054
00055 if (nd > 1)
00056 {
00057
00058 State *test = si_->allocState();
00059
00060 for (int j = 1 ; j < nd ; ++j)
00061 {
00062 stateSpace_->interpolate(s1, s2, (double)j / (double)nd, test);
00063 if (!si_->isValid(test))
00064 {
00065 lastValid.second = (double)(j - 1) / (double)nd;
00066 if (lastValid.first)
00067 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
00068 result = false;
00069 break;
00070 }
00071 }
00072 si_->freeState(test);
00073 }
00074
00075 if (result)
00076 if (!si_->isValid(s2))
00077 {
00078 lastValid.second = (double)(nd - 1) / (double)nd;
00079 if (lastValid.first)
00080 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
00081 result = false;
00082 }
00083
00084 if (result)
00085 valid_++;
00086 else
00087 invalid_++;
00088
00089 return result;
00090 }
00091
00092 bool ompl::base::DiscreteMotionValidator::checkMotion(const State *s1, const State *s2) const
00093 {
00094
00095 if (!si_->isValid(s2))
00096 return false;
00097
00098 bool result = true;
00099 int nd = stateSpace_->validSegmentCount(s1, s2);
00100
00101
00102 std::queue< std::pair<int, int> > pos;
00103 if (nd >= 2)
00104 {
00105 pos.push(std::make_pair(1, nd - 1));
00106
00107
00108 State *test = si_->allocState();
00109
00110
00111 while (!pos.empty())
00112 {
00113 std::pair<int, int> x = pos.front();
00114
00115 int mid = (x.first + x.second) / 2;
00116 stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, test);
00117
00118 if (!si_->isValid(test))
00119 {
00120 result = false;
00121 break;
00122 }
00123
00124 pos.pop();
00125
00126 if (x.first < mid)
00127 pos.push(std::make_pair(x.first, mid - 1));
00128 if (x.second > mid)
00129 pos.push(std::make_pair(mid + 1, x.second));
00130 }
00131
00132 si_->freeState(test);
00133 }
00134
00135 if (result)
00136 valid_++;
00137 else
00138 invalid_++;
00139
00140 return result;
00141 }