Point_3 xz_swap_neg(const Point_3& p)
{
return Point_3(p.z(), p.y(), -p.x());
}
// Distance entre 2 points
double DegradeAnObject::squared_distance(Point_3 p1, Point_3 p2) {
return to_double((p1.x() - p2.x()) * (p1.x() - p2.x()) + (p1.y() - p2.y()) * (p1.y() - p2.y()) + (p1.z() - p2.z()) * (p1.z() - p2.z()));
}
Point_3 xz_swap_pos(const Point_3& p)
{
return Point_3(-p.z(), p.y(), p.x());
}
bool isPositivelyDecomposable(const Point_3 &a, const Point_3 &b,
const Point_3 &c, const Point_3 &decomposed)
{
DEBUG_START;
Eigen::Matrix3d matrix;
matrix << a.x(), b.x(), c.x(),
a.y(), b.y(), c.y(),
a.z(), b.z(), c.z();
Eigen::Vector3d vector;
vector << decomposed.x(), decomposed.y(), decomposed.z();
Eigen::Vector3d coefficients = matrix.inverse() * vector;
std::cout << "Tried to decompose vector, result: " << std::endl
<< coefficients << std::endl;
DEBUG_END;
return coefficients(0) >= 0.
&& coefficients(1) >= 0.
&& coefficients(2) >= 0.;
}
// Calcule une moyenne de deux points
Point_3 DegradeAnObject::meanPoints(Point_3 p1, Point_3 p2) {
Point_3 pt(p2.x() + p1.x(), p2.y() + p1.y(), p2.z() + p1.z());
pt = Point_3(pt.x()/2, pt.y()/2, pt.z()/2);
return pt;
}
static FT sphere_function (const Point_3& p)
{
const FT x2=p.x()*p.x(), y2=p.y()*p.y(), z2=p.z()*p.z();
return x2 + y2 + z2 - 1;
}
void DegradeAnObject::impactAFace(Facet &fs, int index) {
Halfedge_handle h = barycentricMesh(fs, index);
Point_3 pt = h->vertex()->point();
h->vertex()->point() = Point_3(pt.x() - 0.1, pt.y(), pt.z());
}
Vector_3 Origin::operator-(const Point_3& p) const {return Vector_3( CGAL::ORIGIN-p.get_data() ); }
static double signedDist(const Plane_3 &p, const Point_3 &C)
{
return p.a() * C.x() + p.b() * C.y() + p.c() * C.z() + p.d();
}
std::array<double,3> convert_to_array(const Point_3& p)
{
return std::array<double,3>({p.x(),p.y(),p.z()});
}
Vector_3 point_to_vec(const Point_3& v)
{
return Vector_3(v.x(),v.y(),v.z());
}
vector<vector<double>> HermiteRBF::HesCubicBasisFunc(Point_3 Pi,Point_3 Pj)
{
vector<vector<double>> HessianMat;
vector<double> Row0,Row1,Row2;
if (Pi==Pj)
{
Row0.insert(Row0.begin(),3,0);
Row1.insert(Row1.begin(),3,0);
Row2.insert(Row2.begin(),3,0);
HessianMat.push_back(Row0);
HessianMat.push_back(Row1);
HessianMat.push_back(Row2);
return HessianMat;
}
double dDiff=sqrt((Pi.x()-Pj.x())*(Pi.x()-Pj.x())+(Pi.y()-Pj.y())*(Pi.y()-Pj.y())+
(Pi.z()-Pj.z())*(Pi.z()-Pj.z()));
Row0.push_back(3*(dDiff+(Pi.x()-Pj.x())*(Pi.x()-Pj.x())/dDiff));
Row0.push_back(3*(Pi.x()-Pj.x())*(Pi.y()-Pj.y())/dDiff);
Row0.push_back(3*(Pi.x()-Pj.x())*(Pi.z()-Pj.z())/dDiff);
Row1.push_back(Row0.at(1));
Row1.push_back(3*(dDiff+(Pi.y()-Pj.y())*(Pi.y()-Pj.y())/dDiff));
Row1.push_back(3*(Pi.y()-Pj.y())*(Pi.z()-Pj.z())/dDiff);
Row2.push_back(Row0.at(2));
Row2.push_back(Row1.at(2));
Row2.push_back(3*(dDiff+(Pi.z()-Pj.z())*(Pi.z()-Pj.z())/dDiff));
HessianMat.push_back(Row0);
HessianMat.push_back(Row1);
HessianMat.push_back(Row2);
return HessianMat;
}
vector<double> HermiteRBF::GradCubicBasisFunc(SMDT3GTPoint_3 Pi,Point_3 Pj)
{
vector<double> GradVec;
if (Pi.x()==Pj.x() && Pi.y()==Pj.y() && Pi.z()==Pj.z())
{
GradVec.insert(GradVec.begin(),3,0);
return GradVec;
}
double dDiff=sqrt((Pi.x()-Pj.x())*(Pi.x()-Pj.x())+(Pi.y()-Pj.y())*(Pi.y()-Pj.y())+
(Pi.z()-Pj.z())*(Pi.z()-Pj.z()));
GradVec.push_back(3*dDiff*(Pi.x()-Pj.x()));
GradVec.push_back(3*dDiff*(Pi.y()-Pj.y()));
GradVec.push_back(3*dDiff*(Pi.z()-Pj.z()));
return GradVec;
}
double HermiteRBF::CubicBasisFunc(Point_3 Pi,Point_3 Pj)
{
if (Pi==Pj)
{
return 0;
}
double dDiff=sqrt((Pi.x()-Pj.x())*(Pi.x()-Pj.x())+(Pi.y()-Pj.y())*(Pi.y()-Pj.y())+
(Pi.z()-Pj.z())*(Pi.z()-Pj.z()));
dDiff=pow(dDiff,3);
return dDiff;
}
void Viewer::drawEdge(const Point_3& from, const Point_3& to, const QColor& clr, float r)
{
/* Draw regular lines */
if( m_isFlat ) {
// disable lighting
::glDisable( GL_LIGHTING );
::glLineWidth(1.0);
qglColor( clr );
::glBegin(GL_LINES);
::glVertex3f( from.x(), from.y(), from.z() );
::glVertex3f( to.x(), to.y(), to.z() );
::glEnd();
// resume lighting
::glEnable( GL_LIGHTING );
return;
}
/* Draw edges as 3D cylinders */
GLboolean lighting, colorMaterial;
::glGetBooleanv( GL_LIGHTING, &lighting );
::glGetBooleanv( GL_COLOR_MATERIAL, &colorMaterial );
::glEnable( GL_LIGHTING );
::glDisable(GL_COLOR_MATERIAL);
float color[4];
color[0] = clr.redF();
color[1] = clr.greenF();
color[2] = clr.blueF();
color[3] = clr.alphaF();
Vector_3 v = to - from;
// compute the length of the edge
// method 1:
// float length = sqrt( CGAL::squared_distance( from, to ) );
// method 2:
float length = sqrt( v.squared_length() );
// normalize
v = v / length;
// compute the angle: cos theta = v.z/1.0
GLfloat angle = acos( v.z() ) / 3.1415927 * 180;
::glPushMatrix();
// move to "from" point
::glTranslatef( from.x(), from.y(), from.z() );
// rotate from z-axis to from-->to
// axis: cross product of z-axis and from-->to
::glRotatef( angle, -v.y(), v.x(), 0.0f );
// draw
GLUquadricObj* quadratic = ::gluNewQuadric(); // Create A Pointer To The Quadric Object
::gluQuadricNormals( quadratic, GLU_SMOOTH ); // Create Smooth Normals
::glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color );
// gluCylinder draws a cylinder oriented along the z-axis
::gluCylinder( quadratic, r, r, length, 16, 4 );
// move back to origin
::glPopMatrix();
if ( colorMaterial )
::glEnable( GL_COLOR_MATERIAL );
if ( !lighting )
::glDisable( GL_LIGHTING );
}
/* FIXME: Copied from Polyhedron_io.cpp with slight modifications. */
static std::shared_ptr<Polyhedron> convertWithAssociation(Polyhedron_3 p,
const Point_3 &C, const std::vector<Plane_3> &initPlanes)
{
/* Check for non-emptiness. */
ASSERT(p.size_of_vertices());
ASSERT(p.size_of_facets());
int numVertices = p.size_of_vertices();
int numFacets = p.size_of_facets();
/* Allocate memory for arrays. */
Vector3d *vertices = new Vector3d[numVertices];
Facet *facets = new Facet[numFacets];
/* Transform vertexes. */
int iVertex = 0;
for (auto vertex = p.vertices_begin(); vertex != p.vertices_end(); ++vertex)
{
Point_3 point = C + vertex->point();
vertices[iVertex++] = Vector3d(point.x(), point.y(), point.z());
}
/*
* Transform facets.
* This algorithm is based on example kindly provided at CGAL online user
* manual. See example Polyhedron/polyhedron_prog_off.cpp
*/
int iFacet = 0;
auto plane = p.planes_begin();
auto facet = p.facets_begin();
/* Iterate through the std::lists of planes and facets. */
do
{
int id = p.indexPlanes_[iFacet];
facets[id].id = id;
/* Transform current plane. */
Plane_3 pi = centerizePlane(*plane,
Point_3(-C.x(), -C.y(), -C.z()),
signedDist(initPlanes[id], C));
facets[id].plane = Plane(Vector3d(pi.a(), pi.b(), pi.c()),
pi.d());
/*
* Iterate through the std::list of halfedges incident to the curent CGAL
* facet.
*/
auto halfedge = facet->facet_begin();
/* Facets in polyhedral surfaces are at least triangles. */
CGAL_assertion(CGAL::circulator_size(halfedge) >= 3);
facets[id].numVertices = CGAL::circulator_size(halfedge);
facets[id].indVertices =
new int[3 * facets[id].numVertices + 1];
/*
* TODO: It's too unsafe architecture if we do such things as setting
* the size of internal array outside the API functions. Moreover, it
* can cause us to write memory leaks.
* indFacets and numFacets should no be public members.
*/
int iFacetVertex = 0;
do
{
facets[id].indVertices[iFacetVertex++] =
std::distance(p.vertices_begin(), halfedge->vertex());
} while (++halfedge != facet->facet_begin());
/* Add cycling vertex to avoid assertion during printing. */
facets[id].indVertices[facets[id].numVertices] =
facets[id].indVertices[0];
ASSERT(facets[id].correctPlane());
/* Increment the ID of facet. */
++iFacet;
} while (++plane != p.planes_end() && ++facet != p.facets_end());
return std::make_shared<Polyhedron>(numVertices, numFacets, vertices,
facets);
}
void Viewer::wheelEvent(QWheelEvent *event)
{
// Get event modifiers key
#if QT_VERSION < 0x040000
// Bug in Qt : use 0x0f00 instead of Qt::KeyButtonMask with Qt versions < 3.1
const Qt::ButtonState modifiers = (Qt::ButtonState)(event->state() & Qt::KeyButtonMask);
#else
const Qt::KeyboardModifiers modifiers = event->modifiers();
#endif
if( (m_curMode == INSERT_V || m_curMode == FINDNB || m_curMode == EMPTYSPH )
&& modifiers == Qt::SHIFT ) {
// delta() returns the distance that the wheel is rotated, in eighths of a degree.
// note: most mouse types work in steps of 15 degrees
// positive value: rotate forwards away from the user;
// negative value: rotate backwards toward the user.
m_fRadius += (event->delta()*1. / m_iStep ); // inc-/decrease by 0.1 per step
if( m_fRadius < 0.1 )
m_fRadius = 0.1f;
// redraw
updateGL();
}//end-if-insv
else if( m_curMode == INSERT_PT && modifiers == Qt::SHIFT ) {
// delta() returns the distance that the wheel is rotated, in eighths of a degree.
// note: most mouse types work in steps of 15 degrees
// positive value: rotate forwards away from the user;
// negative value: rotate backwards toward the user.
float origR = m_fRadius;
m_fRadius += (event->delta()*1. / m_iStep ); // inc-/decrease by 0.1 per step
if( m_fRadius < 0.1 )
m_fRadius = 0.1f;
// update the new point and its conflict region
if( m_hasNewPt ) {
origR = m_fRadius / origR;
m_newPt = Point_3( m_newPt.x()*origR, m_newPt.y()*origR, m_newPt.z()*origR );
// compute the conflict hole induced by point p
computeConflict( m_newPt );
}//end-if-conflict
// redraw
updateGL();
}//end-if-inspt
// resize the trackball when moving a point
else if( m_curMode == MOVE && modifiers == Qt::SHIFT && m_isMoving ) {
float origR = m_fRadius;
m_fRadius += (event->delta()*1. / m_iStep ); // inc-/decrease by 0.1 per step
if( m_fRadius < 0.1 )
m_fRadius = 0.1f;
origR = m_fRadius / origR;
Point_3 pt = m_pScene->m_vhArray.at( m_vidMoving )->point();
// note: QList::operator[] return a modifiable reference;
// while QList::at return a const reference
#if CGAL_VERSION_NR < 1030700000
// move_point moves the point stored in v to p while preserving the Delaunay property
// it calls remove(v) followed by insert(p) and return the new handle
// it supposely faster when the point has not moved much
m_pScene->m_vhArray[m_vidMoving] = m_pScene->m_dt.move_if_no_collision(
m_pScene->m_vhArray.at( m_vidMoving ),
Point_3( pt.x()*origR, pt.y()*origR, pt.z()*origR ) );
#else
// move_if_no_collision moves the point stored in v to pt
// if there is not already another vertex placed on pt,
// the triangulation is modified s.t. the new position of v is pt;
// otherwise, the vertex at point pt is returned.
Vertex_handle vh = m_pScene->m_dt.move_if_no_collision(
m_pScene->m_vhArray.at( m_vidMoving ),
Point_3( pt.x()*origR, pt.y()*origR, pt.z()*origR ) );
int id1 = m_pScene->m_vhArray.indexOf( vh );
int id2 = m_pScene->m_vhArray.indexOf( vh, m_vidMoving+1 );
// remove the duplicate in vhArray
if( id1 != m_vidMoving )
m_pScene->m_vhArray.removeAt( id1 );
else if( id2 != -1 )
m_pScene->m_vhArray.removeAt( id2 );
m_pScene->m_vhArray[m_vidMoving] = vh;
#endif
// redraw
updateGL();
}//end-if-move
else
QGLViewer::wheelEvent(event);
}
CGAL::Point_3<CGAL::Cartesian<double> > to_cgal(const Point_3& p)
{
return CGAL::Point_3<CGAL::Cartesian<double> >(p.x(), p.y(), p.z());
}
Polyhedron_triangulation::Polyhedron_triangulation(const std::vector<Packing_polyhedron>& objects, const Packing_polyhedron& container)
: nb_groups(0)
{
typedef Packing_polyhedron::Point_3 Point_3;
typedef Packing_polyhedron::Vertex_const_iterator Vertex_const_iterator;
typedef Packing_polyhedron::Facet_const_iterator Facet_const_iterator;
tetgenio bndmesh;
bndmesh.firstnumber = 0;
bndmesh.numberofpoints = 0;
bndmesh.numberoffacets = 0;
for (size_t i = 0; i < objects.size(); i++)
{
bndmesh.numberofpoints += objects[i].size_of_vertices();
bndmesh.numberoffacets += objects[i].size_of_facets();
}
bndmesh.numberofpoints += container.size_of_vertices();
bndmesh.numberoffacets += container.size_of_facets();
bndmesh.pointlist = new REAL[bndmesh.numberofpoints * 3];
bndmesh.pointmarkerlist = new int[bndmesh.numberofpoints];
int *marker_ptr = bndmesh.pointmarkerlist;
REAL *pnt_ptr = bndmesh.pointlist;
// construct point list
for (size_t i = 0; i < objects.size(); i++)
{
for (Vertex_const_iterator vit = objects[i].vertices_begin(); vit != objects[i].vertices_end(); ++vit)
{
Point_3 v = vit->point();
*pnt_ptr++ = v.x();
*pnt_ptr++ = v.y();
*pnt_ptr++ = v.z();
*marker_ptr++ = i;
}
nb_groups++;
}
for (Vertex_const_iterator vit = container.vertices_begin(); vit != container.vertices_end(); ++vit)
{
Point_3 v = vit->point();
*pnt_ptr++ = v.x();
*pnt_ptr++ = v.y();
*pnt_ptr++ = v.z();
*marker_ptr++ = -1;
}
// construct facets
bndmesh.facetlist = new tetgenio::facet[bndmesh.numberoffacets];
//bndmesh.facetmarkerlist = new int[bndmesh.numberoffacets];
bndmesh.facetmarkerlist = NULL;
tetgenio::facet *fct_ptr = bndmesh.facetlist;
for (size_t i = 0; i < objects.size(); i++)
{
for (Facet_const_iterator fit = objects[i].facets_begin(); fit != objects[i].facets_end(); ++fit)
{
fct_ptr->numberofpolygons = 1;
fct_ptr->polygonlist = new tetgenio::polygon[fct_ptr->numberofpolygons];
fct_ptr->numberofholes = 0;
fct_ptr->holelist = NULL;
tetgenio::polygon *p = fct_ptr->polygonlist;
p->numberofvertices = 3; // assume triangle polyhedra
p->vertexlist = new int[3];
Packing_polyhedron::Halfedge_const_handle hh = fit->halfedge();
p->vertexlist[0] = hh->vertex()->idx;
hh = hh->next();
p->vertexlist[1] = hh->vertex()->idx;
hh = hh->next();
p->vertexlist[2] = hh->vertex()->idx;
++fct_ptr;
}
}
for (Facet_const_iterator fit = container.facets_begin(); fit != container.facets_end(); ++fit)
{
fct_ptr->numberofpolygons = 1;
fct_ptr->polygonlist = new tetgenio::polygon[fct_ptr->numberofpolygons];
fct_ptr->numberofholes = 0;
fct_ptr->holelist = NULL;
tetgenio::polygon *p = fct_ptr->polygonlist;
std::list<Packing_polyhedron::Vertex_const_handle> verts;
p->numberofvertices = 0; // assume triangle polyhedra
Packing_polyhedron::Halfedge_const_handle hh = fit->halfedge(), hs;
hs = hh;
do
{
p->numberofvertices++;
verts.push_back(hh->vertex());
hh = hh->next();
} while (hh != hs);
p->vertexlist = new int[p->numberofvertices];
int *vl_ptr = p->vertexlist;
for (std::list<Packing_polyhedron::Vertex_const_handle>::const_iterator it = verts.begin(); it != verts.end(); ++it)
*vl_ptr++ = (*it)->idx;
++fct_ptr;
}
//std::fill(bndmesh.facetmarkerlist, bndmesh.facetmarkerlist + bndmesh.numberoffacets, 0);
tetrahedralize("pnY", &bndmesh, &tetra_mesh);
//std::cout<<"Number of tetrahedra in tetrahedron mesh: " <<tetra_mesh.numberoftetrahedra<<std::endl;
// debug
//for (int i = 0; i < bndmesh.numberofpoints; i++)
// assert(bndmesh.pointmarkerlist[i] == tetra_mesh.pointmarkerlist[i]);
//vtx_groups.resize(nb_group);
//for (int i = 0; i < tetra_mesh.numberofpoints; i++)
// vtx_groups[tetra
cat_cells.resize(nb_groups);
extract_CAT();
}
double DegradeAnObject::distanceBetweenPointAndFacet(Point_3 p, Point_3 pfs) {
return sqrt((p.x() - pfs.x()) * (p.x() - pfs.x()) + (p.y() - pfs.y()) * (p.y() - pfs.y()) + (p.z() - pfs.z()) * (p.z() - pfs.z()));
}
