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);
}
