/* This file is part of the source code for the following publication:
 * Assembling Self-Supporting Structures, Deuss et al., SIGGRAPH Asia 2014
 *
 * Copyright (C) 2014 Mario Deuss <mario.deuss@epfl.ch>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */

#include "Assembly.h"
#include <set>

#include "AABB.h"

#include <map>
#include "PolygonPartition.h"
#include "Utils.h"

namespace SelfAssembly {

I::	I(unsigned int _b0, unsigned int _b1, Assembly& _a ){
	selected_=false;
	scale_=1.;

	assert(_b0!=_b1);
	bI_[0] = std::min(_b0,_b1);
	bI_[1] = std::max(_b0,_b1);
	
	findFaces( _a);
	computeBasis( _a );
	computeInterface( _a);
	useMultiplicities(false);
}

I::I(unsigned int _b0, unsigned int _b1, int _f0, int _f1, Assembly & _a , 
	 std::vector<unsigned int> _multiplicities){
	selected_=false;
	scale_=1.;

	assert(_b0!=_b1);
	bI_[0] = std::min(_b0,_b1);
	bI_[1] = std::max(_b0,_b1);

	bool invert=(_b0>_b1);
	fI_[0] = invert?_f1:_f0;
	fI_[1] = invert?_f0:_f1;
	
	computeBasis( _a );
	computeInterface( _a );
	
	multiplicities_=_multiplicities;
	useMultiplicities(false);
}

bool I::findFaces(Assembly& _a, bool debug/*=false*/ )
{
	const B& b0 = _a.b(bI_[0]);
	const B& b1 = _a.b(bI_[1]);
	
	std::vector<std::pair<int,int> > fP;
	
	//for each face f0 in the b0
	for( unsigned int i0=0;i0<b0.nFi();++i0){
		const F& f0=_a.f(b0.fi(i0));
		
		//Check if any face f1 in b1 matches
		for( unsigned int i1=0;i1<b1.nFi();++i1){
			const F& f1=_a.f(b1.fi(i1));
			
			//A match needs to have same nV
			if(f0.nVi() != f1.nVi()){ continue; }
			
			//Check if each vertex in f0 matches f1
			bool matchF=true;
			for(unsigned int fi0=0;fi0<f0.nVi();++fi0){
				const V& v0=_a.v(f0.vi(fi0));
				
				//Check if v0 matches f1
				bool matchV=false;
				for(unsigned int fi1=0;fi1<f0.nVi();++fi1){
					const V& v1=_a.v(f1.vi(fi1));
					
					//Check if v0 matches v1
					if((v1-v0).norm() < 0.0000001){
						matchV=true;
						break; //we found a match
					}
				}
				
				if(!matchV){
					matchF=false;
					break; //one of the vertices in v0 does not match f1
				}
			}
			
			if(matchF){
				fP.push_back( std::pair<int,int>(b0.fi(i0),b1.fi(i1)));
			}
		}
	}
	
	if(fP.empty()){
		MYERR << __FUNCTION__ << ": Could not find matching faces." << std::endl;
		return false;
	}else{
		if(fP.size()>1){
			if(debug) { MYOUT << __FUNCTION__ << ": Multiple matching faces found:" << std::endl; }
		}
		
		//Compute multiplicity: Put all vertex indices of the first block into a map
		std::map<unsigned int,unsigned int> m;
		for( unsigned int i=0;i<fP.size();++i ){
			const F& fi=_a.f(fP.at(i).first);
			for( unsigned int vit=0; vit<fi.nVi();++vit ){
				int vi=fi.vi(vit);
				if(m.find(vi)==m.end()){
					m[vi]=1;
				}else{
					m[vi]++;
				}
			}
		}
		
		//Add faces and multiplicities
		for( unsigned int i=0;i<fP.size();++i){
			const F& fi=_a.f(fP.at(i).first);
			std::vector<unsigned int> mult;
			//MYOUT << "\n f" << fP.at(i).first << ": ";
			for( unsigned int vit=0; vit<fi.nVi();++vit ){
				int vi=fi.vi(vit);
				assert(m.find(vi)!=m.end());
				mult.push_back( m[vi]);
				//MYOUT << ", m[" << vi << "]=" << m[vi];
			}
			
			if(i>0){
				_a.addI(bI_[0],bI_[1],fP[i].first,fP[i].second,mult);
			}else{
				fI_[0]=fP[0].first;
				fI_[1]=fP[0].second;
				multiplicities_=mult;
			}
			if(debug){ MYOUT << __FUNCTION__ << ":     face: " << fP[i].first << ", " << fP[i].second << std::endl; }
		}
	}
	
	return true;
}

bool I::computeBasis( Assembly& _a )
{
	_a.f(fI_[0]).lsqPlane(_a,basis_,mean_);
	
	//orient basis such that the normal (basis.col(2)) points away from b0_
	const B& b0=_a.b(bI_[0]);
	const B& b1=_a.b(bI_[1]);
	if(basis_.col(2).dot(b0.centroid()-mean_) > 0){
		basis_ = -basis_;
		if(basis_.col(2).dot(b1.centroid()-mean_) <= 0){
			MYERR << __FUNCTION__ << ": Basis normal is pointing away from b[" << bI_[1] << "], to b[" << bI_[0] << "] instead. This should not be, debug it!" <<std::endl;
		}
	}
	
	return true;
}

void I::computeInterface( Assembly& _a )
{
	//compute intersection polygon in fitted plane to determine vertices
	
	//For now we assume faces of adjencent blocks perfectly match,
	//so we only need to project the vertices of one face onto the basis
	
	//Put all vertices of the first face into a matrix
	const F& f=_a.f(fI_[0]);
	Matrix3Xd input(3,f.nVi());
	for(unsigned int i=0;i<f.nVi();++i){
		input.col(i) = _a.v(f.vi(i));
	}
	
	//Project
	for(unsigned int i = 0; i < input.cols(); ++i) input.col(i) -= mean_;
	input = basis_.transpose()*input;
	input.row(2) = Eigen::VectorXd::Constant(input.cols(), 0.0);
	input = basis_*input;
	for(unsigned int i = 0; i < input.cols(); ++i) input.col(i) += mean_;
	
	//Store in this
	for(unsigned int i=0;i<input.cols();++i){
		vertices_.push_back(input.col(i));
	}
	
	//Initialize multiplicities
	multiplicities_.resize(vertices_.size(),-1);
}

bool I::enabled(const Assembly &_a) const
{
	return ( !fixed(_a) && _a.b(bI_[0]).enabled() && _a.b(bI_[1]).enabled() );
}

bool I::fixed(const Assembly &_a) const
{
	return  _a.b(bI_[0]).fixed() && _a.b(bI_[1]).fixed();
}

int Assembly::addF(const F &_f)
{
	//for polygons, check if planar
	if(_f.nVi()>3){
		V e0 = v(_f.vi(0))-v(_f.vi(1));
		V e1 = v(_f.vi(1))-v(_f.vi(2));
		V n = e0.cross(e1);
		
		for(unsigned int i=2; i<_f.nVi();++i){
			V ei= v(_f.vi(i))-v(_f.vi((i%(_f.nVi()))) );
			if( std::fabs( n.dot(ei)) > 0.000001 ){
				MYERR << __FUNCTION__ << ": Non-planar face added to assembly. Not supported and results might be wrong." << std::endl;
			}
		}
	}
	
	fs_.push_back(_f);
	return fs_.size()-1;
}

unsigned int Assembly::nBFree() const
{
	unsigned int n=0;
	for(unsigned int bi=0;bi<nB();++bi){
		if(!(b(bi).fixed())){ ++n; }
	}
	return n;
}

void Assembly::enableBlock(Idx bi)
{
	if(b(bi).enabled()){
		return;
	}else{
		bs_.at(bi).setEnabled(true);
	}
}

void Assembly::disableBlock(Idx bi)
{
	if(b(bi).enabled()){
		bs_.at(bi).setEnabled(false);
	}else{
		return;
	}
}

void Assembly::addNeighbors(Idx bi, Assembly::Neighbors &n, unsigned int nrings ) const
{
	n.insert(bi);
	for(unsigned int i=0; i<nrings; ++i){
		Neighbors n0=n;
		for(auto it=n0.begin();it!=n0.end();++it){
			const B& bl=b(*it);
			for(unsigned int iii=0;iii!=bl.nIi();++iii){
				const I& ifs = is_.at(bl.ii(iii));
				if(static_cast<int>(ifs.bi(0))!=bi){
					n.insert(ifs.bi(0));
				}else{
					n.insert(ifs.bi(1));
				}
			}
		}
	}
	n.erase(bi);
}

bool Assembly::areNeighbors(Idx bi, Idx bj) const
{
	const B& bl=b(bi);
	for(unsigned int iii=0;iii!=bl.nIi();++iii){
		const I& ifs = is_.at(bl.ii(iii));
		bool b01 = (static_cast<int>(ifs.bi(0))==bi && static_cast<int>(ifs.bi(1))==bj);
		bool b10 = (static_cast<int>(ifs.bi(1))==bi && static_cast<int>(ifs.bi(0))==bj);
		if( b01 || b10 ){
			return true;
		}
	}
	return false;
}

void Assembly::clearInterfaces()
{
	//remove interface indices stored in blocks
	for(unsigned int bi=0;bi<nB();++bi ){
		bs_.at(bi).clearInterfaces();
	}
	
	//remove interfaces
	is_.clear();
}

void Assembly::addI(unsigned int _b0, unsigned int _b1)
{
	I ifs(_b0,_b1,*this);
	if(existsI(ifs.bi(0),ifs.bi(1),ifs.fi(0),ifs.fi(1))){
		return;
	}
	bs_.at(_b0).addIi(is_.size());
	bs_.at(_b1).addIi(is_.size());
	is_.push_back(ifs);
}

void Assembly::addI(unsigned int _b0, unsigned int _b1, int _f0, int _f1, std::vector<unsigned int> _multiplicities)
{
	if(existsI(_b0,_b1,_f0,_f1)){
		return;
	}
	I ifs(_b0,_b1,_f0,_f1,*this,_multiplicities);
	bs_.at(_b0).addIi(is_.size());
	bs_.at(_b1).addIi(is_.size());
	is_.push_back(ifs);
}

bool Assembly::existsI( unsigned int _b0, unsigned int _b1, int _f0, int _f1 ){
	const B& bl = b(_b0);
	for(unsigned int iii=0;iii<bl.nIi();++iii){
		const I& ifs= i(bl.ii(iii));
		bool sameb = ( (ifs.bi(0) == _b0 && ifs.bi(1) == _b1) || (ifs.bi(0) == _b1 && ifs.bi(1) == _b0) );
		bool samef = ( (ifs.fi(0) == _f0 && ifs.fi(1) == _f1) || (ifs.fi(0) == _f1 && ifs.fi(1) == _f0) );
		if( sameb && samef ){
			return true;
		}
	}
	return false;
}

void Assembly::enableBlocks( bool disable/*=false*/, bool nonFixedOnly/*=false*/ )
{
	for(unsigned int i=0;i<nB();++i){
		if(!b(i).fixed() || !nonFixedOnly){
			if(!disable){
				enableBlock(i);
			}else{
				disableBlock(i);
			}
		}
	}
}

void Assembly::finalizeBlocks()
{
	for(unsigned int bi=0;bi<nB();++bi){
		B& b=bs_[bi];
		for(unsigned int fii=0;fii<b.nFi();++fii){
			bool s=fs_.at(b.fi(fii)).triangulate(*this);
			if(!s){ std::cerr << __FUNCTION__ << "triangulation failed in b[" << bi <<"]." <<std::endl; }
		}
	}
	computeCentroidsAndVolumes();
	computeValidChains();
	finalizedBlocks_=true;
}

void Assembly::finalizeInterfaces()
{
	recomputeTotalVertices();
	//diabled for now, needs debugging:
	//computeAssemblyTopology();
	finalizedInterfaces_=true;
}

void Assembly::volumeStatistics( Statistics& s, bool nonFixedOnly/*=false*/ ){
	for(unsigned int bi=0;bi<nB();++bi){
		if(nonFixedOnly && b(bi).fixed()){ continue; }
		s.add(b(bi).volume());
	}
}

void Assembly::setHooksAboveCentroids()
{
	for(unsigned int bi=0;bi<nB();++bi){
		B& b= bs_[bi];
		V h=b.centroid();
		h[2] = b.hook()[2];
		b.setHook(h,-1,-1);
	}
}

bool Assembly::setHookByRay(Idx bi, const V &o, const V &dir, const std::vector<int> &faces)
{
	B& bl=bs_.at(bi);
	//intersect ray with all faces
	for(unsigned int fii=0;fii<faces.size();++fii){
		int fi =bl.fi(faces.at(fii));
		F& f=fs_.at(fi);
		if(!f.triangulate(*this)){
			MYERR << __FUNCTION__ << ": Polypartition of face " << faces.at(fii) << " in block " << bi << " failed." << std::endl;
		}
		for(unsigned int ti=0;ti!=f.nT();++ti){
			const T& t=f.t(ti);
			V bc;
			const V& v0=v(t(0));
			const V& v1=v(t(1));
			const V& v2=v(t(2));
			if( Utils::intersects(v0,v1,v2,o,dir,&bc)){
				V h = bc(0)*v0 + bc(1)*v1 + bc(2)*v2;
				bs_.at(bi).setHook(h,fi,ti);
				return true;
			}else if( Utils::intersects(v0,v1,v2,o,-dir,&bc)){
				V h = bc(0)*v0 + bc(1)*v1 + bc(2)*v2;
				bs_.at(bi).setHook(h,fi,ti);
				return true;
			}
		}
	}
	
	return false;
}

void Assembly::clearClusters()
{
	for(unsigned int bi=0;bi<nB();++bi){
		bs_.at(bi).clearClusters();
		bs_.at(bi).addCluster(-1);
	}
	maxCluster_=-1;
}

void Assembly::addCluster(Idx bi, int cl)
{
	bs_.at(bi).addCluster(cl);
}

void Assembly::mergeClusters(Idx bi)
{
	B& bl=bs_.at(bi);
	if(bl.nClusters()==1 && bl.cluster()==-1){
		++maxCluster_;
		bl.clearClusters();
		bl.addCluster(maxCluster_);
		return;
	}
	//merge into one cluster
	bl.removeCluster(-1); //if not it gets removed everywhere :-(
	B::Clusters cl(bl.clustersBegin(),bl.clustersEnd());
	int c = *std::max_element(cl.begin(),cl.end()); //max avoids having -1. -1 only would be covered in the case above
	for(unsigned int bj=0;bj<nB();++bj){
		for( auto it = cl.begin(); it!=cl.end(); ++it ){
			if( b(bj).hasCluster(*it) ){
				bs_.at(bj).removeCluster(*it);
				bs_.at(bj).addCluster(c);
			}
		}
	}
}

void Assembly::computeCentroidsAndVolumes()
{	
	for( unsigned int bIt=0;bIt<nB();++bIt){
		V ctr=V::Zero();
		double vol=0.0;
		computeVolumeAndCentroid(bIt,vol,ctr);
		bs_.at(bIt).setCentroid(ctr);
		bs_.at(bIt).setVolume(std::fabs(vol));
	}
}

void Assembly::computeVolumeAndCentroid( Idx bi, double& vol, V& ctr){
	const B& bl=b(bi);
	
	//using en.wikipedia.org/wiki/Centroid
	ctr=V::Zero();
	vol=0.0;
	
	for( unsigned int fIt=0;fIt<bl.nFi();++fIt){
		const F& f=this->f(bl.fi(fIt));
		
		for( unsigned int t=0;t<f.nT();++t){
			const T& ti=f.t(t);
			
			//triangulate polygon by connecting each edge with the first vertex
			const V& a=v(ti(0));
			const V& b=v(ti(1));
			const V& c=v(ti(2));
			
			//The signed volume of the tetrahedron (with (0,0,0)) is given by a*(bxc)/6.
			double vi = a.dot(b.cross(c))/6.;
			vol += vi;
			
			//The centroid of the tetrahedron (by connecting (0,0,0) with the triangle)
			//is given by the average.
			V ci= (a+b+c)/(4.);
			ctr += ci*vi;
		}
	}
	
	if(vol!=0.0){
		ctr /= vol;
	}
}

void Assembly::setInterfaceScales(double s)
{
	for(unsigned int ii=0;ii<nI();++ii){
		is_.at(ii).setScale(s);
	}
}

double Assembly::computeRegistrationSpheres( double radius, bool doTxt)
{
	double minR= std::numeric_limits<double>::max();
	for(unsigned int ii=0;ii<nI();++ii){
		I& ifs=is_.at(ii);
		ifs.clearRegistrationSpheres();
		ifs.clearFrames();

		//work on face of interface in first block
		F& f = fs_.at(i(ii).fi(0));
		if(f.nT()==0){ f.triangulate(*this); }
		for(unsigned int ti=0;ti<f.nT();++ti){

			const T& t = f.t(ti);
			V v0 = v(t(0)); V v1 = v(t(1)); V v2 = v(t(2));
			const F::B basis = f.basis(ti,*this);

			std::vector<V> vs(12);
			vs.at(0) = v0; vs.at(1) = 0.5*(v0+v1); vs.at(2) = 0.5*(v0+v2);
			vs.at(3) = v1; vs.at(4) = 0.5*(v1+v2); vs.at(5) = 0.5*(v1+v0);
			vs.at(6) = v2; vs.at(7) = 0.5*(v2+v1); vs.at(8) = 0.5*(v2+v0);
			vs.at(9) = 0.5*(v0+v1); vs.at(10) = 0.5*(v1+v2); vs.at(11) = 0.5*(v0+v2);

			if(ti==1 && doTxt){
				vs.clear();
				vs.push_back(v0); vs.push_back(v1); vs.push_back(v2);
			}
			
			//registration spheres for subtriangles:
			for(int sti=0;sti<static_cast<int>(vs.size());sti += 3){
				V a = vs.at(sti+0);
				V b = vs.at(sti+1);
				V c = vs.at(sti+2);
				V ci=Utils::incircleCenter(a,b,c);
				double r=Utils::incircleRadius(a,b,c);
				double sRad = radius;
				if(sRad>r){
					sRad=r;
					MYOUT << "Registration sphere radius: " << sRad <<std::endl;
				}
				if(sRad<minR){ minR=sRad;}
				V offset = basis*(Utils::sampleCircle());
				if(ti==1 && doTxt){
					ifs.addFrame(ci,r*basis);
				}else{
					ifs.addRegistrationSphere(ci + offset*(r-sRad),sRad);
				}
			}
		}
	}
	return minR;
}

unsigned int Assembly::computeValidChains()
{
	std::vector<int> nl3(0);
	unsigned int n=0;
	for(unsigned int bi=0;bi<nB();++bi){
		unsigned int nb = addChains(bi);
		if(nb<3 && !b(bi).fixed() ){ nl3.push_back(bi); }
		n += nb;
	}
	return n;
}

unsigned int Assembly::addChains(unsigned int bi){
	unsigned int n=0;
	for( unsigned int ai=0;ai<nA();++ai){
		int ci=addChain(bi,ai);
		if(ci>-1){ ++n; }
	}
	return n;
}

int Assembly::addChain(unsigned int bi, unsigned int ai ){
	
	const B& bl=b(bi);
	if(bl.fixed() || !bl.hasHook()){ return -1;}
	
	const V& h=bl.hook();
	const V& anc=a(ai);
	V d = anc-h;
	double l=d.norm();
	d = d.normalized();
	if(!intersects(h,d,1e-6,l)){
		C _c(bi,ai);
		bs_.at(bi).addCi(cs_.size());
		cs_.push_back(_c);
		return bs_.size()-1;
	}else{
		return -1;
	}
	
}



bool Assembly::intersects(const V &o, const V &dir, double tMin /*=10e-6*/, double tMax /*= std::numeric_limits<double>::max()*/, V* bc/*=NULL*/)
{
	for(unsigned int bi=0;bi<nB();++bi){
		const B& bl=b(bi);
		for(unsigned int fi=0;fi<bl.nFi();++fi){
			const F& fa=f(bl.fi(fi));
			for(unsigned int ti=0;ti<fa.nT();++ti){
				const T& t=fa.t(ti);
				bool in=Utils::intersects(v(t(0)),v(t(1)),v(t(2)),o,dir,bc,tMin,tMax);
				if(in){ return true;}
			}
		}
	}
	return false;
}


void Assembly::recomputeTotalVertices()
{
	totalVertices_.resize(nI());
	unsigned int t=0;
	for(unsigned int ii=0;ii<nI();++ii){
		totalVertices_.at(ii)=t;
		t += i(ii).nV();
	}
	totalVertices_.push_back(t);
}

bool F::triangulate( const Assembly& a )
{
	bool success=true;
	ts_.clear();
	unsigned int n= nVi();
	if(n>3){

		//Put all vertices of face into a matrix
		std::vector<int> vis;
		Eigen::Matrix3Xd f(3,n);
		for(unsigned int i=0;i<n;++i){
			f.col(i) = a.v(vi(i));
			vis.push_back(vi(i));
		}

		std::vector<std::vector<int> > newF;
		int ret=PolygonPartition::partition(1,f,newF);
		if(ret==1){
			for(unsigned int i=0; i<newF.size();++i){
				assert(newF.at(i).size()==3);
				T t; t << vis.at(newF[i][0]) , vis.at(newF[i][1]) , vis.at(newF[i][2]);
				ts_.push_back(t);
			}
		}else{
			success =false;
			//triangulate by connecting points sequentially:
			for(unsigned int i=1;i<n-1;++i){
				T t; t << vis.at(0), vis.at(i), vis.at(i+1);
				ts_.push_back(t);
			}
		}

	}else{
		T t; t << vi(0) , vi(1) , vi(2);
		ts_.push_back(t);
	}
	assert(ts_.size()==(n-2));
	return success;
}

void F::vertices(const Assembly &a, Eigen::Matrix3Xd &verts) const
{
	//Put all vertices of the interface into a matrix
	verts = Eigen::Matrix3Xd(3,nVi());
	for(unsigned int i=0;i<nVi();++i){	verts.col(i) = a.v(vi(i)); }
}

void F::lsqPlane( const Assembly& a,Eigen::Matrix3d &basis, V &mean ) const
{
	//Put all vertices of the interface into a matrix
	Eigen::Matrix3Xd input; vertices(a,input);
	
	//fit a plane to vertices of both faces of the interface
	mean = input.rowwise().mean();
	for(unsigned int i = 0; i < input.cols(); ++i) input.col(i) -= mean;
	Eigen::JacobiSVD<Eigen::Matrix3Xd > jSVD;
	jSVD.compute(input, Eigen::ComputeFullU);
	basis = jSVD.matrixU();
	
	//Make sure last singular value is smallest
	assert( jSVD.singularValues()(0)>=jSVD.singularValues()(2) && jSVD.singularValues()(1)>=jSVD.singularValues()(2) );
	if(jSVD.singularValues()(2) > 0.000001 ){
//		MYERR << __FUNCTION__ << ": Non-planar interface detected. Result will be incorrect. 3rd singular value=" << jSVD.singularValues()(2) <<std::endl;
	}
	
	//normalize basis vectors
	for(int d=0;d<3;++d){
		basis.col(d).normalize();
	}
}

V F::cog(const Assembly &a) const
{
	//Put all vertices of the interface into a matrix
	Eigen::Matrix3Xd input(3,nVi());
	for(unsigned int i=0;i<nVi();++i){	input.col(i) = a.v(vi(i)); }
	
	//Project to 2d, store in basis coordinates, loc
	Eigen::Matrix3Xd output(3,nVi());
	Eigen::Matrix2Xd loc(2,nVi());
	output = input;
	Eigen::Vector3d mean = output.rowwise().mean();
	for(unsigned int i = 0; i < output.cols(); ++i) output.col(i) -= mean;
	Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::HouseholderQRPreconditioner> jSVD;
	jSVD.compute(output, Eigen::ComputeFullU);
	Eigen::MatrixXd basis = jSVD.matrixU();
	output = basis.transpose()*output;
	loc = output.block(0,0,2,nVi());
	output.row(2) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Constant(output.cols(), 0.0);
	output = basis*output;
	for(unsigned int i = 0; i < output.cols(); ++i) output.col(i) += mean;
	
	//determine cog of projected polygon
	// following: http://paulbourke.net/geometry/polygonmesh/
	double area=0.;
	Eigen::Vector2d cog=Eigen::Vector2d::Zero();
	for( unsigned int c=0;c<output.cols();++c){
		if(c>0 && c<output.cols()-1){
			Eigen::Vector2d p0=loc.col(0);
			Eigen::Vector2d e1=loc.col(c)-p0;
			Eigen::Vector2d e2=loc.col(c+1)-p0;
			double ai= 0.5*(e1(0)*e2(1)-e1(1)*e2(0));
			area += ai;
			Eigen::Vector2d cogi = (e1+e2+3.*p0)/3;
			cog += ai*cogi;
		}
	}
	if(std::fabs(area)>1e-10){
		cog /= area;
	}
	Eigen::Vector3d cog3; cog3 << cog, 0.;
	cog3 = basis*cog3 + mean;
	return cog3;
}

F::B F::basis(int ti, const Assembly& a) const
{
	const T& tr=ts_.at(ti);
	V v0=a.v((tr(0)));
	V v1=a.v((tr(1)));
	V v2=a.v((tr(2)));
	V e0 = v1-v0;
	e0 = e0/e0.norm();
	V e1 = v2-v0;
	e1 = e1 - e0*(e1.dot(e0));
	e1 = e1/e1.norm();
	
	B b;
	b.col(0) = e0;
	b.col(1) = e1;
	b.col(2) = e0.cross(e1);
	
	return b;
}

bool C::enabled(const Assembly &a) const
{
	return (a.b(bI_).enabled() && enabled_);
}

void C::setEnabled(bool e)
{
	enabled_=e;
}

double Assembly::aabbRadius() const{

	AABB aabb;
	for(Idx i=0; i<nV(); ++i){
		aabb.extend(v(i));
	}
	return aabb.getRadius();
}

}
