// ImplicitFunction.cpp: ImplicitFunction クラスのインプリメンテーション
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MeshEditor.h"
#include "ImplicitFunction.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

#define PI 3.14159265f

//////////////////////////////////////////////////////////////////////
// 構築/消滅
//////////////////////////////////////////////////////////////////////

ImplicitFunction::ImplicitFunction()
{

}

ImplicitFunction::~ImplicitFunction()
{

}

void ImplicitFunction::EvaluateGradient(float x[], float g[])
{	
	/*
	float R = 3;
	float r = 1;

	float P[3];
	double l = sqrt(x[0]*x[0] + x[1]*x[1]);
	P[0] = x[0] - R*x[0]/l;
	P[1] = x[1] - R*x[1]/l;
	P[2] = x[2];

	l = MeshData::LENGTH(P);

	g[0] = -P[0]/l;
	g[1] = -P[1]/l;
	g[2] = -P[2]/l;

	return;
	*/

	/*
	double l = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
	g[0] = -x[0]/l;
	g[1] = -x[1]/l;
	g[2] = -x[2]/l;

	return;
	*/

	float p1[3], p2[3];

	p1[0] = x[0] - h; 
	p1[1] = x[1];
	p1[2] = x[2];
	
	p2[0] = x[0] + h; 
	p2[1] = x[1];
	p2[2] = x[2];

	g[0] = (EvaluateFunction(p2) - EvaluateFunction(p1))/(2.0f*h);

	p1[0] = x[0]; 
	p1[1] = x[1] - h;
	p1[2] = x[2];
	
	p2[0] = x[0]; 
	p2[1] = x[1] + h;
	p2[2] = x[2];

	g[1] = (EvaluateFunction(p2) - EvaluateFunction(p1))/(2.0f*h);

	p1[0] = x[0]; 
	p1[1] = x[1];
	p1[2] = x[2] - h;
	
	p2[0] = x[0]; 
	p2[1] = x[1];
	p2[2] = x[2] + h;

	g[2] = (EvaluateFunction(p2) - EvaluateFunction(p1))/(2.0f*h);
}

float ImplicitFunction::EvaluateFunction(float x[])
{
	/*
	float R = 3;
	float r = 1;
	float c[3] = {0,0,0};

	return hfTorusZ(x, c, R, r);
	*/
	
	/*
	float P[3];
	double l = sqrt(x[0]*x[0] + x[1]*x[1]);
	P[0] = x[0] - R*x[0]/l;
	P[1] = x[1] - R*x[1]/l;
	P[2] = x[2];

	return r - MeshData::LENGTH(P);
	*/

	//return 1.0 - x[0]*x[0]/100 - x[1]*x[1]/100 - x[2]*x[2]/100;

	//return 10.0 - sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);

	//#include "models/5triangle.txt";
	#include "models/syuriken.txt"
	//#include "2way_twist.txt"
	//#include "models/model.txt"
	//#include "star.txt"
	//#include "models/block.txt"
	//#include "models/sphere_plane.txt"
	//#include "twist.txt"
	//#include "doraemonH.txt"
	//#include "sphere_tori.txt"
	//#include "cube_sphere.txt"
}

float ImplicitFunction::R_int(float f1, float f2)
{
	//return min(f1,f2);
	return f1+f2-sqrt(f1*f1+f2*f2);
}

float ImplicitFunction::R_uni(float f1, float f2)
{
	//return max(f1,f2);
	return f1+f2+sqrt(f1*f1+f2*f2);
}

float ImplicitFunction::hfEllipsoid(float x[], float cen[], float a, float b, float c)
{
	return 1.0 - ((x[0]-cen[0])/a)*((x[0]-cen[0])/a) 
		- ((x[1]-cen[1])/b)*((x[1]-cen[1])/b)
		- ((x[2]-cen[2])/c)*((x[2]-cen[2])/c);
}

float ImplicitFunction::hfEllCylZ(float x[], float cen[], float a, float b)
{
	return 1 - ((x[0]-cen[0])/a)*((x[0]-cen[0])/a) 
		- ((x[1]-cen[1])/b)*((x[1]-cen[1])/b);
}

float ImplicitFunction::hfTorusX(float x[], float cen[], float R, float r)
{
	return r*r-(x[0]-cen[0])*(x[0]-cen[0])
		-(x[1]-cen[1])*(x[1]-cen[1])
		-(x[2]-cen[2])*(x[2]-cen[2]) 
		-R*R + 2*R*sqrt((x[1]-cen[1])*(x[1]-cen[1])+(x[2]-cen[2])*(x[2]-cen[2]));
}

float ImplicitFunction::hfTorusY(float x[], float cen[], float R, float r)
{
	return r*r-(x[0]-cen[0])*(x[0]-cen[0])
		-(x[1]-cen[1])*(x[1]-cen[1])
		-(x[2]-cen[2])*(x[2]-cen[2]) 
		-R*R + 2*R*sqrt((x[2]-cen[2])*(x[2]-cen[2])+(x[0]-cen[0])*(x[0]-cen[0]));
}

float ImplicitFunction::hfTorusZ(float x[], float cen[], float R, float r)
{
	return r*r-(x[0]-cen[0])*(x[0]-cen[0])
		-(x[1]-cen[1])*(x[1]-cen[1])
		-(x[2]-cen[2])*(x[2]-cen[2]) 
		-R*R + 2*R*sqrt((x[0]-cen[0])*(x[0]-cen[0])+(x[1]-cen[1])*(x[1]-cen[1]));
}

float ImplicitFunction::hfSphere(float x[], float cen[], float r)
{
	return r*r - (x[0]-cen[0])*(x[0]-cen[0])
		- (x[1]-cen[1])*(x[1]-cen[1]) 
		- (x[2]-cen[2])*(x[2]-cen[2]);
}

//Meno: Prof. Pasko's handout pp.8 in "Skelton Model"
inline float ImplicitFunction::distanceToSegment(float A[], float B[], float P[])
{
	float AC[3];

	double AB2 = (B[0]-A[0])*(B[0]-A[0]) + (B[1]-A[1])*(B[1]-A[1]) + (B[2]-A[2])*(B[2]-A[2]);
	double dot = (P[0]-A[0])*(B[0]-A[0]) + (P[1]-A[1])*(B[1]-A[1]) + (P[2]-A[2])*(B[2]-A[2]);
	
	AC[0] = (float)(dot*(B[0]-A[0])/AB2);
	AC[1] = (float)(dot*(B[1]-A[1])/AB2);
	AC[2] = (float)(dot*(B[2]-A[2])/AB2);

	double dot2 = AC[0]*(B[0]-A[0]) + AC[1]*(B[1]-A[1]) + AC[2]*(B[2]-A[2]);
	if(dot2 < 0)
		return (A[0]-P[0])*(A[0]-P[0]) + (A[1]-P[1])*(A[1]-P[1]) + (A[2]-P[2])*(A[2]-P[2]);
	else if(AB2 < AC[0]*AC[0]+AC[1]*AC[1]+AC[2]*AC[2])
		return (B[0]-P[0])*(B[0]-P[0]) + (B[1]-P[1])*(B[1]-P[1]) + (B[2]-P[2])*(B[2]-P[2]);
	else
		return (AC[0]+A[0]-P[0])*(AC[0]+A[0]-P[0]) + 
				(AC[1]+A[1]-P[1])*(AC[1]+A[1]-P[1]) + 
					(AC[2]+A[2]-P[2])*(AC[2]+A[2]-P[2]);
}

inline float ImplicitFunction::metaball(float r, float d)
{
	if(r <= d/3.0f)
		return (1.0f-3.0f*r*r/(d*d));
	else if(r <= d)
		return 1.5f*(1.0f-r/d)*(1.0f-r/d);
	else 
		return 0;
}

void ImplicitFunction::EvaluateDD(float x[], float dd[][3])
{
	float p[3];
	p[0] = x[0] + h;
	p[1] = x[1];
	p[2] = x[2];
	float xf[3];
	EvaluateGradient(p, xf);

	p[0] = x[0] - h;
	float xb[3];
	EvaluateGradient(p, xb);

	p[0] = x[0];
	p[1] = x[1] + h;
	float yf[3];
	EvaluateGradient(p, yf);

	p[1] = x[1] - h;
	float yb[3];
	EvaluateGradient(p, yb);

	p[1] = x[1];
	p[2] = x[2] + h;
	float zf[3];
	EvaluateGradient(p, zf);

	p[2] = x[2] - h;
	float zb[3];
	EvaluateGradient(p, zb);

	dd[0][0] = (xf[0] - xb[0])/(2.0f*h);
	dd[1][0] = dd[0][1] = (xf[1] - xb[1])/(2.0f*h);
	dd[2][0] = dd[0][2] = (xf[2] - xb[2])/(2.0f*h);
	dd[1][1] = (yf[1] - yb[1])/(2.0f*h);
	dd[1][2] = dd[2][1] = (yf[2] - yb[2])/(2.0f*h);
	dd[2][2] = (zf[2] - zf[2])/(2.0f*h);
}

float ImplicitFunction::model(float x[])
{
	return 0;
}

void ImplicitFunction::nabla(float x[], float g[])
{	
	float p1[3], p2[3];
	float H = h ;//sqrt(h);

	p1[0] = x[0] - H; 
	p1[1] = x[1];
	p1[2] = x[2];
	
	p2[0] = x[0] + H; 
	p2[1] = x[1];
	p2[2] = x[2];

	g[0] = (model(p2) - model(p1))/(2.0f*H);

	p1[0] = x[0]; 
	p1[1] = x[1] - H;
	p1[2] = x[2];
	
	p2[0] = x[0]; 
	p2[1] = x[1] + H;
	p2[2] = x[2];

	g[1] = (model(p2) - model(p1))/(2.0f*H);

	p1[0] = x[0]; 
	p1[1] = x[1];
	p1[2] = x[2] - H;
	
	p2[0] = x[0]; 
	p2[1] = x[1];
	p2[2] = x[2] + H;

	g[2] = (model(p2) - model(p1))/(2.0f*H);
}

void ImplicitFunction::doubleNabla(float x[], float dd[][3])
{
	float p[3];
	p[0] = x[0] + h;
	p[1] = x[1];
	p[2] = x[2];
	float xf[3];
	nabla(p, xf);

	p[0] = x[0] - h;
	float xb[3];
	nabla(p, xb);

	p[0] = x[0];
	p[1] = x[1] + h;
	float yf[3];
	nabla(p, yf);

	p[1] = x[1] - h;
	float yb[3];
	nabla(p, yb);

	p[1] = x[1];
	p[2] = x[2] + h;
	float zf[3];
	nabla(p, zf);

	p[2] = x[2] - h;
	float zb[3];
	nabla(p, zb);

	dd[0][0] = (xf[0] - xb[0])/(2.0f*h);
	dd[1][0] = dd[0][1] = (xf[1] - xb[1])/(2.0f*h);
	dd[2][0] = dd[0][2] = (xf[2] - xb[2])/(2.0f*h);
	dd[1][1] = (yf[1] - yb[1])/(2.0f*h);
	dd[1][2] = dd[2][1] = (yf[2] - yb[2])/(2.0f*h);
	dd[2][2] = (zf[2] - zf[2])/(2.0f*h);
}

float ImplicitFunction::EvaluateVertexError(MeshData *mesh, double &max)
{
	float err = 0;
	int vertex_N = mesh->vertex_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	float total_area = 0;
	max = -1;
	
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	float (*vertex)[3] = mesh->vertex;

	for(i=0; i<vertex_N; i++){
		if(mesh->isBound[i])
			continue;
		int *nei, nei_N;
		float area = 0;
		mesh->getConsistent1Ring(i, nei, nei_N);
		for(int j=0; j<nei_N; j++)
			area += MeshData::AREA(vertex[i], vertex[nei[j]], vertex[nei[(j+1)%nei_N]]);
		float f = this->EvaluateFunction(vertex[i]);
		float g[3];
		this->EvaluateGradient(vertex[i], g);
		double len = g[0]*g[0] + g[1]*g[1] + g[2]*g[2];
		if(len == 0)
			continue;
		err += area*f*f/len;
		total_area += area;
		
		double e = fabs(f)/sqrt(len);
		if(max < e)
			max = e;
	}

	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	return sqrt(err/(3.0*total_area));
}

float ImplicitFunction::EvaluateNormalError(MeshData *mesh, double &max)
{
	float err = 0;
	int vertex_N = mesh->vertex_N;
	int face_N = mesh->face_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	int (*face)[3] = mesh->face;
	BOOL *isBound = mesh->isBound;
	float total_area = 0;
	max = -1;

	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	float (*vertex)[3] = mesh->vertex;

	for(i=0; i<face_N; i++){
		int* fi = face[i];
		//if(isBound[fi[0]] || isBound[fi[1]] || isBound[fi[2]])
			//continue;

		/*
		float c[3];
		c[0] = (vertex[fi[0]][0] + vertex[fi[1]][0] + vertex[fi[2]][0])/3.0f;
		c[1] = (vertex[fi[0]][1] + vertex[fi[1]][1] + vertex[fi[2]][1])/3.0f;
		c[2] = (vertex[fi[0]][2] + vertex[fi[1]][2] + vertex[fi[2]][2])/3.0f;
		float g[3];
		this->EvaluateGradient(c, g);
		float len1 = MeshData::LENGTH(g);
		*/
		float *g = mesh->normal_f[i];

		float v1[3], v2[3], n[3];
		MeshData::VEC(v1, vertex[fi[0]], vertex[fi[1]]);
		MeshData::VEC(v2, vertex[fi[0]], vertex[fi[2]]);
		MeshData::CROSS(n, v1, v2);
		double len2 = MeshData::LENGTH(n);
		
		if((float)len2 == 0)
			continue;

		float dot = MeshData::DOT(g,n)/len2;

		err += 2.0*len2*(1.0f+dot);
		total_area += len2;

		float nm[3];
		nm[0] = g[0] + n[0]/len2;
		nm[1] = g[1] + n[1]/len2;
		nm[2] = g[2] + n[2]/len2;
		double e = MeshData::LENGTH(nm);
		if(max < e)
			max = e;
	}

	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	return sqrt(err/total_area);
}

void ImplicitFunction::setNormalAsGrd(MeshData* mesh)
{
	int vertex_N = mesh->vertex_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();

	int face_N = mesh->face_N;
	float (*normal)[3] = mesh->normal_f;
	for(i=0; i<face_N; i++){
		float c[3];
		mesh->faceCenter(c, i);
		this->EvaluateGradient(c, normal[i]);
		double len = MeshData::LENGTH(normal[i]);
		if((float)len != 0){
			normal[i][0] /= len;
			normal[i][1] /= len;
			normal[i][2] /= len;
		}
	}

	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;
}

float ImplicitFunction::EvaluateCurvatureError(MeshData *mesh, double &max)
{
	float err = 0;
	int vertex_N = mesh->vertex_N;
	int face_N = mesh->face_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	int (*link)[3] = mesh->face_link_E;
	float total_area = 0;
	max = -1;

	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	float (*vertex)[3] = mesh->vertex;

	for(i=0; i<face_N; i++){
		float *g = mesh->normal_f[i];
		float n[3];
		mesh->faceNormal(n, i);
		double a = mesh->faceArea(i);
		float c[3];
		mesh->faceCenter(c, i);

		total_area += a;

		for(int j=0; j<3; j++){
			int pair = link[i][j];
			if(pair > i)
				continue;
			float *g1 = mesh->normal_f[pair];
			float n1[3];
			mesh->faceNormal(n1, pair);
			double a1 = mesh->faceArea(pair);
			float c1[3];
			mesh->faceCenter(c1, pair);

			double dot1 = MeshData::DOT(n, n1);
			if(dot1 > 1.0)
				dot1 = 1.0;
			else if(dot1 < -1.0)
				dot1 = -1.0;
			
			double dot2 = MeshData::DOT(g, g1);
			if(dot2 > 1.0)
				dot2 = 1.0;
			else if(dot2 < -1.0)
				dot2 = -1.0;

			//if(dot1*dot2 < 0)
				//continue;

			double e = fabs(acos(dot1) - acos(dot2))/MeshData::DIST(c, c1);

			err += (a + a1)*e*e;

			if(max < e)
				max = e;
		}
	}
	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	return err/(3.0*total_area);
}

void ImplicitFunction::EvaluateNormalError(MeshData *mesh, float *error)
{
	int vertex_N = mesh->vertex_N;
	int face_N = mesh->face_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	int (*face)[3] = mesh->face;
	BOOL *isBound = mesh->isBound;

	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	float (*vertex)[3] = mesh->vertex;

	for(i=0; i<face_N; i++){
		int* fi = face[i];
		
		float *g = mesh->normal_f[i];

		float v1[3], v2[3], n[3];
		MeshData::VEC(v1, vertex[fi[0]], vertex[fi[1]]);
		MeshData::VEC(v2, vertex[fi[0]], vertex[fi[2]]);
		MeshData::CROSS(n, v1, v2);
		double len2 = MeshData::LENGTH(n);
		
		if((float)len2 == 0){
			error[i] = 0;
			continue;
		}

		float nm[3];
		nm[0] = g[0] + n[0]/len2;
		nm[1] = g[1] + n[1]/len2;
		nm[2] = g[2] + n[2]/len2;
		error[i] = MeshData::LENGTH(nm);

		if(error[i] > 1)
			int k  = 0;
	}

	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;
}


inline float ImplicitFunction::distanceToTriangle(float A[], float B[], float C[], float P[])
{
	float AB[3], AC[3];
	MeshData::VEC(AB, A, B);
	MeshData::VEC(AC, A, C);
	double n[3];
	MeshData::CROSS(n, AB, AC);
	double len2 = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
	if((float)len2 != 0){
		float AP[3];
		MeshData::VEC(AP, A, P);
		double dot = MeshData::DOT(AP, n);
		double Q[3];
		Q[0] = P[0] - dot*n[0]/len2;
		Q[1] = P[1] - dot*n[1]/len2;
		Q[2] = P[2] - dot*n[2]/len2;

		double c[3], v[3];
		MeshData::CROSS(c, AB, n);
		MeshData::VEC(v, A, Q);
		if(MeshData::DOT(c, v) > 0){
			goto OUTER;
		}
		float BC[3];
		MeshData::VEC(BC, B, C);
		MeshData::CROSS(c, BC, n);
		MeshData::VEC(v, B, Q);
		if(MeshData::DOT(c, v) > 0){
			goto OUTER;
		}
		MeshData::CROSS(c, AC, n);
		MeshData::VEC(v, C, Q);
		if(MeshData::DOT(c, v) < 0){
			goto OUTER;
		}	
		return MeshData::DIST2(Q, P);
	}
	OUTER:
	double min = distanceToSegment(A, B, P);
	double d2 = distanceToSegment(B, C, P);
	if(d2 < min)
		min = d2;
	d2 = distanceToSegment(C, A, P);
	if(d2 < min)
		min = d2;
	return min;
}