geometry.cpp

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

const std::string normal_name = "PROTECTED_GEOM_UTIL_NORMAL";
const std::string measure_name = "PROTECTED_GEOM_UTIL_MEASURE";
const std::string centroid_name = "PROTECTED_GEOM_UTIL_CENTROID";
const std::string barycenter_name = "PROTECTED_GEOM_UTIL_BARYCENTER";

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_diff(const Storage::real_array & vecin1,const Storage::real_array & 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;
		int dmax = -1, dmin = 4;
		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:
				for (INMOST_DATA_ENUM_TYPE k = 0; k < s; ++k)
				{
					int d = Element::GetGeometricDimension(GetGeometricType(lc[k]));
					if (dmax < d) dmax = d;
					if (dmin > d) dmin = d;
				}
				if (dmax != dmin)
				{
					ret = Element::MultiLine;
				}
				else if( dmax == 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:
				for (INMOST_DATA_ENUM_TYPE k = 0; k < s; ++k)
				{
					int d = Element::GetGeometricDimension(GetGeometricType(lc[k]));
					if (dmax < d) dmax = d;
					if (dmin > d) dmin = d;
				}
				if (dmax != dmin)
				{
					ret = Element::MultiPolygon;
				}
				else if(  dmax == 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(measure_name);    break;
				case CENTROID:     centroid_tag = GetTag(centroid_name);   break;
				case BARYCENTER: barycenter_tag = GetTag(barycenter_name); break;
				case NORMAL:         normal_tag = GetTag(normal_name);     break;
				}
			}
		}
	}

	void Mesh::RepairGeometricTags()
	{
		if( HaveTag(measure_name) )
		{
			measure_tag = GetTag(measure_name);
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( measure_tag.isDefined(etype) && !HaveGeometricData(MEASURE,etype) )
					ShowGeometricData(MEASURE,etype);
		}
		if( HaveTag(centroid_name) )
		{
			centroid_tag = GetTag(centroid_name);
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( centroid_tag.isDefined(etype) && !HaveGeometricData(CENTROID,etype) )
					ShowGeometricData(CENTROID,etype);
		}
		if( HaveTag(barycenter_name) )
		{
			barycenter_tag = GetTag(barycenter_name);
			for(ElementType etype = EDGE; etype <= CELL; etype = NextElementType(etype))
				if( barycenter_tag.isDefined(etype) && !HaveGeometricData(BARYCENTER,etype) )
					ShowGeometricData(BARYCENTER,etype);
		}
		if( HaveTag(normal_name) )
		{
			normal_tag = GetTag(normal_name);
			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(measure_name,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(centroid_name,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(barycenter_name,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(normal_name,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::FacesOrientation(ElementArray<Face> & faces, MarkerType rev)
	{
		//can copy orientation-independent algorithm from
		//incident_matrix.hpp: incident_matrix::compute_measure
		//assume mdim is of size 3 at most
		if( !faces.empty() )
		{
			//real was = *ret/3.0;
			Face cur = faces[0];
			Mesh * mesh = faces.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 )
						{
							if( isPrivate(rev) )
								r.face->SetPrivateMarker(rev);
							else
								r.face->SetMarker(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);
			//faces.RemPrivateMarker(rev);
		}
	}
	
	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 )
						{
							real c[3] = {0,0,0};
							real_array v0 = nodes[0].Coords();
							real_array v1 = nodes[1].Coords();
							vec_diff(v0.data(),v1.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 )
						{
							*ret = 0;
							real nt[3] = {0,0,0}, l1[3] = {0,0,0}, l2[3] = {0,0,0},n0[3] = {0,0,0}, ss;
							real_array v0 = nodes[0].Coords();
							real_array v1 = nodes[1].Coords();
							real_array v2 = nodes[2].Coords();
							vec_diff(v1,v0,l1,mdim);
							vec_diff(v2,v0,l2,mdim);
							vec_cross_product(l1,l2,n0);
							for(int i = 1; i < (int)nodes.size()-1; i++)
							{
								v1 = nodes[i].Coords();
								v2 = nodes[i+1].Coords();
								vec_diff(v1,v0,l1,mdim);
								vec_diff(v2,v0,l2,mdim);
								vec_cross_product(l1,l2,nt);
								ss = vec_dot_product(n0,nt,3);
								ss /= fabs(ss);
								*ret += sqrt(vec_dot_product(nt,nt,3))*0.5*ss;
							}
							*ret = fabs(*ret);
						} 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();
						bool ornt = true;//!HaveGeometricData(ORIENTATION,FACE);
						MarkerType rev = 0;
						if( ornt )
						{
							rev = CreatePrivateMarker();
							FacesOrientation(faces,rev);
						}
						real vol = 0, a, at;
						real x[3] = {0,0,0}, n[3] = {0,0,0}, n0[3] = {0,0,0}, s, ss;
						real l1[3] = {0,0,0}, l2[3] = {0,0,0};
						real nt[3] = {0,0,0}, cx[3] = {0,0,0};
						me.Centroid(cx);
						for(unsigned j = 0; j < faces.size(); j++)
						{
							//compute normal to face
							ElementArray<Node> nodes = faces[j].getNodes();
							if( ornt )
								s = faces[j].GetPrivateMarker(rev) ? -1.0 : 1.0;
							else
								s = faces[j].FaceOrientedOutside(me) ? 1.0 : -1.0;
							x[0] = x[1] = x[2] = 0;
							n[0] = n[1] = n[2] = 0;
							a = 0;
							real_array v0 = nodes[0].Coords();
							real_array v1 = nodes[1].Coords();
							real_array v2 = nodes[2].Coords();
							vec_diff(v1,v0,l1,mdim);
							vec_diff(v2,v0,l2,mdim);
							vec_cross_product(l1,l2,n0);
							for(int k = 1; k < (int)nodes.size()-1; k++)
							{
								v1 = nodes[k+0].Coords();
								v2 = nodes[k+1].Coords();
								vec_diff(v1,v0,l1,mdim);
								vec_diff(v2,v0,l2,mdim);
								vec_cross_product(l1,l2,nt);
								for(int q = 0; q < 3; ++q) nt[q] *= 0.5;
								ss = vec_dot_product(n0,nt,3);
								ss /= fabs(ss);
								at = sqrt(vec_dot_product(nt,nt,3))*ss;
								//same as faces[j].Normal(n)
								for(int q = 0; q < 3; ++q)
									n[q] += nt[q];
								//same as faces[j].Centroid(x)
								for(int q = 0; q < 3; ++q)
									x[q] += at*((v0[q]-cx[q])+(v1[q]-cx[q])+(v2[q]-cx[q]))/3.0;
								a += at;
							}
							for(int q = 0; q < 3; ++q) x[q] /= a;
							vol += s*vec_dot_product(x,n,3);
						}
						if( ornt )
						{
							if( vol < 0.0 ) vol = -vol;
							faces.RemPrivateMarker(rev);
							ReleasePrivateMarker(rev);
						}
						*ret = vol/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);
					for(unsigned k = 0; k < nodes.size(); ++k)
					{
						for(int q = 0; q < mdim; ++q)
							ret[q] += nodes[k].Coords()[q];
					}
					for(int q = 0; q < mdim; ++q)
						ret[q] /= (real)nodes.size();
				}
			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 )
					{
						real_array v0 = n[0].Coords();
						real_array v1 = n[1].Coords();
						for(integer j = 0; j < dim; j++)
							ret[j] = (v0[j] + v1[j])*0.5;
					}
					else if( n.size() == 1 )
					{
						real_array v0 = n[0].Coords();
						for(integer j = 0; j < dim; j++) ret[j] = v0[j];
					}
				}
				else if( edim == 2 )
				{
					ElementArray<Node> nodes = Element(this,e)->getNodes();
					if( nodes.size() > 2 )
					{
						*ret = 0;
						real nt[3] = {0,0,0}, l1[3] = {0,0,0}, l2[3] = {0,0,0};
						real c[3] = {0,0,0}, n0[3] = {0,0,0}, ss, cx[3] = {0,0,0};
						real_array v0 = nodes[0].Coords();
						real_array v1 = nodes[1].Coords();
						real_array v2 = nodes[2].Coords();
						vec_diff(v1,v0,l1,mdim);
						vec_diff(v2,v0,l2,mdim);
						vec_cross_product(l1,l2,n0);
						real a = 0, at;
						Element(this,e).Centroid(cx);
						for(int i = 1; i < (int)nodes.size()-1; i++)
						{
							real_array v1 = nodes[i].Coords();
							real_array v2 = nodes[i+1].Coords();
							vec_diff(v1,v0,l1,mdim);
							vec_diff(v2,v0,l2,mdim);
							vec_cross_product(l1,l2,nt);
							ss = vec_dot_product(n0,nt,3);
							ss /= fabs(ss);
							at = sqrt(vec_dot_product(nt,nt,3))*0.5*ss;
							for(int q = 0; q < mdim; ++q)
								c[q] += at*((v0[q]-cx[q])+(v1[q]-cx[q])+(v2[q]-cx[q]))/3.0;
							a += at;
						}
						for(int q = 0; q < mdim; ++q) ret[q] = c[q]/a+cx[q];
					}
					//std::cout << ret[0] << " " << ret[1] << " " << ret[2] << std::endl;
				}
				else if( edim == 3 )
				{
					Cell me = Cell(this,e);
					ElementArray<Face> faces = me->getFaces();
					bool ornt = true;//!HaveGeometricData(ORIENTATION,FACE);
					MarkerType rev = 0;
					if( ornt )
					{
						rev = CreatePrivateMarker();
						FacesOrientation(faces,rev);
					}
					real vol = 0, a, at;
					real x[3] = {0,0,0}, nt[3] = {0,0,0}, s;
					real c[3] = {0,0,0}, n[3] = {0,0,0};
					real n0[3] = {0,0,0}, ss;
					real l1[3] = {0,0,0}, l2[3] = {0,0,0};
					real cx[3] = {0,0,0};
					me.Centroid(cx);
					for(unsigned j = 0; j < faces.size(); j++)
					{
						//compute normal to face
						ElementArray<Node> nodes = faces[j].getNodes();
						if( ornt )
							s = faces[j].GetPrivateMarker(rev) ? -1.0 : 1.0;
						else
							s = faces[j].FaceOrientedOutside(me) ? 1.0 : -1.0;
						real_array v0 = nodes[0].Coords();
						real_array v1 = nodes[1].Coords();
						real_array v2 = nodes[2].Coords();
						vec_diff(v1,v0,l1,mdim);
						vec_diff(v2,v0,l2,mdim);
						vec_cross_product(l1,l2,n0);
						x[0] = x[1] = x[2] = 0;
						n[0] = n[1] = n[2] = 0;
						a = 0;
						for(int i = 1; i < (int)nodes.size()-1; i++)
						{
							real_array v1 = nodes[i].Coords();
							real_array v2 = nodes[i+1].Coords();
							vec_diff(v1,v0,l1,mdim);
							vec_diff(v2,v0,l2,mdim);
							vec_cross_product(l1,l2,nt);
							for(int q = 0; q < 3; ++q)
								nt[q] *= 0.5;
							ss = vec_dot_product(n0,nt,3);
							ss /= fabs(ss);
							at = sqrt(vec_dot_product(nt,nt,3))*ss;
							//same as faces[j].Normal(n)
							for(int q = 0; q < 3; ++q)
								n[q] += nt[q];
							//same as faces[j].Centroid(x)
							for(int q = 0; q < 3; ++q)
								x[q] += at*((v0[q]-cx[q])+(v1[q]-cx[q])+(v2[q]-cx[q]))/3.0;
							a += at;
							//second-order midpoint formula
							for(int q = 0; q < 3; ++q)
								c[q] += s*nt[q]*(pow((v0[q]-cx[q])+(v1[q]-cx[q]),2)+pow((v0[q]-cx[q])+(v2[q]-cx[q]),2)+pow((v1[q]-cx[q])+(v2[q]-cx[q]),2))/24.0;
						}
						for(int q = 0; q < 3; ++q) x[q] = x[q]/a;
						vol += s*vec_dot_product(x,n,3);
					}
					if( ornt )
					{
						if( vol < 0.0 )
						{
							vol = -vol;
							for(int q = 0; q < 3; ++q)
								c[q] = -c[q];
						}
						faces.RemPrivateMarker(rev);
						ReleasePrivateMarker(rev);
					}
					vol /= 3.0;
					for(int q = 0; q < mdim; ++q)
						ret[q] = c[q]/(vol) + cx[q];
					//std::cout << ret[0] << " " << ret[1] << " " << ret[2] << std::endl;
				}
			}
			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();
						real n[3] = {0,0,0}, l1[3] = {0,0,0}, l2[3] = {0,0,0};
						real nt[3] = {0,0,0};
						real_array v0 = nodes[0].Coords();
						for(int i = 1; i < (int)nodes.size()-1; i++)
						{
							real_array v1 = nodes[i].Coords();
							real_array v2 = nodes[i+1].Coords();
							vec_diff(v1,v0,l1,mdim);
							vec_diff(v2,v0,l2,mdim);
							vec_cross_product(l1,l2,nt);
							for(int q = 0; q < 3; ++q)
								n[q] += nt[q]*0.5;
						}
						for(int q = 0; q < mdim; ++q)
							ret[q] = n[q];
					}
					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
			{