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

#include "stdafx.h"
#include "MeshEditor.h"
#include "ThinPlate.h"

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

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

ThinPlate::ThinPlate()
{
	count = 0;
}

ThinPlate::~ThinPlate()
{

}


float ThinPlate::func(float p[])
{
	int face_N = mesh->face_N;
	float (*normal)[3] = mesh->normal_f;
	int (*face)[3] = mesh->face;
	
	float E = 0;
	for(int i=0; i<face_N; i++){
		if(face[i][0] < 0)
			continue;

		float p0[3];
		p0[0] = p[3*face[i][0]];
		p0[1] = p[3*face[i][0]+1];
		p0[2] = p[3*face[i][0]+2];
		float p1[3];
		p1[0] = p[3*face[i][1]];
		p1[1] = p[3*face[i][1]+1];
		p1[2] = p[3*face[i][1]+2];
		float p2[3];
		p2[0] = p[3*face[i][2]];
		p2[1] = p[3*face[i][2]+1];
		p2[2] = p[3*face[i][2]+2];

		float v1[3], v2[3];
		MeshData::VEC(v1, p0, p1);
		MeshData::VEC(v2, p0, p2);
		double n[3];
		MeshData::CROSS(n, v1, v2);
		double area = MeshData::LENGTH(n);
		E += (float)(area - MeshData::DOT(n, normal[i]));
	}

	return E;
}

void ThinPlate::dfunc(float p[], float d[])
{
	count++;
	int vertex_N = mesh->vertex_N;
	int **link = mesh->vertex_link_v;
	int **link_f = mesh->vertex_link_f;
	int *degree = mesh->degree_v;
	int *isBound = mesh->isBound;
	float (*normal)[3] = mesh->normal_f;

	for(int i=0; i<vertex_N; i++){
		if(isBound[i]){
			d[3*i] = d[3*i+1] = d[3*i+2] = 0;
			continue;
		}
		double dx, dy, dz;
		dx = dy = dz = 0;
		int deg = degree[i];
		int* l = link[i];
		int* l_f = link_f[i];
		float p0[3], p1[3], p2[3];
		p0[0] = p[3*i];
		p0[1] = p[3*i+1];
		p0[2] = p[3*i+2];

		for(int j=0; j<deg; j++){
			int f = l_f[j];
			int i1 = l[j];
			int i2 = l[(j+1)%deg];
			p1[0] = p[3*i1];
			p1[1] = p[3*i1+1];
			p1[2] = p[3*i1+2];
			p2[0] = p[3*i2];
			p2[1] = p[3*i2+1];
			p2[2] = p[3*i2+2];

			float v1[3], v2[3], v3[3];
			MeshData::VEC(v1, p0, p1);
			MeshData::VEC(v2, p0, p2);
			MeshData::VEC(v3, p1, p2);

			double c[3];
			MeshData::CROSS(c, v1, v2);
			double A = MeshData::LENGTH(c);
			if((float)A != 0){
				double cot1 = -MeshData::DOT(v1, v3)/A;
				double cot2 =  MeshData::DOT(v2, v3)/A;

				dx += cot2*v1[0] + cot1*v2[0];
				dy += cot2*v1[1] + cot1*v2[1];
				dz += cot2*v1[2] + cot1*v2[2];
			}

			double m[3];
			m[0] = normal[f][0];
			m[1] = normal[f][1];
			m[2] = normal[f][2];

			double dot1 = MeshData::DOT(v1, m);
			double dot2 = MeshData::DOT(v2, m);

			double vp1[3], vp2[3], vp3[3];
			vp1[0] = v1[0] - dot1*m[0];
			vp1[1] = v1[1] - dot1*m[1];
			vp1[2] = v1[2] - dot1*m[2];

			vp2[0] = v2[0] - dot2*m[0];
			vp2[1] = v2[1] - dot2*m[1];
			vp2[2] = v2[2] - dot2*m[2];

			vp3[0] = vp2[0] - vp1[0];
			vp3[1] = vp2[1] - vp1[1];
			vp3[2] = vp2[2] - vp1[2];

			MeshData::CROSS(c, vp1, vp2);
			A = MeshData::LENGTH(c);
			if((float)A != 0){
				double cot1 = -MeshData::DOT(vp1, vp3)/A;
				double cot2 =  MeshData::DOT(vp2, vp3)/A;

				dx -= cot2*vp1[0] + cot1*vp2[0];
				dy -= cot2*vp1[1] + cot1*vp2[1];
				dz -= cot2*vp1[2] + cot1*vp2[2];
			}
		}
		d[3*i] = -(float)dx;
		d[3*i+1] = -(float)dy;
		d[3*i+2] = -(float)dz;
	}
}

/*
float ThinPlate::func(float p[])
{
	int n = mesh->vertex_N;
	int **link = mesh->vertex_link_v;
	int *degree = mesh->degree_v; 
	double E = 0;
	for(int i=0; i<n; i++){
		double L[3] = {0,0,0};
		int deg = degree[i];
		int* l = link[i];
		if(deg == 0)
			continue;
		for(int j=0; j<deg; j++){
			int pair = l[j];
			L[0] += p[3*pair];
			L[1] += p[3*pair+1];
			L[2] += p[3*pair+2];
		}
		L[0] = p[3*i] - L[0]/deg;
		L[1] = p[3*i+1] - L[1]/deg;
		L[2] = p[3*i+2] - L[2]/deg;

		E += (L[0]*L[0] + L[1]*L[1] + L[2]*L[2]);
	}
	return (float)E;
}

void ThinPlate::dfunc(float p[], float d[])
{
	int n = mesh->vertex_N;
	int **link = mesh->vertex_link_v;
	int *degree = mesh->degree_v; 
	float (*L)[3] = new float[n][3];
	for(int i=0; i<n; i++){
		int deg = degree[i];
		int* l = link[i];
		if(deg == 0)
			continue;
		float* Li = L[i];
		Li[0] = Li[1] = Li[2] = 0;
		for(int j=0; j<deg; j++){
			int pair = l[j];
			Li[0] += p[3*pair];
			Li[1] += p[3*pair+1];
			Li[2] += p[3*pair+2];
		}
		Li[0] = p[3*i] - Li[0]/deg;
		Li[1] = p[3*i+1] - Li[1]/deg;
		Li[2] = p[3*i+2] - Li[2]/deg;
	}

	for(i=0; i<n; i++){
		int deg = degree[i];
		int* l = link[i];
		d[3*i] = d[3*i+1] = d[3*i+2] = 0;
		if(deg == 0)
			continue;
		for(int j=0; j<deg; j++){
			int pair = l[j];
			d[3*i] += L[i][0] - L[pair][0];
			d[3*i+1] += L[i][1] - L[pair][1];
			d[3*i+2] += L[i][2] - L[pair][2];
		}
		d[3*i] /= deg;
		d[3*i+1] /= deg;
		d[3*i+2] /= deg;
	}

	delete[] L;
}
*/