Commit a31bc901 authored by pitiot's avatar pitiot

adding part cell

parent d69b68dc
#ifndef PARTCELL_3D_MEMO_H
#define PARTCELL_3D_MEMO_H
#include "Algo/MovingObjects/particle_cell_3D.h"
#include "Algo/Geometry/inclusion.h"
#include "Geometry/intersection.h"
#include "Geometry/orientation.h"
#include "Geometry/plane_3d.h"
#include <iostream>
/* A particle cell is a particle base within a map, within a precise cell, the displacement function should indicate
after each displacement wherein lies the new position of the particle */
namespace CGoGN
{
namespace Algo
{
namespace Volume
{
namespace MovingObjects
{
template <typename PFP>
class ParticleCell3DMemo : public Algo::Volume::MovingObjects::ParticleCell3D<PFP>
{
public :
typedef typename PFP::MAP MAP;
typedef typename PFP::VEC3 VEC3;
typedef VertexAttribute<VEC3, MAP> TAB_POS;
ParticleCell3DMemo(MAP& map, Dart belonging_cell, VEC3 pos, const TAB_POS& tabPos) :
ParticleCell3D<PFP>(map, belonging_cell, pos, tabPos)
{
}
virtual ~ParticleCell3DMemo()
{
}
void vertexState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross);
void edgeState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross);
void faceState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross, Geom::Orientation3D sideOfFace = Geom::ON);
void volumeState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross);
void volumeSpecialCase(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross);
std::vector<Dart> move(const VEC3& newCurrent, CellMarkerMemo<MAP, VOLUME>& memo_cross);
std::vector<Dart> move(const VEC3& newCurrent);
};
} // namespace MovingObjects
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
#include "Algo/MovingObjects/particle_cell_3D_memo.hpp"
#endif
/*******************************************************************************
* 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
{
template <typename PFP>
std::vector<Dart> ParticleCell3DMemo<PFP>::move(const VEC3& newCurrent)
{
this->crossCell = NO_CROSS ;
if(!Geom::arePointsEquals(newCurrent, this->getPosition()))
{
CellMarkerMemo<MAP, VOLUME> memo_cross(this->m);
switch(this->state) {
case VERTEX : vertexState(newCurrent,memo_cross); break;
case EDGE : edgeState(newCurrent,memo_cross); break;
case FACE : faceState(newCurrent,memo_cross); break;
case VOLUME : volumeState(newCurrent,memo_cross); break;
}
return memo_cross.get_markedCells();
}
else
this->Algo::MovingObjects::ParticleBase<PFP>::move(newCurrent) ;
std::vector<Dart> res;
res.push_back(this->d);
return res;
}
template <typename PFP>
std::vector<Dart> ParticleCell3DMemo<PFP>::move(const VEC3& newCurrent, CellMarkerMemo<MAP, VOLUME>& memo_cross)
{
this->crossCell = NO_CROSS ;
if(!Geom::arePointsEquals(newCurrent, this->getPosition()))
{
switch(this->state) {
case VERTEX : vertexState(newCurrent,memo_cross); break;
case EDGE : edgeState(newCurrent,memo_cross); break;
case FACE : faceState(newCurrent,memo_cross); break;
case VOLUME : volumeState(newCurrent,memo_cross); break;
}
return memo_cross.get_markedCells();
}
else
this->Algo::MovingObjects::ParticleBase<PFP>::move(newCurrent) ;
std::vector<Dart> res;
res.push_back(this->d);
return res;
}
/**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 <typename PFP>
void ParticleCell3DMemo<PFP>::vertexState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross)
{
#ifdef DEBUG
std::cout << "vertexState" << this->d << std::endl;
#endif
if(!memo_cross.isMarked(this->d)) memo_cross.mark(this->d);
this->crossCell = CROSS_OTHER ;
VEC3 som = this->position[this->d];
if(Geom::arePointsEquals(current, this->m_position)) {
this->m_position = this->m_positionFace = som;
this->state = VERTEX;
return;
}
Dart dd=this->d;
Geom::Orientation3D wsof;
CellMarkerStore<MAP, FACE> mark(this->m);
do {
VEC3 dualsp = (som+ Algo::Surface::Geometry::vertexNormal<PFP>(this->m,this->d,this->position));
Dart ddd=this->d;
mark.mark(this->d);
//searching the good orientation in a volume
if(this->isLeftENextVertex(current,this->d,dualsp)!=Geom::UNDER) {
this->d=this->m.phi1(this->m.phi2(this->d));
while(this->isLeftENextVertex(current,this->d,dualsp)!=Geom::UNDER && ddd!=this->d)
this->d=this->m.phi1(this->m.phi2(this->d));
if(ddd==this->d) {
if(isnan(current[0]) || isnan(current[1]) || isnan(current[2])) {
std::cout << __FILE__ << " " << __LINE__ << " NaN !" << std::endl;
}
bool verif = true;
do {
if(this->whichSideOfFace(current,this->d)!=Geom::OVER)
verif = false;
else
this->d=this->m.alpha1(this->d);
} while(verif && this->d!=ddd);
if(verif) {
this->volumeState(current,memo_cross);
return;
}
}
}
else {
while(this->isRightVertex(current,this->d,dualsp) && dd!=this->m.alpha_1(this->d))
this->d=this->m.phi2(this->m.phi_1(this->d));
}
wsof = this->whichSideOfFace(current,this->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=this->position[this->d];
VEC3 p2=this->position[this->m.phi1(this->d)];
VEC3 norm(p2-p1);
norm.normalize();
Geom::Plane3D<typename PFP::REAL> 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(this->d);
if(!mark.isMarked(this->m.alpha1(this->d)))
this->d=this->m.alpha1(this->d);
else
{
Dart dtmp=this->d;
this->d = this->nextDartOfVertexNotMarked(this->d,mark);
if(dtmp==this->d) {
std::cout << "numerical rounding ?" << std::endl;
this->d = dd;
this->m_position = this->pointInFace(this->d);
this->m_positionFace = this->m_position;
this->volumeState(current, memo_cross);
return;
}
}
}
} while(wsof == 1);
if(wsof != 0)
{
this->m_position = this->pointInFace(this->d);
this->d = this->nextNonPlanar(this->d);
this->m_positionFace = this->pointInFace(this->d);
this->volumeState(current,memo_cross);
}
else
{
//Dart ddd=d;
this->edgeState(current,memo_cross);
}
}
template <typename PFP>
void ParticleCell3DMemo<PFP>::edgeState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross)
{
#ifdef DEBUG
std::cout << "edgeState" << this->d << " " << this->m_position << std::endl;
#endif
if(!memo_cross.isMarked(this->d)) memo_cross.mark(this->d);
this->crossCell = CROSS_OTHER ;
bool onEdge=false;
Dart dd=this->d;
Geom::Orientation3D wsof = this->whichSideOfFace(current,this->m.alpha2(this->d));
if(wsof!=Geom::UNDER) {
do {
this->d = this->m.alpha2(this->d);
wsof = this->whichSideOfFace(current,this->m.alpha2(this->d));
}while(wsof!=1 && dd!=this->d);
if(this->d==dd)
onEdge = true;
if(wsof==Geom::ON) {
switch(this->whichSideOfEdge(current,this->d)) {
case Geom::ON :
onEdge=true;
break;
case Geom::UNDER :
this->d = this->m.phi2(this->d);
break;
default :
break;
}
}
wsof = this->whichSideOfFace(current,this->d);
}
else {
wsof = this->whichSideOfFace(current,this->d);
while(wsof==Geom::UNDER && dd != this->m.alpha_2(this->d)) {
this->d = this->m.alpha_2(this->d);
wsof = this->whichSideOfFace(current,this->d);
}
switch(this->whichSideOfEdge(current,this->d)) {
case Geom::ON : onEdge=true;
break;
default :
break;
}
}
if(wsof==-1) {
this->m_position = this->pointInFace(this->d);
this->d = this->nextNonPlanar(this->m.phi1(this->d));
this->m_positionFace = this->pointInFace(this->d);
volumeState(current, memo_cross);
return;
}
else {
if(onEdge) {
if(this->isOnHalfEdge(current,this->d))
if (this->isOnHalfEdge(current,this->m.phi3(this->d))) {
this->state=2;
this->m_position = this->m_positionFace = current;
}
else {
this->d=this->m.phi1(this->d);
vertexState(current, memo_cross);
}
else {
vertexState(current, memo_cross);
}
}
else {
this->m_positionFace = this->m_position;
this->d=this->m.phi1(this->d);
faceState(current, memo_cross,wsof);
}
}
}
template <typename PFP>
void ParticleCell3DMemo<PFP>::faceState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross, Geom::Orientation3D wsof)
{
#ifdef DEBUG
std::cout << "faceState" << this->d << std::endl;
#endif
if(!memo_cross.isMarked(this->d)) memo_cross.mark(this->d);
this->crossCell = CROSS_FACE ;
if(wsof==Geom::ON)
wsof = this->whichSideOfFace(current,this->d);
if (wsof==Geom::OVER) {
#ifdef DEBUG
std::cout << "wsof OVER" << std::endl;
#endif
this->d = this->m.phi3(this->d);
this->d = this->nextNonPlanar(this->d);
this->m_positionFace = this->pointInFace(this->d);
volumeState(current, memo_cross);
return;
}
else if(wsof==Geom::UNDER) {
#ifdef DEBUG
std::cout << "wsof UNDER" << std::endl;
#endif
this->d = this->nextNonPlanar(this->d);
this->m_positionFace = this->pointInFace(this->d);
volumeState(current, memo_cross);
return;
}
VEC3 norm = Algo::Surface::Geometry::faceNormal<PFP>(this->m,this->d,this->position);
Dart dd=this->d;
if(this->isLeftL1DFace(current,this->d,this->m_positionFace,norm)!=Geom::UNDER) {
this->d = this->m.phi_1(this->d);
while(this->isLeftL1DFace(current,this->d,this->m_positionFace,norm)!=Geom::UNDER && dd!=this->d)
this->d = this->m.phi_1(this->d);
if(dd==this->d) {
std::cout << "sortie ?(1)" << std::endl;
do {
switch (this->whichSideOfEdge(current,this->d)) {
case Geom::OVER : this->d=this->m.phi_1(this->d);
break;
case Geom::ON :this->m_position = current;
this->state = EDGE;
return;
default :
Geom::Plane3D<typename PFP::REAL> pl = Algo::Surface::Geometry::facePlane<PFP>(this->m,this->d,this->position);
VEC3 p3 = pl.normal()+this->m_position;
Geom::Plane3D<typename PFP::REAL> plOrtho(this->m_position,current,p3);
VEC3 e(this->position[this->m.phi1(this->d)]-this->position[this->d]);
Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position);
edgeState(current, memo_cross);
return;
}
} while(this->d!=dd);
this->m_position = this->m_positionFace = current;
this->state = FACE;
return;
}
}
else {
while(this->isRightDFace(current,this->d,this->m_positionFace,norm) && this->m.phi1(this->d)!=dd)
this->d = this->m.phi1(this->d);
if(this->m.phi_1(this->d)==dd) {
this->d = this->m.phi1(this->d);
do {
switch (this->whichSideOfEdge(current,this->d))
{
case Geom::OVER :
this->d=this->m.phi_1(this->d);
break;
case Geom::ON :this->m_position = current;
this->state = EDGE;
return;
default :
Geom::Plane3D<typename PFP::REAL> pl = Algo::Surface::Geometry::facePlane<PFP>(this->m,this->d,this->position);
VEC3 p3 = pl.normal()+this->m_position;
Geom::Plane3D<typename PFP::REAL> plOrtho(this->m_position,current,p3);
VEC3 e(this->position[this->m.phi1(this->d)]-this->position[this->d]);
Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position);
edgeState(current, memo_cross);
return;
}
}while(this->d!=dd);
this->m_position = this->m_positionFace = current;
this->state = FACE;
return;
}
}
switch (this->whichSideOfEdge(current,this->d))
{
case Geom::OVER :
this->m_position = this->m_positionFace = current;
this->state = FACE;
break;
case Geom::ON :
this->m_position = this->m_positionFace = current;
this->state = EDGE;
break;
default :
Geom::Plane3D<typename PFP::REAL> pl = Algo::Surface::Geometry::facePlane<PFP>(this->m,this->d,this->position);
VEC3 p3 = pl.normal()+this->m_position;
Geom::Plane3D<typename PFP::REAL> plOrtho(this->m_position,current,p3);
VEC3 e(this->position[this->m.phi1(this->d)]-this->position[this->d]);
Geom::intersectionPlaneRay(plOrtho,this->m_position,current-this->m_position,this->m_position);
this->m_positionFace = this->m_position;
edgeState(current, memo_cross);
}
}
template <typename PFP>
void ParticleCell3DMemo<PFP>::volumeState(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross)
{
#ifdef DEBUG
std::cout << "volumeState " << this->d << std::endl;
#endif
if(!memo_cross.isMarked(this->d)) memo_cross.mark(this->d);
CellMarkerStore<MAP, FACE> mark(this->m);
bool above;
Geom::Orientation3D testRight=Geom::OVER;
do {
above=false;
Dart dd=this->d;
bool particularcase=false;
Geom::Orientation3D testLeft = this->isLeftL1DVol(current,this->d,this->m_positionFace,this->m_position);
if(testLeft!=Geom::UNDER) {
this->d = this->m.phi_1(this->d);
while(dd!=this->d && this->isLeftL1DVol(current,this->d,this->m_positionFace,this->m_position)!=Geom::UNDER)
this->d = this->m.phi_1(this->d);
if(dd==this->d)
particularcase=true;
}
else {
testRight = this->isRightDVol(current,this->d,this->m_positionFace,this->m_position);
while(testRight!=Geom::OVER && dd!=this->m.phi1(this->d)) {
this->d = this->m.phi1(this->d);
testRight = this->isRightDVol(current,this->d,this->m_positionFace,this->m_position);
}
if(testLeft==0 && dd==this->m.phi1(this->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;
this->m_position = current;
return;
}
#ifdef DEBUG
std::cout << "particularCase" << std::endl;
#endif
this->volumeSpecialCase(current, memo_cross);
return;
}
Geom::Orientation3D testAbove = this->isAbove(current,this->d,this->m_position);
if(testAbove!=Geom::UNDER || (testRight==Geom::ON && this->isAbove(current,this->m.phi_1(this->d),this->m_position)!=Geom::UNDER)) {
if(testAbove==Geom::OVER || this->whichSideOfFace(current,this->d)==Geom::UNDER) {
mark.mark(this->d);
above=true;
this->d = this->m.phi2(this->d);
if(mark.isMarked(this->d)) {
dd = this->d;
this->d = this->nextFaceNotMarked(this->d,mark);
mark.mark(this->d);
if(this->d==dd) {
#ifdef DEBUG
std::cout << "dd=d" << std::endl;
#endif
this->volumeSpecialCase(current,memo_cross);
return;
}
}
this->m_positionFace = this->pointInFace(this->d);
}
}
} while(above);
Geom::Orientation3D wsof = this->whichSideOfFace(current,this->d);
if(wsof==Geom::UNDER) {
this->m_position = current;
this->state = VOLUME;
}
else if(wsof==Geom::ON) {
if(this->isAbove(current,this->d,this->m_position)==Geom::UNDER) {
this->m_position = this->m_positionFace = current;
this->state = FACE;
}
else {
this->m_position = this->m_positionFace = current;
edgeState(current, memo_cross);
}
}
else {
if(this->isAbove(current,this->d,this->m_position)==Geom::UNDER) {
// this->m_position = m.intersectDartPlaneLine(d,this->m_position,current);
Algo::Surface::Geometry::intersectionLineConvexFace<PFP>(this->m,this->d,this->position,this->m_position,current-this->m_position,this->m_position);
faceState(current, memo_cross,wsof);
}
else {
// this->m_position = m.intersectDartPlaneLineEdge(d,this->m_position,current);
Algo::Surface::Geometry::intersectionLineConvexFace<PFP>(this->m,this->d,this->position,this->m_position,current-this->m_position,this->m_position);
edgeState(current, memo_cross);
}
}
}
template <typename PFP>
void ParticleCell3DMemo<PFP>::volumeSpecialCase(const VEC3& current, CellMarkerMemo<MAP, VOLUME>& memo_cross)
{
#ifdef DEBUG
std::cout << "volumeSpecialCase " << this->d << std::endl;
#endif
if(!memo_cross.isMarked(this->d)) memo_cross.mark(this->d);
Dart dd;
CellMarkerStore<MAP, FACE> mark(this->m);
Dart d_min;
std::vector<Dart> dart_list;
std::vector<float> dist_list;
std::list<Dart> visitedFaces; // Faces that are traversed
visitedFaces.push_back(this->d); // Start with the face of d
std::list<Dart>::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 = this->whichSideOfFace(current,*face);
if(wsof==Geom::OVER)
{
this->d = *face;
if(this->isAbove(current,this->d,this->m_position)==Geom::UNDER)
{
// this->m_position = m.intersectDartPlaneLine(d,this->m_position,current);
Algo::Surface::Geometry::intersectionLineConvexFace<PFP>(this->m,this->d,this->position,this->m_position,current-this->m_position,this->m_position);
faceState(current, memo_cross,Geom::OVER);
}
else
{
// this->m_position = m.intersectDartPlaneLineEdge(d,this->m_position,current);
Algo::Surface::Geometry::intersectionLineConvexFace<PFP>(this->m,this->d,this->position,this->m_position,current-this->m_position,this->m_position);
edgeState(current, memo_cross);
}
return;
}
else if(wsof==Geom::ON)
{
this->m_position = current;
this->d = *face;
faceState(current, memo_cross);
return;
}
Geom::Plane3D<typename PFP::REAL> pl = Algo::Surface::Geometry::facePlane<PFP>(this->m,*face,this->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 = this->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;i<dist_list.size();++i)
{
if(dist_list[i]<min)
{
this->d=dart_list[i];
min = dist_list[i];
}