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

#include "stdafx.h"
#include "MeshEditor.h"
#include "NormalError.h"

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

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

NormalError::NormalError()
{

}

NormalError::~NormalError()
{

}

float NormalError::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++){
		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 NormalError::dfunc(float p[], float d[])
{
	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;
	}
}