VectorXd calculateSolution(SupportFunctionDataPtr data, VectorXd values)
{
DEBUG_START;
VectorXd difference = values - data->supportValues();
std::vector<double> epsilons;
for (int i = 0; i < (int) difference.size(); ++i)
{
epsilons.push_back(difference(i));
}
auto points = data->getShiftedDualPoints_3(epsilons);
auto innerIndex = collectInnerPointsIDs(points);
std::cerr << innerIndex.size() << " points are inner" << std::endl;
std::vector<Plane_3> planes;
auto directions = data->supportDirections<Point_3>();
for (int i = 0; i < (int) directions.size(); ++i)
{
auto direction = directions[i];
Plane_3 plane(direction.x(), direction.y(), direction.z(),
-values(i));
planes.push_back(plane);
}
Polyhedron_3 polyhedron(planes);
globalPCLDumper(PCL_DUMPER_LEVEL_DEBUG,
"recovered-by-native-estimator.ply") << polyhedron;
VectorXd solution =
polyhedron.findTangientPointsConcatenated(directions);
DEBUG_END;
return solution;
}
int main() {
Eigen::MatrixXd A(4,3);
A << -1, 0, 0,
0, -1, 0,
0, 0, -1,
1, 1, 1;
Eigen::VectorXd b(4);
b << 0, 0, 0, 1;
iris::Polyhedron polyhedron(A, b);
iris::Ellipsoid ellipsoid(3);
iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid);
Eigen::MatrixXd C_expected(3,3);
C_expected << 0.2523, -0.0740, -0.0740,
-0.0740, 0.2523, -0.0740,
-0.0740, -0.0740, 0.2523;
Eigen::VectorXd d_expected(3);
d_expected << 0.2732, 0.2732, 0.2732;
valuecheckMatrix(ellipsoid.getC(), C_expected, 1e-3);
valuecheckMatrix(ellipsoid.getD(), d_expected, 1e-3);
return 0;
}
void Scene_polyhedron_shortest_path_item::recreate_shortest_path_object()
{
if (m_shortestPaths)
{
delete m_shortestPaths;
}
m_shortestPaths = new Surface_mesh_shortest_path(*polyhedron(),
CGAL::get(boost::vertex_index, *polyhedron()),
CGAL::get(CGAL::halfedge_index, *polyhedron()),
CGAL::get(CGAL::face_index, *polyhedron()),
CGAL::get(CGAL::vertex_point, *polyhedron()));
//m_shortestPaths->m_debugOutput = true;
m_isTreeCached = false;
}
void on_paint_mesh(const k3d::mesh& Mesh, const k3d::gl::painter_render_state& RenderState, k3d::iproperty::changed_signal_t& ChangedSignal)
{
return_if_fail(k3d::gl::extension::query_vbo());
if(!has_non_empty_polyhedra(Mesh))
return;
const color_t color = RenderState.node_selection ? selected_mesh_color() : unselected_mesh_color(RenderState.parent_selection);
const color_t selected_color = RenderState.show_component_selection ? selected_component_color() : color;
k3d::gl::store_attributes attributes;
glDisable(GL_LIGHTING);
enable_blending();
clean_vbo_state();
edge_selection& selected_edges = get_data<edge_selection>(&Mesh, this);
get_data<point_vbo>(&Mesh, this).bind();
for(k3d::mesh::primitives_t::const_iterator primitive = Mesh.primitives.begin(); primitive != Mesh.primitives.end(); ++primitive)
{
boost::scoped_ptr<k3d::polyhedron::const_primitive> polyhedron(k3d::polyhedron::validate(Mesh, **primitive));
if(!polyhedron.get())
continue;
get_data<edge_vbo>(&Mesh, this).bind(primitive->get());
assert_not_implemented();
/*
const k3d::uint_t edge_count = polyhedron->edge_points.size();
const selection_records_t& edge_selection_records = selected_edges.records(primitive->get());
if (!edge_selection_records.empty())
{
for (selection_records_t::const_iterator record = edge_selection_records.begin(); record != edge_selection_records.end(); ++record)
{
color4d(record->weight ? selected_color : color);
size_t start = record->begin * 2;
size_t end = record->end;
end = end > edge_count ? edge_count : end;
end *= 2;
size_t count = end - start;
glDrawElements(GL_LINES, count, GL_UNSIGNED_INT, static_cast<GLuint*>(0) + start);
}
}
else
{
color4d(color);
glDrawElements(GL_LINES, polyhedron->edge_points.size() * 2, GL_UNSIGNED_INT, 0);
}
*/
}
clean_vbo_state();
disable_blending();
}
bool Scene_polyhedron_shortest_path_item::deferred_load(Scene_polyhedron_item* polyhedronItem, Scene_interface* sceneInterface, Messages_interface* messages, QMainWindow* mainWindow)
{
initialize(polyhedronItem, sceneInterface, messages, mainWindow);
std::ifstream inFile(m_deferredLoadFilename.c_str());
if (!inFile)
{
return false;
}
m_shortestPaths->clear();
std::vector<face_descriptor> listOfFaces;
listOfFaces.reserve(CGAL::num_faces(*polyhedron()));
face_iterator current, end;
for (boost::tie(current, end) = CGAL::faces(*polyhedron()); current != end; ++current)
{
listOfFaces.push_back(*current);
}
std::string line;
std::size_t faceId;
Barycentric_coordinate location;
Construct_barycentric_coordinate construct_barycentric_coordinate;
while (std::getline(inFile, line))
{
std::istringstream lineStream(line);
FT coords[3];
lineStream >> faceId >> coords[0] >> coords[1] >> coords[2];
location = construct_barycentric_coordinate(coords[0], coords[1], coords[2]);
// std::cout << "Read in face: " << faceId << " , " << location << std::endl;
m_shortestPaths->add_source_point(listOfFaces[faceId], location);
}
return true;
}
void Scene_edit_polyhedron_item::reset_drawing_data()
{
positions.clear();
positions.resize(num_vertices(*polyhedron()) * 3);
normals.clear();
normals.resize(positions.size());
std::size_t counter = 0;
BOOST_FOREACH(vertex_descriptor vb, vertices(*polyhedron()))
{
positions[counter * 3] = vb->point().x();
positions[counter * 3 + 1] = vb->point().y();
positions[counter * 3 + 2] = vb->point().z();
const Polyhedron::Traits::Vector_3& n =
CGAL::Polygon_mesh_processing::compute_vertex_normal(vb, deform_mesh->halfedge_graph());
normals[counter * 3] = n.x();
normals[counter * 3 + 1] = n.y();
normals[counter * 3 + 2] = n.z();
++counter;
}
const k3d::selection::set create_mesh_selection(const k3d::mesh& Mesh)
{
k3d::selection::set results;
boost::scoped_ptr<k3d::geometry::primitive_selection::storage> primitive_selection(k3d::geometry::primitive_selection::create(results));
// For each primitive in the mesh ...
k3d::uint_t primitive = 0;
for(k3d::mesh::primitives_t::const_iterator p = Mesh.primitives.begin(); p != Mesh.primitives.end(); ++p, ++primitive)
{
boost::scoped_ptr<k3d::polyhedron::const_primitive> polyhedron(k3d::polyhedron::validate(Mesh, **p));
if(polyhedron)
{
// Convert point and face selections to edge selections ...
const k3d::uint_t face_begin = 0;
const k3d::uint_t face_end = face_begin + polyhedron->face_first_loops.size();
for(k3d::uint_t face = face_begin; face != face_end; ++face)
{
const k3d::uint_t face_loop_begin = polyhedron->face_first_loops[face];
const k3d::uint_t face_loop_end = face_loop_begin + polyhedron->face_loop_counts[face];
for(k3d::uint_t face_loop = face_loop_begin; face_loop != face_loop_end; ++face_loop)
{
const k3d::uint_t first_edge = polyhedron->loop_first_edges[face_loop];
for(k3d::uint_t edge = first_edge; ; )
{
if(polyhedron->face_selections[face])
{
k3d::geometry::primitive_selection::append(*primitive_selection, primitive, primitive+1, k3d::selection::EDGE, edge, edge+1, 1.0);
}
else if((*Mesh.point_selection)[polyhedron->vertex_points[edge]] || (*Mesh.point_selection)[polyhedron->vertex_points[polyhedron->clockwise_edges[edge]]])
{
k3d::geometry::primitive_selection::append(*primitive_selection, primitive, primitive+1, k3d::selection::EDGE, edge, edge+1, 1.0);
}
edge = polyhedron->clockwise_edges[edge];
if(edge == first_edge)
break;
}
}
}
continue;
}
}
return results;
}
void on_select_mesh(const k3d::mesh& Mesh, const k3d::gl::painter_render_state& RenderState, const k3d::gl::painter_selection_state& SelectionState, k3d::iproperty::changed_signal_t& ChangedSignal)
{
return_if_fail(k3d::gl::extension::query_vbo());
if(!has_non_empty_polyhedra(Mesh))
return;
if (!SelectionState.select_split_edges)
return;
k3d::gl::store_attributes attributes;
glDisable(GL_LIGHTING);
clean_vbo_state();
get_data<point_vbo>(&Mesh, this).bind();
for(k3d::mesh::primitives_t::const_iterator primitive = Mesh.primitives.begin(); primitive != Mesh.primitives.end(); ++primitive)
{
boost::scoped_ptr<k3d::polyhedron::const_primitive> polyhedron(k3d::polyhedron::validate(Mesh, **primitive));
if(!polyhedron.get())
get_data<edge_vbo>(&Mesh, this).bind(primitive->get());
const k3d::mesh::indices_t& edge_points = polyhedron->edge_points;
const k3d::mesh::indices_t& clockwise_edges = polyhedron->clockwise_edges;
const k3d::mesh::points_t& points = *Mesh.points;
const size_t edge_count = polyhedron->edge_points.size();
for(size_t edge = 0; edge < edge_count; ++edge)
{
if (SelectionState.select_backfacing ||
(!SelectionState.select_backfacing &&
!backfacing(points[edge_points[edge]] * RenderState.matrix, RenderState.camera, get_data<normal_cache>(&Mesh, this).point_normals(this).at(edge_points[edge]))
&& !backfacing(points[edge_points[clockwise_edges[edge]]] * RenderState.matrix, RenderState.camera, get_data<normal_cache>(&Mesh, this).point_normals(this).at(edge_points[clockwise_edges[edge]]))))
{
k3d::gl::push_selection_token(k3d::selection::SPLIT_EDGE, edge);
glDrawElements(GL_LINES, 2, GL_UNSIGNED_INT, static_cast<GLuint*>(0) + 2*edge);
k3d::gl::pop_selection_token();
}
}
}
clean_vbo_state();
}
void cached_triangulation::on_execute(const k3d::mesh& Mesh, k3d::inode* Painter)
{
m_input_points = Mesh.points;
if (m_points.empty())
{
m_progress = 0;
m_point_links.resize(Mesh.points->size());
for(k3d::mesh::primitives_t::const_iterator primitive = Mesh.primitives.begin(); primitive != Mesh.primitives.end(); ++primitive)
{
boost::scoped_ptr<k3d::polyhedron::const_primitive> polyhedron(k3d::polyhedron::validate(Mesh, **primitive));
if(polyhedron.get() && !k3d::polyhedron::is_sds(*polyhedron))
{
m_points.resize(m_points.size() + polyhedron->edge_points.size());
k3d::triangulator::process(Mesh, *polyhedron);
}
}
}
else
{
if (m_affected_indices.empty())
{
for (k3d::uint_t index = 0; index != m_point_links.size(); ++index)
{
k3d::mesh::indices_t& linked_points = m_point_links[index];
for (k3d::uint_t i = 0; i != linked_points.size(); ++i)
{
m_points[linked_points[i]] = m_input_points->at(index);
}
}
}
else
{
for (k3d::uint_t index = 0; index != m_affected_indices.size(); ++index)
{
k3d::mesh::indices_t& linked_points = m_point_links[m_affected_indices[index]];
for (k3d::uint_t i = 0; i != linked_points.size(); ++i)
{
m_points[linked_points[i]] = m_input_points->at(m_affected_indices[index]);
}
}
}
}
m_affected_indices.clear();
}
{
positions[counter * 3] = vb->point().x();
positions[counter * 3 + 1] = vb->point().y();
positions[counter * 3 + 2] = vb->point().z();
const Polyhedron::Traits::Vector_3& n =
CGAL::Polygon_mesh_processing::compute_vertex_normal(vb, deform_mesh->halfedge_graph());
normals[counter * 3] = n.x();
normals[counter * 3 + 1] = n.y();
normals[counter * 3 + 2] = n.z();
++counter;
}
tris.clear();
tris.resize(polyhedron()->size_of_facets() * 3);
counter = 0;
BOOST_FOREACH(face_descriptor fb, faces(*polyhedron()))
{
tris[counter * 3] = static_cast<unsigned int>(fb->halfedge()->vertex()->id());
tris[counter * 3 + 1] = static_cast<unsigned int>(fb->halfedge()->next()->vertex()->id());
tris[counter * 3 + 2] = static_cast<unsigned int>(fb->halfedge()->prev()->vertex()->id());
++counter;
}
edges.clear();
edges.resize(polyhedron()->size_of_halfedges());
counter = 0;
for (Polyhedron::Edge_iterator eb = polyhedron()->edges_begin();
eb != polyhedron()->edges_end(); ++eb, ++counter)
{
polyhedron make_sv(const polyhedron &oc, idVec4 light)
{
static polyhedron lut[64];
int index = 0;
for (unsigned int i = 0; i < 6; i++) {
if ((oc.p[i].plane * light) > 0)
index |= 1<<i;
}
if (lut[index].e.size() == 0) {
polyhedron &ph = lut[index];
ph = oc;
int V = ph.v.size();
for (int j = 0; j < V; j++) {
idVec3 proj = homogeneous_difference(light, ph.v[j]);
ph.v.push_back(idVec4(proj.x, proj.y, proj.z, 0));
}
ph.p.empty();
for (unsigned int i=0; i < oc.p.size(); i++) {
if ((oc.p[i].plane * light) > 0) {
ph.p.push_back(oc.p[i]);
}
}
if (ph.p.size() == 0)
return ph = polyhedron();
ph.compute_neighbors();
MyArrayPoly vpg;
int I = ph.p.size();
for (int i=0; i < I; i++) {
MyArrayInt &vi = ph.p[i].vi;
MyArrayInt &ni = ph.p[i].ni;
int S = vi.size();
for (int j = 0; j < S; j++) {
if (ni[j] == -1) {
poly pg;
int a = vi[(j+1)%S];
int b = vi[j];
pg.vi = four_ints(a, b, b+V, a+V);
pg.ni = four_ints(-1, -1, -1, -1);
vpg.push_back(pg);
}
}
}
for (unsigned int i = 0; i < vpg.size(); i++)
ph.p.push_back(vpg[i]);
ph.compute_neighbors();
ph.v.empty(); // no need to copy this data since it'll be replaced
}
polyhedron ph2 = lut[index];
// initalize vertices
ph2.v = oc.v;
int V = ph2.v.size();
for (int j = 0; j < V; j++) {
idVec3 proj = homogeneous_difference(light, ph2.v[j]);
ph2.v.push_back(idVec4(proj.x, proj.y, proj.z, 0));
}
// need to compute planes for the shadow volume (sv)
ph2.recompute_planes();
return ph2;
}
const k3d::selection::set operator()(k3d::inode* const Node, const k3d::mesh& Mesh, const k3d::selection::set& CurrentSelection, const k3d::selection::records& InteractiveSelection) const
{
k3d::selection::set results = CurrentSelection;
// Extract the set of edges to be selected ...
mesh::indices_t edges;
mesh::indices_t edge_primitives;
for(k3d::selection::records::const_iterator record = InteractiveSelection.begin(); record != InteractiveSelection.end(); ++record)
{
if(k3d::selection::get_node(*record) != Node)
continue;
for(k3d::selection::record::tokens_t::const_iterator primitive_token = record->tokens.begin(); primitive_token != record->tokens.end(); ++primitive_token)
{
if(primitive_token->type != k3d::selection::PRIMITIVE)
continue;
for(k3d::selection::record::tokens_t::const_iterator edge_token = primitive_token + 1; edge_token != record->tokens.end(); ++edge_token)
{
if(edge_token->type != k3d::selection::EDGE)
continue;
edges.push_back(edge_token->id);
edge_primitives.push_back(primitive_token->id);
break;
}
break;
}
}
// Get the set of unique primitives ...
std::set<uint_t> primitives(edge_primitives.begin(), edge_primitives.end());
// Optionally select adjacent edges ...
if(select_adjacent)
{
const uint_t edge_begin = 0;
const uint_t edge_end = edge_begin + edges.size();
for(std::set<uint_t>::const_iterator primitive = primitives.begin(); primitive != primitives.end(); ++primitive)
{
if(Mesh.primitives.size() <= *primitive)
continue;
boost::scoped_ptr<polyhedron::const_primitive> polyhedron(polyhedron::validate(Mesh, *Mesh.primitives[*primitive]));
if(!polyhedron)
continue;
mesh::bools_t boundary_edges;
mesh::indices_t adjacent_edges;
polyhedron::create_edge_adjacency_lookup(polyhedron->vertex_points, polyhedron->clockwise_edges, boundary_edges, adjacent_edges);
std::set<uint_t> primitive_edges;
for(uint_t edge = edge_begin; edge != edge_end; ++edge)
{
if(edge_primitives[edge] != *primitive)
continue;
primitive_edges.insert(edges[edge]);
}
for(std::set<uint_t>::const_iterator edge = primitive_edges.begin(); edge != primitive_edges.end(); ++edge)
{
if(boundary_edges[*edge])
continue;
if(primitive_edges.count(adjacent_edges[*edge]))
continue;
edges.push_back(adjacent_edges[*edge]);
edge_primitives.push_back(*primitive);
}
}
}
// Add all our edges to the output selection ...
if(edges.size())
{
const uint_t edge_begin = 0;
const uint_t edge_end = edge_begin + edges.size();
boost::scoped_ptr<geometry::primitive_selection::storage> primitive_selection(geometry::primitive_selection::create(results));
for(std::set<uint_t>::const_iterator primitive = primitives.begin(); primitive != primitives.end(); ++primitive)
{
primitive_selection->primitive_begin.push_back(*primitive);
primitive_selection->primitive_end.push_back(*primitive + 1);
primitive_selection->primitive_selection_type.push_back(k3d::selection::EDGE);
primitive_selection->primitive_first_range.push_back(primitive_selection->index_begin.size());
primitive_selection->primitive_range_count.push_back(0);
for(uint_t edge = edge_begin; edge != edge_end; ++edge)
{
if(edge_primitives[edge] != *primitive)
continue;
primitive_selection->primitive_range_count.back() += 1;
primitive_selection->index_begin.push_back(edges[edge]);
primitive_selection->index_end.push_back(edges[edge] + 1);
primitive_selection->weight.push_back(weight);
}
}
}
return results;
}
void paint_mesh(const k3d::mesh& Mesh, const k3d::ri::render_state& RenderState)
{
for(k3d::mesh::primitives_t::const_iterator primitive = Mesh.primitives.begin(); primitive != Mesh.primitives.end(); ++primitive)
{
boost::scoped_ptr<k3d::polyhedron::const_primitive> polyhedron(k3d::polyhedron::validate(Mesh, **primitive));
if(!polyhedron)
continue;
const k3d::mesh::points_t& points = *Mesh.points;
const k3d::mesh::table_t& point_attributes = Mesh.point_attributes;
const k3d::uint_t shell_begin = 0;
const k3d::uint_t shell_end = shell_begin + polyhedron->shell_types.size();
for(k3d::uint_t shell = shell_begin; shell != shell_end; ++shell)
{
if(polyhedron->shell_types[shell] != k3d::polyhedron::POLYGONS)
continue;
// Get the set of all materials used in this polyhedron ...
typedef std::set<k3d::imaterial*> materials_t;
materials_t materials;
const k3d::uint_t faces_begin = 0;
const k3d::uint_t faces_end = faces_begin + polyhedron->face_shells.size();
for(k3d::uint_t face = faces_begin; face != faces_end; ++face)
{
if(polyhedron->face_shells[face] != shell)
continue;
materials.insert(polyhedron->face_materials[face]);
}
// Iterate over each material, rendering all the faces that use that material in a single pass
for(materials_t::iterator m = materials.begin(); m != materials.end(); ++m)
{
k3d::imaterial* const material = *m;
k3d::ri::unsigned_integers loop_counts;
k3d::ri::unsigned_integers vertex_counts;
k3d::ri::unsigned_integers vertex_ids;
array_copier ri_constant_attributes;
ri_constant_attributes.add_arrays(polyhedron->constant_attributes);
array_copier ri_uniform_attributes;
ri_uniform_attributes.add_arrays(polyhedron->face_attributes);
array_copier ri_facevarying_attributes;
ri_facevarying_attributes.add_arrays(polyhedron->vertex_attributes);
array_copier ri_vertex_attributes;
ri_vertex_attributes.add_array(k3d::ri::RI_P(), points);
ri_vertex_attributes.add_arrays(point_attributes);
ri_constant_attributes.push_back(0);
for(k3d::uint_t face = faces_begin; face != faces_end; ++face)
{
if(polyhedron->face_shells[face] != shell)
continue;
if(polyhedron->face_materials[face] != material)
continue;
loop_counts.push_back(polyhedron->face_loop_counts[face]);
ri_uniform_attributes.push_back(face);
const k3d::uint_t loop_begin = polyhedron->face_first_loops[face];
const k3d::uint_t loop_end = loop_begin + polyhedron->face_loop_counts[face];
for(k3d::uint_t loop = loop_begin; loop != loop_end; ++loop)
{
k3d::uint_t vertex_count = 0;
const k3d::uint_t first_edge = polyhedron->loop_first_edges[loop];
for(k3d::uint_t edge = first_edge; ; )
{
ri_facevarying_attributes.push_back(edge);
++vertex_count;
vertex_ids.push_back(polyhedron->vertex_points[edge]);
edge = polyhedron->clockwise_edges[edge];
if(edge == first_edge)
break;
}
vertex_counts.push_back(vertex_count);
}
}
ri_vertex_attributes.insert(0, points.size());
k3d::ri::parameter_list ri_parameters;
ri_constant_attributes.copy_to(k3d::ri::CONSTANT, ri_parameters);
ri_uniform_attributes.copy_to(k3d::ri::UNIFORM, ri_parameters);
ri_facevarying_attributes.copy_to(k3d::ri::FACEVARYING, ri_parameters);
ri_vertex_attributes.copy_to(k3d::ri::VERTEX, ri_parameters);
k3d::ri::setup_material(material, RenderState);
RenderState.stream.RiPointsGeneralPolygonsV(loop_counts, vertex_counts, vertex_ids, ri_parameters);
}
}
}
}
