geometry.cpp

#include "inmost.h"
#if defined(USE_MESH)
#include <deque>
using namespace std;

namespace INMOST
{
	typedef struct orient_face_t
	{
		Edge bridge;
		Node first;
		Face face;
		orient_face_t(Edge _bridge, Node _first, Face _face)
		:bridge(_bridge),first(_first),face(_face)
		{
		}
	} orient_face;

	__INLINE static void vec_diff(const Storage::real * vecin1, const Storage::real * vecin2, Storage::real * vecout, unsigned int size)
	{
		for(unsigned int i = 0; i < size; i++)
			vecout[i] = vecin1[i] - vecin2[i];
	}
	
	__INLINE static void vec_cross_product(const Storage::real * vecin1, const Storage::real * vecin2, Storage::real * vecout)
	{
		Storage::real temp[3];
		temp[0] = vecin1[1]*vecin2[2] - vecin1[2]*vecin2[1];
		temp[1] = vecin1[2]*vecin2[0] - vecin1[0]*vecin2[2];
		temp[2] = vecin1[0]*vecin2[1] - vecin1[1]*vecin2[0];
		vecout[0] = temp[0];
		vecout[1] = temp[1];
		vecout[2] = temp[2];
	}
	
	__INLINE static Storage::real vec_dot_product(const Storage::real * vecin1,const Storage::real * vecin2, unsigned int size)
	{
		Storage::real ret = 0;
		for(unsigned int i = 0; i < size; i++)
			ret += vecin1[i]*vecin2[i];
		return ret;
	}
	
	__INLINE static Storage::real vec_len2(const Storage::real * vecin, unsigned int size)
	{
		return vec_dot_product(vecin,vecin,size);
	}
	
	__INLINE static Storage::real vec_len(const Storage::real * vecin, unsigned int size)
	{
		return ::sqrt(vec_len2(vecin,size));
	}
	
	__INLINE static Storage::real det3d(Storage::real a, Storage::real b, Storage::real c,
	                         Storage::real d, Storage::real e, Storage::real f,
	                         Storage::real g, Storage::real h, Storage::real i ) 
	{
		return a*e*i - c*e*g + b*f*g - a*f*h + c*d*h - b*d*i;
	}
	
	__INLINE static Storage::real det3v(const Storage::real * x,const Storage::real * y,const Storage::real * z) 
	{
		return det3d(x[0], x[1], x[2],  y[0], y[1], y[2],  z[0], z[1], z[2]);
	}
	
	__INLINE static Storage::real det4v(const Storage::real * w, const Storage::real * x, const Storage::real * y, const Storage::real * z) 
	{
		return det3d(x[0]-w[0], x[1]-w[1], x[2]-w[2],  y[0]-w[0], y[1]-w[1], y[2]-w[2],  z[0]-w[0], z[1]-w[1], z[2]-w[2]);
	}	
	
	
	__INLINE static Storage::real vec_normalize(Storage::real * vecin, unsigned int size)
	{
		Storage::real len = 0;
		for(unsigned int i = 0; i < size; i++)
			len += vecin[i]*vecin[i];
		len = ::sqrt(len);
		for(unsigned int i = 0; i < size; i++)
			vecin[i] /= len;
		return len;
	}
	
	
	ElementArray<Cell> Cell::NeighbouringCells() const
	{
		ElementArray<Cell> ret(GetMeshLink());
		ElementArray<Face> faces = getFaces();
		for(ElementArray<Face>::iterator f = faces.begin(); f != faces.end(); f++)
		{
			Cell c = Neighbour(f->self());
			if( c.isValid() ) ret.push_back(c);
		}
		return ret;
	}
	
	
	Cell Cell::Neighbour(Face f) const
	{
		Cell b = f->BackCell();
		if( b == self() )
			return f->FrontCell();
		return b;
	}
	
	bool Cell::Inside(const Storage::real * point) const//check for 2d case
	{
		Mesh * mesh = GetMeshLink();
		integer dim = GetElementDimension();
		if( dim == 3 )
		{
			/*
			tiny_map<HandleType,real,16> hits;
			real ray[3];
			ray[0] = rand()/(real)RAND_MAX;
			ray[1] = rand()/(real)RAND_MAX;
			ray[2] = rand()/(real)RAND_MAX;
			CastRay(point,ray,hits);
			if( hits.size()%2 == 0 ) return false;
			return true;
			 */
			assert(mesh->GetDimensions() == 3);
			integer vp = 0;
			integer vm = 0;
			integer vz = 0;
			real eps = mesh->GetEpsilon();
			real c,d, fcnt[3];
			real_array v1,v2;
			ElementArray<Face> data = getFaces();
			Face cur = data[0];
			MarkerType mrk = mesh->CreatePrivateMarker();
			MarkerType rev = mesh->CreatePrivateMarker(); //reverse orientation
			data.SetPrivateMarker(mrk); //0-th face orientation is default
			cur->RemPrivateMarker(mrk);
			Node n1,n2; //to retrive edge
			bool reverse = false; //reverse orientation in considered face
			std::deque< orient_face > stack; //edge and first node and face for visiting
			//todo: can do faster by retriving edges and going over their nodes
			//should not use FindSharedAdjacency
			ElementArray<Edge> edges = cur->getEdges();
			do
			{
				//figure out starting node order
				if( edges[0]->getBeg() == edges[1]->getBeg() ||
				   edges[0]->getBeg() == edges[1]->getEnd() )
				{
					n1 = edges[0]->getEnd();
					n2 = edges[0]->getBeg();
				}
				else
				{
					n1 = edges[0]->getBeg();
					n2 = edges[0]->getEnd();
				}
				//schedule unvisited adjacent faces
				for(unsigned j = 0; j < edges.size(); j++)
				{
					//schedule face adjacent to considered edge
					ElementArray<Face> adjacent = edges[j]->getFaces(mrk);
					assert(adjacent.size() <= 1);
					if( !adjacent.empty() && adjacent[0].GetPrivateMarker(mrk))
					{
						adjacent.RemPrivateMarker(mrk);
						stack.push_back(orient_face(edges[j],reverse ? n2 : n1,adjacent[0]));
					}
					//update edge nodes
					n1 = n2; //current end is new begin
					//find new end
					if( n2 == edges[(j+1)%edges.size()]->getBeg() )
						n2 = edges[(j+1)%edges.size()]->getEnd();
					else
						n2 = edges[(j+1)%edges.size()]->getBeg();
				}
				if( stack.empty() ) break;
				//get entry from stack
				orient_face r = stack.front();
				//remove face from stack
				stack.pop_front();
				//retrive edges for new face
				edges = r.face->getEdges();
				reverse = false;
				//figure out starting node order
				if( edges[0]->getBeg() == edges[1]->getBeg() ||
				   edges[0]->getBeg() == edges[1]->getEnd() )
				{
					n1 = edges[0]->getEnd();
					n2 = edges[0]->getBeg();
				}
				else
				{
					n1 = edges[0]->getBeg();
					n2 = edges[0]->getEnd();
				}
				//find out common edge orientation
				for(unsigned j = 0; j < edges.size(); j++)
				{
					if( edges[j] == r.bridge ) //found the edge
					{
						//reverse ordering on this face
						if( r.first == n1 )
						{
							r.face->SetPrivateMarker(rev);
							reverse = true;
						}
						break;
					}
					//update edge nodes
					n1 = n2; //current end is new begin
					//find new end
					if( n2 == edges[(j+1)%edges.size()]->getBeg() )
						n2 = edges[(j+1)%edges.size()]->getEnd();
					else
						n2 = edges[(j+1)%edges.size()]->getBeg();
				}
			} while(true);
			data.RemPrivateMarker(mrk);
			mesh->ReleasePrivateMarker(mrk);
			for(ElementArray<Face>::size_type f = 0; f < data.size(); f++)
			{
				d = 0.0;
				data[f]->Centroid(fcnt);
				ElementArray<Node> nodes = data[f]->getNodes();
				v1 = nodes[0].Coords();
				for (ElementArray<Node>::size_type i=0; i<nodes.size(); i++)
				{
					v2 = nodes[(i+1)%nodes.size()].Coords();
					d += c = det4v(point, fcnt, v1.data(), v2.data());
					v1.swap(v2);
				}
				//if(!data[f]->FaceOrientedOutside(self()))
				if( data[f]->GetPrivateMarker(rev) )
					c = -1.0;
				else
					c = 1.0;
				if(c*d > eps)
					vp++;
				else if(c*d < -eps)
					vm++;
				else
					vz++;
			}
			data.RemPrivateMarker(rev);
			mesh->ReleasePrivateMarker(rev);
			if(vp*vm > 0) return false;
			else if( vz == 0 ) return true;
			else return true;
		}
		else
		{
			int mdim = mesh->GetDimensions();
			assert(mdim <= 3);
			Storage::real data[9][3];
			if( mdim < 3 )
			{
				memset(data,0,sizeof(Storage::real)*9*3);
			}
			Centroid(data[0]);
			ElementArray<Node> nodes = getNodes();
			for(int i = 0; i < static_cast<int>(nodes.size()); i++)
			{
				int j = (i+1)%nodes.size();
				nodes[i].Centroid(data[1]);
				nodes[j].Centroid(data[2]);
				vec_diff(point,data[0],data[3],mdim);
				vec_diff(point,data[1],data[4],mdim);
				vec_diff(point,data[2],data[5],mdim);
				vec_cross_product(data[3],data[4],data[6]);
				vec_cross_product(data[4],data[5],data[7]);
				vec_cross_product(data[5],data[3],data[8]);
				
				if( vec_dot_product(data[6],data[7],mdim) >= 0 &&
				   vec_dot_product(data[7],data[8],mdim) >= 0 &&
				   vec_dot_product(data[6],data[8],mdim) >= 0 )
					return true; //inside one of the triangles
			}
			return false;
		}
	}
	
	
	void Face::UnitNormal(real * nrm) const
	{
		Mesh * m = GetMeshLink();
		m->GetGeometricData(GetHandle(),NORMAL,nrm); 
		integer dim = m->GetDimensions();
		real    l   = ::sqrt(vec_dot_product(nrm,nrm,dim)); 
		if(::fabs(l) > m->GetEpsilon()) 
		{
			for(integer i = 0; i < dim; i++) 
				nrm[i] /= l; 
		}
	}
	
	void Face::OrientedNormal(Cell c, Storage::real * nrm) const
	{
		Normal(nrm); 
		if( !FaceOrientedOutside(c) )
		{
			integer dim = GetMeshLink()->GetDimensions();
			for(integer i = 0; i < dim; i++) 
				nrm[i] = -nrm[i];
		}
	}
	
	
	void Face::OrientedUnitNormal(Cell c, Storage::real * nrm) const
	{
		UnitNormal(nrm); 
		if( !FaceOrientedOutside(c) ) 
		{
			integer dim = GetMeshLink()->GetDimensions();
			for(integer i = 0; i < dim; i++) 
				nrm[i] = -nrm[i];
		}
	}
	
	
	
	bool Mesh::TestClosure(const HandleType * elements, integer num) const
	{
		integer i;
		tiny_map<HandleType,int,64> e_visit;
		tiny_map<HandleType,int,64>::iterator it;
		if( !HideMarker() )
		{
			for(i = 0; i < num; i++)
			{
				Element::adj_type const & lc = LowConn(elements[i]);
				for(Element::adj_type::size_type jt = 0; jt < lc.size(); jt++)
					e_visit[lc[jt]]++;
			}
		}
		else
		{
			for(i = 0; i < num; i++) if( !GetMarker(elements[i],HideMarker()) )
			{
				Element::adj_type const & lc = LowConn(elements[i]);
				for(Element::adj_type::size_type jt = 0; jt < lc.size(); jt++) 
				{
					if( !GetMarker(lc[jt],HideMarker()) ) 
						e_visit[lc[jt]]++;
				}
			}
		}
		for(it = e_visit.begin(); it != e_visit.end(); it++)
			if( it->second != 2 ) return false;
		return true;
	}
	
	Element::GeometricType Mesh::ComputeGeometricType(ElementType etype, const HandleType * lc, INMOST_DATA_ENUM_TYPE s) const
	{
		Element::GeometricType ret = Element::Unset;
		if( s == 0 && etype != NODE) return ret;
		switch(etype)
		{
			case NODE: ret = Element::Vertex; break;
			case EDGE:
				if( s == 1 )
					ret = Element::Vertex;
				else if( s == 2 )
					ret = Element::Line;
				break;
			case FACE:

				if( Element::GetGeometricDimension(GetGeometricType(lc[0])) == 0 )
				{ 
					ret = Element::Line;
				}
				else
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) || TestClosure(lc,s) )
					{
						if( s == 3 )
							ret = Element::Tri;
						else if( s == 4 )
							ret = Element::Quad;
						else
							ret = Element::Polygon;
					}
					else ret = Element::MultiLine;
				}
				break;
			case CELL:
				if(  Element::GetGeometricDimension(GetGeometricType(lc[0])) == 1 )
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) || TestClosure(lc,s) )
					{
						if( s == 3 )
							ret = Element::Tri;
						else if( s == 4 )
							ret = Element::Quad;
						else
							ret = Element::Polygon;
					}
					else ret = Element::MultiLine;
				}
				else 
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) ||  TestClosure(lc,s) )
					{
						//test c_faces closure, if no closure, set as MultiPolygon
						INMOST_DATA_ENUM_TYPE quads = 0,tris = 0,i;
						for(i = 0; i < s; i++)
						{
							if( GetGeometricType(lc[i]) == Element::Tri )
								tris++;
							else if( GetGeometricType(lc[i]) == Element::Quad )
								quads++;
						}
						if( tris == 4 && s == 4 )
							ret = Element::Tet;
						else if( quads == 6 && s == 6 )
							ret = Element::Hex;
						else if( tris == 4 && quads == 1 && s == tris+quads)
							ret = Element::Pyramid;
						else if( quads == 3 && tris == 2 && s == tris+quads)
							ret = Element::Prism;
						else
							ret = Element::Polyhedron;
					}
					else ret = Element::MultiPolygon;
				}
				break;
			case ESET: ret = Element::Set; break;
		}
		return ret;
	}

	Storage::real Edge::Length() const 
	{
		Storage::real ret; 
		GetMeshLink()->GetGeometricData(GetHandle(),MEASURE,&ret); 
		return ret;
	}

	Storage::real Face::Area() const 
	{
		real ret; 
		GetMeshLink()->GetGeometricData(GetHandle(),MEASURE,&ret); 
		return ret;
	}
	void Face::Normal(real * nrm) const 
	{
		GetMeshLink()->GetGeometricData(GetHandle(),NORMAL,nrm);
	}

	Storage::real Cell::Volume() const 
	{
		real ret; 
		GetMeshLink()->GetGeometricData(GetHandle(),MEASURE,&ret); 
		return ret;
	}

	void Element::ComputeGeometricType() const
	{
		GetMeshLink()->ComputeGeometricType(GetHandle());
	}
	
	void Mesh::ComputeGeometricType(HandleType h) 
	{
		SetGeometricType(h,Element::Unset);
		Element::adj_type const & lc = LowConn(h);
		if( !lc.empty() )
			SetGeometricType(h,ComputeGeometricType(GetHandleElementType(h),lc.data(),static_cast<integer>(lc.size())));
	}
	
	void Mesh::RecomputeGeometricData(HandleType e)
	{
		//static std::map<Element *, int> numfixes;
		GeometricData d ;
		for(d = CENTROID; d <= NORMAL; d++) // first compute centroids and normals 
		{
			if( HaveGeometricData(d,GetHandleElementType(e)) ) //compute centroid first
			{
				Tag t = GetGeometricTag(d);
				Storage::real * a = static_cast<Storage::real *>(MGetDenseLink(e,t));
				HideGeometricData(d,GetHandleElementType(e));
				GetGeometricData(e,d,a);
				ShowGeometricData(d,GetHandleElementType(e));
			}
		}


		if( GetHandleElementType(e) == CELL && HaveGeometricData(ORIENTATION,FACE)) //then correct the normal
		{
			Element::adj_type & lc = LowConn(e);
			for(Element::adj_type::iterator it = lc.begin(); it != lc.end(); ++it)
				if( !GetMarker(*it,HideMarker()) && HighConn(*it).size() == 1 )
				{
					Face(this,*it)->FixNormalOrientation();
				}
		}
		for(d = MEASURE; d <= BARYCENTER; d++) // compute the rest
		{
			if( HaveGeometricData(d,GetHandleElementType(e)) )
			{
				Tag t = GetGeometricTag(d);
				Storage::real * a = static_cast<Storage::real *>(MGetDenseLink(e,t));
				HideGeometricData(d,GetHandleElementType(e));
				GetGeometricData(e,d,a);
				ShowGeometricData(d,GetHandleElementType(e));
			}
		}
	}
	
	
	void Mesh::RemoveGeometricData(GeomParam table)
	{
		for(GeomParam::iterator it = table.begin(); it != table.end(); ++it)
		{
			if( it->first == MEASURE    ) 
			{
				if(measure_tag.isValid())    
					measure_tag    = DeleteTag(measure_tag   ,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(MEASURE,etype);
			}
			if( it->first == CENTROID   ) 
			{
				if(centroid_tag.isValid())   
					centroid_tag   = DeleteTag(centroid_tag  ,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(CENTROID,etype);
			}
			if( it->first == BARYCENTER ) 
			{
				if(barycenter_tag.isValid()) 
					barycenter_tag = DeleteTag(barycenter_tag,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(BARYCENTER,etype);
			}
			if( it->first == NORMAL     ) 
			{
				if(normal_tag.isValid())
					normal_tag     = DeleteTag(normal_tag    ,it->second);
				for(ElementType etype = FACE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(NORMAL,etype);
			}
			if( it->first == ORIENTATION) 
				if( FACE & it->second) HideGeometricData(ORIENTATION,FACE);
		}
	}
	
	void Mesh::RestoreGeometricTags()
	{
		for(GeometricData gtype = MEASURE; gtype <= NORMAL; gtype++)
		{
			bool restore = false;
			for(ElementType etype = EDGE; etype <= CELL && !restore; etype = NextElementType(etype))
				if( HaveGeometricData(gtype,etype) )
					restore = true;
			if( restore )
			{
				switch(gtype)
				{
				case MEASURE:       measure_tag = GetTag("GEOM_UTIL_MEASURE");    break;
				case CENTROID:     centroid_tag = GetTag("GEOM_UTIL_CENTROID");   break;
				case BARYCENTER: barycenter_tag = GetTag("GEOM_UTIL_BARYCENTER"); break;
				case NORMAL:         normal_tag = GetTag("GEOM_UTIL_NORMAL");     break;
				}
			}
		}
	}

	void Mesh::RepairGeometricTags()
	{
		if( HaveTag("GEOM_UTIL_MEASURE") ) 
		{
			measure_tag = GetTag("GEOM_UTIL_MEASURE");
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( measure_tag.isDefined(etype) && !HaveGeometricData(MEASURE,etype) )
					ShowGeometricData(MEASURE,etype);
		}
		if( HaveTag("GEOM_UTIL_CENTROID") )
		{
			centroid_tag = GetTag("GEOM_UTIL_CENTROID");
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( centroid_tag.isDefined(etype) && !HaveGeometricData(CENTROID,etype) )
					ShowGeometricData(CENTROID,etype);
		}
		if( HaveTag("GEOM_UTIL_BARYCENTER") )
		{
			barycenter_tag = GetTag("GEOM_UTIL_BARYCENTER");
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( barycenter_tag.isDefined(etype) && !HaveGeometricData(BARYCENTER,etype) )
					ShowGeometricData(BARYCENTER,etype);
		}
		if( HaveTag("GEOM_UTIL_NORMAL") )
		{
			normal_tag = GetTag("GEOM_UTIL_NORMAL");
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( normal_tag.isDefined(etype) && !HaveGeometricData(NORMAL,etype) )
					ShowGeometricData(NORMAL,etype);
		}
	}
	
	void Mesh::PrepareGeometricData(GeomParam table)
	{
		std::sort(&*table.begin(),&*table.end());
		for(GeomParam::iterator it = table.begin(); it != table.end(); ++it)
		{
			GeometricData types = it->first;
			ElementType mask = it->second;
			if( types == ORIENTATION )
			{
				//std::cout << "ORIENTATION" << std::endl;
				if( mask & FACE )
				{
#if defined(USE_OMP)
#pragma omp parallel for
#endif
					for(integer e = 0; e < FaceLastLocalID(); ++e) 
					{
						if( isValidElement(FACE,e) )
							Face(this,ComposeHandle(FACE,e))->FixNormalOrientation();
					}
				}
				ShowGeometricData(ORIENTATION,FACE);
			}
			if( types == MEASURE )
			{
				//std::cout << "MEASURE" << std::endl;
				for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				{
					if( (mask & etype) && !HaveGeometricData(MEASURE,etype))
					{
						measure_tag = CreateTag("GEOM_UTIL_MEASURE",DATA_REAL,etype,NONE,1);
#if defined(USE_OMP)
#pragma omp parallel for
#endif
						for(integer e = 0; e < LastLocalID(etype); ++e) if( isValidElement(etype,e) )
						{
							HandleType h = ComposeHandle(etype,e);
							GetGeometricData(h,MEASURE,static_cast<Storage::real *>(MGetDenseLink(h,measure_tag)));
						}
						ShowGeometricData(MEASURE,etype);
					}
				}
			}
			if( types == CENTROID )
			{
				//std::cout << "CENTROID" << std::endl;
				for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				{
					if( (mask & etype) && !HaveGeometricData(CENTROID,etype))
					{
						centroid_tag = CreateTag("GEOM_UTIL_CENTROID",DATA_REAL,etype,NONE,GetDimensions());
#if defined(USE_OMP)
#pragma omp parallel for
#endif
						for(integer k = 0; k < LastLocalID(etype); ++k) if( isValidElement(etype,k) )
						{
							HandleType h = ComposeHandle(etype,k);
							GetGeometricData(h,CENTROID,static_cast<Storage::real *>(MGetDenseLink(h,centroid_tag)));
						}
						ShowGeometricData(CENTROID,etype);
					}
				}
			}
			if( types == BARYCENTER )
			{
				//std::cout << "BARYCENTER" << std::endl;
				for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				{
					if( (mask & etype) && !HaveGeometricData(BARYCENTER,etype))
					{
						barycenter_tag = CreateTag("GEOM_UTIL_BARYCENTER",DATA_REAL,etype,NONE,GetDimensions());
#if defined(USE_OMP)
#pragma omp parallel for
#endif
						for(integer e = 0; e < LastLocalID(etype); ++e) if( isValidElement(etype,e) )
						{
							HandleType h = ComposeHandle(etype,e);
							GetGeometricData(h,BARYCENTER,static_cast<Storage::real *>(MGetDenseLink(h,barycenter_tag)));
						}
						ShowGeometricData(BARYCENTER,etype);
					}
				}	
			}
			if( types == NORMAL )
			{
				//std::cout << "NORMAL" << std::endl;
				for(ElementType etype = FACE; etype <= CELL; etype = NextElementType(etype))
				{
					if( (mask & etype) && !HaveGeometricData(NORMAL,etype))
					{
						normal_tag = CreateTag("GEOM_UTIL_NORMAL",DATA_REAL,etype,NONE,GetDimensions());
#if defined(USE_OMP)
#pragma omp parallel for
#endif
						for(integer e = 0; e < LastLocalID(etype); ++e) if( isValidElement(etype,e) )
						{
							HandleType h = ComposeHandle(etype,e);
							GetGeometricData(h,NORMAL,static_cast<Storage::real *>(MGetDenseLink(h,normal_tag)));
						}
						ShowGeometricData(NORMAL,etype);
					}
				}
			}
		}
	}
	
	void Mesh::GetGeometricData(HandleType e, GeometricData type, Storage::real * ret)
	{
		assert(e != InvalidHandle());
		assert(ret != NULL);
		assert(type == MEASURE || type == CENTROID || type == BARYCENTER || type == NORMAL);
		ElementType etype = GetHandleElementType(e);
		integer edim = Element::GetGeometricDimension(GetGeometricType(e));
		integer mdim = GetDimensions();
		switch(type)
		{
			case MEASURE:
			if( HaveGeometricData(MEASURE,etype) )
			{
				*ret = static_cast<Storage::real *>(MGetDenseLink(e,measure_tag))[0];
				//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
			}
			else
			{
				switch(edim)
				{
					case 0: *ret = 0; break;
					case 1: //length of edge
					{
						ElementArray<Node> nodes = Element(this,e)->getNodes();
						if( nodes.size() > 1 )
						{
							Storage::real c[3];
							vec_diff(nodes[0]->Coords().data(),nodes[1]->Coords().data(),c,mdim);
							*ret = vec_len(c,mdim);
						}
						else *ret = 0;
						//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
						break;
					}
					case 2: //area of face
					{
						ElementArray<Node> nodes = Element(this,e)->getNodes();
						if( nodes.size() > 2 )
						{
							real x[3] = {0,0,0};
							Storage::real_array x0 = nodes[0].Coords();
							for(ElementArray<Node>::size_type i = 1; i < nodes.size()-1; i++)
							{
								Storage::real_array v1 = nodes[i].Coords();
								Storage::real_array v2 = nodes[i+1].Coords();
								if( mdim == 3 )
								{
									x[0] += (v1[1]-x0[1])*(v2[2]-x0[2]) - (v1[2]-x0[2])*(v2[1]-x0[1]);
									x[1] += (v1[2]-x0[2])*(v2[0]-x0[0]) - (v1[0]-x0[0])*(v2[2]-x0[2]);
								}
								x[2] += (v1[0]-x0[0])*(v2[1]-x0[1]) - (v1[1]-x0[1])*(v2[0]-x0[0]);
							}
							*ret = ::sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2])*0.5;
						} else *ret = 0;
						//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
						break;
					}
					case 3: //volume of cell
					{
						
						Cell me = Cell(this,e);
						ElementArray<Face> faces = me->getFaces();
						*ret = 0;
						//can copy orientation-independent algorithm from
						//incident_matrix.hpp: incident_matrix::compute_measure
						//assume mdim is of size 3 at most
						dynarray<Storage::real,3> fcnt(mdim), fnrm(mdim), ccnt(mdim);
						/*
						me->Centroid(ccnt.data());
						for(ElementArray<Face>::size_type i = 0; i < faces.size(); i++)
						{
							faces[i]->Centroid(fcnt.data());
							for(int r = 0; r < mdim; ++r)
								fcnt[r] = fcnt[r]-ccnt[r];
							faces[i]->OrientedNormal(me,fnrm.data());
							*ret += vec_dot_product(fcnt.data(),fnrm.data(),mdim);
						}
						if( *ret < 0.0 ) //a robust algorithm that can handle unoriented cell
						 */
						if( !faces.empty() )
						{
							//real was = *ret/3.0;
							*ret = 0;
							Face cur = faces[0];
							Cell c1 = me;
							Mesh * mesh = c1.GetMeshLink();
							//firstly, have to figure out orientation of each face
							//mark all faces, so that we can perform adjacency retrival
							MarkerType mrk = mesh->CreatePrivateMarker();
							MarkerType rev = mesh->CreatePrivateMarker(); //reverse orientation
							faces.SetPrivateMarker(mrk); //0-th face orientation is default
							cur->RemPrivateMarker(mrk);
							Node n1,n2; //to retrive edge
							bool reverse = false; //reverse orientation in considered face
							std::deque< orient_face > stack; //edge and first node and face for visiting
							ElementArray<Edge> edges = cur->getEdges();
							do
							{
								//figure out starting node order
								if( edges[0]->getBeg() == edges[1]->getBeg() ||
								    edges[0]->getBeg() == edges[1]->getEnd() )
								{
									n1 = edges[0]->getEnd();
									n2 = edges[0]->getBeg();
								}
								else
								{
									n1 = edges[0]->getBeg();
									n2 = edges[0]->getEnd();
								}
								//schedule unvisited adjacent faces
								for(unsigned j = 0; j < edges.size(); j++)
								{
									//schedule face adjacent to considered edge
									ElementArray<Face> adjacent = edges[j]->getFaces(mrk);
									assert(adjacent.size() <= 1);
									if( !adjacent.empty() )
									{
										adjacent.RemPrivateMarker(mrk);
										stack.push_back(orient_face(edges[j],reverse ? n2 : n1,adjacent[0]));
									}
									//update edge nodes
									n1 = n2; //current end is new begin
									//find new end
									if( n2 == edges[(j+1)%edges.size()]->getBeg() )
										n2 = edges[(j+1)%edges.size()]->getEnd();
									else
										n2 = edges[(j+1)%edges.size()]->getBeg();
								}
								if( stack.empty() ) break;
								//get entry from stack
								orient_face r = stack.front();
								//remove face from stack
								stack.pop_front();
								//retrive edges for new face
								edges = r.face->getEdges();
								reverse = false;
								//figure out starting node order
								if( edges[0]->getBeg() == edges[1]->getBeg() ||
								   edges[0]->getBeg() == edges[1]->getEnd() )
								{
									n1 = edges[0]->getEnd();
									n2 = edges[0]->getBeg();
								}
								else
								{
									n1 = edges[0]->getBeg();
									n2 = edges[0]->getEnd();
								}
								//find out common edge orientation
								for(unsigned j = 0; j < edges.size(); j++)
								{
									if( edges[j] == r.bridge ) //found the edge
									{
										//reverse ordering on this face
										if( r.first == n1 )
										{
											r.face->SetPrivateMarker(rev);
											reverse = true;
										}
										break;
									}
									//update edge nodes
									n1 = n2; //current end is new begin
									//find new end
									if( n2 == edges[(j+1)%edges.size()]->getBeg() )
										n2 = edges[(j+1)%edges.size()]->getEnd();
									else
										n2 = edges[(j+1)%edges.size()]->getBeg();
								}
							} while(true);
							faces.RemPrivateMarker(mrk);
							mesh->ReleasePrivateMarker(mrk);
							c1->Centroid(ccnt.data());
							for(unsigned j = 0; j < faces.size(); j++)
							{
								//compute normal to face
								faces[j].Centroid(fcnt.data());
								faces[j].Normal(fnrm.data());
								for(int r = 0; r < mdim; ++r)
									fcnt[r] = fcnt[r]-ccnt[r];
								*ret += (faces[j]->GetPrivateMarker(rev) ? -1.0 : 1.0)*vec_dot_product(fcnt.data(),fnrm.data(),3);
							}
							*ret = fabs(*ret);
							faces.RemPrivateMarker(rev);
							mesh->ReleasePrivateMarker(rev);
							
							//std::cout << "volume is " << *ret/3.0 << " was " << was << " for " << me->LocalID() << std::endl;
						}
						*ret /= 3.0;
						
						break;
					}
				}
			}
			//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
			break;
			case CENTROID:
			if(etype == NODE )
				memcpy(ret,MGetDenseLink(e,CoordsTag()),sizeof(real)*mdim);
			else if(HaveGeometricData(CENTROID,etype))
			{
				memcpy(ret,MGetDenseLink(e,centroid_tag),sizeof(real)*mdim);
			}
			else
			{
				ElementArray<Node> nodes = Element(this,e)->getNodes();
				memset(ret,0,sizeof(real)*mdim);
				if(nodes.size() != 0)
				{
					Storage::real div = 1.0/nodes.size();
					for(ElementArray<Node>::size_type i = 0; i < nodes.size(); i++)
					{
						Storage::real_array c =nodes[i].Coords();
						for(integer j = 0; j < mdim; j++) ret[j] += c[j];
					}
					for(integer j = 0; j < mdim; j++) ret[j] *= div;
				}
			}
			break;
			case BARYCENTER:
			if( etype == NODE )
				memcpy(ret,MGetDenseLink(e,CoordsTag()),sizeof(real)*mdim);
			else if(HaveGeometricData(BARYCENTER,etype))
				memcpy(ret,MGetDenseLink(e,barycenter_tag),sizeof(real)*mdim);
			else
			{
				memset(ret,0,sizeof(real)*mdim);
				if( edim == 1 )
				{
					ElementArray<Node> n = Element(this,e)->getNodes();
					if( n.size() == 2 )
					{
						Storage::real_array a = n[0].Coords();
						Storage::real_array b = n[1].Coords();
						for(integer j = 0; j < dim; j++) 
							ret[j] = (a[j] + b[j])*0.5;
					}
					else if( n.size() == 1 )
					{
						Storage::real_array a = n[0].Coords();
						for(integer j = 0; j < dim; j++) ret[j] = a[j];
					}
				}
				else if( edim == 2 )
				{
					ElementArray<Node> nodes = Element(this,e)->getNodes();
					real s,d, x1[3] = {0,0,0},x2[3] = {0,0,0},x[3] = {0,0,0};
					//here we compute area of polygon
					//~ if( HaveGeometricData(MEASURE,etype) && HaveGeometricData(NORMAL,etype) )
					//~ {
						//~ s = e->RealDF(measure_tag);
						//~ memcpy(x,&e->RealDF(normal_tag),sizeof(Storage::real)*mdim);
					//~ }
					//~ else
					//~ {
					if( nodes.size() > 2 )
					{
						Storage::real_array x0 = nodes[0].Coords();
						for(ElementArray<Node>::size_type i = 1; i < nodes.size()-1; i++)
						{
							Storage::real_array v1 = nodes[i].Coords();
							Storage::real_array v2 = nodes[i+1].Coords();
							if( mdim == 3 )
							{
								x[0] += (v1[1]-x0[1])*(v2[2]-x0[2]) - (v1[2]-x0[2])*(v2[1]-x0[1]);
								x[1] += (v1[2]-x0[2])*(v2[0]-x0[0]) - (v1[0]-x0[0])*(v2[2]-x0[2]);
							}
							x[2] += (v1[0]-x0[0])*(v2[1]-x0[1]) - (v1[1]-x0[1])*(v2[0]-x0[0]);
						}
						s = ::sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
						x[0] /= s; x[1] /= s; x[2] /= s; //here we obtain the unit normal
						//~ }
						//here we compute the center
						Storage::real_array v0 = nodes[0].Coords();
						for(ElementArray<Node>::size_type j = 1; j < nodes.size()-1; j++)
						{
							Storage::real_array v1 = nodes[j].Coords();
							Storage::real_array v2 = nodes[j+1].Coords();
							for(integer k = 0; k < mdim; k++)
							{
								x1[k] = v0[k] - v1[k];
								x2[k] = v0[k] - v2[k];
							}
							d = det3v(x1,x2,x); //here we use unit normal
							for(integer k = 0; k < mdim; k++) 
								ret[k] += d*(v0[k]+v1[k]+v2[k]);
						}
						for(integer k = 0; k < mdim; k++) ret[k] /= 3.0 * s;
					}
					else
					{
						for(ElementArray<Node>::size_type i = 0; i < nodes.size(); i++)
						{
							Storage::real_array c = nodes[i].Coords();
							for(integer k = 0; k < mdim; k++) ret[k] += c[k];
						}
						for(integer k = 0; k < mdim; k++) ret[k] /= static_cast<Storage::real>(nodes.size());
					}
				}
				else if( edim == 3 )
				{
					ElementArray<Face> faces = Element(this,e)->getFaces();
					real d,c,vol = 0, y[3];
					for(ElementArray<Face>::size_type i = 0; i < faces.size(); i++)
					{
						d = y[0] = y[1] = y[2] = 0;
						ElementArray<Node> nodes = faces[i].getNodes();
						Storage::real_array v0 = nodes[0].Coords();
						for(ElementArray<Node>::size_type j = 1; j < nodes.size()-1; j++)
						{
							Storage::real_array v1 = nodes[j].Coords();
							Storage::real_array v2 = nodes[j+1].Coords();
							c = det3v(v0.data(),v1.data(),v2.data());
							d += c;
							y[0] += c * (v0[0] + v1[0] + v2[0]);
							y[1] += c * (v0[1] + v1[1] + v2[1]);
							y[2] += c * (v0[2] + v1[2] + v2[2]);
						}
						c = faces[i].FaceOrientedOutside(Cell(this,e)) ? 1 : -1;
						ret[0] += c * y[0];
						ret[1] += c * y[1];
						ret[2] += c * y[2];
						vol += c*d;
					}
					ret[0] /= vol*4;
					ret[1] /= vol*4;
					ret[2] /= vol*4;
				}
			}
			break;
			case NORMAL:
			if( HaveGeometricData(NORMAL,etype) )
				memcpy(ret,MGetDenseLink(e,normal_tag),sizeof(real)*mdim);
			else
			{
				memset(ret,0,sizeof(real)*mdim);
				if( edim == 2 )//&& mdim == 3)
				{
					ElementArray<Node> nodes = Element(this,e)->getNodes();
					
					Storage::real_array x0 = nodes[0].Coords(), a = x0, b;
					for(ElementArray<Node>::size_type i = 0; i < nodes.size(); i++)
					{
						b = nodes[(i+1)%nodes.size()].Coords();
						ret[0] += (a[1]-x0[1])*(b[2]-x0[2]) - (a[2]-x0[2])*(b[1]-x0[1]);
						ret[1] += (a[2]-x0[2])*(b[0]-x0[0]) - (a[0]-x0[0])*(b[2]-x0[2]);
						ret[2] += (a[0]-x0[0])*(b[1]-x0[1]) - (a[1]-x0[1])*(b[0]-x0[0]);
						a.swap(b);
					}
					/*
					for(unsigned i = 0; i < nodes.size(); i++)
					{
						Storage::real_array a = nodes[i].Coords();
						Storage::real_array b = nodes[(i+1)%nodes.size()].Coords();
						ret[0] += a[1]*b[2] - a[2]*b[1];
						ret[1] += a[2]*b[0] - a[0]*b[2];
						ret[2] += a[0]*b[1] - a[1]*b[0];
					}
					*/
					ret[0] *= 0.5;
					ret[1] *= 0.5;
					ret[2] *= 0.5;
				}
				else if( edim == 1 )//&& mdim == 2 )
				{
					ElementArray<Node> nodes = Element(this,e)->getNodes();
					if( nodes.size() > 1 )
					{
						Storage::real_array a = nodes[0].Coords();
						Storage::real_array b = nodes[1].Coords();
						ret[0] = b[1] - a[1];
						ret[1] = a[0] - b[0];
						Storage::real l = ::sqrt(ret[0]*ret[0]+ret[1]*ret[1]);
						if( l )
						{
							ret[0] /= l;
							ret[1] /= l;
						}
						l = ::sqrt((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1]));
						ret[0] *= l;
						ret[1] *= l;
					}
				}
			}
			break;
		}
		//~ if( type == MEASURE )
		//~ {
			//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
		//~ }
	}
	


	bool Element::Planarity() const
	{
		Mesh * m = GetMeshLink();
		integer dim = m->GetDimensions();
		if( dim < 3 ) return true;
		ElementArray<Node> p = getNodes();
		if( p.size() <= 3 ) return true;
		ElementArray<Node>::size_type i, s = p.size();
		Storage::real v[2][3] = {{0,0,0},{0,0,0}};
		vec_diff(p[1].Coords().data(),p[0].Coords().data(),v[0],3);
		vec_diff(p[2].Coords().data(),p[0].Coords().data(),v[1],3);
		vec_cross_product(v[0],v[1],v[1]);
		for(i = 3; i < s; i++)
		{
			vec_diff(p[i].Coords().data(),p[0].Coords().data(),v[0],3);
			if( ::fabs(vec_dot_product(v[0],v[1],3)) > m->GetEpsilon() ) return false;
		}
		return true;
	}


	bool Cell::Closure() const
	{
		adj_type & lc = GetMeshLink()->LowConn(GetHandle());
		return lc.size() > 0 ? GetMeshLink()->TestClosure(lc.data(),static_cast<integer>(lc.size())) : false;
	}

	bool Face::Closure() const
	{
		adj_type & lc = GetMeshLink()->LowConn(GetHandle());
		return lc.size() > 0 ? GetMeshLink()->TestClosure(lc.data(),static_cast<integer>(lc.size())) : false;
	}



	

	

	bool Element::Boundary() const
	{
		switch(GetElementType())
		{
			case FACE:
				if( !getAsFace()->FrontCell().isValid() )
					return true;
				return false;
			case CELL:
			case EDGE:
			case NODE:
			{
				ElementArray<Element> faces = getAdjElements(FACE);
				for(ElementArray<Element>::iterator it = faces.begin(); it != faces.end(); it++)
					if( it->Boundary() ) return true;
				return false;
			}
			default: return false;
		}
		return false;
	}
	
	
	
	std::ostream & spaces(std::ostream & out, int print)
	{
		while(print) {out << " "; print--;}
		return out;
	}
	
	
	bool Face::CheckNormalOrientation() const
	{
		Mesh * mesh = GetMeshLink();
        //integer dim = mesh->GetDimensions();
		Cell c1 = BackCell();
		if( c1.isValid() )
		{
			Storage::real measure = 0;
			ElementArray<Face> data = c1.getFaces();
			Face cur = *this;
			//firstly, have to figure out orientation of each face
			//mark all faces, so that we can perform adjacency retrival
			MarkerType mrk = mesh->CreatePrivateMarker();
			MarkerType rev = mesh->CreatePrivateMarker(); //reverse orientation
			data.SetPrivateMarker(mrk); //0-th face orientation is default
			cur->RemPrivateMarker(mrk);
			Node n1,n2; //to retrive edge
			bool reverse = false; //reverse orientation in considered face
			std::deque< orient_face > stack; //edge and first node and face for visiting
			ElementArray<Edge> edges = cur->getEdges();
			do
			{
				//figure out starting node order
				if( edges[0]->getBeg() == edges[1]->getBeg() ||
				    edges[0]->getBeg() == edges[1]->getEnd() )
				{
					n1 = edges[0]->getEnd();
					n2 = edges[0]->getBeg();
				}
				else
				{
					n1 = edges[0]->getBeg();
					n2 = edges[0]->getEnd();
				}
				//schedule unvisited adjacent faces
				for(unsigned j = 0; j < edges.size(); j++)
				{
					//schedule face adjacent to considered edge
					ElementArray<Face> adjacent = edges[j]->getFaces(mrk);
					assert(adjacent.size() <= 1);
					if( !adjacent.empty() )
					{
						adjacent.RemPrivateMarker(mrk);
						stack.push_back(orient_face(edges[j],reverse ? n2 : n1,adjacent[0]));
					}
					//update edge nodes
					n1 = n2; //current end is new begin
					//find new end
					if( n2 == edges[(j+1)%edges.size()]->getBeg() )
						n2 = edges[(j+1)%edges.size()]->getEnd();
					else
						n2 = edges[(j+1)%edges.size()]->getBeg();
				}
				if( stack.empty() ) break;
				//get entry from stack
				orient_face r = stack.front();
				//remove face from stack
				stack.pop_front();
				//retrive edges for new face
				edges = r.face->getEdges();
				reverse = false;
				//figure out starting node order
				if( edges[0]->getBeg() == edges[1]->getBeg() ||
				   edges[0]->getBeg() == edges[1]->getEnd() )
				{
					n1 = edges[0]->getEnd();
					n2 = edges[0]->getBeg();
				}
				else
				{
					n1 = edges[0]->getBeg();
					n2 = edges[0]->getEnd();
				}
				//find out common edge orientation
				for(unsigned j = 0; j < edges.size(); j++)
				{
					if( edges[j] == r.bridge ) //found the edge
					{
						//reverse ordering on this face
						if( r.first == n1 )
						{
							r.face->SetPrivateMarker(rev);
							reverse = true;
						}
						break;
					}
					//update edge nodes
					n1 = n2; //current end is new begin
					//find new end
					if( n2 == edges[(j+1)%edges.size()]->getBeg() )
						n2 = edges[(j+1)%edges.size()]->getEnd();
					else
						n2 = edges[(j+1)%edges.size()]->getBeg();
				}
			} while(true);
			data.RemPrivateMarker(mrk);
			mesh->ReleasePrivateMarker(mrk);
			Storage::real nrm[3], cnt[3], ccnt[3];
			c1->Centroid(ccnt);
			for(unsigned j = 0; j < data.size(); j++)
			{
				//compute normal to face
				data[j].Centroid(cnt);
				data[j].Normal(nrm);
				for(int r = 0; r < 3; ++r)
					cnt[r] = cnt[r]-ccnt[r];
				measure += (data[j]->GetPrivateMarker(rev) ? -1.0 : 1.0)*vec_dot_product(cnt,nrm,3);
			}
            //bool have_rev = cur->GetPrivateMarker(rev);
			data.RemPrivateMarker(rev);
			mesh->ReleasePrivateMarker(rev);
			if( (measure < 0 ))// && !have_rev) || (measure > 0 && have_rev))
				return false;
		}
		return true;
	}
	
	bool Face::FixNormalOrientation() const
	{
		if( !CheckNormalOrientation() )
		{
			if( FrontCell().isValid() )
				SwapCells();
			else
			{
				ReorderEdges(); //this is not thread-safe with respect to CheckNormalOrientation
				if( GetMeshLink()->HaveGeometricData(NORMAL,FACE) )
				{
					real_array nrm = GetMeshLink()->RealArrayDF(GetHandle(),GetMeshLink()->GetGeometricTag(NORMAL));
					for(real_array::size_type it = 0; it < nrm.size(); ++it)
						nrm[it] = -nrm[it];
				}
			}
			return true;
		}
		return false;
	}

	Storage::real meantri(Storage::real * v0, Storage::real * v1, Storage::real * v2, Storage::integer dim, Storage::real (*func)(Storage::real* x,Storage::real), Storage::real time)
	{
		Storage::real value = 0;
		static const Storage::real w[4] =   { -0.149570044467670, 0.175615257433204, 0.053347235608839 , 0.077113760890257};
		static const Storage::real a[4][3] =
		{
			{0.333333333333333,0.333333333333333,0.333333333333333},
			{0.479308067841923,0.260345966079038,0.260345966079038},
			{0.869739794195568,0.065130102902216,0.065130102902216},
			{0.638444188569809,0.312865496004875,0.048690315425316}
		};
		Storage::real XYG[13][3];
		for (Storage::integer i = 0 ; i < dim; i++)
			XYG[0][i] = 0.33333333333333333333*(v0[i]+v1[i]+v2[i]);
		 value += w[0] * func(XYG[0],time);
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (Storage::integer j = 0 ; j < dim; j++)
				XYG[1+i][j] = v0[j] + (v1[j] - v0[j]) * a[1][i] + (v2[j] - v0[j])*a[1][(i+1)%3];
			value += w[1] * func(XYG[1+i],time);
		}
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (Storage::integer j = 0 ; j < dim; j++)
				XYG[4+i][j] = v0[j] + (v1[j] - v0[j]) * a[2][i] + (v2[j] - v0[j])*a[2][(i+1)%3];
			value += w[2] * func(XYG[4+i],time);
		}
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (Storage::integer j = 0 ; j < dim; j++)