static I impl(I begin, I end, C pred, P proj, D len, Pair const p, concepts::ForwardIterator *fi) { // *begin is known to be false // len >= 1 if(len == 1) return begin; if(len == 2) { I tmp = begin; if(invoke(pred, invoke(proj, *++tmp))) { ranges::iter_swap(begin, tmp); return tmp; } return begin; } if(len <= p.second) { // The buffer is big enough to use // Move the falses into the temporary buffer, and the trues to the front of the line // Update begin to always point to the end of the trues auto tmpbuf = make_raw_buffer(p.first); auto buf = tmpbuf.begin(); *buf = iter_move(begin); ++buf; auto res = partition_copy(make_move_iterator(next(begin)), make_move_sentinel(end), begin, buf, std::ref(pred), std::ref(proj)); // All trues now at start of range, all falses in buffer // Move falses back into range, but don't mess up begin which points to first false ranges::move(p.first, res.out2().base().base(), res.out1()); // h destructs moved-from values out of the temp buffer, but doesn't deallocate buffer return res.out1(); } // Else not enough buffer, do in place // len >= 3 D half = len / 2; // half >= 2 I middle = next(begin, half); // recurse on [begin, middle), *begin know to be false // F????????????????? // f m l I begin_false = stable_partition_fn::impl(begin, middle, pred, proj, half, p, fi); // TTTFFFFF?????????? // f ff m l // recurse on [middle, end], except increase middle until *(middle) is false, *end know to be true I m1 = middle; D len_half = len - half; while(invoke(pred, invoke(proj, *m1))) { if(++m1 == end) return ranges::rotate(begin_false, middle, end).begin(); --len_half; } // TTTFFFFFTTTF?????? // f ff m m1 l I end_false = stable_partition_fn::impl(m1, end, pred, proj, len_half, p, fi); // TTTFFFFFTTTTTFFFFF // f ff m sf l return ranges::rotate(begin_false, middle, end_false).begin(); // TTTTTTTTFFFFFFFFFF // | }
void String::reallocate() { auto newcapacity = size() ? size()*2 : 1; // auto new_data = alloc.allocate(newcapacity); // auto dest = new_data; // auto elem = elements; // for(size_t i=0; i !=size(); i++){ // alloc.construct(dest++, std::move(*elem++)); // } auto first = alloc.allocate(newcapacity); auto last = uninitialized_copy(make_move_iterator(begin()), make_move_iterator(end()), first); free(); // elements = new_data; // first_free = dest; elements = first; first_free = last; cap = elements + newcapacity; }
static void merge_sort_loop(I1 begin, I1 end, I2 result, D step_size, C &pred, P &proj) { D two_step = 2 * step_size; while(end - begin >= two_step) { result = merge(make_move_iterator(begin), make_move_iterator(begin + step_size), make_move_iterator(begin + step_size), make_move_iterator(begin + two_step), result, std::ref(pred), std::ref(proj), std::ref(proj)).out(); begin += two_step; } step_size = ranges::min(D(end - begin), step_size); merge(make_move_iterator(begin), make_move_iterator(begin + step_size), make_move_iterator(begin + step_size), make_move_iterator(end), result, std::ref(pred), std::ref(proj), std::ref(proj)); }
static I impl(I begin, I end, C pred, P proj, D len, Pair p, detail::bidirectional_iterator_tag_ bi) { // *begin is known to be false // *end is known to be true // len >= 2 if(len == 2) { ranges::iter_swap(begin, end); return end; } if(len == 3) { I tmp = begin; if(invoke(pred, invoke(proj, *++tmp))) { ranges::iter_swap(begin, tmp); ranges::iter_swap(tmp, end); return end; } ranges::iter_swap(tmp, end); ranges::iter_swap(begin, tmp); return tmp; } if(len <= p.second) { // The buffer is big enough to use // Move the falses into the temporary buffer, and the trues to the front // of the line Update begin to always point to the end of the trues auto tmpbuf = ranges::make_raw_buffer(p.first); auto buf = tmpbuf.begin(); *buf = iter_move(begin); ++buf; auto res = partition_copy(make_move_iterator(next(begin)), make_move_sentinel(end), begin, buf, std::ref(pred), std::ref(proj)); begin = res.out1; // move *end, known to be true *begin = iter_move(res.in); ++begin; // All trues now at start of range, all falses in buffer // Move falses back into range, but don't mess up begin which points to // first false ranges::move(p.first, res.out2.base().base(), begin); // h destructs moved-from values out of the temp buffer, but doesn't // deallocate buffer return begin; } // Else not enough buffer, do in place // len >= 4 I middle = begin; D half = len / 2; // half >= 2 advance(middle, half); // recurse on [begin, middle-1], except reduce middle-1 until *(middle-1) is // true, *begin know to be false F????????????????T f m l I m1 = middle; I begin_false = begin; D len_half = half; while(!invoke(pred, invoke(proj, *--m1))) { if(m1 == begin) goto first_half_done; --len_half; } // F???TFFF?????????T // f m1 m l begin_false = stable_partition_fn::impl(begin, m1, pred, proj, len_half, p, bi); first_half_done: // TTTFFFFF?????????T // f ff m l // recurse on [middle, end], except increase middle until *(middle) is false, // *end know to be true m1 = middle; len_half = len - half; while(invoke(pred, invoke(proj, *m1))) { if(++m1 == end) return ranges::rotate(begin_false, middle, ++end).begin(); --len_half; } // TTTFFFFFTTTF?????T // f ff m m1 l I end_false = stable_partition_fn::impl(m1, end, pred, proj, len_half, p, bi); // TTTFFFFFTTTTTFFFFF // f ff m sf l return ranges::rotate(begin_false, middle, end_false).begin(); // TTTTTTTTFFFFFFFFFF // | }
bool PFLocalPlanner::select_goal(geometry_msgs::PoseStamped *_goal) { if (! this->local_reached) { *_goal = this->current_local_goal; return true; } if (this->global_plan.size() == 0) { //reached = true; return false; } geometry_msgs::PoseStamped pose; if (!this->my_pose(&pose)) { ROS_ERROR("Cannot get pose on the goal frame: %s", this->goal_frame_id.c_str()); return false; } tf::StampedTransform goalToLocal; try { this->tf->lookupTransform(this->cmd_frame_id, this->goal_frame_id, ros::Time(0), goalToLocal); } catch (tf::TransformException ex) { ROS_ERROR("Goal To Local: %s", ex.what()); return false; } std::vector<geometry_msgs::PoseStamped>::iterator it; double d, dx, dy; bool has_goal = false; tf::Vector3 candidate; tf::Quaternion orientation; int i=0; for (it = this->global_plan.begin(); it != this->global_plan.end(); it++) { candidate.setX(it->pose.position.x); candidate.setY(it->pose.position.y); orientation = tf::Quaternion(it->pose.orientation.x,it->pose.orientation.y,it->pose.orientation.z,it->pose.orientation.w); candidate = goalToLocal * candidate; geometry_msgs::Point tmp; tmp.x = candidate.x(); tmp.y = candidate.y(); // if(it->pose.position.z == 1.0) continue; d = this->dist(pose.pose.position,tmp); if (d > max_local_goal_dist) break; i++; } //if(it != global_plan.end()) // it->pose.position.z = 1.0 orientation = goalToLocal*orientation; _goal->pose.position.x = candidate.x(); _goal->pose.position.y = candidate.y(); _goal->pose.orientation.x = orientation.x(); _goal->pose.orientation.y = orientation.y(); _goal->pose.orientation.z = orientation.z(); _goal->pose.orientation.w = orientation.w(); _goal->header.frame_id = this->cmd_frame_id; global_plan = vector<geometry_msgs::PoseStamped>( make_move_iterator(global_plan.begin() + i), make_move_iterator(global_plan.end())); local_goal_pub.publish(*_goal); this->current_local_goal = *_goal; return true; }