static void reverse_subpath(vector<BezierPath::Type>& types, vector<BezierPath::Point>& vertices, unsigned int from_element, unsigned int from_vertex, unsigned int to_element, unsigned int to_vertex) { vector<BezierPath::Type> new_types(to_element - from_element); vector<BezierPath::Point> new_vertices(to_vertex - from_vertex); bool was_closed = false; new_types.push_back(BezierPath::move); new_vertices.push_back(vertices[to_vertex-1]); for (unsigned int element = to_element - 1, vertex = to_vertex - 2; element >= from_element; element--) { BezierPath::Type t = types[element]; switch (t) { case BezierPath::move: if (was_closed) { new_types.push_back(BezierPath::close); } break; case BezierPath::cubic: new_vertices.push_back(vertices[vertex]); vertex--; // fall through case BezierPath::parabolic: new_vertices.push_back(vertices[vertex]); vertex--; // fall through case BezierPath::line: new_vertices.push_back(vertices[vertex]); vertex--; new_types.push_back(t); break; case BezierPath::close: was_closed = true; break; } } assert(new_types.size() == (to_element - from_element)); assert(new_vertices.size() == (to_vertex - from_vertex)); for (unsigned int i = 0; i < new_types.size(); i++) { types[from_element + i] = new_types[i]; } for (unsigned int j = 0; j < new_vertices.size(); j++) { vertices[from_vertex + j] = new_vertices[j]; } }
ConvexPolygon::Ptr ConvexPolygon::magnifyByDistance(const double distance) { // compute centroid Eigen::Vector3f c = centroid(); Vertices new_vertices(vertices_.size()); for (size_t i = 0; i < vertices_.size(); i++) { new_vertices[i] = (vertices_[i] - c).normalized() * distance + vertices_[i]; // ROS_INFO("old v: [%f, %f, %f]", vertices_[i][0], vertices_[i][1], vertices_[i][2]); // ROS_INFO("new v: [%f, %f, %f]", new_vertices[i][0], new_vertices[i][1], new_vertices[i][2]); // ROS_INFO(""); } ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices)); return ret; }
std::pair<random_spine_generator*, solution*> generate_solution( random_spine_generator * current_spine_generator, solution *current_solution, int stepsize, unsigned num_random_changes, const std::vector<std::vector<vertex_t> > & adjacency_list ) { random_spine_generator * random_spine= new random_spine_generator( *current_spine_generator ); solution * random_solution= new solution( *current_solution ); // generate new random vertices std::vector<vertex_t> new_vertices(stepsize); //random_spine->add_random_to_spine(stepsize,new_vertices); random_spine->random_change_block( stepsize, num_random_changes, new_vertices); // enlarge our solution random_solution->add_vertices_to_spine_order( new_vertices ); greedy_edge_add( *random_solution, new_vertices, adjacency_list ); return std::make_pair(random_spine,random_solution); }