/// Computes the Minkowski sum of two convex polyhedra
PolyhedronPtr Polyhedron::minkowski(Polyhedron& p1, const Matrix4& T1, Polyhedron& p2, const Matrix4& T2, bool reflect_p2)
{
// verify that both polyhedra are convex
if (!p1.is_convex() || !p2.is_convex())
throw std::runtime_error("Polyhedron::minkowski() only operates on convex polyhedra");
// we'll transform p2 to p1's frame
Matrix4 T2_to_T1 = Matrix4::inverse_transform(T1) * T2;
Polyhedron p2_copy = p2;
p2_copy.transform(T2_to_T1);
// compute the minkowski sum
std::list<Vector3> points;
for (unsigned i=0; i< p1.get_vertices().size(); i++)
for (unsigned j=0; j< p2_copy.get_vertices().size(); j++)
{
// add vertex from p1 to vertex from p2
Vector3 v = p1.get_vertices()[i];
if (reflect_p2)
v -= p2_copy.get_vertices()[j];
else
v += p2_copy.get_vertices()[j];
points.push_back(v);
}
// compute the convex hull of the points
return CompGeom::calc_convex_hull(points.begin(), points.end());
}
/// Sends this polyhedron to the specified stream using VRML
void Polyhedron::to_vrml(std::ostream& out, const Polyhedron& p, Vector3 diffuse_color, bool wireframe)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the vertices and the facets
const std::vector<Vector3>& vertices = p.get_vertices();
const std::vector<IndexedTri>& facets = p.get_facets();
out << "Shape {" << std::endl;
out << " appearance Appearance { material Material { diffuseColor " << diffuse_color[0] << " " << diffuse_color[1] << " " << diffuse_color[2] << " } }" << std::endl;
out << " geometry ";
if (!wireframe)
out << "IndexedFaceSet {" << std::endl;
else
out << "IndexedLineSet {" << std::endl;
out << " coord Coordinate { point [ ";
for (unsigned i=0; i< vertices.size(); i++)
out << vertices[i][X] << " " << vertices[i][Y] << " " << vertices[i][Z] << ", ";
out << " ] }" << std::endl;
out << " coordIndex [ ";
if (!wireframe)
for (unsigned i=0; i< facets.size(); i++)
out << facets[i].a << " " << facets[i].b << " " << facets[i].c << " -1, ";
else
for (unsigned i=0; i< facets.size(); i++)
out << facets[i].a << " " << facets[i].b << " " << facets[i].c << " " << facets[i].a << " -1, ";
out << " ] } }" << std::endl;
}
static void write_poly2(const Polyhedron& p, std::ostream& out)
{
const unsigned X = 0, Y = 1, Z = 2;
const vector<Vector3>& v = p.get_mesh().get_vertices();
for (unsigned i=0; i< v.size(); i++)
{
out << "Transform {" << std::endl;
out << " translation " << v[i][0] << " " << v[i][1] << " " << v[i][2] << std::endl;
out << " scale 0.1 0.1 0.1" << std::endl;
out << " children Shape { " << std::endl;
out << " geometry Text { " << std::endl;
out << " string \"" << i << "\"" << std::endl;
out << " } } }" << std::endl;
}
// get the vertices and the facets
const std::vector<Vector3>& vertices = p.get_vertices();
const std::vector<IndexedTri>& facets = p.get_facets();
// first write the consistent edges
out << "Shape {" << std::endl;
out << " appearance Appearance { material Material { diffuseColor 1 1 1 } }" << std::endl;
out << " geometry ";
out << "IndexedLineSet {" << std::endl;
out << " coord Coordinate { point [ ";
for (unsigned i=0; i< vertices.size(); i++)
out << vertices[i][X] << " " << vertices[i][Y] << " " << vertices[i][Z] << ", ";
out << " ] }" << std::endl;
out << " coordIndex [ ";
for (unsigned i=0; i< facets.size(); i++)
{
if (consistent(facets, facets[i].a, facets[i].b))
out << facets[i].a << " " << facets[i].b << " -1, ";
if (consistent(facets, facets[i].b, facets[i].c))
out << facets[i].b << " " << facets[i].c << " -1, ";
if (consistent(facets, facets[i].a, facets[i].c))
out << facets[i].a << " " << facets[i].c << " -1, ";
}
out << " ] } }" << std::endl;
// now write the inconsistent edges
std::set<pair<unsigned, unsigned> > marked;
out << "Shape {" << std::endl;
out << " appearance Appearance { material Material { diffuseColor 1 0 0 } }" << std::endl;
out << " geometry ";
out << "IndexedLineSet {" << std::endl;
out << " coord Coordinate { point [ ";
for (unsigned i=0; i< vertices.size(); i++)
out << vertices[i][X] << " " << vertices[i][Y] << " " << vertices[i][Z] << ", ";
out << " ] }" << std::endl;
out << " coordIndex [ ";
for (unsigned i=0; i< facets.size(); i++)
{
if (marked.find(make_pair(facets[i].a, facets[i].b)) == marked.end() &&
!consistent(facets, facets[i].a, facets[i].b))
{
out << facets[i].a << " " << facets[i].b << " -1, ";
marked.insert(make_pair(facets[i].a, facets[i].b));
std::cout << "edge: " << facets[i].a << " " << facets[i].b << std::endl;
}
if (marked.find(make_pair(facets[i].b, facets[i].c)) == marked.end() &&
!consistent(facets, facets[i].b, facets[i].c))
{
out << facets[i].b << " " << facets[i].c << " -1, ";
marked.insert(make_pair(facets[i].b, facets[i].c));
std::cout << "edge: " << facets[i].b << " " << facets[i].c << std::endl;
}
if (marked.find(make_pair(facets[i].a, facets[i].c)) == marked.end() &&
!consistent(facets, facets[i].a, facets[i].c))
{
out << facets[i].a << " " << facets[i].c << " -1, ";
marked.insert(make_pair(facets[i].a, facets[i].c));
std::cout << "edge: " << facets[i].a << " " << facets[i].c << std::endl;
}
}
out << " ] } }" << std::endl;
}
