//Find the barycenter for each mesh and translate each mesh into the barycenter, returning mesh and barycenter
std::list<TranslatedMeshData> centerMeshesInBarycenter(std::list<MeshData> &listMeshData)
{
std::list< std::pair<MeshData,PointCGAL> > result;
std::list<MeshData>::iterator meshDataInter;
for(meshDataInter = listMeshData.begin(); meshDataInter != listMeshData.end(); ++meshDataInter)
{
MeshData meshData = *meshDataInter;
PointCGAL barycenter = getMeshBarycenter(meshData);
MeshData meshDataTranslated = translateMesh(meshData, PointCGAL(-barycenter.x(), -barycenter.y(), -barycenter.z()));
result.push_back(TranslatedMeshData(meshDataTranslated, barycenter));
}
return result;
}
cv::Point2d DelaunayTri::GetCircumcenter(const cv::Point2d &a, const cv::Point2d &b, const cv::Point2d &c) {
PointCGAL p = dt.circumcenter(PointCGAL(a.x, a.y), PointCGAL(b.x, b.y), PointCGAL(c.x, c.y));
return cv::Point2d(p.x(), p.y());
}
DelaunayTri::DelaunayTri(const std::vector<cv::Point2i> &XYpoints,
const int &width, const int &height, const int &size)
{
this->width = width;
this->height = height;
img = cv::Mat::zeros(cv::Size(width, height), CV_8UC1);
int i;
points.resize(size);
for(i = 0; i < size; ++i)
{
points[i] = std::make_pair(PointCGAL(XYpoints[i].x, XYpoints[i].y), i + 1);
}
dt.insert(points.begin(),points.end());
int idxSize = 0;
size_ = dt.number_of_faces();
indices.resize(size_*3, -1);
centers.resize(size_, cv::Point2d(0, 0));
neighbors.resize(size_*3, 0.0);
radii.resize(size_, 0.0);
for(Delaunay::Finite_faces_iterator it = dt.finite_faces_begin(); it != dt.finite_faces_end(); it++) {
cv::Point2d t[3];
int idx[3];
Delaunay::Face_handle face = it;
int offset = idxSize * 3;
for(i = 0; i < 3; i++) {
t[i] = cv::Point2d(dt.triangle(face)[i].x(), dt.triangle(face)[i].y());
*(idx + i) = face->vertex(i)->info();
indices [offset + i] = face->vertex(i)->info();
}
this->insertMap(idx, idxSize);
cv::Point2d circumCenter = this->GetCircumcenter(t[0], t[1], t[2]);
centers[idxSize] = circumCenter;
radii[idxSize] = distPoint2Point<double>(circumCenter.x, circumCenter.y,
t[1].x, t[1].y);
idxSize++;
}
//this->draw_subdiv();
//cv::imwrite("delaunay.png", img);
this->computeNeighbors();
}
PointCGAL converPointInexactToExact(KDPoint point)
{
return PointCGAL(CGAL::to_double(point.x()), CGAL::to_double(point.y()), CGAL::to_double(point.z()));
}
TrianglesList meshSimplification(TrianglesList &triangles, int stopPredicate) {
#ifdef MESHSIMPLIFICATION_LOG
CGAL::Timer timer;
timer.start();
#endif
TrianglesList result;
try
{
Polyhedron P;
#ifdef MESHSIMPLIFICATION_LOG
std::cout << "Start Building Polyhedron surface... " << std::endl;
#endif
Build_triangle_mesh_coherent_surface<HalfedgeDS> triangle(triangles);
P.delegate(triangle);
P.normalize_border();
#ifdef MESHSIMPLIFICATION_LOG
std::cout << "Completed Building Polyhedron surface:" << std::endl;
std::cout << "Polyhedron is_pure_triangle: " << P.is_pure_triangle() << std::endl;
std::cout << "Polyhedron is_closed: " << P.is_closed() << std::endl;
std::cout << "Polyhedron is_pure_bivalent : " << P.is_pure_bivalent () << std::endl;
std::cout << "Polyhedron is_pure_trivalent: " << P.is_pure_trivalent() << std::endl;
std::cout << "Polyhedron is_valid 0: " << P.is_valid(false, 0) << std::endl;
std::cout << "Polyhedron is_valid 1: " << P.is_valid(false, 1) << std::endl;
std::cout << "Polyhedron is_valid 2: " << P.is_valid(false, 2) << std::endl;
std::cout << "Polyhedron is_valid 3: " << P.is_valid(false, 3) << std::endl;
std::cout << "Polyhedron is_valid 4: " << P.is_valid(false, 4) << std::endl;
std::cout << "Polyhedron normalized_border_is_valid : " << P.normalized_border_is_valid(false) << std::endl;
#endif
#ifdef MESHSIMPLIFICATION_LOG
std::cout << "Start edge_collapse... " << std::endl;
#endif
SMS::Count_stop_predicate<Polyhedron> stop(stopPredicate);
int removedEdges = SMS::edge_collapse(P, stop,
CGAL::vertex_index_map(boost::get(CGAL::vertex_external_index, P)).edge_index_map(boost::get(CGAL::edge_external_index ,P))
);
#ifdef MESHSIMPLIFICATION_LOG
std::cout << "Completed edge_collapse:" << std::endl;
std::cout << "Finished with: " << removedEdges << " edges removed and " << (P.size_of_halfedges()/2) << " final edges." << std::endl;
#endif
//Build output result
for ( Polyhedron::Facet_iterator fit( P.facets_begin() ), fend( P.facets_end() ); fit != fend; ++fit )
{
if ( fit->is_triangle() )
{
PointCGAL verts[3];
int tick = 0;
Polyhedron::Halfedge_around_facet_circulator hit( fit->facet_begin() ), hend( hit );
do
{
if ( tick < 3 )
{
verts[tick++] = PointCGAL( hit->vertex()->point().x(), hit->vertex()->point().y(), hit->vertex()->point().z() );
}
else
{
std::cout << "meshSimplification: We've got facets with more than 3 vertices even though the facet reported to be triangular..." << std::endl;
}
} while( ++hit != hend );
result.push_back( Triangle(verts[0], verts[1], verts[2]) );
}
else
{
std::cout << "meshSimplification: Skipping non-triangular facet" << std::endl;
}
}
}
catch (CGAL::Assertion_exception e)
{
std::cout << "ERROR: meshSimplification CGAL::Assertion_exception" << e.message() << std::endl;
}
#ifdef MESHSIMPLIFICATION_LOG
timer.stop();
std::cout << "meshSimplification result with: " << result.size() << " triangles." << std::endl;
std::cout << "Total meshSimplification time: " << timer.time() << std::endl;
#endif
return result;
}
//The return values are the mass, the center of mass, and the inertia tensor relative to the
// center of mass. The code assumes that the rigid body has constant density 1. If the rigid body has constant
// density D, then you need to multiply the output mass by D and the output inertia tensor by D.
void calculateMassCenterInertia(TrianglesList &triangles, double &massReturn, PointCGAL &cmReturn, Vector &inertiaReturn)
{
const REALD mult[10] = {1./6., 1./24., 1./24., 1./24., 1./60., 1./60., 1/60., 1./120., 1./120., 1./120.};
REALD intg[10] = {0., 0., 0., 0., 0., 0., 0., 0., 0., 0.}; // order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
std::list<Triangle>::iterator triangleIter;
for(triangleIter = triangles.begin(); triangleIter != triangles.end(); ++triangleIter)
{
Triangle t = *triangleIter;
REALD x0 = CGAL::to_double(t.vertex(0).x());
REALD y0 = CGAL::to_double(t.vertex(0).y());
REALD z0 = CGAL::to_double(t.vertex(0).z());
REALD x1 = CGAL::to_double(t.vertex(1).x());
REALD y1 = CGAL::to_double(t.vertex(1).y());
REALD z1 = CGAL::to_double(t.vertex(1).z());
REALD x2 = CGAL::to_double(t.vertex(2).x());
REALD y2 = CGAL::to_double(t.vertex(2).y());
REALD z2 = CGAL::to_double(t.vertex(2).z());
// get edges and cross product of edges
REALD a1 = x1-x0;
REALD b1 = y1-y0;
REALD c1 = z1-z0;
REALD a2 = x2-x0;
REALD b2 = y2-y0;
REALD c2 = z2-z0;
REALD d0 = b1*c2-b2*c1;
REALD d1 = a2*c1-a1*c2;
REALD d2 = a1*b2-a2*b1;
// compute integral terms
REALD f1x, f2x, f3x, g0x, g1x, g2x, f1y, f2y, f3y, g0y, g1y, g2y, f1z, f2z, f3z, g0z, g1z, g2z;
subexpressions_integral_terms(x0, x1, x2, f1x, f2x, f3x, g0x, g1x, g2x);
subexpressions_integral_terms(y0, y1, y2, f1y, f2y, f3y, g0y, g1y, g2y);
subexpressions_integral_terms(z0, z1, z2, f1z, f2z, f3z, g0z, g1z, g2z);
// update integrals
intg[0] += d0*f1x;
intg[1] += d0*f2x;
intg[2] += d1*f2y;
intg[3] += d2*f2z;
intg[4] += d0*f3x;
intg[5] += d1*f3y;
intg[6] += d2*f3z;
intg[7] += d0*(y0*g0x+y1*g1x+y2*g2x);
intg[8] += d1*(z0*g0y+z1*g1y+z2*g2y);
intg[9] += d2*(x0*g0z+x1*g1z+x2*g2z);
}
//Multiple the coefficients
for (int i = 0; i < 10; i++)
{
intg[i] *= mult[i];
}
//Mass of the body with a constant density 1
REALD mass = intg[0];
//Center of mass
REALD cm_x = intg[1]/mass;
REALD cm_y = intg[2]/mass;
REALD cm_z = intg[3]/mass;
//Inertia tensor relative to center of mass
REALD inertia_xx = intg[5]+intg[6]-mass*(cm_y*cm_y+cm_z*cm_z);
REALD inertia_yy = intg[4]+intg[6]-mass*(cm_z*cm_z+cm_x*cm_x);
REALD inertia_zz = intg[4]+intg[5]-mass*(cm_x*cm_x+cm_y*cm_y);
//REALD inertia_xy = -(intg[7]-mass*cm_x*cm_y);
//REALD inertia_yz = -(intg[8]-mass*cm_y*cm_z);
//REALD inertia_xz = -(intg[9]-mass*cm_z*cm_x);
//Return the references values
massReturn = fabs(mass);
cmReturn = PointCGAL(cm_x, cm_y, cm_z);
inertiaReturn = Vector(inertia_xx, inertia_yy, inertia_zz);
}
