prevent GoalStates::sampleGoal from rolling over prematurely. Fix tha…

…nks to Jonathan Gammell. Closes #155
ompl · Jul 30, 2015 · 6686801 · 6686801
1 parent 7b3fa36
commit 6686801
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/src/ompl/base/goals/src/GoalStates.cpp b/src/ompl/base/goals/src/GoalStates.cpp
@@ -83,8 +83,12 @@ void ompl::base::GoalStates::sampleGoal(base::State *st) const
 {
     if (states_.empty())
         throw Exception("There are no goals to sample");
+
+    // Roll over the samplePosition_ if it points past the number of states.
+    samplePosition_ = samplePosition_ % states_.size();
+    // Get the next state.
     si_->copyState(st, states_[samplePosition_]);
-    samplePosition_ = (samplePosition_ + 1) % states_.size();
+    samplePosition_++;
 }
 
 unsigned int ompl::base::GoalStates::maxSampleCount() const