/**
* True if u is steeper than v. Uses the square of slope to avoid sqrt.
*/
bool is_steeper(Vector_3 u, Vector_3 v)
{
Vector_2 u_2 = Vector_2(u.x(), u.y());
Vector_2 v_2 = Vector_2(v.x(), v.y());
return ((u.z() * u.z() / u_2.squared_length()) >
(v.z() * v.z() / v_2.squared_length()));
}
// Computes the intersection C+uV of the planes M*x+D=0 and N*x+E=0.
// Returns -1 if there are no intersections (parallel), 0 for a line
// intersection and 1 for co-planarity.
// precondition: A, B are normalized (hessian form)
int plane_plane_intersection(const Vector_3& M, const double D,
const Vector_3& N, const double E,
Point_3& C, Vector_3& V)
{
typedef CGAL::Cartesian<double> K;
typedef CGAL::Plane_3<K> Plane_3;
typedef CGAL::Line_3<K> Line_3;
Plane_3 P1(M.x(), M.y(), M.z(), D);
Plane_3 P2(N.x(), N.y(), N.z(), E);
CGAL::Object result = CGAL::intersection(P1, P2);
if (const Line_3 *iline = CGAL::object_cast<Line_3>(&result)) {
CGAL::Point_3<K> p = iline->point(0);
CGAL::Vector_3<K> v = iline->to_vector();
C = Point_3(p.x(), p.y(), p.z());
V = Vector_3(v.x(), v.y(), v.z());
return 0;
}
else if (const Plane_3 *iplane = CGAL::object_cast<Plane_3>(&result)) {
return 1;
}
else {
return -1;
}
}
int line_plane_intersection(const Point_3& B, const Vector_3& M,
const Vector_3& N, const double D,
Point_3& p)
{
typedef CGAL::Cartesian<double> K;
typedef CGAL::Plane_3<K> Plane_3;
typedef CGAL::Line_3<K> Line_3;
CGAL::Point_3<K> CB(B.x(), B.y(), B.z());
CGAL::Vector_3<K> CM(M.x(), M.y(), M.z());
Line_3 L(CB, CM);
Plane_3 P(N.x(), N.y(), N.z(), D);
CGAL::Object result = CGAL::intersection(L, P);
if (const CGAL::Point_3<K> *ipoint = CGAL::object_cast<CGAL::Point_3<K> >(&result)) {
p = Point_3(ipoint->x(), ipoint->y(), ipoint->z());
return 0;
}
else if (const Line_3 *iline = CGAL::object_cast<Line_3>(&result)) {
return 1;
}
else {
return -1;
}
}
static std::vector<Point_3> mapPointsFromOXYplane(std::vector<Point_2> points,
Vector_3 nu)
{
DEBUG_START;
ASSERT(!!Vector3d(nu.x(), nu.y(), nu.z()) && "nu is null vector");
Vector_3 ez(0., 0, 1.);
double length = sqrt(nu.squared_length());
ASSERT(std::fpclassify(length) != FP_ZERO);
nu = nu * 1. / length; /* Normalize std::vector \nu. */
ASSERT(std::isfinite(nu.x()));
ASSERT(std::isfinite(nu.y()));
ASSERT(std::isfinite(nu.z()));
Vector_3 tau = cross_product(nu, ez);
std::vector<Point_3> pointsMapped;
CGAL::Origin o;
for (auto &point : points)
{
pointsMapped.push_back(o + tau * point.x() + ez * point.y());
}
DEBUG_END;
return pointsMapped;
}
void Viewer::alphaChanged()
{
normals.resize(0);
pos_poly.resize(0);
std::list<Facet> facets;
scene->alpha_shape.get_alpha_shape_facets(std::back_inserter(facets), Alpha_shape_3::REGULAR);
for(std::list<Facet>::iterator fit = facets.begin();
fit != facets.end();
++fit) {
const Cell_handle& ch = fit->first;
const int index = fit->second;
//const Vector_3& n = ch->normal(index); // must be unit vector
const Point_3& a = ch->vertex((index+1)&3)->point();
const Point_3& b = ch->vertex((index+2)&3)->point();
const Point_3& c = ch->vertex((index+3)&3)->point();
Vector_3 v = CGAL::unit_normal(a,b,c);
normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
pos_poly.push_back(a.x()); pos_poly.push_back(a.y()); pos_poly.push_back(a.z());
pos_poly.push_back(b.x()); pos_poly.push_back(b.y()); pos_poly.push_back(b.z());
pos_poly.push_back(c.x()); pos_poly.push_back(c.y()); pos_poly.push_back(c.z());
}
initialize_buffers();
}
// Assign either ceiling, floor or the default semantic based on input booleans and normal vector
void assignCeilVloor(std::set<Polyhedron::Facet_handle>& fhSet, bool canBeUp, bool canBeDown) {
pointVector facetPoints;
Vector_3 ortVec;
for (std::set<Polyhedron::Facet_handle>::iterator sfIt=fhSet.begin();sfIt!=fhSet.end();++sfIt) {
if (!canBeUp && !canBeDown) {
(*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC; continue;
}
facetPoints = comp_facetPoints(*sfIt);
CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),ortVec);
if (!normalizeVector(ortVec)) continue;
if (canBeDown && ortVec.z() <= -HORIZONTAL_ANGLE_RANGE ) (*sfIt)->semanticBLA = DEFAULT_DOWN_SEMANTIC;
else if (canBeUp && ortVec.z() >= HORIZONTAL_ANGLE_RANGE ) (*sfIt)->semanticBLA = DEFAULT_UP_SEMANTIC;
else (*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC;
}
}
void rotate_points3d(const Eigen::MatrixXd& ptsin, const Point_3& center, const Vector_3& direction, double angle, Eigen::MatrixXd& ptsout)
{
Eigen::Vector3d c(center.x(), center.y(), center.z());
Eigen::Vector3d dir(direction.x(), direction.y(), direction.z());
Eigen::Matrix4d rotMat = create_rotation3d_line_angle(center, direction, angle);
ptsout.resize(ptsin.rows(), ptsin.cols() );
ptsout = transform_point3d(ptsin, rotMat);
}
// Génère le vecteur qui a subi une rotation d'angle teta
Vector_3 DegradeAnObject::rotationVector(Vector_3 v, Vector_3 normal, double teta) {
double c = cos(teta);
double s = sin(teta);
Kernel::RT m00 = normal.x() * normal.x() * (1-c) + c;
Kernel::RT m01 = normal.x() * normal.y() * (1-c) - normal.z() * s;
Kernel::RT m02 = normal.x() * normal.z() * (1-c) + normal.y() * s;
Kernel::RT m10 = normal.x() * normal.y() * (1-c) + normal.z() * s;
Kernel::RT m11 = normal.y() * normal.y() * (1-c) + c;
Kernel::RT m12 = normal.z() * normal.y() * (1-c) - normal.x() * s;
Kernel::RT m20 = normal.x() * normal.z() * (1-c) - normal.y() * s;
Kernel::RT m21 = normal.z() * normal.y() * (1-c) + normal.x() * s;
Kernel::RT m22 = normal.z() * normal.z() * (1-c) + c;
CGAL::Aff_transformation_3<Kernel> rotate(m00, m01, m02, m10, m11, m12, m20, m21, m22);
return rotate.transform(v);
}
Polyhedron_3 obtainPolyhedron(Polyhedron_3 initialP, std::map<int, int> map,
IpoptTopologicalCorrector *FTNLP)
{
DEBUG_START;
std::vector<Vector_3> directions = FTNLP->getDirections();
std::vector<double> values = FTNLP->getValues();
std::vector<Plane_3> planes(initialP.size_of_facets());
unsigned iFacet = 0;
for (auto I = initialP.facets_begin(), E = initialP.facets_end();
I != E; ++I)
{
auto it = map.find(iFacet);
if (it != map.end())
{
int i = it->second;
Vector_3 u = directions[i];
double h = values[i];
ASSERT(h > 0);
planes[iFacet] = Plane_3(-u.x(), -u.y(), -u.z(), h);
std::cout << "Changing plane #" << iFacet << ": "
<< I->plane() << " |--> " << planes[iFacet]
<< std::endl;
}
else
{
planes[iFacet] = I->plane();
}
++iFacet;
}
Polyhedron_3 intersection(planes);
std::cout << "Change in facets number: " << initialP.size_of_facets()
<< " -> " << intersection.size_of_facets() << std::endl;
ASSERT(initialP.size_of_facets() - intersection.size_of_facets()
< map.size() &&
"It seems that all extracted facets have gone");
DEBUG_END;
return intersection;
}
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 );
}
//compute the cross point
Point_3 compute_cross_point(Plane_3 plane, Point_3 start, Point_3 end)
{
Vector_3 normal = plane.orthogonal_vector();
Vector_3 line_direction = end - start;
Point_3 p= plane.point();
double t;
double a = (start.x() - p.x()) * normal.x() + (start.y() - p.y()) * normal.y() + (start.z() - p.z()) * normal.z();
double b = line_direction.x() * normal.x() + line_direction.y() * normal.y() + line_direction.z() * normal.z();
assert(b != 0);
t = -a / b;
return start + t * line_direction;
}
// Réalise un impact sur la face fs à partir d'une liste de points à déplacer, répartis par couronne (pts[0] = première couronne intérieure, pts[0][0] = premier point de la première couronne)
void DegradeAnObject::impactTheFacetArea(std::vector< std::vector<Point_3> > pts, Facet fs, double ray, int index) {
double str = 0.02;
Vector_3 normal = normalizeVector(getNormalOfFacet(fs));
Kernel::Plane_3 pl(fs.halfedge()->vertex()->point(), normal);
for(int i = 0 ; i < pts.size() ; i++) {
for(int j = 0 ; j < pts[i].size() ; j++) {
bool chk = false;
Point_iterator pi = polys[index].points_begin();
while(!chk) {
++pi;
if(*pi == pts[i][j]) {
*pi = Point_3(pi->x() - (impactStrengh(str, i))*normal.x(), pi->y() - (impactStrengh(str, i))*normal.y(), pi->z() - (impactStrengh(str, i))*normal.z());
chk = true;
}
}
}
}
}
// Normalise un vecteur
Vector_3 DegradeAnObject::normalizeVector(Vector_3 v) {
double norm = sqrt(to_double(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()));
return v/norm;
}
void apply_semanticRequirements(Polyhedron& exteriorPolyhe) {
std::map<std::string,std::vector<bool>> semanticNormals;
semanticNormals["Roof"] = std::vector<bool>(3,true);
semanticNormals["Roof"][2] = false; // down
semanticNormals["Ground"] = std::vector<bool>(3,false);
semanticNormals["Ground"][2] = true;
semanticNormals["Ceiling"] = std::vector<bool>(3,false);
semanticNormals["Ceiling"][2] = true;
semanticNormals["Floor"] = std::vector<bool>(3,false);
semanticNormals["Floor"][0] = true;
semanticNormals["Wall"] = std::vector<bool>(3,true);
semanticNormals["Window"] = std::vector<bool>(3,true);
semanticNormals["Door"] = std::vector<bool>(3,true);
semanticNormals["Closure"] = std::vector<bool>(3,true);
semanticNormals[TO_DIST_SEMANTIC] = std::vector<bool>(3,true);
//semanticNormals["Anything"] = std::vector<bool>(3,true);
std::map<std::string,int> semanticEnum;
semanticEnum["Roof"] = 1;
semanticEnum["Window"] = 2;
semanticEnum["Door"] = 3;
semanticEnum["Wall"] = 4;
semanticEnum["Ground"] = 5;
semanticEnum["Ceiling"] = 6;
semanticEnum["Floor"] = 7;
semanticEnum["Closure"] = 8;
semanticEnum["Anything"] = 9;
//semanticEnum["Install"] = 10;
Vector_3 ortVec;
Polyhedron::Facet_iterator exfIt; // Iterate over exterior faces
for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt) {
int minSem = 99;
for (std::vector<std::string>::iterator svIt=exfIt->equidistSems.begin();svIt!=exfIt->equidistSems.end();++svIt) { // Add equidist semantics
std::map<std::string,int>::iterator seIt = semanticEnum.find(*svIt);
if (seIt!=semanticEnum.end()) { // ......
if (seIt->second<minSem) {
minSem = seIt->second;
exfIt->semanticBLA = *svIt;
}
}else { // This seems wrong...
exfIt->semanticBLA = *svIt;
break;
}
}
pointVector facetPoints = comp_facetPoints(exfIt);
CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),ortVec); // Calculate normal vector, ortVec set to zero in newell
if (!normalizeVector(ortVec)) continue;
std::map<std::string,std::vector<bool>>::iterator snIt = semanticNormals.find(exfIt->semanticBLA);
if (snIt!=semanticNormals.end()) {
if (!snIt->second[0] && ortVec.z() > HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_UP_SEMANTIC;
else if (!snIt->second[1] && ortVec.z() <= HORIZONTAL_ANGLE_RANGE && ortVec.z() >= -HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_HOR_SEMANTIC;
else if (!snIt->second[2] && ortVec.z() <= -HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_DOWN_SEMANTIC;
} else {
if (ortVec.z() > HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_UP_SEMANTIC;
else if (ortVec.z() <= HORIZONTAL_ANGLE_RANGE && ortVec.z() >= -HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_HOR_SEMANTIC;
else if (ortVec.z() <= -HORIZONTAL_ANGLE_RANGE)
exfIt->semanticBLA = DEFAULT_DOWN_SEMANTIC;
}
}
// Set semantics of facets created due to closing (too far from original geometry)
for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt) {
if (exfIt->semanticBLA==TO_DIST_SEMANTIC) {
std::set<Polyhedron::Facet_handle> fhSet;
connectedSemFacets(exfIt,TO_DIST_SEMANTIC,false,fhSet);
bool canBeUp = indirectlyTouchingFindSem(DEFAULT_UP_SEMANTIC,fhSet);
bool canBeDown = indirectlyTouchingFindSem(DEFAULT_DOWN_SEMANTIC,fhSet);
assignCeilVloor(fhSet,canBeUp,canBeDown);
}
}
}
Point_3 vec_to_point(const Vector_3& v)
{
return Point_3(v.x(),v.y(),v.z());
}
std::array<double,3> convert_to_array(const Vector_3& p)
{
return std::array<double,3>({p.x(),p.y(),p.z()});
}