ajout particle 3D

include/Algo/MovingObjects/particle_cell_2D.h

@@ -49,7 +49,7 @@ public :
 	{}
 
 	ParticleCell2D(Map& map, Dart belonging_cell, VEC3 pos, const TAB_POS& tabPos) :
-		ParticleBase(pos), m(map), m_positions(tabPos), d(belonging_cell), lastCrossed(belonging_cell), state(2), crossCell(NO_CROSS)
+		ParticleBase(pos), m(map), m_positions(tabPos), d(belonging_cell), lastCrossed(belonging_cell), state(FACE_ORBIT), crossCell(NO_CROSS)
 	{}
 
 	Dart getCell() { return d; }

include/Algo/MovingObjects/particle_cell_3D.h

+#ifndef PARTCELL_H
+#define PARTCELL_H
+
+#include "particle_base.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 MovingObjects
+{
+
+template <typename PFP>
+class ParticleCell3D : public ParticleBase
+{
+public :
+	typedef typename PFP::MAP Map;
+	typedef typename PFP::VEC3 VEC3;
+	typedef typename PFP::TVEC3 TAB_POS;
+
+	Map& m;
+
+	const TAB_POS& position;
+
+	Dart d;
+	Dart lastCrossed;
+
+	VEC3 m_positionFace;
+
+	unsigned int state;
+
+	unsigned int crossCell ;
+
+	ParticleCell3D(Map& map) : m(map)
+	{}
+
+	ParticleCell3D(Map& map, Dart belonging_cell, VEC3 pos, const TAB_POS& tabPos) :
+		ParticleBase(pos), m(map), position(tabPos), d(belonging_cell), state(3)
+	{
+		m_positionFace = pointInFace(d);
+	}
+
+	void display();
+
+	Dart getCell() { return d; }
+
+	VEC3 pointInFace(Dart d);
+
+	Geom::Orientation3D isLeftENextVertex(VEC3 c, Dart d, VEC3 base);
+
+	bool isRightVertex(VEC3 c, Dart d, VEC3 base);
+
+	Geom::Orientation3D whichSideOfFace(VEC3 c, Dart d);
+
+	Geom::Orientation3D isLeftL1DVol(VEC3 c, Dart d, VEC3 base, VEC3 top);
+
+	Geom::Orientation3D isRightDVol(VEC3 c, Dart d, VEC3 base, VEC3 top);
+
+	Geom::Orientation3D isAbove(VEC3 c, Dart d, VEC3 top);
+
+	int isLeftL1DFace(VEC3 c, Dart d, VEC3 base, VEC3 normal);
+
+	bool isRightDFace(VEC3 c, Dart d, VEC3 base, VEC3 normal);
+
+	Dart nextDartOfVertexNotMarked(Dart d, CellMarker& mark);
+
+	Dart nextNonPlanar(Dart d);
+
+	Dart nextFaceNotMarked(Dart d,CellMarker& mark);
+
+	Geom::Orientation3D whichSideOfEdge(VEC3 c, Dart d);
+
+	bool isOnHalfEdge(VEC3 c, Dart d);
+
+	void vertexState(const VEC3& current);
+
+	void edgeState(const VEC3& current);
+
+	void faceState(const VEC3& current, Geom::Orientation3D sideOfFace=Geom::ON);
+
+	void volumeState(const VEC3& current);
+
+	void volumeSpecialCase(const VEC3& current);
+
+
+	void move(const VEC3& newCurrent)
+	{
+		if(!Geom::arePointsEquals(newCurrent, m_position))
+		{
+			switch(state) {
+			case VERTEX_ORBIT : vertexState(newCurrent); break;
+			case EDGE_ORBIT : 	edgeState(newCurrent);   break;
+			case FACE_ORBIT : 	faceState(newCurrent);   break;
+			case VOLUME_ORBIT : volumeState(newCurrent);   break;
+			}
+
+			display();
+		}
+	}
+};
+
+#include "particle_cell_3D.hpp"
+
+}
+
+}
+
+}
+
+#endif

include/Algo/MovingObjects/particle_cell_3D.hpp

+// #define DEBUG
+
+#define DELTA 0.00001
+//static const float DELTA=0.00001;
+
+template <typename PFP>
+void ParticleCell3D<PFP>::display()
+{
+//	std::cout << "position : " << m_position << std::endl;
+}
+
+template <typename PFP>
+typename PFP::VEC3 ParticleCell3D<PFP>::pointInFace(Dart d)
+{
+	return Algo::Geometry::faceCentroid<PFP>(m,d,position);
+//	const VEC3& p1(m_positions[d]);
+//	Dart dd=m.phi1(d);
+//	const VEC3& p2(m_positions[dd]);
+//	dd=m.phi1(dd);
+//	VEC3& p3(m_positions[dd]);
+//
+//	while(Geom::testOrientation2D(p3,p1,p2)==Geom::ALIGNED) {
+//		dd = m.phi1(dd);
+//		p3 = m_positions[dd];
+//	}
+//
+//	CGoGNout << "pointInFace " << (p1+p3)*0.5f << CGoGNendl;
+//
+//	return (p1+p3)*0.5f;
+}
+
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::isLeftENextVertex(VEC3 c, Dart d, VEC3 base)
+{
+	VEC3 p = position[d];
+	VEC3 p1 = position[m.phi1(m.phi2(d))];
+
+	Geom::Plane3D<typename PFP::REAL> pl(p,base,p1);
+	return pl.orient(c);
+}
+
+template <typename PFP>
+bool ParticleCell3D<PFP>::isRightVertex(VEC3 c, Dart d, VEC3 base)
+{
+	VEC3 p = position[d];
+	VEC3 p1 = position[m.phi_1(d)];
+
+	Geom::Plane3D<typename PFP::REAL> pl(p,base,p1);
+	return pl.orient(c)==Geom::UNDER;
+}
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::whichSideOfFace(VEC3 c, Dart d)
+{
+	Geom::Plane3D<typename PFP::REAL> pl = Algo::Geometry::facePlane<PFP>(m,d,position);
+	return pl.orient(c);
+}
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::isLeftL1DVol(VEC3 c, Dart d, VEC3 base, VEC3 top)
+{
+	VEC3 p2 = position[d];
+
+	Geom::Plane3D<typename PFP::REAL> pl(top,p2,base);
+
+	return pl.orient(c);
+}
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::isRightDVol(VEC3 c, Dart d, VEC3 base, VEC3 top)
+{
+	VEC3 p1= position[m.phi1(d)];
+
+	Geom::Plane3D<typename PFP::REAL> pl(top,p1,base);
+
+	return pl.orient(c);
+}
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::isAbove(VEC3 c, Dart d, VEC3 top)
+{
+	VEC3 p1 = position[d];
+	VEC3 p2 = position[m.phi1(d)];
+
+	Geom::Plane3D<typename PFP::REAL> pl(top,p2,p1);
+
+
+	return pl.orient(c);
+}
+
+template <typename PFP>
+int ParticleCell3D<PFP>::isLeftL1DFace(VEC3 c, Dart d, VEC3 base, VEC3 normal)
+{
+	VEC3 p2 = position[d];
+
+	VEC3 v2(p2-base);
+
+	VEC3 np = normal ^ v2;
+
+	Geom::Plane3D<typename PFP::REAL> pl(np,base*np);
+
+	return pl.orient(c);
+}
+
+template <typename PFP>
+bool ParticleCell3D<PFP>::isRightDFace(VEC3 c, Dart d, VEC3 base, VEC3 normal)
+{
+	VEC3 p1 = position[m.phi1(d)];
+
+	VEC3 np = normal ^ VEC3(p1-base);
+
+	Geom::Plane3D<typename PFP::REAL> pl(np,base*np);
+
+	return pl.orient(c)==Geom::UNDER;
+}
+
+
+template <typename PFP>
+Dart ParticleCell3D<PFP>::nextDartOfVertexNotMarked(Dart d, CellMarker& mark)
+{
+	// lock a marker
+	Dart d1;
+	DartMarkerNoUnmark markCC(m);
+
+	// init algo with parameter dart
+	std::list<Dart> darts_list;
+	darts_list.push_back(d);
+	markCC.mark(d);
+
+	// use iterator for begin of not yet treated darts
+	std::list<Dart>::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<Dart>::iterator it=darts_list.begin(); it!=darts_list.end(); ++it)
+				markCC.unmark(*it);
+			return d1;
+		}
+	}
+
+	// clear markers
+	for (std::list<Dart>::iterator it=darts_list.begin(); it!=darts_list.end(); ++it)
+		markCC.unmark(*it);
+
+	return d;
+}
+
+template <typename PFP>
+Dart ParticleCell3D<PFP>::nextNonPlanar(Dart d)
+{
+	// lock a marker
+	Dart d1;
+	DartMarkerNoUnmark markCC(m);
+
+	// init algo with parameter dart
+	std::list<Dart> darts_list;
+	darts_list.push_back(d);
+	markCC.mark(d);
+
+	// use iterator for begin of not yet treated darts
+	std::list<Dart>::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<typename PFP::REAL> pl1 = Algo::Geometry::facePlane<PFP>(m,d,position);
+		Geom::Plane3D<typename PFP::REAL> pl2 = Algo::Geometry::facePlane<PFP>(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<Dart>::iterator it=darts_list.begin(); it!=darts_list.end(); ++it)
+		markCC.unmark(*it);
+
+	return d1;
+}
+
+template <typename PFP>
+Dart ParticleCell3D<PFP>::nextFaceNotMarked(Dart d,CellMarker& mark)
+{
+	// lock a marker
+	Dart d1;
+	DartMarkerNoUnmark markCC(m);
+
+	// init algo with parameter dart
+	std::list<Dart> darts_list;
+	darts_list.push_back(d);
+	markCC.mark(d);
+
+	// use iterator for begin of not yet treated darts
+	std::list<Dart>::iterator beg = darts_list.begin();
+
+	// until all darts treated
+	while (beg != darts_list.end())
+	{
+		d1 = *beg;
+
+		if(!mark.isMarked(d1))
+		{
+			break;
+		}
+
+		// 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<Dart>::iterator it=darts_list.begin(); it!=darts_list.end(); ++it)
+	{
+		markCC.unmark(*it);
+	}
+
+	if(beg==darts_list.end())
+		return d;
+
+	return d1;
+}
+
+template <typename PFP>
+Geom::Orientation3D ParticleCell3D<PFP>::whichSideOfEdge(VEC3 c, Dart d)
+{
+	VEC3 p1 = position[m.phi1(d)];
+	VEC3 p2 = position[d];
+
+	Geom::Plane3D<typename PFP::REAL> pl = Algo::Geometry::facePlane<PFP>(m,d,position);
+	VEC3 norm = pl.normal();
+	VEC3 n2 = norm ^ VEC3(p2-p1);
+
+	Geom::Plane3D<typename PFP::REAL> pl2(n2,p1*n2);
+
+	return pl2.orient(c);
+}
+
+template <typename PFP>
+bool ParticleCell3D<PFP>::isOnHalfEdge(VEC3 c, Dart d)
+{
+	VEC3 p1 = position[d];
+	VEC3 p2 = position[m.phi1(d)];
+
+	VEC3 norm(p2-p1);
+	norm.normalize();
+
+	Geom::Plane3D<typename PFP::REAL> 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 <typename PFP>
+void ParticleCell3D<PFP>::vertexState(const VEC3& current)
+{
+	#ifdef DEBUG
+	std::cout << "vertexState" << d << std::endl;
+	#endif
+
+	VEC3 som = position[d];
+
+	if(Geom::arePointsEquals(current, m_position)) {
+		m_position = m_positionFace = som;
+		state = VERTEX_ORBIT;
+		return;
+	}
+
+	Dart dd=d;
+	Geom::Orientation3D wsof;
+	CellMarkerStore mark(m, FACE_CELL);
+
+	do {
+		VEC3 dualsp = (som+ Algo::Geometry::vertexNormal<PFP>(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<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(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;
+					m_position = pointInFace(d);
+					m_positionFace = m_position;
+					volumeState(current);
+					return;
+				}
+			}
+		}
+	}while(wsof==1);
+
+	if(wsof!=0)
+	{
+		m_position = pointInFace(d);
+		d = nextNonPlanar(d);
+		m_positionFace = pointInFace(d);
+		volumeState(current);
+	}
+	else
+	{
+		Dart ddd=d;
+
+		edgeState(current);
+	}
+}
+
+template <typename PFP>
+void ParticleCell3D<PFP>::edgeState(const VEC3& current)
+{
+	#ifdef DEBUG
+	std::cout << "edgeState" <<  d <<  " " << m_position << std::endl;
+
+	#endif
+
+	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==0) {
+			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==1 && dd != m.alpha_2(d)) {
+			d = m.alpha_2(d);
+			wsof = whichSideOfFace(current,d);
+		}
+
+		switch(whichSideOfEdge(current,d)) {
+			case Geom::ON : onEdge=true;