/******************************************************************************* * CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps * * version 0.1 * * Copyright (C) 2009-2012, IGG Team, LSIIT, University of Strasbourg * * * * This library is free software; you can redistribute it and/or modify it * * under the terms of the GNU Lesser General Public License as published by the * * Free Software Foundation; either version 2.1 of the License, or (at your * * option) any later version. * * * * This library is distributed in the hope that it will be useful, but WITHOUT * * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * * for more details. * * * * You should have received a copy of the GNU Lesser General Public License * * along with this library; if not, write to the Free Software Foundation, * * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * * * * Web site: http://cgogn.unistra.fr/ * * Contact information: cgogn@unistra.fr * * * *******************************************************************************/ // #define DEBUG namespace CGoGN { namespace Algo { namespace Volume { namespace MovingObjects { #define DELTA 0.00001 //static const float DELTA=0.00001; template void ParticleCell3D::display() { // std::cout << "position : " << this->m_position << std::endl; } template typename PFP::VEC3 ParticleCell3D::pointInFace(Dart d) { return Algo::Surface::Geometry::faceCentroid(m,d,position); // const VEC3& p1(this->m_positions[d]); // Dart dd=m.phi1(d); // const VEC3& p2(this->m_positions[dd]); // dd=m.phi1(dd); // VEC3& p3(this->m_positions[dd]); // // while(Geom::testOrientation2D(p3,p1,p2)==Geom::ALIGNED) { // dd = m.phi1(dd); // p3 = this->m_positions[dd]; // } // // CGoGNout << "pointInFace " << (p1+p3)*0.5f << CGoGNendl; // // return (p1+p3)*0.5f; } template Geom::Orientation3D ParticleCell3D::isLeftENextVertex(VEC3 c, Dart d, VEC3 base) { VEC3 p = position[d]; VEC3 p1 = position[m.phi1(m.phi2(d))]; Geom::Plane3D pl(p,base,p1); return pl.orient(c); } template bool ParticleCell3D::isRightVertex(VEC3 c, Dart d, VEC3 base) { VEC3 p = position[d]; VEC3 p1 = position[m.phi_1(d)]; Geom::Plane3D pl(p,base,p1); return pl.orient(c)==Geom::UNDER; } template Geom::Orientation3D ParticleCell3D::whichSideOfFace(VEC3 c, Dart d) { Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,d,position); return pl.orient(c); } template Geom::Orientation3D ParticleCell3D::isLeftL1DVol(VEC3 c, Dart d, VEC3 base, VEC3 top) { VEC3 p2 = position[d]; Geom::Plane3D pl(top,p2,base); return pl.orient(c); } template Geom::Orientation3D ParticleCell3D::isRightDVol(VEC3 c, Dart d, VEC3 base, VEC3 top) { VEC3 p1= position[m.phi1(d)]; Geom::Plane3D pl(top,p1,base); return pl.orient(c); } template Geom::Orientation3D ParticleCell3D::isAbove(VEC3 c, Dart d, VEC3 top) { VEC3 p1 = position[d]; VEC3 p2 = position[m.phi1(d)]; Geom::Plane3D pl(top,p2,p1); return pl.orient(c); } template int ParticleCell3D::isLeftL1DFace(VEC3 c, Dart d, VEC3 base, VEC3 normal) { VEC3 p2 = position[d]; VEC3 v2(p2-base); VEC3 np = normal ^ v2; Geom::Plane3D pl(np,base*np); return pl.orient(c); } template bool ParticleCell3D::isRightDFace(VEC3 c, Dart d, VEC3 base, VEC3 normal) { VEC3 p1 = position[m.phi1(d)]; VEC3 np = normal ^ VEC3(p1-base); Geom::Plane3D pl(np,base*np); return pl.orient(c)==Geom::UNDER; } template Dart ParticleCell3D::nextDartOfVertexNotMarked(Dart d, CellMarkerStore& mark) { // lock a marker Dart d1; DartMarkerNoUnmark markCC(m); // init algo with parameter dart std::list darts_list; darts_list.push_back(d); markCC.mark(d); // use iterator for begin of not yet treated darts std::list::iterator beg = darts_list.begin(); // until all darts treated while (beg != darts_list.end()) { d1 = *beg; // add phi1, phi2 and phi3 successor if they are not yet marked Dart d2 = m.phi1(m.phi2(d1)); Dart d3 = m.phi1(m.phi3(d1)); if (!markCC.isMarked(d2)) { darts_list.push_back(d2); markCC.mark(d2); } if (!markCC.isMarked(d3)) { darts_list.push_back(d3); markCC.mark(d3); } beg++; // apply functor if (!mark.isMarked(d1)) { for (std::list::iterator it=darts_list.begin(); it!=darts_list.end(); ++it) markCC.unmark(*it); return d1; } } // clear markers for (std::list::iterator it=darts_list.begin(); it!=darts_list.end(); ++it) markCC.unmark(*it); return d; } template Dart ParticleCell3D::nextNonPlanar(Dart d) { // lock a marker Dart d1; DartMarkerNoUnmark markCC(m); // init algo with parameter dart std::list darts_list; darts_list.push_back(d); markCC.mark(d); // use iterator for begin of not yet treated darts std::list::iterator beg = darts_list.begin(); // until all darts treated while (beg != darts_list.end()) { d1 = *beg; // add phi1, phi2 and phi3 successor if they are not yet marked Dart d2 = m.phi1(d1); Dart d3 = m.phi2(d1); Dart d4 = m.phi_1(d1); if (!markCC.isMarked(d2)) { darts_list.push_back(d2); markCC.mark(d2); } if (!markCC.isMarked(d3)) { darts_list.push_back(d3); markCC.mark(d3); } if (!markCC.isMarked(d4)) { darts_list.push_back(d4); markCC.mark(d4); } beg++; // apply functor Geom::Plane3D pl1 = Algo::Surface::Geometry::facePlane(m,d,position); Geom::Plane3D pl2 = Algo::Surface::Geometry::facePlane(m,d1,position); if ((pl1.normal()-pl2.normal()).norm2()>0.000001) beg = darts_list.end(); // remove the head of the list } // clear markers for (std::list::iterator it=darts_list.begin(); it!=darts_list.end(); ++it) markCC.unmark(*it); return d1; } template Dart ParticleCell3D::nextFaceNotMarked(Dart d, CellMarkerStore& mark) { // lock a marker Dart d1; DartMarkerNoUnmark markCC(m); // init algo with parameter dart std::list darts_list; darts_list.push_back(d); markCC.mark(d); // use iterator for begin of not yet treated darts std::list::iterator beg = darts_list.begin(); // until all darts treated while (beg != darts_list.end()) { d1 = *beg; if(!mark.isMarked(d1)) { for (std::list::iterator it=darts_list.begin(); it!=darts_list.end(); ++it) { markCC.unmark(*it); } return d1; } // add phi1, phi2 and phi3 successor if they are not yet marked Dart d2 = m.phi1(d1); Dart d3 = m.phi2(d1); Dart d4 = m.phi_1(d1); if (!markCC.isMarked(d2)) { darts_list.push_back(d2); markCC.mark(d2); } if (!markCC.isMarked(d3)) { darts_list.push_back(d3); markCC.mark(d3); } if (!markCC.isMarked(d4)) { darts_list.push_back(d4); markCC.mark(d4); } beg++; } // clear markers for (std::list::iterator it=darts_list.begin(); it!=darts_list.end(); ++it) { markCC.unmark(*it); } // if(beg==darts_list.end()) return d; // return d1; } template Geom::Orientation3D ParticleCell3D::whichSideOfEdge(VEC3 c, Dart d) { VEC3 p1 = position[m.phi1(d)]; VEC3 p2 = position[d]; Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,d,position); VEC3 norm = pl.normal(); VEC3 n2 = norm ^ VEC3(p2-p1); Geom::Plane3D pl2(n2,p1*n2); return pl2.orient(c); } template bool ParticleCell3D::isOnHalfEdge(VEC3 c, Dart d) { VEC3 p1 = position[d]; VEC3 p2 = position[m.phi1(d)]; VEC3 norm(p2-p1); norm.normalize(); Geom::Plane3D pl(norm,p1*norm); return pl.orient(c)==Geom::OVER && !Geom::arePointsEquals(c,p1); } /**when the ParticleCell3D trajectory go through a vertex * searching the good volume "umbrella" where the ParticleCell3D is * if the ParticleCell3D is on the vertex, do nothing */ template void ParticleCell3D::vertexState(const VEC3& current) { #ifdef DEBUG std::cout << "vertexState" << d << std::endl; #endif crossCell = CROSS_OTHER ; VEC3 som = position[d]; if(Geom::arePointsEquals(current, this->m_position)) { this->m_position = this->m_positionFace = som; state = VERTEX; return; } Dart dd=d; Geom::Orientation3D wsof; CellMarkerStore mark(m); do { VEC3 dualsp = (som+ Algo::Surface::Geometry::vertexNormal(m,d,position)); Dart ddd=d; mark.mark(d); //searching the good orientation in a volume if(isLeftENextVertex(current,d,dualsp)!=Geom::UNDER) { d=m.phi1(m.phi2(d)); while(isLeftENextVertex(current,d,dualsp)!=Geom::UNDER && ddd!=d) d=m.phi1(m.phi2(d)); if(ddd==d) { if(isnan(current[0]) || isnan(current[1]) || isnan(current[2])) { std::cout << __FILE__ << " " << __LINE__ << " NaN !" << std::endl; } bool verif = true; do { if(whichSideOfFace(current,d)!=Geom::OVER) verif = false; else d=m.alpha1(d); } while(verif && d!=ddd); if(verif) { volumeState(current); return; } } } else { while(isRightVertex(current,d,dualsp) && dd!=m.alpha_1(d)) d=m.phi2(m.phi_1(d)); } wsof = whichSideOfFace(current,d); //if c is before the vertex on the edge, we have to change of umbrella, we are symetric to the good umbrella if(wsof != Geom::OVER) { VEC3 p1=position[d]; VEC3 p2=position[m.phi1(d)]; VEC3 norm(p2-p1); norm.normalize(); Geom::Plane3D plane(norm,p1*norm); wsof = plane.orient(current); } //if c is on the other side of the face, we have to change umbrella //if the umbrella has already been tested, we just take another arbitrary one if(wsof == 1) { mark.mark(d); if(!mark.isMarked(m.alpha1(d))) d=m.alpha1(d); else { Dart dtmp=d; d = nextDartOfVertexNotMarked(d,mark); if(dtmp==d) { std::cout << "numerical rounding ?" << std::endl; d = dd; this->m_position = pointInFace(d); this->m_positionFace = this->m_position; volumeState(current); return; } } } } while(wsof == 1); if(wsof != 0) { this->m_position = pointInFace(d); d = nextNonPlanar(d); this->m_positionFace = pointInFace(d); volumeState(current); } else { //Dart ddd=d; edgeState(current); } } template void ParticleCell3D::edgeState(const VEC3& current) { #ifdef DEBUG std::cout << "edgeState" << d << " " << this->m_position << std::endl; #endif crossCell = CROSS_OTHER ; bool onEdge=false; Dart dd=d; Geom::Orientation3D wsof = whichSideOfFace(current,m.alpha2(d)); if(wsof!=Geom::UNDER) { do { d = m.alpha2(d); wsof = whichSideOfFace(current,m.alpha2(d)); }while(wsof!=1 && dd!=d); if(d==dd) onEdge = true; if(wsof==Geom::ON) { switch(whichSideOfEdge(current,d)) { case Geom::ON : onEdge=true; break; case Geom::UNDER : d = m.phi2(d); break; default : break; } } wsof = whichSideOfFace(current,d); } else { wsof = whichSideOfFace(current,d); while(wsof==Geom::UNDER && dd != m.alpha_2(d)) { d = m.alpha_2(d); wsof = whichSideOfFace(current,d); } switch(whichSideOfEdge(current,d)) { case Geom::ON : onEdge=true; break; default : break; } } if(wsof==-1) { this->m_position = pointInFace(d); d = nextNonPlanar(m.phi1(d)); this->m_positionFace = pointInFace(d); volumeState(current); return; } else { if(onEdge) { if(isOnHalfEdge(current,d)) if (isOnHalfEdge(current,m.phi3(d))) { state=2; this->m_position = this->m_positionFace = current; } else { d=m.phi1(d); vertexState(current); } else { vertexState(current); } } else { this->m_positionFace = this->m_position; d=m.phi1(d); faceState(current,wsof); } } } template void ParticleCell3D::faceState(const VEC3& current, Geom::Orientation3D wsof) { #ifdef DEBUG std::cout << "faceState" << d << std::endl; #endif crossCell = CROSS_FACE ; if(wsof==Geom::ON) wsof = whichSideOfFace(current,d); if (wsof==Geom::OVER) { d = m.phi3(d); d = nextNonPlanar(d); this->m_positionFace = pointInFace(d); volumeState(current); return; } else if(wsof==Geom::UNDER) { d = nextNonPlanar(d); this->m_positionFace = pointInFace(d); volumeState(current); return; } VEC3 norm = Algo::Surface::Geometry::faceNormal(m,d,position); Dart dd=d; if(isLeftL1DFace(current,d,this->m_positionFace,norm)!=Geom::UNDER) { d = m.phi_1(d); while(isLeftL1DFace(current,d,this->m_positionFace,norm)!=Geom::UNDER && dd!=d) d = m.phi_1(d); if(dd==d) { std::cout << "sortie ?(1)" << std::endl; do { switch (whichSideOfEdge(current,d)) { case Geom::OVER : d=m.phi_1(d); break; case Geom::ON :this->m_position = current; state = EDGE; return; default : Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,d,position); VEC3 p3 = pl.normal()+this->m_position; Geom::Plane3D plOrtho(this->m_position,current,p3); VEC3 e(position[m.phi1(d)]-position[d]); Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position); edgeState(current); return; } } while(d!=dd); this->m_position = this->m_positionFace = current; state = FACE; return; } } else { while(isRightDFace(current,d,this->m_positionFace,norm) && m.phi1(d)!=dd) d = m.phi1(d); if(m.phi_1(d)==dd) { d = m.phi1(d); do { switch (whichSideOfEdge(current,d)) { case Geom::OVER : d=m.phi_1(d); break; case Geom::ON :this->m_position = current; state = EDGE; return; default : Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,d,position); VEC3 p3 = pl.normal()+this->m_position; Geom::Plane3D plOrtho(this->m_position,current,p3); VEC3 e(position[m.phi1(d)]-position[d]); Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position); edgeState(current); return; } }while(d!=dd); this->m_position = this->m_positionFace = current; state = FACE; return; } } switch (whichSideOfEdge(current,d)) { case Geom::OVER : this->m_position = this->m_positionFace = current; state = FACE; break; case Geom::ON : this->m_position = this->m_positionFace = current; state = EDGE; break; default : Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,d,position); VEC3 p3 = pl.normal()+this->m_position; Geom::Plane3D plOrtho(this->m_position,current,p3); VEC3 e(position[m.phi1(d)]-position[d]); Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position); this->m_positionFace = this->m_position; edgeState(current); } } template void ParticleCell3D::volumeState(const VEC3& current) { #ifdef DEBUG std::cout << "volumeState " << d << std::endl; #endif CellMarkerStore mark(m); bool above; Geom::Orientation3D testRight=Geom::OVER; do { above=false; Dart dd=d; bool particularcase=false; Geom::Orientation3D testLeft = isLeftL1DVol(current,d,this->m_positionFace,this->m_position); if(testLeft!=Geom::UNDER) { d = m.phi_1(d); while(dd!=d && isLeftL1DVol(current,d,this->m_positionFace,this->m_position)!=Geom::UNDER) d = m.phi_1(d); if(dd==d) particularcase=true; } else { testRight = isRightDVol(current,d,this->m_positionFace,this->m_position); while(testRight!=Geom::OVER && dd!=m.phi1(d)) { d = m.phi1(d); testRight = isRightDVol(current,d,this->m_positionFace,this->m_position); } if(testLeft==0 && dd==m.phi1(d)) particularcase=true; } if(particularcase) //(this->m_position,this->m_positionFace,c) presque alignés et si c est proche de la face //aucun des "above" sur les dart ne va donner de résultats concluant (proche de 0 pour tous) { if(isnan(current[0]) || isnan(current[1]) || isnan(current[2])) { std::cout << __FILE__ << " " << __LINE__ << " NaN !" << std::endl; display(); this->m_position = current; return; } volumeSpecialCase(current); return; } Geom::Orientation3D testAbove = isAbove(current,d,this->m_position); if(testAbove!=Geom::UNDER || (testRight==Geom::ON && isAbove(current,m.phi_1(d),this->m_position)!=Geom::UNDER)) { if(testAbove==Geom::OVER || whichSideOfFace(current,d)==Geom::UNDER) { mark.mark(d); above=true; d = m.phi2(d); if(mark.isMarked(d)) { dd = d; d = nextFaceNotMarked(d,mark); mark.mark(d); if(d==dd) { volumeSpecialCase(current); return; } } this->m_positionFace = pointInFace(d); } } } while(above); Geom::Orientation3D wsof = whichSideOfFace(current,d); if(wsof==Geom::UNDER) { this->m_position = current; state = VOLUME; } else if(wsof==Geom::ON) { if(isAbove(current,d,this->m_position)==Geom::UNDER) { this->m_position = this->m_positionFace = current; state = FACE; } else { this->m_position = this->m_positionFace = current; edgeState(current); } } else { if(isAbove(current,d,this->m_position)==Geom::UNDER) { // this->m_position = m.intersectDartPlaneLine(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); faceState(current,wsof); } else { // this->m_position = m.intersectDartPlaneLineEdge(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); edgeState(current); } } } template void ParticleCell3D::volumeSpecialCase(const VEC3& current) { #ifdef DEBUG std::cout << "volumeSpecialCase " << d << std::endl; #endif Dart dd; CellMarkerStore mark(m); Dart d_min; std::vector dart_list; std::vector dist_list; std::list visitedFaces; // Faces that are traversed visitedFaces.push_back(d); // Start with the face of d std::list::iterator face; // For every face added to the list for (face = visitedFaces.begin();face != visitedFaces.end(); ++face) { if (!mark.isMarked(*face)) { // Face has not been visited yet Geom::Orientation3D wsof = whichSideOfFace(current,*face); if(wsof==Geom::OVER) { d = *face; if(isAbove(current,d,this->m_position)==Geom::UNDER) { // this->m_position = m.intersectDartPlaneLine(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); faceState(current,Geom::OVER); } else { // this->m_position = m.intersectDartPlaneLineEdge(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); edgeState(current); } return; } else if(wsof==Geom::ON) { this->m_position = current; d = *face; faceState(current); return; } Geom::Plane3D pl = Algo::Surface::Geometry::facePlane(m,*face,position); if(pl.normal()*VEC3(current-this->m_position)>0) { dist_list.push_back(-pl.distance(current)); dart_list.push_back(*face); } // If the face wasn't crossed then mark visited darts (current face) // and add non visited adjacent faces to the list of face Dart ddd = *face; do { mark.mark(ddd); Dart adj = m.phi2(ddd); // Get adjacent face if (adj != ddd && !mark.isMarked(adj)) visitedFaces.push_back(adj);// Add it } while(ddd!=*face); } } if(dist_list.size()>0) { float min=dist_list[0]; for(unsigned int i = 1;im_positionFace = pointInFace(d); Geom::Orientation3D wsof = whichSideOfFace(current,d); if(wsof==Geom::UNDER) { this->m_position = current; state = VOLUME; } else if(wsof==Geom::ON) { if(isAbove(current,d,this->m_position)==Geom::UNDER) { this->m_position = current; state = FACE; } else { this->m_position = current; state = EDGE; } } else { if(isAbove(current,d,this->m_position)==Geom::UNDER) { // this->m_position = m.intersectDartPlaneLine(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); faceState(current,wsof); } else { // this->m_position = m.intersectDartPlaneLineEdge(d,this->m_position,current); Algo::Surface::Geometry::intersectionLineConvexFace(m,d,position,this->m_position,current-this->m_position,this->m_position); edgeState(current); } } } } // namespace MovingObjects } // namespace Surface } // namespace Algo } // namespace CGoGN