#include "precompile.h"
#include "constraint.h"
#include <iostream>
#include <eigen\Eigenvalues>

//#define DEBUG_W11_ALWAYS_POSITIVE
#define DEBUG_DET_ALWAYS_POSITIVE
//#define DEBUG_FINITE_DIFFERENCE

#ifdef ENABLE_MATLAB_DEBUGGING
#include "../matlab_debugger.h"
extern MatlabDebugger* g_debugger;
#endif

ScalarType sp_function_spline_psi_test(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x - LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return x + 0.5*k*x*x;
	}
	else
	{
		return (-0.25*std::log(-2 * k*x) - 0.375) / k;
	}
}

//----------Constraint Class----------//
Constraint::Constraint()
{
}

Constraint::Constraint(ConstraintType type) :
	m_constraint_type(type)
{
}

Constraint::Constraint(const Constraint& other) : 
	m_constraint_type(other.m_constraint_type)
{
}

Constraint::~Constraint()
{
}

//----------Attachment Constraint-------//
AttachmentConstraint::AttachmentConstraint(ScalarType *stiffness, unsigned int p0, unsigned int handle_id, unsigned int handle_vertex_id, unsigned int dim) : 
	Constraint(CONSTRAINT_TYPE_ATTACHMENT),
	p_k(stiffness),
	m_p0(p0),
	m_handle_id(handle_id),
	m_handle_vertex_id(handle_vertex_id),
	m_dim(dim)
{
	m_penalty_gradient.resize(m_dim);
}
AttachmentConstraint::AttachmentConstraint(const AttachmentConstraint& other) :
	Constraint(other),
	p_k(other.p_k),
	m_p0(other.m_p0),
	m_handle_id(other.m_handle_id),
	m_handle_vertex_id(other.m_handle_vertex_id),
	m_dim(other.m_dim)
{
	m_penalty_gradient.resize(m_dim);
}
AttachmentConstraint::~AttachmentConstraint()
{
}

void AttachmentConstraint::UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_vertex_rotation_2d, const std::vector<Eigen::Quaternion<ScalarType, Eigen::DontAlign>>& per_vertex_rotation_3d)
{
	m_fixed_point = handle_vertices[m_handle_id].block(m_handle_vertex_id*m_dim, 0, m_dim, 1);
}

// energy = 0.5 * k * ||x - p||^2
ScalarType AttachmentConstraint::EvaluateEnergy(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX p = x.block(m_p0*m_dim, 0, m_dim, 1);

	VectorX diff = (p-m_fixed_point) /(1.0 + regularization_mu*regularization_mu);

	ScalarType e_i = 0.5*(*(p_k))*multiplier*(diff).squaredNorm();
	return e_i;
}
void AttachmentConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX p = x.block(m_p0*m_dim, 0, m_dim, 1);

	VectorX diff = (p-m_fixed_point) /(1.0 + regularization_mu*regularization_mu);

	VectorX g_i = (*(p_k))*multiplier*diff;
	gradient.block(m_dim*m_p0, 0, m_dim, 1) += g_i;
}
void AttachmentConstraint::EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	// TODO: change it back 
	ScalarType ks = *(p_k)*multiplier;

	for (unsigned int i = 0; i < m_dim; ++i)
	{
		hessian_triplets.push_back(SparseMatrixTriplet(m_dim*m_p0+i, m_dim*m_p0+i, ks));
	}
}
void AttachmentConstraint::EvaluateEnergyGradientHessianPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType),  \
	ScalarType (*d_psi)(ScalarType, ScalarType),  \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	m_penalty_hessian_triplets.clear();

	// preparation
	VectorX p = x.block(m_p0*m_dim, 0, m_dim, 1);
	VectorX diff = p-m_fixed_point;

	// energy
	m_penalty_energy = 0.5*(*(p_k))*diff.squaredNorm();
	// gradient
	m_penalty_gradient = (*(p_k))*diff;
	// Hessian
	ScalarType ks = *(p_k);
	for (unsigned int i = 0; i < m_dim; ++i)
	{
		m_penalty_hessian_triplets.push_back(SparseMatrixTriplet(m_dim*m_p0+i, m_dim*m_p0+i, ks));
	}
}
ScalarType AttachmentConstraint::RetrieveEnergy()
{
	return m_penalty_energy;
}
void AttachmentConstraint::RetrieveGradient(VectorX& gradient)
{
	gradient.block(m_dim*m_p0, 0, m_dim, 1) += m_penalty_gradient;
}
void AttachmentConstraint::RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	for (unsigned int i = 0; i < m_dim; ++i)
	{
		hessian_triplets.push_back(m_penalty_hessian_triplets[i]);
	}
}


// hard constraints
void AttachmentConstraint::EvaluateHardConstraintTargetPoints(VectorX& x, VectorX& target_points, int& index)
{
	target_points.block(index, 0, m_dim, 1) = m_fixed_point;

	//// move the corresponding point to the target
	//x.block(m_p0*m_dim, 0, m_dim, 1) = m_fixed_point;
	index += m_dim;
}
void AttachmentConstraint::EvaluateHardConstraintAttachmentMatrix(std::vector<SparseMatrixTriplet>& target_triplets, int& index)
{
	for (unsigned int i = 0; i != m_dim; i++)
	{
		target_triplets.push_back(SparseMatrixTriplet(m_p0*m_dim+i, index+i, 1));
	}
	index += m_dim;
}

// local step
bool AttachmentConstraint::CorrectToNoninversion(VectorX& f, unsigned int& index)
{
	bool result = (f.block(index, 0, m_dim, 1) - m_fixed_point).squaredNorm() > LARGER_EPSILON ? false : true;
	f.block(index, 0, m_dim, 1) = m_fixed_point;

	index += m_dim;

	return result;
}
// global 
void AttachmentConstraint::EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index)
{
	for (unsigned int i = 0; i != m_dim; ++i)
	{
		g_triplets.push_back(SparseMatrixTriplet(index + i, m_dim*m_p0 + i, 1));
	}
	index += m_dim;
}

void AttachmentConstraint::GetVectorizedExrotationField(VectorX& r, unsigned int& index)
{
	r.block(index, 0, m_dim, 1) = m_fixed_point;
	index += m_dim;
}

//----------!Attachment Constraint-------//

//----------Glue Constraint-------//
GlueConstraint::GlueConstraint(ScalarType *stiffness, unsigned int p0, unsigned int p1, unsigned int dim) : 
	Constraint(CONSTRAINT_TYPE_GLUE),
	p_k(stiffness),
	m_p0(p0),
	m_p1(p1),
	m_dim(dim)
{

}
GlueConstraint::GlueConstraint(const GlueConstraint& other) :
	Constraint(other),
	p_k(other.p_k),
	m_p0(other.m_p0),
	m_p1(other.m_p1),
	m_dim(other.m_dim)
{

}
GlueConstraint::~GlueConstraint()
{
}

// energy = 0.5 * k * ||x - p||^2
ScalarType GlueConstraint::EvaluateEnergy(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX p0 = x.block(m_p0*m_dim, 0, m_dim, 1);
	VectorX p1 = x.block(m_p1*m_dim, 0, m_dim, 1);

	ScalarType e_i = 0.5*(*(p_k))*multiplier*(p0 - p1).squaredNorm();
	return e_i;
}
void GlueConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX p0 = x.block(m_p0*m_dim, 0, m_dim, 1);
	VectorX p1 = x.block(m_p1*m_dim, 0, m_dim, 1);

	VectorX g_i0 = (*(p_k))*multiplier*(p0 - p1);
	gradient.block(m_dim*m_p0, 0, m_dim, 1) += g_i0;
	gradient.block(m_dim*m_p1, 0, m_dim, 1) -= g_i0;
}
void GlueConstraint::EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	ScalarType ks = *(p_k)*multiplier;

	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p0, 2*m_p0, ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p0+1, 2*m_p0+1, ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p0, 2*m_p1, -ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p0+1, 2*m_p1+1, -ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p1, 2*m_p0, -ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p1+1, 2*m_p0+1, -ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p1, 2*m_p1, ks));
	hessian_triplets.push_back(SparseMatrixTriplet(2*m_p1+1, 2*m_p1+1, ks));
}
//----------!Glue Constraint-------//

//----------Triangle Constraint-------//
TriangleElementConstraint::TriangleElementConstraint(\
	unsigned element_id, \
	ScalarType *mu, \
	ScalarType *lambda, \
	bool *enable_non_inversion, \
	ScalarType *alpha, \
	ScalarType *beta, \
	ScalarType *slackness, \
	unsigned int p1, \
	unsigned int p2, \
	unsigned int p3, \
	const VectorX& x) :
	Constraint(CONSTRAINT_TYPE_TRIANGLE_ELEMENT),
	m_element_id(element_id),
	p_mu(mu),
	p_lambda(lambda),
	p_enable_non_inversion(enable_non_inversion),
	p_alpha(alpha),
	p_beta(beta),
	p_slackness(slackness)
{
	m_p[0] = p1;
	m_p[1] = p2;
	m_p[2] = p3;

	getEdgeMatrix(x, m_Dm);
	m_area = std::abs(0.5 * m_Dm.determinant());
	m_Dm_inv = m_Dm.inverse(); // assume there's not degenerated triangle in the reference pose

	m_ExR.setIdentity();

	m_penalty_gradient.resize(6);
}

TriangleElementConstraint::TriangleElementConstraint(const TriangleElementConstraint& other) :
	Constraint(other),
	m_element_id(other.m_element_id),
	p_mu(other.p_mu),
	p_lambda(other.p_lambda),
	p_enable_non_inversion(other.p_enable_non_inversion),
	p_enable_modified_barrier(other.p_enable_modified_barrier),
	p_alpha(other.p_alpha),
	p_beta(other.p_beta),
	p_slackness(other.p_slackness)
{
	m_p[0] = other.m_p[0];
	m_p[1] = other.m_p[1];
	m_p[2] = other.m_p[2];

	m_Dm = other.m_Dm;
	m_area = other.m_area;
	m_Dm_inv = other.m_Dm_inv;

	m_ExR.setIdentity();

	m_penalty_gradient.resize(6);
}

TriangleElementConstraint::~TriangleElementConstraint()
{
}

void TriangleElementConstraint::UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotatino_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d)
{
	ScalarType theta = per_element_rotatino_2d[m_element_id];

	m_ExR(0, 0) = cos(theta);
	m_ExR(0, 1) = -sin(theta);
	m_ExR(1, 0) = sin(theta);
	m_ExR(1, 1) = cos(theta);
}

void TriangleElementConstraint::UpdateRotation(const VectorX& x)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Eigen::JacobiSVD<EigenMatrix2> svd(F,  Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	EigenMatrix2 U = svd.matrixU();
	EigenMatrix2 V = svd.matrixV();

	m_R = U*V.transpose();
	m_R.block<2, 1>(0, 1) *= m_R.determinant();
}

ScalarType TriangleElementConstraint::EvaluateEnergyNoninversionConstraintCore(const VectorX& x)
{
	ScalarType energy;

	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		energy =  W.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		energy = F.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix2 W = 0.5 * (m_R.transpose() * F + F.transpose() * m_R);

		energy = W.determinant();
	}

	return energy;
}
void TriangleElementConstraint::EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_block)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 dW11dW; dW11dW.setZero(); dW11dW(0,0) = 1;

		P += (m_ExR)* (W.determinant() * W.inverse());
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		P += F.determinant() * F.inverse().transpose();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix2 W = 0.5 * (m_R.transpose()*F + F.transpose()*m_R);
		EigenMatrix2 dW11dW; dW11dW.setZero(); dW11dW(0,0) = 1;

		P += (m_R)* (W.determinant() * W.inverse());
	}

	EigenMatrix2 H = P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	gradient_block.resize(6);
	for (unsigned int i = 0; i < 3; i++)
	{
		gradient_block.block<2, 1>(2*i, 0) = g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_block)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix2222 dPdF;
	dPdF.SetZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		ScalarType detW = W.determinant();

		EigenMatrix2 Winv = W.inverse();

		//if (detW < 1e-20)
		//{
		//	//std::cout << "element_id: " << m_element_id << "detW: " << detW << std::endl;
		//}
		ScalarType OneOverDetW = 1/detW;

		EigenMatrix2 detWWinv = detW * Winv;

		Matrix2222 I;
		I.SetIdentity();
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix2222 dPBdW; // PB = tensor of barrier function
		dPBdW.SetZero();

		// constructing block by block:
		// detW
		directProduct(dPBdW, detWWinv, detWWinv);
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j) -= detWWinv * dWdWij * detWWinv;
			}
		}
		dPBdW = dPBdW * OneOverDetW;
		// multiply the exrotation term
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPBdW(i, j) = m_ExR * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		ScalarType detF = F.determinant();
		EigenMatrix2 Finv = F.inverse();
		EigenMatrix2 FinvT = Finv.transpose();
		//if (detF < 1e-20)
		//{
		//	std::cout << "element_id: " << m_element_id << "detF: " << detF << std::endl;
		//}
		ScalarType OneOverDetF = 1/detF;
		EigenMatrix2 detFFinv = detF * Finv;
		EigenMatrix2 detFFinvT = detF * FinvT;

		// constructing block by block:
		// detW
		directProduct(dPdF, detFFinvT, detFFinvT);
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dFdFij; dFdFij.setZero();
				dFdFij(i, j) = 1;

				dPdF(i, j) -= (detFFinv * dFdFij * detFFinv).transpose();
			}
		}
		dPdF = dPdF * OneOverDetF;
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix2 W = 0.5 * (m_R.transpose()*F + F.transpose()*m_R);

		ScalarType detW = W.determinant();

		EigenMatrix2 Winv = W.inverse();

		//if (detW < 1e-20)
		//{
		//	//std::cout << "element_id: " << m_element_id << "detW: " << detW << std::endl;
		//}
		ScalarType OneOverDetW = 1/detW;

		EigenMatrix2 detWWinv = detW * Winv;

		Matrix2222 I;
		I.SetIdentity();
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_R.transpose() * I(i,j) + I(j, i) * m_R);
			}
		}

		Matrix2222 dPBdW; // PB = tensor of barrier function
		dPBdW.SetZero();

		// constructing block by block:
		// detW
		directProduct(dPBdW, detWWinv, detWWinv);
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j) -= detWWinv * dWdWij * detWWinv;
			}
		}
		dPBdW = dPBdW * OneOverDetW;
		// multiply the exrotation term
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPBdW(i, j) = m_R * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()));

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	hessian_block.resize(6, 6);
	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			hessian_block.block<2, 2>(i*2, j*2) = H_blocks[i][j];
		}
	}
}

void TriangleElementConstraint::EvaluateEnergyNoninversionConstraintCore(const VectorX& x, ScalarType& W11, ScalarType& detW)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		W11 = W(0,0);
		detW =  W.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		W11 = 0;
		detW = F.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix2 W = 0.5 * (m_R.transpose()*F + F.transpose()*m_R);
		
		W11 = W(0,0);
		detW =  W.determinant();
	}
}
void TriangleElementConstraint::EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_W11_block, VectorX& gradient_detW_block)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 P_W11;
	EigenMatrix2 P_detW; // stress tensor
	P_W11.setZero();
	P_detW.setZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 dW11dW; dW11dW.setZero(); dW11dW(0,0) = 1;

		P_W11 = (m_ExR) * dW11dW;
		P_detW = (m_ExR)* (W.determinant() * W.inverse());
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		P_W11.setZero();
		P_detW = F.determinant() * F.inverse().transpose();
	}

	EigenMatrix2 H_W11 = P_W11 * m_Dm_inv.transpose();
	EigenMatrix2 H_detW = P_detW * m_Dm_inv.transpose();

	EigenVector2 g_W11[3];
	g_W11[0] = H_W11.block<2,1>(0, 0);
	g_W11[1] = H_W11.block<2,1>(0, 1);
	g_W11[2] = -g_W11[0]-g_W11[1];
	EigenVector2 g_detW[3];
	g_detW[0] = H_detW.block<2,1>(0, 0);
	g_detW[1] = H_detW.block<2,1>(0, 1);
	g_detW[2] = -g_detW[0]-g_detW[1];

	gradient_W11_block.resize(6);
	gradient_detW_block.resize(6);
	for (unsigned int i = 0; i < 3; i++)
	{
		gradient_W11_block.block<2, 1>(2*i, 0) = g_W11[i];
		gradient_detW_block.block<2, 1>(2*i, 0) = g_detW[i];
	}
}
void TriangleElementConstraint::EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_W11_block, EigenMatrix& Hessian_detW_block)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix2222 dP_detWdF;
	dP_detWdF.SetZero();
	Matrix2222 dP_W11dF;
	dP_W11dF.SetZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		ScalarType detW = W.determinant();
		EigenMatrix2 Winv = W.inverse();

		ScalarType OneOverDetW = 1/detW;

		EigenMatrix2 detWWinv = detW * Winv;

		Matrix2222 I;
		I.SetIdentity();
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix2222 dPB_detWdW; // PB = tensor of barrier function
		dPB_detWdW.SetZero();

		// detW
		directProduct(dPB_detWdW, detWWinv, detWWinv);
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPB_detWdW(i, j) -= detWWinv * dWdWij * detWWinv;
			}
		}
		dPB_detWdW = dPB_detWdW * OneOverDetW;
		// multiply the exrotation term
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPB_detWdW(i, j) = m_ExR * dPB_detWdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dP_detWdF(i, j) = dPB_detWdW.FrobeniusProduct(dWdF(i, j));
			}
		}
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		ScalarType detF = F.determinant();
		EigenMatrix2 Finv = F.inverse();
		EigenMatrix2 FinvT = Finv.transpose();
		//if (detF < 1e-20)
		//{
		//	std::cout << "element_id: " << m_element_id << "detF: " << detF << std::endl;
		//}
		ScalarType OneOverDetF = 1/detF;
		EigenMatrix2 detFFinv = detF * Finv;
		EigenMatrix2 detFFinvT = detF * FinvT;

		// constructing block by block:
		// detW
		directProduct(dP_detWdF, detFFinvT, detFFinvT);
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dFdFij; dFdFij.setZero();
				dFdFij(i, j) = 1;

				dP_detWdF(i, j) -= (detFFinv * dFdFij * detFFinv).transpose();
			}
		}
		dP_detWdF = dP_detWdF * OneOverDetF;
	}

	Matrix2222 dH = dP_detWdF * ((m_Dm_inv.transpose()));

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	hessian_W11_block.resize(6, 6);
	hessian_W11_block.setZero();
	Hessian_detW_block.resize(6, 6);
	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			Hessian_detW_block.block<2, 2>(i*2, j*2) = H_blocks[i][j];
		}
	}
}

ScalarType TriangleElementConstraint::EvaluateEnergyNoConstraintCore(const VectorX& x, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*multiplier*(F-target_term).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*multiplier*std::pow((target_term.transpose()*F-I).trace(), 2);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientNoConstraintCore(const VectorX& x, VectorX& gradient_block, ScalarType multiplier)
{
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*multiplier*(F-target_term);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*multiplier*(target_term.transpose()*F-I).trace()*target_term;
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	gradient_block.resize(6);
	for (unsigned int i = 0; i < 3; i++)
	{
		gradient_block.block<2, 1>(2*i, 0) = g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianNoConstraintCore(const VectorX& x, EigenMatrix& hessian_block, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu)*multiplier);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*multiplier*target_term, target_term);
		dPdF = dPdF + dP2dF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	hessian_block.resize(6, 6);
	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			hessian_block.block<2, 2>(i*2, j*2) = H_blocks[i][j];
		}
	}
}


// energy = area * (mu||F-R||^2 + 0.5lambda*tr(F-R)^2 + barrier_function)
ScalarType TriangleElementConstraint::EvaluateEnergy(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*multiplier*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*multiplier*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion) // barrier term: -alpha*log(W11-slackness)-betalog(det(W)-slackness), W = 0.5(RTF + FTR)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 K = (*p_slackness)*EigenMatrix2::Identity();
		ScalarType WpK11 = (W+K)(0, 0);
		ScalarType detWpK = (W+K).determinant();

		if (WpK11 <= 0  || detWpK <= 0 )
		{
			throw EXCEPTION_LOG_OF_NEGATIVE;
		}

		if (*p_enable_modified_barrier)
		{
			energy += (*p_alpha)*(WpK11-std::log(WpK11)) + (*p_beta)*((W+K).trace()-std::log(detWpK));
		}
		else
		{
			energy += -(*p_alpha)*std::log(WpK11) - (*p_beta)*std::log(detWpK);
		}
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
	//EvaluateGradientFiniteDifference(x, gradient);
	//return;

	//VectorX g1=gradient;
	//EvaluateGradientFiniteDifference(x, g1);

	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*multiplier*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*multiplier*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 dW11dW; dW11dW.setZero(); dW11dW(0,0) = 1;
		EigenMatrix2 K = (*p_slackness)*EigenMatrix2::Identity();
		ScalarType WpK11 = (W+K)(0, 0);
		ScalarType detWpK = (W+K).determinant();
		
		if (*p_enable_modified_barrier)
		{
			P += (m_ExR) * ((*p_alpha)*(1-1/WpK11)*dW11dW + (*p_beta)*(EigenMatrix2::Identity()-(W+K).inverse().transpose()));
		}
		else
		{
			P += (m_ExR)* (-(*p_alpha)/WpK11*dW11dW - (*p_beta)*(W+K).inverse().transpose());
		}
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	//EvaluateHessianFiniteDifference(x, hessian_triplets);
	//return;

	//std::vector<SparseMatrixTriplet> triplets = hessian_triplets;
	//EvaluateHessianFiniteDifference(x, triplets);

	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu)*multiplier);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*multiplier*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion) // barrier term
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 K = (*p_slackness)*EigenMatrix2::Identity();

		EigenMatrix2 WpK = W+K;
		ScalarType WpK1 = WpK(0,0);
		ScalarType detWpK = WpK.determinant();

		EigenMatrix2 WpKinv = WpK.inverse();
		ScalarType WpK1inv = 1/WpK1;

		Matrix2222 I;
		I.SetIdentity();
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix2222 dPBdW; // PB = tensor of barrier function
		Matrix2222 dPBdF; 
		dPBdW.SetZero();
		
		if (*p_enable_modified_barrier)
		{
			// constructing block by block:
			// alpha 1x1
			dPBdW(0, 0)(0, 0) += (*p_alpha) * ( WpK1inv * WpK1inv ); // for the first term
			// gamma 3x3
			for (unsigned int i = 0; i < 2; i++)
			{
				for (unsigned int j = 0; j < 2; j++)
				{
					EigenMatrix2 dWdWij; dWdWij.setZero();
					dWdWij(i, j) = 1;

					dPBdW(i, j) += (*p_beta) * WpKinv * dWdWij * WpKinv;
				}
			}
			// multiply the exrotation term
			for (unsigned int i = 0; i < 2; i++)
			{
				for (unsigned int j = 0; j < 2; j++)
				{
					dPBdW(i, j) = m_ExR * dPBdW(i, j);
				}
			}
		}
		else
		{
			// constructing block by block:
			// alpha 1x1
			dPBdW(0, 0)(0, 0) += (*p_alpha) * ( WpK1inv * WpK1inv ); // for the first term
			// gamma 3x3
			for (unsigned int i = 0; i < 2; i++)
			{
				for (unsigned int j = 0; j < 2; j++)
				{
					EigenMatrix2 dWdWij; dWdWij.setZero();
					dWdWij(i, j) = 1;

					dPBdW(i, j) += (*p_beta) * WpKinv * dWdWij * WpKinv;
				}
			}
			// multiply the exrotation term
			for (unsigned int i = 0; i < 2; i++)
			{
				for (unsigned int j = 0; j < 2; j++)
				{
					dPBdW(i, j) = m_ExR * dPBdW(i, j);
				}
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}

void TriangleElementConstraint::LocalGlobalLocalStep(const VectorX& x)
{
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

	Eigen::EigenSolver<EigenMatrix2> evd_solver(W);
	EigenMatrix2 V = evd_solver.eigenvectors().real();
	EigenVector2 eigs = evd_solver.eigenvalues().real();
	//m_W_is_SPD = true;
	for (unsigned int i = 0; i != 2; ++i)
	{
		if (eigs(i) < LARGER_EPSILON)
		{
			eigs(i) = LARGER_EPSILON;
			m_W_is_SPD = false;
		}
	}

	m_W_projection = V*eigs.asDiagonal()*V.transpose();
}

ScalarType TriangleElementConstraint::EvaluateEnergyLocalGlobal(const VectorX& x, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		energy += 0.5 * multiplier * (W-m_W_projection).squaredNorm();
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient, ScalarType multiplier)
{
	//EvaluateGradientFiniteDifference(x, gradient);
	//return;

	//VectorX g1=gradient;
	//EvaluateGradientFiniteDifference(x, g1);

	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		P += multiplier * m_ExR * (W-m_W_projection);
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianLocalGlobal(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier)
{
	//std::vector<SparseMatrixTriplet> h1 = hessian_triplets;
	//EvaluateHessianLocalGlobalFD(x, h1, multiplier);

	// get deformation gradient
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		Matrix2222 I;
		I.SetIdentity();
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix2222 dPBdW; // PB = tensor of barrier function
		Matrix2222 dPBdF; 
		dPBdW.SetIdentity();
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPBdW(i, j) = multiplier * m_ExR * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}

ScalarType TriangleElementConstraint::EvaluateEnergyPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		EigenVector2 lambdas = W.eigenvalues().real();

		energy += psi(lambdas(0), multiplier) + psi(lambdas(1), multiplier);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientPenaltyMethod(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		Eigen::EigenSolver<EigenMatrix2> evd_solver(W);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();
		
		P += m_ExR * (Q * dpsi_L * Q.transpose());
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianPenaltyMethod(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//return EvaluateHessianPenaltyMethodFD(x, hessian_triplets, d_psi, d2_psi, multiplier);

	//// debug finite differences
	//std::vector<SparseMatrixTriplet> hessian_triplets_FD = hessian_triplets;
	//EvaluateHessianPenaltyMethodFD(x, hessian_triplets_FD, d_psi, d2_psi, multiplier);

	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		Eigen::EigenSolver<EigenMatrix2> evd_solver(W);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenVector2 dpsi2_eigs;
		dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
		dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();
		EigenMatrix2 dpsi2_L; 
		dpsi2_L = dpsi2_eigs.asDiagonal();

		Matrix2222 dPBdF; 
		EigenMatrix2 QTdQ;
		QTdQ.setZero();
		EigenMatrix2 delta_Q;
		EigenMatrix2 delta_L;
		delta_L.setZero();

		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				EigenMatrix2 QTdWQ = 0.5 * Q.transpose() * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR) * Q;
				delta_L(0,0) = QTdWQ(0,0);
				delta_L(1,1) = QTdWQ(1,1);
				ScalarType l2_minus_l1 = eigs(1) - eigs(0);
				if (std::abs(l2_minus_l1) < EPSILON)
				{
					dPBdF(i, j) = m_ExR * Q * (dpsi2_L*delta_L)*Q.transpose();
				}
				else
				{
					QTdQ(0, 1) = QTdWQ(0, 1)/l2_minus_l1;
					QTdQ(1, 0) = -QTdQ(0, 1);
					dPBdF(i, j) = m_ExR * Q * (QTdQ*dpsi_L + dpsi_L*QTdQ.transpose() + dpsi2_L*delta_L)*Q.transpose();
				}

				//// sanity check
				//EigenMatrix2 temp = delta_Q * eigs.asDiagonal()*Q.transpose() + Q*delta_L*Q.transpose() + Q*eigs.asDiagonal()*delta_Q.transpose();// temp should be dW
				//EigenMatrix2 diff = temp-0.5 * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR);
				//EigenMatrix2 temp2 = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal()*QTdQ.transpose();
				//EigenMatrix2 diff2 = temp2 - QTdWQ;

			}
		}

		//Matrix2222 I;
		//I.SetIdentity();
		//Matrix2222 dWdF;
		//for (unsigned int i = 0; i < 2; i++)
		//{
		//	for (unsigned int j = 0; j < 2; j++)
		//	{
		//		dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
		//	}
		//}
		//Matrix2222 dPBdW; // PB = tensor of barrier function
		//dPBdW.SetZero();
		//for (unsigned int i = 0; i < 2; i++)
		//{
		//	for (unsigned int j = i; j < 2; j++)
		//	{
		//		// set dW
		//		EigenMatrix2 delta_W;
		//		delta_W.setZero();
		//		if (i == j)
		//		{
		//			delta_W(i, j) = 1;
		//		}
		//		else
		//		{
		//			delta_W(i, j) = 0.5;
		//			delta_W(j, i) = 0.5;
		//		}

		//		// construct dQ and dL
		//		EigenMatrix2 QTdWQ = Q.transpose() * delta_W * Q;
		//		ScalarType l2_minus_l1 = eigs(1) - eigs(0);
		//		if (std::abs(l2_minus_l1) < EPSILON)
		//		{
		//			l2_minus_l1 = (l2_minus_l1>0)?EPSILON:-EPSILON;
		//		}
		//		QTdQ(0, 1) = QTdWQ(0, 1)/l2_minus_l1;
		//		QTdQ(1, 0) = -QTdQ(0, 1);
		//		delta_Q = Q*QTdQ;
		//		delta_L(0,0) = QTdWQ(0,0);
		//		delta_L(1,1) = QTdWQ(1,1);

		//		dPBdW(i, j) = m_ExR * (delta_Q*dpsi_L*Q.transpose() + Q*dpsi_L*delta_Q.transpose() + Q*dpsi2_L*delta_L*Q.transpose());
		//		if (i != j)
		//		{
		//			dPBdW(j, i) = dPBdW(i, j);
		//		}
		//	}
		//}
		////assert((dPBdW(0,1)-dPBdW(0,1).transpose()).squaredNorm() < EPSILON);
		////dPBdW(1, 0) = dPBdW(0, 1);

		//// construct dPBdF
		//for (unsigned int i = 0; i < 2; i++)
		//{
		//	for (unsigned int j = 0; j < 2; j++)
		//	{
		//		dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
		//	}
		//}

		dPdF = dPdF + dPBdF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];
	
	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}
void TriangleElementConstraint::EvaluateHessianPenaltyMethodFD(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradientPenaltyMethod(x, gradient, d_psi, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-6, 0);
	dt[1] = EigenVector2(0, 1e-6);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2,1>(m_p[j]*2,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientPenaltyMethod(x_plus_t, gradient_prime, d_psi, multiplier);
				temp[dim] = (gradient_prime.block<2,1>(m_p[i]*2,0) - gradient.block<2,1>(m_p[i]*2,0)) * 1e6;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}

}

bool TriangleElementConstraint::HasZeroPenaltyEnergy(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType))
{
	EigenMatrix2 F; 
	getDeformationGradient(x, F);
	EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
	EigenVector2 lambdas = W.eigenvalues().real();

	// if energy == zero
	if (psi(lambdas(0), 1) <= 0 && psi(lambdas(1), 1) <= 0)
	//if (lambdas(0) > 0.0 && lambdas(1) > 0.0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void TriangleElementConstraint::EvaluateEnergyGradientHessianPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType),  \
	ScalarType (*d_psi)(ScalarType, ScalarType),  \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	m_penalty_energy = 0.0;
	m_penalty_gradient.setZero();
	m_penalty_hessian_triplets.clear();

	// preparation
	EigenMatrix2 F; 
	getDeformationGradient(x, F);
	EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
	Eigen::EigenSolver<EigenMatrix2> evd_solver(W);
	EigenMatrix2 Q = evd_solver.eigenvectors().real();
	EigenVector2 eigs = evd_solver.eigenvalues().real();
	EigenVector2 dpsi_eigs;
	dpsi_eigs(0) = d_psi(eigs(0), multiplier);
	dpsi_eigs(1) = d_psi(eigs(1), multiplier);
	EigenVector2 dpsi2_eigs;
	dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
	dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
	EigenMatrix2 dpsi_L; 
	dpsi_L = dpsi_eigs.asDiagonal();
	EigenMatrix2 dpsi2_L; 
	dpsi2_L = dpsi2_eigs.asDiagonal();

	EigenMatrix2 P; // stress tensor
	P.setZero();
	Matrix2222 dPdF; // differentiate 
	dPdF.SetZero();
	Matrix2222 I_tensor;
	I_tensor.SetIdentity();

	// assembling
	if ((*p_mu) > 0) // elastic term
	{
		// energy
		m_penalty_energy += (*p_mu)*(F-m_ExR).squaredNorm();
		
		// gradient
		P += 2*(*p_mu)*(F-m_ExR);

		// hessian
		dPdF = dPdF + I_tensor * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		// energy
		EigenMatrix2 I; I.setIdentity();
		m_penalty_energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);

		// gradient
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;

		// hessian
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion)
	{
		// energy
		m_penalty_energy += psi(eigs(0), multiplier) + psi(eigs(1), multiplier);

		// gradient
		P += m_ExR * (Q * dpsi_L * Q.transpose());

		// hessian
		Matrix2222 dWdF;
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I_tensor(i,j) + I_tensor(j, i) * m_ExR);
			}
		}

		Matrix2222 dPBdF; 
		EigenMatrix2 QTdQ;
		QTdQ.setZero();
		EigenMatrix2 delta_Q;
		EigenMatrix2 delta_L;
		delta_L.setZero();

		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				EigenMatrix2 QTdWQ = 0.5 * Q.transpose() * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR) * Q;
				ScalarType l2_minus_l1 = eigs(1) - eigs(0);
				if (std::abs(l2_minus_l1) < EPSILON)
				{
					l2_minus_l1 = (l2_minus_l1>0)?EPSILON:-EPSILON;
				}
				QTdQ(0, 1) = QTdWQ(0, 1)/l2_minus_l1;
				QTdQ(1, 0) = -QTdQ(0, 1);
				delta_Q = Q*QTdQ;
				delta_L(0,0) = QTdWQ(0,0);
				delta_L(1,1) = QTdWQ(1,1);

				//// sanity check
				//EigenMatrix2 temp = delta_Q * eigs.asDiagonal()*Q.transpose() + Q*delta_L*Q.transpose() + Q*eigs.asDiagonal()*delta_Q.transpose();// temp should be dW
				//EigenMatrix2 diff = temp-0.5 * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR);
				//EigenMatrix2 temp2 = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal()*QTdQ.transpose();
				//EigenMatrix2 diff2 = temp2 - QTdWQ;

				dPBdF(i, j) = m_ExR * (delta_Q*dpsi_L*Q.transpose() + Q*dpsi_L*delta_Q.transpose() + Q*dpsi2_L*delta_L*Q.transpose());
			}
		}

		dPdF = dPdF + dPBdF;
	}

	// final touch
	// energy
	m_penalty_energy *= m_area;
	// gradient
	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();
	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];
	for (unsigned int i = 0; i < 3; i++)
	{
		m_penalty_gradient.block<2, 1>(2*i, 0) += g[i];
	}
	// Hessian
	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					m_penalty_hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}
ScalarType TriangleElementConstraint::RetrieveEnergy()
{
	return m_penalty_energy;
}
void TriangleElementConstraint::RetrieveGradient(VectorX& gradient)
{
	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += m_penalty_gradient.block<2,1>(2*i, 0);
	}
}
void TriangleElementConstraint::RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	for (unsigned int i = 0; i < m_penalty_hessian_triplets.size(); ++i)
	{
		hessian_triplets.push_back(m_penalty_hessian_triplets[i]);
	}
}

ScalarType TriangleElementConstraint::EvaluateEnergySpectralSDP(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;
		EigenMatrix2 X = m_V.transpose() * A * m_V;

		EigenVector2 lambdas = X.eigenvalues().real();

		energy += psi(lambdas(0), multiplier) + psi(lambdas(1), multiplier);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientSpectralSDP(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//return EvaluateGradientSpectralSDPFD(x, gradient, sp_function_spline_psi_test, multiplier);

	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;
		EigenMatrix2 X = m_V.transpose() * A * m_V;
		Eigen::EigenSolver<EigenMatrix2> evd_solver(X);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();

		P += - m_ExR * (m_V * Q * dpsi_L * Q.transpose() * m_V.transpose());
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianSpectralSDP(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//return EvaluateHessianSpectralSDPFD(x, hessian_triplets, d_psi, d2_psi, multiplier);

	// get deformation gradient
	EigenMatrix2 F; 
	getDeformationGradient(x, F);

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;
		EigenMatrix2 X = m_V.transpose() * A * m_V;
		Eigen::EigenSolver<EigenMatrix2> evd_solver(X);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenVector2 dpsi2_eigs;
		dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
		dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();
		EigenMatrix2 dpsi2_L; 
		dpsi2_L = dpsi2_eigs.asDiagonal();

		//// sanity check
		//EigenMatrix2 TEMP1 = Q*eigs.asDiagonal()*Q.transpose() - X;

		Matrix2222 dPBdF; 
		EigenMatrix2 QTdQ;
		QTdQ.setZero();
		EigenMatrix2 delta_Q;
		EigenMatrix2 delta_L;
		delta_L.setZero();

		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				EigenMatrix2 QTVTdWVQ = - 0.5 * Q.transpose() * m_V.transpose() * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR) * m_V * Q;
				delta_L(0,0) = QTVTdWVQ(0,0);
				delta_L(1,1) = QTVTdWVQ(1,1);

				ScalarType l2_minus_l1 = eigs(1) - eigs(0);
				if (std::abs(l2_minus_l1) < EPSILON)
				{
					// eigen value the same case:
					dPBdF(i, j) = - m_ExR * m_V * Q* (dpsi2_L*delta_L) * Q.transpose() * m_V.transpose();

					////sanity check
					//EigenMatrix2 LHS = delta_L;
					//EigenMatrix2 RHS = QTVTdWVQ;
					//ScalarType diff = (LHS-RHS).norm();
				}
				else
				{
					QTdQ(0, 1) = QTVTdWVQ(0, 1)/l2_minus_l1;
					QTdQ(1, 0) = -QTdQ(0, 1);
					dPBdF(i, j) = - m_ExR * m_V * Q* (QTdQ*dpsi_L + dpsi_L*QTdQ.transpose() + dpsi2_L*delta_L) * Q.transpose() * m_V.transpose();
					
					////sanity check
					//EigenMatrix2 LHS = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal() * QTdQ.transpose();
					//EigenMatrix2 RHS = QTVTdWVQ;
					//ScalarType diff = (LHS-RHS).norm();
					//if (diff > LARGER_EPSILON)
					//{
					//	std::cout << "diff = " << diff << std::endl;
					//	std::cout << "LHS = \n" << LHS << std::endl;
					//	std::cout << "RHS = \n" << RHS << std::endl;
					//}
				}
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	//// debug finite differences
	//std::vector<SparseMatrixTriplet> hessian_triplets_FD = hessian_triplets;
	//EvaluateHessianSpectralSDPFD(x, hessian_triplets_FD, d_psi, d2_psi, multiplier);
	//// sanity check: check positive definiteness:
	//Eigen::MatrixXd HB(6,6);
	//for (unsigned int i = 0; i < 3; i++)
	//{
	//	for (unsigned int j = 0; j < 3; j++)
	//	{
	//		EigenMatrix2& block = H_blocks[i][j];
	//		for (unsigned int row = 0; row < 2; row ++)
	//		{
	//			for (unsigned int col = 0; col < 2; col ++)
	//			{
	//				HB(i*2+row, j*2+col) = block(row, col);
	//			}
	//		}
	//	}
	//}
	//VectorX eigs_of_HB = HB.eigenvalues().real();
	//for (unsigned int i = 0; i != eigs_of_HB.size(); ++i)
	//{
	//	if(eigs_of_HB(i) < -LARGER_EPSILON)
	//	{
	//		std::cout << "warning, the block of hessian is not SPD" << std::endl;
	//	}
	//}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
 }
void TriangleElementConstraint::EvaluateGradientSpectralSDPFD(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType (*psi)(ScalarType, ScalarType),
	ScalarType multiplier)
{
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergySpectralSDP(x, psi, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(LARGER_EPSILON, 0);
	dt[1] = EigenVector2(0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 3; i++)
	{
		ScalarType temp[2];
		for (unsigned int dim = 0; dim < 2; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<2,1>(m_p[i]*2,0) += dt[dim];
			energy_prime = EvaluateEnergySpectralSDP(x_plus_t, psi, multiplier);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		gradient(m_p[i]*2,0) += temp[0];
		gradient(m_p[i]*2+1,0) += temp[1];
	}
}
void TriangleElementConstraint::EvaluateHessianSpectralSDPFD(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradientSpectralSDP(x, gradient, d_psi, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-8, 0);
	dt[1] = EigenVector2(0, 1e-8);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2,1>(m_p[j]*2,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientSpectralSDP(x_plus_t, gradient_prime, d_psi, multiplier);
				temp[dim] = (gradient_prime.block<2,1>(m_p[i]*2,0) - gradient.block<2,1>(m_p[i]*2,0)) * 1e8;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}

}

void TriangleElementConstraint::InitLagrangian()
{
	m_V = EigenMatrix2::Identity();
	m_U = m_V;
}
void TriangleElementConstraint::UpdateLagrangian(\
	const VectorX& x_old, \
	const VectorX& x_new, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F_old; 
	getDeformationGradient(x_old, F_old);
	EigenMatrix2 W_old = 0.5 * (m_ExR.transpose()*F_old + F_old.transpose()*m_ExR);
	EigenMatrix2 X_old = -m_V.transpose() * W_old * m_V;
	Eigen::EigenSolver<EigenMatrix2> evd_solver_old(X_old);
	EigenMatrix2 S = evd_solver_old.eigenvectors().real();
	
	EigenMatrix2 F_new;
	getDeformationGradient(x_new, F_new);
	EigenMatrix2 W_new = 0.5 * (m_ExR.transpose()*F_new + F_new.transpose()*m_ExR);
	EigenMatrix2 X_new = -m_V.transpose() * W_new * m_V;
	Eigen::EigenSolver<EigenMatrix2> evd_solver(X_new);
	EigenVector2 eigs = evd_solver.eigenvalues().real();
	EigenVector2 dpsi_eigs;
	dpsi_eigs(0) = d_psi(eigs(0), multiplier);
	dpsi_eigs(1) = d_psi(eigs(1), multiplier);
	EigenMatrix2 dpsi_L; 
	dpsi_L = dpsi_eigs.asDiagonal();

	assert(dpsi_eigs(0) > 0);
	assert(dpsi_eigs(1) > 0);

	EigenVector2 u_eigs_new(std::sqrt(dpsi_eigs(0)), std::sqrt(dpsi_eigs(1)));
	EigenMatrix2 V_new = m_V*S*u_eigs_new.asDiagonal();

	EigenMatrix2 U = m_V*S*dpsi_L*((m_V*S).transpose());

	//Eigen::EigenSolver<EigenMatrix2> evd_u(U);
	//EigenVector2 u_eigs = evd_u.eigenvalues().real();
	//assert(u_eigs(0) > 0);
	//assert(u_eigs(1) > 0);
	//EigenMatrix2 Q_eigs = evd_u.eigenvectors().real();
	//EigenMatrix2 V_new = Q_eigs*u_eigs_new.asDiagonal();

	//EigenMatrix2 V_new = U.llt().matrixL();
	if ((V_new*V_new.transpose() - U).norm() >= LARGER_EPSILON)
	{
		std::cout << "V_new*V_new.transpose() - U = \n" << V_new*V_new.transpose() - U << std::endl;
		std::cout << "norm = " << (V_new*V_new.transpose() - U).norm() << std::endl;
	}
	//assert((V_new*V_new.transpose() - U).norm() < LARGER_EPSILON);
	
	//ScalarType lambda_V = 0.5;
	//ScalarType ratio = m_V.norm() / (V_new - m_V).norm();
	//if (ratio < 1)
	//{
	//	lambda_V *= ratio;
	//}

	//m_V += lambda_V * (V_new - m_V);


	m_V = V_new;
}

// pennon
ScalarType TriangleElementConstraint::EvaluateEnergyPennon(\
	const VectorX& x, \
	ScalarType(*psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F - m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F - I).trace(), 2);
	}
	if (*p_enable_non_inversion)
	{
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;

		//EigenMatrix2 X = A;
		//Eigen::EigenSolver<EigenMatrix2> evd_solver(X);
		//EigenMatrix2 S = evd_solver.eigenvectors().real();
		//EigenVector2 lambdas = evd_solver.eigenvalues().real();
		//EigenVector2 psi_lambdas;
		//psi_lambdas(0) = psi(lambdas(0), multiplier);
		//psi_lambdas(1) = psi(lambdas(1), multiplier);
		//EigenMatrix2 psi_L;
		//psi_L = psi_lambdas.asDiagonal();
		
		EigenMatrix2 V = m_U.llt().matrixL();
		EigenMatrix2 X = V.transpose() * A * V;
		EigenVector2 lambdas = X.eigenvalues().real();
		energy += psi(lambdas(0), multiplier) + psi(lambdas(1), multiplier);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientPennon(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType(*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//return EvaluateGradientPennonFD(x, gradient, sp_function_spline_psi_test, multiplier);

	// get deformation gradient
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2 * (*p_mu)*(F - m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F - I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix2 V = m_U.llt().matrixL();
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;
		EigenMatrix2 X = V.transpose() * A * V;
		Eigen::EigenSolver<EigenMatrix2> evd_solver(X);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L;
		dpsi_L = dpsi_eigs.asDiagonal();

		P += -m_ExR * (V * Q * dpsi_L * Q.transpose() * V.transpose());
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2, 1>(0, 0);
	g[1] = H.block<2, 1>(0, 1);
	g[2] = -g[0] - g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2 * m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianPennon(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType(*d_psi)(ScalarType, ScalarType), \
	ScalarType(*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//return EvaluateHessianPennonFD(x, hessian_triplets, d_psi, d2_psi, multiplier);

	// get deformation gradient
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2 * (*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix2 V = m_U.llt().matrixL();
		EigenMatrix2 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix2 A = -W;
		EigenMatrix2 X = V.transpose() * A * V;
		Eigen::EigenSolver<EigenMatrix2> evd_solver(X);
		EigenMatrix2 Q = evd_solver.eigenvectors().real();
		EigenVector2 eigs = evd_solver.eigenvalues().real();
		EigenVector2 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		EigenVector2 dpsi2_eigs;
		dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
		dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
		EigenMatrix2 dpsi_L;
		dpsi_L = dpsi_eigs.asDiagonal();
		EigenMatrix2 dpsi2_L;
		dpsi2_L = dpsi2_eigs.asDiagonal();

		//// sanity check
		//EigenMatrix2 TEMP1 = Q*eigs.asDiagonal()*Q.transpose() - X;

		Matrix2222 dPBdF;
		EigenMatrix2 QTdQ;
		QTdQ.setZero();
		EigenMatrix2 delta_Q;
		EigenMatrix2 delta_L;
		delta_L.setZero();

		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				EigenMatrix2 QTVTdWVQ = -0.5 * Q.transpose() * V.transpose() * (m_ExR.transpose()*delta_F + delta_F.transpose()*m_ExR) * V * Q;
				delta_L(0, 0) = QTVTdWVQ(0, 0);
				delta_L(1, 1) = QTVTdWVQ(1, 1);

				ScalarType l2_minus_l1 = eigs(1) - eigs(0);
				if (std::abs(l2_minus_l1) < EPSILON)
				{
					// eigen value the same case:
					dPBdF(i, j) = -m_ExR * V * Q* (dpsi2_L*delta_L) * Q.transpose() * V.transpose();

					////sanity check
					//EigenMatrix2 LHS = delta_L;
					//EigenMatrix2 RHS = QTVTdWVQ;
					//ScalarType diff = (LHS-RHS).norm();
				}
				else
				{
					QTdQ(0, 1) = QTVTdWVQ(0, 1) / l2_minus_l1;
					QTdQ(1, 0) = -QTdQ(0, 1);
					dPBdF(i, j) = -m_ExR * V * Q* (QTdQ*dpsi_L + dpsi_L*QTdQ.transpose() + dpsi2_L*delta_L) * Q.transpose() * V.transpose();

					////sanity check
					//EigenMatrix2 LHS = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal() * QTdQ.transpose();
					//EigenMatrix2 RHS = QTVTdWVQ;
					//ScalarType diff = (LHS-RHS).norm();
					//if (diff > LARGER_EPSILON)
					//{
					//	std::cout << "diff = " << diff << std::endl;
					//	std::cout << "LHS = \n" << LHS << std::endl;
					//	std::cout << "RHS = \n" << RHS << std::endl;
					//}
				}
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0] - H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j] - H_blocks[1][j];
	}

	//// debug finite differences
	//std::vector<SparseMatrixTriplet> hessian_triplets_FD = hessian_triplets;
	//EvaluateHessianSpectralSDPFD(x, hessian_triplets_FD, d_psi, d2_psi, multiplier);
	//// sanity check: check positive definiteness:
	//Eigen::MatrixXd HB(6,6);
	//for (unsigned int i = 0; i < 3; i++)
	//{
	//	for (unsigned int j = 0; j < 3; j++)
	//	{
	//		EigenMatrix2& block = H_blocks[i][j];
	//		for (unsigned int row = 0; row < 2; row ++)
	//		{
	//			for (unsigned int col = 0; col < 2; col ++)
	//			{
	//				HB(i*2+row, j*2+col) = block(row, col);
	//			}
	//		}
	//	}
	//}
	//VectorX eigs_of_HB = HB.eigenvalues().real();
	//for (unsigned int i = 0; i != eigs_of_HB.size(); ++i)
	//{
	//	if(eigs_of_HB(i) < -LARGER_EPSILON)
	//	{
	//		std::cout << "warning, the block of hessian is not SPD" << std::endl;
	//	}
	//}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row++)
			{
				for (unsigned int col = 0; col < 2; col++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i] * 2 + row, m_p[j] * 2 + col, block(row, col)));
				}
			}
		}
	}
}

void TriangleElementConstraint::EvaluateGradientPennonFD(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType(*psi)(ScalarType, ScalarType),
	ScalarType multiplier)
{
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergyPennon(x, psi, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(LARGER_EPSILON, 0);
	dt[1] = EigenVector2(0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 3; i++)
	{
		ScalarType temp[2];
		for (unsigned int dim = 0; dim < 2; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<2, 1>(m_p[i] * 2, 0) += dt[dim];
			energy_prime = EvaluateEnergyPennon(x_plus_t, psi, multiplier);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		gradient(m_p[i] * 2, 0) += temp[0];
		gradient(m_p[i] * 2 + 1, 0) += temp[1];
	}
}
void TriangleElementConstraint::EvaluateHessianPennonFD(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType(*d_psi)(ScalarType, ScalarType), \
	ScalarType(*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	unsigned int size = x.size();
	VectorX gradient(size);
	VectorX gradient_prime(size);
	gradient.setZero();
	EvaluateGradientPennon(x, gradient, d_psi, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-8, 0);
	dt[1] = EigenVector2(0, 1e-8);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2, 1>(m_p[j] * 2, 0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientPennon(x_plus_t, gradient_prime, d_psi, multiplier);
				temp[dim] = (gradient_prime.block<2, 1>(m_p[i] * 2, 0) - gradient.block<2, 1>(m_p[i] * 2, 0)) * 1e8;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row++)
			{
				for (unsigned int col = 0; col < 2; col++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i] * 2 + row, m_p[j] * 2 + col, block(row, col)));
				}
			}
		}
	}

}

void TriangleElementConstraint::UpdateLagrangianPennon(\
	const VectorX& x_old, \
	const VectorX& x_new, \
	ScalarType(*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	EigenMatrix2 F_new;
	getDeformationGradient(x_new, F_new);
	EigenMatrix2 W_new = 0.5 * (m_ExR.transpose()*F_new + F_new.transpose()*m_ExR);
	EigenMatrix2 A = -W_new;

	ScalarType p = 1.0 / multiplier;
	EigenMatrix2 Z = (A - p*EigenMatrix2::Identity()).inverse();

	m_U = p*p*Z*m_U*Z;
}

ScalarType TriangleElementConstraint::EvaluateEnergyNoConstraint(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	//// get deformation gradient
	//EigenMatrix2 F; 
	//EigenMatrix2 Ds;
	//getEdgeMatrix(x, Ds);
	//F = Ds * m_Dm_inv;

	//ScalarType energy = 0.0;
	//if ((*p_mu) > 0) // elastic term
	//{
	//	energy += (*p_mu)*(*p_multiplier)*(F-m_ExR).squaredNorm();
	//}
	//if ((*p_lambda) > 0) // poisson term
	//{
	//	EigenMatrix2 I; I.setIdentity();
	//	energy += 0.5*(*p_lambda)*(*p_multiplier)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	//}

	//energy *= m_area;

	//return energy;

	return EvaluateEnergyNoConstraintCore(x, multiplier);
}
void TriangleElementConstraint::EvaluateGradientNoConstraint(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
//	//EvaluateGradientFiniteDifferencePenalty(x, gradient);
//	//return;
//
//	// get deformation gradient
//	EigenMatrix2 F; 
//	EigenMatrix2 Ds;
//	getEdgeMatrix(x, Ds);
//	F = Ds * m_Dm_inv;
//
//	EigenMatrix2 P; // stress tensor
//	P.setZero();
//
//	if ((*p_mu) > 0) // elastic term
//	{
//		P += 2*(*p_mu)*(*p_multiplier)*(F-m_ExR);
//	}
//	if ((*p_lambda) > 0) // poisson term
//	{
//		EigenMatrix2 I; I.setIdentity();
//		P += (*p_lambda)*(*p_multiplier)*(m_ExR.transpose()*F-I).trace()*m_ExR;
//	}
//
//	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();
//
//	EigenVector2 g[3];
//	g[0] = H.block<2,1>(0, 0);
//	g[1] = H.block<2,1>(0, 1);
//	g[2] = -g[0]-g[1];
//
//	for (unsigned int i = 0; i < 3; i++)
//	{
//		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
//	}
	VectorX gradient_block;
	EvaluateGradientNoConstraintCore(x, gradient_block, multiplier);

	for (unsigned int i = 0; i != 3; i ++)
	{
		gradient.block<2,1>(2*m_p[i], 0) += gradient_block.block<2,1>(2*i, 0);
	}
}
void TriangleElementConstraint::EvaluateHessianNoConstraint(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	//// get deformation gradient
	//EigenMatrix2 F; 
	//EigenMatrix2 Ds;
	//getEdgeMatrix(x, Ds);
	//F = Ds * m_Dm_inv;

	//Matrix2222 dPdF;
	//dPdF.SetZero();

	//if ((*p_mu) > 0) // elastic term
	//{
	//	Matrix2222 I;
	//	I.SetIdentity();
	//	dPdF = dPdF + I * (2*(*p_mu)*(*p_multiplier));
	//}
	//if ((*p_lambda) > 0) // poisson term
	//{
	//	Matrix2222 dP2dF;
	//	directProduct(dP2dF, (*p_lambda)*(*p_multiplier)*m_ExR, m_ExR);
	//	dPdF = dPdF + dP2dF;
	//}

	//Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	//EigenMatrix2 H_blocks[3][3];

	//// i == 0 to 1 case 
	//for (unsigned int i = 0; i < 2; i++)
	//{
	//	// j == 0 to 1 case
	//	for (unsigned int j = 0; j < 2; j++)
	//	{
	//		EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
	//		EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

	//		H_blocks[i][j].block<2, 1>(0, 0) = v0;
	//		H_blocks[i][j].block<2, 1>(0, 1) = v1;
	//	}
	//	// j == 2 case
	//	H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	//}
	//// i == 2 case
	//for (unsigned int j = 0; j < 3; j++)
	//{
	//	H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	//}

	//// set to triplets
	//for (unsigned int i = 0; i < 3; i++)
	//{
	//	for (unsigned int j = 0; j < 3; j++)
	//	{
	//		EigenMatrix2& block = H_blocks[i][j];
	//		for (unsigned int row = 0; row < 2; row ++)
	//		{
	//			for (unsigned int col = 0; col < 2; col ++)
	//			{
	//				hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
	//			}
	//		}
	//	}
	//}
	EigenMatrix hessian_block;
	EvaluateHessianNoConstraintCore(x, hessian_block, multiplier);

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, hessian_block(i*2+row, j*2+col)));
				}
			}
		}
	}
}

// energy gradient hessian for a gerneral barrier function
ScalarType TriangleElementConstraint::EvaluateEnergyGeneralBarrier(const VectorX& x, ScalarType (*psi)(ScalarType), ScalarType multiplier, bool throwable, ScalarType regularization_mu)
{
	ScalarType objective_energy = EvaluateEnergyNoConstraintCore(x, 1);

	ScalarType barrier_energy = 0;
	if ((*p_enable_non_inversion))
	{
		ScalarType W11, detW;
		EvaluateEnergyNoninversionConstraintCore(x, W11, detW);
		if (throwable)
		{
			if (detW < 0)
				throw EXCEPTION_LOG_OF_NEGATIVE;
			if (m_noninversion_criteria == NC_DETERMINANT_W || m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
			{
				if (W11 < 0)
				{
					throw EXCEPTION_LOG_OF_NEGATIVE;
				}
			}
		}
		barrier_energy = m_area*(*p_beta)*psi(detW);
		barrier_energy = barrier_energy / multiplier;
	}

	return objective_energy + barrier_energy;
}
void TriangleElementConstraint::EvaluateGradientGeneralBarrier(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX objective_gradient;
	EvaluateGradientNoConstraintCore(x, objective_gradient);

	VectorX barrier_gradient(6);
	barrier_gradient.setZero();
	if ((*p_enable_non_inversion))
	{
		VectorX detW_gradient;
		EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
		barrier_gradient = m_area*(*p_beta)*d_psi(EvaluateEnergyNoninversionConstraintCore(x))*detW_gradient;

		barrier_gradient = barrier_gradient /multiplier;
	}

	VectorX gradient_block = objective_gradient+barrier_gradient;
	for (unsigned int i = 0; i != 3; i ++)
	{
		gradient.block<2,1>(2*m_p[i], 0) += gradient_block.block<2,1>(2*i, 0);
	}
}
void TriangleElementConstraint::EvaluateHessianGeneralBarrier(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier, ScalarType regularization_mu)
{
	EigenMatrix objective_hessian;
	EvaluateHessianNoConstraintCore(x, objective_hessian);

	EigenMatrix barrier_hessian(6,6);
	barrier_hessian.setZero();

	if ((*p_enable_non_inversion))
	{
		ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);
		VectorX detW_gradient;
		EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
		EigenMatrix detW_hessian;
		EvaluateHessianNoninversionConstraintCore(x, detW_hessian);
		barrier_hessian = m_area*(*p_beta)*(d2_psi(detW)*detW_gradient*detW_gradient.transpose()+d_psi(detW)*detW_hessian);

		barrier_hessian = barrier_hessian / multiplier;
		//// DEBUG Finite difference version of hessian
		//EigenMatrix det_hessian_FD;
		//EvaluateHessianNoninversionConstraintCoreFD(x, det_hessian_FD);
		//// DEBUG for positive definiteness of hessian
		//VectorX ev1 = objective_hessian.eigenvalues().real();
		//VectorX evdetW = detW_hessian.eigenvalues().real();
		//VectorX ev2 = barrier_hessian.eigenvalues().real();
	}

	EigenMatrix hessian_block = objective_hessian + barrier_hessian;

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, hessian_block(i*2+row, j*2+col)));
				}
			}
		}
	}
}

void TriangleElementConstraint::EvaluateGradientPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier)
{
	ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);

	VectorX objective_gradient;
	EvaluateGradientNoConstraintCore(x, objective_gradient);

	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	VectorX barrier_gradient = m_area * detW_gradient * lambda(m_element_id);

	// dE/dx part
	VectorX gradient_block = objective_gradient-barrier_gradient;
	for (unsigned int i = 0; i != 3; i ++)
	{
		gradient.block<2,1>(2*m_p[i], 0) += gradient_block.block<2,1>(2*i, 0);
	}

	// dE/dlambda part
	gradient(x.size() + m_element_id) += detW * lambda(m_element_id) - 1/multiplier;
}

void TriangleElementConstraint::EvaluateHessianPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier)
{
	//EvaluateHessianPrimalDualGeneralBarrierFD(x, lambda, hessian_triplets, d_psi, d2_psi, multiplier);
	//return;

	//std::vector<SparseMatrixTriplet> ht = hessian_triplets;
	//EvaluateHessianPrimalDualGeneralBarrierFD(x, lambda, ht, d_psi, d2_psi, multiplier);

	EigenMatrix objective_hessian;
	EvaluateHessianNoConstraintCore(x, objective_hessian);

	ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);
	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	EigenMatrix detW_hessian;
	EvaluateHessianNoninversionConstraintCore(x, detW_hessian);
	EigenMatrix barrier_hessian = m_area * detW_hessian*lambda(m_element_id);

	EigenMatrix hessian_block = objective_hessian - barrier_hessian;

	////debug a = 1
	//if (detW < 1e-20)
	//{
	//	std::cout << "Element id:" << m_element_id << std::endl;
	//	std::cout << "detW or detF:" << detW << std::endl;
	//	std::cout << hessian_block << std::endl;
	//}

	// d2L/dx2 set to triplets 
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, hessian_block(i*2+row, j*2+col)));
				}
			}
		}
	}

	// d2L/dxdlambda bottom and right part
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int row = 0; row < 2; row++)
		{
			// right
			hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2 + row, x.size()+m_element_id, -m_area * detW_gradient(i*2+row)));
			// bottm
			hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, m_p[i]*2 + row, lambda(m_element_id)*detW_gradient(i*2+row)));
		}
	}

	//VectorX x_plus_dx = x;
	//VectorX problem_row(6);
	//for (unsigned int i = 0; i < 3; i++)
	//{
	//	for (unsigned int dim = 0; dim != 2; dim ++)
	//	{
	//		x_plus_dx = x;
	//		x_plus_dx(m_p[i]*2+dim) += 1e-6;

	//		ScalarType detW_x_plus_dx = EvaluateEnergyNoninversionConstraintCore(x_plus_dx);
	//		problem_row(i*2+dim) = lambda(m_element_id)*(detW_x_plus_dx-detW)*1e6;
	//	}
	//}


	// d2L/dlambda2
	hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, x.size()+m_element_id, detW));
//
//#ifdef ENABLE_MATLAB_DEBUGGING
//	//g_debugger->SendSparseMatrixUsingTriplets(ht, "HFD", x.size()+lambda.size(), x.size()+lambda.size());
//	g_debugger->SendSparseMatrixUsingTriplets(hessian_triplets, "H", x.size()+lambda.size(), x.size()+lambda.size());
//#endif
}

void TriangleElementConstraint::EvaluateHessianPrimalDualGeneralBarrierFD(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier)
{
	VectorX x_plus_dx = x;
	VectorX lambda_plus_dlambda = lambda;
	VectorX g(x.size()+lambda.size());
	VectorX g_prime(x.size()+lambda.size());
	g.setZero();
	EvaluateGradientPrimalDualGeneralBarrier(x, lambda, g, d_psi, multiplier);

	for (unsigned int i = 0; i != 3; i ++)
	{
		for (unsigned int dim = 0; dim != 2; dim ++)
		{
			x_plus_dx = x;
			x_plus_dx(m_p[i]*2+dim) += 1e-6;

			g_prime.setZero();
			EvaluateGradientPrimalDualGeneralBarrier(x_plus_dx, lambda, g_prime, d_psi, multiplier);
			VectorX diff = (g_prime-g)*1e6;

			for (unsigned int j = 0; j!=3; j++)
			{
				for (unsigned int dimj = 0; dimj!=2; dimj++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[j]*2+dimj, m_p[i]*2+dim, diff(m_p[j]*2+dimj)));
				}
			}
			hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, m_p[i]*2+dim, diff(x.size()+m_element_id)));
		}
	}

	lambda_plus_dlambda = lambda;
	lambda_plus_dlambda(m_element_id) += 1e-6;
	g_prime.setZero();
	EvaluateGradientPrimalDualGeneralBarrier(x, lambda_plus_dlambda, g_prime, d_psi, multiplier);
	VectorX diff = (g_prime-g)*1e6;

	for (unsigned int j = 0; j!=3; j++)
	{
		for (unsigned int dimj = 0; dimj!=2; dimj++)
		{
			hessian_triplets.push_back(SparseMatrixTriplet(m_p[j]*2+dimj, x.size()+m_element_id, diff(m_p[j]*2+dimj)));
		}
	}
	hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, x.size()+m_element_id, diff(x.size()+m_element_id)));
}

ScalarType TriangleElementConstraint::EvaluateEnergyCorot(const VectorX& x)
{
	EigenMatrix2 F;
	EigenMatrix2 R;
	getDeformationGradient(x, F);
	getRotationMatrix(F, R);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-R).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((R.transpose()*F-I).trace(), 2);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientCorot(const VectorX& x, VectorX& gradient)
{
	VectorX g2 = gradient;
	// FD corot gradient
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergyCorot(x);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(LARGER_EPSILON, 0);
	dt[1] = EigenVector2(0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 3; i++)
	{
		ScalarType temp[2];
		for (unsigned int dim = 0; dim < 2; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<2,1>(m_p[i]*2,0) += dt[dim];
			energy_prime = EvaluateEnergyCorot(x_plus_t);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		g2(m_p[i]*2,0) += temp[0];
		g2(m_p[i]*2+1,0) += temp[1];
	}

	gradient = g2;
	return;

	EigenMatrix2 F;
	EigenMatrix2 R;
	getDeformationGradient(x, F);
	getRotationMatrix(F, R);

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-R);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix2 I; I.setIdentity();
		P += (*p_lambda)*(R.transpose()*F-I).trace()*R;
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}
void TriangleElementConstraint::EvaluateHessianCorot(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	EigenMatrix2 F;
	EigenMatrix2 R;
	getDeformationGradient(x, F);
	Eigen::JacobiSVD<EigenMatrix2> svd(F,  Eigen::ComputeFullU | Eigen::ComputeFullV);
	EigenMatrix2 U = svd.matrixU();
	EigenMatrix2 V = svd.matrixV();
	EigenVector2 SIGMA = svd.singularValues();
	ScalarType detU = U.determinant();
	ScalarType detV = V.determinant();
	if (detU < 0)
	{
		U.block<2,1>(0,1) *= -1;	
		SIGMA[1] *= -1;
	}
	if (detV < 0)
	{
		V.block<2,1>(0,1) *= -1;
		SIGMA[1] *= -1;
	}
	R = U*V.transpose();

	Matrix2222 dPdF;
	dPdF.SetZero();

	// preparation:
	// svd Derivative 
	Matrix2222 I;
	I.SetIdentity();
	Matrix2222 dRdF;
	dRdF.SetZero();
	bool similar_singular_value = false;
	EigenMatrix2 M;
	if (abs(SIGMA(0)-SIGMA(1)) > EPSILON)
	{
		M(0,0) = M(1,1) = SIGMA(1);
		M(0,1) = M(1,0) = -SIGMA(0);
		similar_singular_value = false;
	}
	else
	{
		similar_singular_value = true;
	}
	for (unsigned int i = 0; i != 2; i++)
	{
		for (unsigned int j = 0; j != 2; j++)
		{
			EigenMatrix2 rhs = U*I(i, j)*V.transpose();
			rhs(0,0)=0; rhs(1,1) = 0;
			EigenMatrix2 U_tilde_plus_V_tilde_transpose;
			if (similar_singular_value == true)
			{
				U_tilde_plus_V_tilde_transpose = rhs/SIGMA(0);
			}
			else
			{
				EigenVector2 rhs_ab;
				rhs_ab(0) = rhs(0, 1);
				rhs_ab(1) = rhs(1, 0);
				EigenVector2 uv = M.inverse() * rhs_ab;

				EigenMatrix2 U_tilde;
				U_tilde.setZero();
				U_tilde(0, 1) = uv(0);
				U_tilde(1, 0) = -uv(0);
				EigenMatrix2 V_tilde;
				V_tilde.setZero();
				V_tilde(0, 1) = uv(1);
				V_tilde(1, 0) = -uv(1);

				U_tilde_plus_V_tilde_transpose = U_tilde + V_tilde.transpose();
			}
			dRdF(i,j) = U*U_tilde_plus_V_tilde_transpose*V.transpose();
		}
	}

	if ((*p_mu) > 0) // elastic term
	{
		dPdF = dPdF + (I-dRdF) * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, R, R);

		Matrix2222 dPdR;
		directProduct(dPdR, F, R);
		dPdR = dPdR + I * ((R.transpose()*F-EigenMatrix2::Identity()).trace());

		Matrix2222 dP3dF;
		dP3dF.SetZero();
		for (unsigned int i = 0; i != 2; i++)
		{
			for (unsigned int j = 0; j != 2; j++)
			{
				dP3dF(i, j) = dPdR.FrobeniusProduct(dRdF(i,j));
			}
		}

		dPdF = dPdF + (dP2dF + dP3dF)*(*p_lambda);
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}
void TriangleElementConstraint::EvaluateHessianCorotLocalGlobalStyle(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	EigenMatrix2 F;
	EigenMatrix2 R;
	getDeformationGradient(x, F);
	getRotationMatrix(F, R);

	Matrix2222 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix2222 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix2222 dP2dF;
		directProduct(dP2dF, (*p_lambda)*R, R);
		dPdF = dPdF + dP2dF;
	}

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}
void TriangleElementConstraint::EvaluateHessianCorotConstant1(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	Matrix2222 dPdF;
	dPdF.SetZero();

	Matrix2222 I;
	I.SetIdentity();
	dPdF = dPdF + I * (2*(*p_mu) + (*p_lambda));

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}
void TriangleElementConstraint::EvaluateHessianCorotConstant2(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	Matrix2222 dPdF;
	dPdF.SetZero();

	Matrix2222 I;
	I.SetIdentity();
	dPdF = dPdF + I * (2*(*p_mu));

	Matrix2222 dH = dPdF * ((m_Dm_inv.transpose()) * m_area);

	EigenMatrix2 H_blocks[3][3];

	// i == 0 to 1 case 
	for (unsigned int i = 0; i < 2; i++)
	{
		// j == 0 to 1 case
		for (unsigned int j = 0; j < 2; j++)
		{
			EigenVector2 v0 = dH(0, j)*m_Dm_inv.transpose().block<2, 1>(0, i);
			EigenVector2 v1 = dH(1, j)*m_Dm_inv.transpose().block<2, 1>(0, i);

			H_blocks[i][j].block<2, 1>(0, 0) = v0;
			H_blocks[i][j].block<2, 1>(0, 1) = v1;
		}
		// j == 2 case
		H_blocks[i][2] = -H_blocks[i][0]-H_blocks[i][1];
	}
	// i == 2 case
	for (unsigned int j = 0; j < 3; j++)
	{
		H_blocks[2][j] = -H_blocks[0][j]-H_blocks[1][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}

ScalarType TriangleElementConstraint::EvaluateEnergyNeoHookean(const VectorX& x)
{
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	//Eigen::JacobiSVD<EigenMatrix2> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
	//EigenMatrix2 U = svd.matrixU();
	//EigenMatrix2 V = svd.matrixV();
	//EigenVector2 SIGMA = svd.singularValues();
	//ScalarType detU = U.determinant();
	//ScalarType detV = V.determinant();
	//if (detU < 0)
	//{
	//	U.block<2, 1>(0, 1) *= -1;
	//	SIGMA[1] *= -1;
	//}
	//if (detV < 0)
	//{
	//	V.block<2, 1>(0, 1) *= -1;
	//	SIGMA[1] *= -1;
	//}

	//ScalarType clamp_point = 0.01;
	//ScalarType I1 = 0;
	//ScalarType J = 1;
	//for (unsigned int i = 0; i != 2; i++)
	//{
	//	if (SIGMA(i) < clamp_point)
	//	{
	//		SIGMA(i) = clamp_point;
	//	}
	//	//if (i == 2 && SIGMA(i) < clamp_point)
	//	//{
	//	//	SIGMA(i) = clamp_point;
	//	//}
	//	//assert(SIGMA(i) > 0);
	//	I1 += SIGMA(i) * SIGMA(i);
	//	J *= SIGMA(i);
	//}
	//ScalarType logJ = std::log(J);

	EigenMatrix2 FTF = F.transpose()*F;
	ScalarType I1 = FTF.trace();
	ScalarType I3 = FTF.determinant();
	ScalarType J = F.determinant();

	ScalarType logJ;
	//// neo hookean
	//if (J <= 0 )
	//{
	//	throw EXCEPTION_LOG_OF_NEGATIVE;
	//}
	//logJ = std::log(J);

	// clamped neo hookean
	const ScalarType clamp_point = 0.01;
	if (J > clamp_point)
	{
		logJ = std::log(J);
	}
	else
	{
		//logJ = std::log(clamp_point) + 1.0 / clamp_point*(J - clamp_point) - 0.5 / (clamp_point*clamp_point)*(J - clamp_point)*(J - clamp_point);
		logJ = std::log(clamp_point);
		//std::cout << "objective clamped" << std::endl;
	}

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += 0.5*(*p_mu)*(I1-3) - (*p_mu)*logJ;
	}
	if ((*p_lambda) > 0) // poisson term
	{
		energy += 0.5*(*p_lambda)*std::pow(logJ, 2);
	}

	energy *= m_area;

	return energy;
}
void TriangleElementConstraint::EvaluateGradientNeoHookean(const VectorX& x, VectorX& gradient)
{
	EigenMatrix2 F;
	getDeformationGradient(x, F);

	//Eigen::JacobiSVD<EigenMatrix2> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
	//EigenMatrix2 U = svd.matrixU();
	//EigenMatrix2 V = svd.matrixV();
	//EigenVector2 SIGMA = svd.singularValues();
	//ScalarType detU = U.determinant();
	//ScalarType detV = V.determinant();
	//if (detU < 0)
	//{
	//	U.block<2, 1>(0, 1) *= -1;
	//	SIGMA[1] *= -1;
	//}
	//if (detV < 0)
	//{
	//	V.block<2, 1>(0, 1) *= -1;
	//	SIGMA[1] *= -1;
	//}
	//ScalarType clamp_point = 0.01;
	//ScalarType J = 1;
	//for (unsigned int i = 0; i != 2; i++)
	//{
	//	if (SIGMA(i) < clamp_point)
	//	{
	//		SIGMA(i) = clamp_point;
	//	}
	//	//if (i == 2 && SIGMA(i) < clamp_point)
	//	//{
	//	//	SIGMA(i) = clamp_point;
	//	//}
	//	//assert(SIGMA(i) > 0);
	//	J *= SIGMA(i);
	//}
	//ScalarType logJ = std::log(J);
	//F = U*SIGMA.asDiagonal()*V.transpose();
	//EigenMatrix2 FinvT = F.inverse().transpose();

	EigenMatrix2 FinvT = F.inverse().transpose();
	ScalarType J = F.determinant();

	ScalarType logJ;

	//// neo hookean
	//if (J <= 0 )
	//{
	//	throw EXCEPTION_LOG_OF_NEGATIVE;
	//}
	//logJ = std::log(J);

	// clamped neo hookean
	const ScalarType clamp_point = 0.01;
	if (J > clamp_point)
	{
		logJ = std::log(J);
	}
	else
	{
		//logJ = std::log(clamp_point) + 1.0 / clamp_point*(J - clamp_point) - 0.5 / (clamp_point*clamp_point)*(J - clamp_point)*(J - clamp_point);
		logJ = std::log(clamp_point);
		FinvT.setZero();
		//std::cout << "gradient clamped" << std::endl;
	}

	EigenMatrix2 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += (*p_mu)*(F - FinvT);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		P += (*p_lambda)*logJ*FinvT;
	}

	EigenMatrix2 H = m_area * P * m_Dm_inv.transpose();

	EigenVector2 g[3];
	g[0] = H.block<2,1>(0, 0);
	g[1] = H.block<2,1>(0, 1);
	g[2] = -g[0]-g[1];

	for (unsigned int i = 0; i < 3; i++)
	{
		gradient.block<2, 1>(2*m_p[i], 0) += g[i];
	}
}


// alternating projection
bool TriangleElementConstraint::CorrectToNoninversion(VectorX& f, unsigned int& index)
{
	bool result = true;
	EigenMatrix2 F;

	// retrieve F from f
	for (unsigned int i = 0; i < 2; ++i)
	{
		F.block<2, 1>(0, i) = f.block<2, 1>(index + i * 2, 0);
	}

	EigenMatrix2 M = m_ExR.transpose() * F;
	EigenMatrix2 Ms = 0.5 * (M + M.transpose());
	EigenMatrix2 Ma = M - Ms;

	Eigen::EigenSolver<EigenMatrix2> evd_solver(Ms);
	EigenMatrix2 V = evd_solver.eigenvectors().real();
	EigenVector2 eigs = evd_solver.eigenvalues().real();

	// **key** modify the eigenvalues to make Ms positive definite.
	for (unsigned int i = 0; i != 2; ++i)
	{
		if (eigs(i) < LARGER_EPSILON)
		{
			//std::cout << "The " << i << "_th eigenvalue of the " << m_element_id << "_th element: " << eigs(i) << std::endl; 
			eigs(i) = LARGER_EPSILON;
			result = false;
		}
	}

	EigenMatrix2 S = eigs.asDiagonal();

	F = m_ExR * (V*S*V.transpose() + Ma);

	// vectorize back to f
	for (unsigned int i = 0; i < 2; ++i)
	{
		f.block<2, 1>(index + i * 2, 0) = F.block<2, 1>(0, i);
	}

	index += 4;
	return result;
}

void TriangleElementConstraint::EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index)
{
	Eigen::Matrix<ScalarType, 2, 3> IND;
	IND.block<2, 2>(0, 0) = EigenMatrix2::Identity();
	IND.block<2, 1>(0, 2) = EigenVector2(-1, -1);

	Eigen::Matrix<ScalarType, 2, 3> G = m_Dm_inv.transpose() * IND;

	for (unsigned int i = 0; i < 2; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			ScalarType coeff = G(i, j);

			g_triplets.push_back(SparseMatrixTriplet(index + i * 2 + 0, 2 * m_p[j] + 0, coeff));
			g_triplets.push_back(SparseMatrixTriplet(index + i * 2 + 1, 2 * m_p[j] + 1, coeff));
		}
	}
	index += 4;
}

void TriangleElementConstraint::GetVectorizedExrotationField(VectorX& r, unsigned int& index)
{
	for (unsigned int i = 0; i < 2; ++i)
	{
		r.block<2, 1>(index + i * 2, 0) = m_ExR.block<2, 1>(0, i);
	}
	index += 4;
}

void TriangleElementConstraint::EvaluateVectorizedDeformationGradient(const VectorX& x, VectorX& f)
{
	EigenMatrix2 F;
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	// vectorize to f
	for (unsigned int i = 0; i < 2; ++i)
	{
		f.block<2, 1>(m_element_id * 4 + i * 2, 0) = F.block<2, 1>(0, i);
	}
}

#pragma region tri_debugging
void TriangleElementConstraint::EvaluateGradientFiniteDifference(const VectorX& x, VectorX& gradient)
{
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergy(x);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(LARGER_EPSILON, 0);
	dt[1] = EigenVector2(0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 3; i++)
	{
		ScalarType temp[2];
		for (unsigned int dim = 0; dim < 2; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<2,1>(m_p[i]*2,0) += dt[dim];
			energy_prime = EvaluateEnergy(x_plus_t);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		gradient(m_p[i]*2,0) += temp[0];
		gradient(m_p[i]*2+1,0) += temp[1];
	}
}

void  TriangleElementConstraint::EvaluateHessianFiniteDifference(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradient(x, gradient);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-10, 0);
	dt[1] = EigenVector2(0, 1e-10);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2,1>(m_p[j]*2,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradient(x_plus_t, gradient_prime);
				temp[dim] = (gradient_prime.block<2,1>(m_p[i]*2,0) - gradient.block<2,1>(m_p[i]*2,0)) * 1e10;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}

void  TriangleElementConstraint::EvaluateHessianLocalGlobalFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradientLocalGlobal(x, gradient, multiplier);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-10, 0);
	dt[1] = EigenVector2(0, 1e-10);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2,1>(m_p[j]*2,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientLocalGlobal(x_plus_t, gradient_prime, multiplier);
				temp[dim] = (gradient_prime.block<2,1>(m_p[i]*2,0) - gradient.block<2,1>(m_p[i]*2,0)) * 1e10;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}

void TriangleElementConstraint::EvaluateGradientFiniteDifferencePenalty(const VectorX& x, VectorX& gradient)
{
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergyNoConstraint(x);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(LARGER_EPSILON, 0);
	dt[1] = EigenVector2(0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 3; i++)
	{
		ScalarType temp[2];
		for (unsigned int dim = 0; dim < 2; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<2,1>(m_p[i]*2,0) += dt[dim];
			energy_prime = EvaluateEnergyNoConstraint(x_plus_t);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		gradient(m_p[i]*2,0) += temp[0];
		gradient(m_p[i]*2+1,0) += temp[1];
	}
}

void  TriangleElementConstraint::EvaluateHessianFiniteDifferencePenalty(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradientNoConstraint(x, gradient);

	EigenVector2 dt[2];
	dt[0] = EigenVector2(1e-10, 0);
	dt[1] = EigenVector2(0, 1e-10);
	VectorX x_plus_t;

	EigenMatrix2 H_blocks[3][3];
	// finite difference p0, x
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector2 temp[2];
			for (unsigned int dim = 0; dim < 2; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<2,1>(m_p[j]*2,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientNoConstraint(x_plus_t, gradient_prime);
				temp[dim] = (gradient_prime.block<2,1>(m_p[i]*2,0) - gradient.block<2,1>(m_p[i]*2,0)) * 1e10;
			}

			H_blocks[i][j].block<2, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<2, 1>(0, 1) = temp[1];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenMatrix2& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 2; row ++)
			{
				for (unsigned int col = 0; col < 2; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*2+row, m_p[j]*2+col, block(row, col)));
				}
			}
		}
	}
}


EigenMatrix2 TriangleElementConstraint::GetW(const VectorX& x)
{
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	return 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
}

EigenMatrix2 TriangleElementConstraint::GetF(const VectorX& x)
{
	EigenMatrix2 F; 
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	return F;
}
#pragma endregion

void TriangleElementConstraint::getEdgeMatrix(const VectorX& x, EigenMatrix2& D)
{
	D.block<2, 1>(0, 0) = x.block<2, 1>(2*m_p[0], 0) - x.block<2, 1>(2*m_p[2], 0);
	D.block<2, 1>(0, 1) = x.block<2, 1>(2*m_p[1], 0) - x.block<2, 1>(2*m_p[2], 0);
}

void TriangleElementConstraint::getDeformationGradient(const VectorX& x, EigenMatrix2& F)
{
	EigenMatrix2 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;
}

void TriangleElementConstraint::getRotationMatrix(const EigenMatrix2& F, EigenMatrix2& R)
{
	Eigen::JacobiSVD<EigenMatrix2> svd(F,  Eigen::ComputeFullU | Eigen::ComputeFullV);
	EigenMatrix2 U = svd.matrixU();
	EigenMatrix2 V = svd.matrixV();
	R = U*V.transpose();
	R.block<2, 1>(0, 1) *= R.determinant();
}

//----------!Triangle Constraint-------//

//----------Tet Constraint-------//
TetElementConstraint::TetElementConstraint(unsigned int element_id, ScalarType *mu, ScalarType *lambda, bool *enable_non_inversion, ScalarType *alpha, ScalarType *beta, ScalarType *gamma, ScalarType *slackness, unsigned int p1, unsigned int p2, unsigned int p3, unsigned int p4, const VectorX& x) :
	Constraint(CONSTRAINT_TYPE_TET_ELEMENT),
	m_element_id(element_id),
	p_mu(mu),
	p_lambda(lambda),
	p_enable_non_inversion(enable_non_inversion),
	p_alpha(alpha),
	p_beta(beta),
	p_gamma(gamma),
	p_slackness(slackness)
{
	m_p[0] = p1;
	m_p[1] = p2;
	m_p[2] = p3;
	m_p[3] = p4;

	getEdgeMatrix(x, m_Dm);
	m_volume = std::abs(m_Dm.determinant() / 6);
	m_Dm_inv = m_Dm.inverse(); // assume there's not degenerated triangle in the reference pose

	m_penalty_gradient.resize(12);
}
TetElementConstraint::TetElementConstraint(const TetElementConstraint& other) :
	Constraint(other),
	m_element_id(other.m_element_id),
	p_mu(other.p_mu),
	p_lambda(other.p_lambda),
	p_enable_non_inversion(other.p_enable_non_inversion),
	p_alpha(other.p_alpha),
	p_beta(other.p_beta),
	p_gamma(other.p_gamma),
	p_slackness(other.p_slackness)
{
	m_p[0] = other.m_p[0];
	m_p[1] = other.m_p[1];
	m_p[2] = other.m_p[2];
	m_p[3] = other.m_p[3];

	m_Dm = other.m_Dm;
	m_volume = other.m_volume;
	m_Dm_inv = other.m_Dm_inv;

	m_penalty_gradient.resize(12);
}
TetElementConstraint::~TetElementConstraint()
{

}

void TetElementConstraint::UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotatino_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d)
{
	EigenQuaternion q;
	q = per_element_rotation_3d[m_element_id];
	m_ExR = q.matrix();
}
void TetElementConstraint::UpdateRotation(const VectorX& x)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Eigen::JacobiSVD<EigenMatrix3> svd(F,  Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	EigenMatrix3 U = svd.matrixU();
	EigenMatrix3 V = svd.matrixV();

	m_R = U*V.transpose();
	m_R.block<3, 1>(0, 2) *= m_R.determinant();
}
ScalarType  TetElementConstraint::EvaluateEnergy(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*multiplier*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*multiplier*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion) // barrier term: -alpha*log(W11-slackness)-betalog(det(W)-slackness), W = 0.5(RTF + FTR)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix3 K = (*p_slackness)*EigenMatrix3::Identity();
		ScalarType WpK11 = (W+K)(0, 0);
		ScalarType detWpK22 = (W+K).block<2,2>(0,0).determinant();
		ScalarType detWpK = (W+K).determinant();

		if (WpK11 <= 0  || detWpK22 <= 0  || detWpK <= 0 )
		{
			throw EXCEPTION_LOG_OF_NEGATIVE;
		}

		energy += -(*p_alpha)*std::log(WpK11) - (*p_beta)*std::log(detWpK22) - (*p_gamma)*std::log(detWpK);
	}

	energy *= m_volume;

	return energy;
}
void  TetElementConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
	// TEST: see if g1 = g at the end of the function
	//VectorX g1=gradient;
	//EvaluateGradientFiniteDifference(x, g1);

	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*multiplier*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		P += (*p_lambda)*multiplier*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix3 K = (*p_slackness)*EigenMatrix3::Identity();
		ScalarType WpK11 = (W+K)(0, 0);
		ScalarType detWpK22 = (W+K).block<2,2>(0,0).determinant();
		ScalarType detWpK = (W+K).determinant();

		EigenMatrix3 PB = EigenMatrix3::Zero();
		PB(0,0) += -(*p_alpha) * 1/WpK11;
		PB.block<2,2>(0,0) += -(*p_beta) * (W+K).block<2,2>(0,0).inverse();
		PB += -(*p_gamma) * (W+K).inverse();
		PB = (m_ExR)*PB;
		
		P += PB;
	}

	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	for (unsigned int i = 0; i < 4; i++)
	{
		gradient.block<3, 1>(3*m_p[i], 0) += g[i];
	}
}
void  TetElementConstraint::EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	// TEST: to see if hessian_triplets matches triplets at the end of this function
	//std::vector<SparseMatrixTriplet> triplets = hessian_triplets;
	//EvaluateHessianFiniteDifference(x, triplets);

	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix3333 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix3333 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu)*multiplier);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*multiplier*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion) // barrier term
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		EigenMatrix3 K = (*p_slackness)*EigenMatrix3::Identity();

		EigenMatrix3 WpK = W+K;
		EigenMatrix2 WpK2 = WpK.block<2,2>(0,0);
		ScalarType WpK1 = WpK(0,0);
		
		EigenMatrix3 WpKinv = WpK.inverse();
		EigenMatrix2 WpK2inv = WpK2.inverse();
		ScalarType WpK1inv = 1/WpK1;

		Matrix3333 I;
		I.SetIdentity();
		Matrix3333 dWdF;
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix3333 dPBdW; // PB = tensor of barrier function
		Matrix3333 dPBdF; 
		dPBdW.SetZero();
		// constructing block by block:
		// alpha 1x1
		dPBdW(0, 0)(0, 0) += (*p_alpha) * ( WpK1inv * WpK1inv ); // for the first term
		// beta 2x2
		for (unsigned int i = 0; i < 2; i++)
		{
			for (unsigned int j = 0; j < 2; j++)
			{
				EigenMatrix2 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j).block<2,2>(0,0) += (*p_beta) * (WpK2inv * dWdWij * WpK2inv);
			}
		}
		// gamma 3x3
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				EigenMatrix3 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j) += (*p_gamma) * WpKinv * dWdWij * WpKinv;
			}
		}
		// multiply the exrotation term
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdW(i, j) = m_ExR * dPBdW(i, j);
			}
		}
		// construct dPBdF
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}

void TetElementConstraint::LocalGlobalLocalStep(const VectorX& x)
{
	EigenMatrix3 F;
	getDeformationGradient(x, F);

	EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

	Eigen::EigenSolver<EigenMatrix3> evd_solver(W);
	EigenMatrix3 V = evd_solver.eigenvectors().real();
	EigenVector3 eigs = evd_solver.eigenvalues().real();
	for (unsigned int i = 0; i != 3; ++i)
	{
		if (eigs(i) < LARGER_EPSILON)
		{
			eigs(i) = LARGER_EPSILON;
			m_W_is_SPD = false;
		}
	}

	m_W_projection = V*eigs.asDiagonal()*V.transpose();
}

ScalarType TetElementConstraint::EvaluateEnergyLocalGlobal(const VectorX& x, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F;
	getDeformationGradient(x, F);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		energy += 0.5 * multiplier * (W-m_W_projection).squaredNorm();
	}

	energy *= m_volume;

	return energy;
}
void TetElementConstraint::EvaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient, ScalarType multiplier)
{
	//EvaluateGradientFiniteDifference(x, gradient);
	//return;

	//VectorX g1=gradient;
	//EvaluateGradientFiniteDifference(x, g1);

	// get deformation gradient
	EigenMatrix3 F;
	getDeformationGradient(x, F);

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		P += multiplier * m_ExR * (W-m_W_projection);
	}

	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	for (unsigned int i = 0; i < 4; i++)
	{
		gradient.block<3, 1>(3*m_p[i], 0) += g[i];
	}
}
void TetElementConstraint::EvaluateHessianLocalGlobal(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F;
	getDeformationGradient(x, F);

	Matrix3333 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix3333 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		Matrix3333 I;
		I.SetIdentity();
		Matrix3333 dWdF;
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix3333 dPBdW; // PB = tensor of barrier function
		Matrix3333 dPBdF; 
		dPBdW.SetIdentity();
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdW(i, j) = multiplier * m_ExR * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}

		dPdF = dPdF + dPBdF;
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}

ScalarType TetElementConstraint::EvaluateEnergyPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F; 
	getDeformationGradient(x, F);

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}
	if (*p_enable_non_inversion)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		EigenVector3 lambdas = W.eigenvalues().real();

		energy += psi(lambdas(0), multiplier) + psi(lambdas(1), multiplier) + psi(lambdas(2), multiplier);
	}

	energy *= m_volume;

	return energy;
}
void TetElementConstraint::EvaluateGradientPenaltyMethod(\
	const VectorX& x, \
	VectorX& gradient, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F; 
	getDeformationGradient(x, F);

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term 0.5*R*((-alpha/W11*[1 0; 0 0]-beta*W^{-T})+(-alpha/W11*[1 0; 0 0]-beta*W^{-T})^T)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		Eigen::EigenSolver<EigenMatrix3> evd_solver(W);
		EigenMatrix3 Q = evd_solver.eigenvectors().real();
		EigenVector3 eigs = evd_solver.eigenvalues().real();
		EigenVector3 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		dpsi_eigs(2) = d_psi(eigs(2), multiplier);
		EigenMatrix3 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();

		P += m_ExR * (Q * dpsi_L * Q.transpose());
	}

	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	for (unsigned int i = 0; i < 4; i++)
	{
		gradient.block<3, 1>(3*m_p[i], 0) += g[i];
	}
}
void TetElementConstraint::EvaluateHessianPenaltyMethod(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	//// debug finite differences
	//std::vector<SparseMatrixTriplet> hessian_triplets_FD = hessian_triplets;
	//EvaluateHessianPenaltyMethodFD(x, hessian_triplets_FD, d_psi, d2_psi, multiplier);

	// get deformation gradient
	EigenMatrix3 F; 
	getDeformationGradient(x, F);

	Matrix3333 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix3333 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion/* && !m_W_is_SPD*/) // barrier term
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
		Eigen::EigenSolver<EigenMatrix3> evd_solver(W);
		EigenMatrix3 Q = evd_solver.eigenvectors().real();
		EigenVector3 eigs = evd_solver.eigenvalues().real();
		EigenVector3 dpsi_eigs;
		dpsi_eigs(0) = d_psi(eigs(0), multiplier);
		dpsi_eigs(1) = d_psi(eigs(1), multiplier);
		dpsi_eigs(2) = d_psi(eigs(2), multiplier);
		EigenVector3 dpsi2_eigs;
		dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
		dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
		dpsi2_eigs(2) = d2_psi(eigs(2), multiplier);
		EigenMatrix3 dpsi_L; 
		dpsi_L = dpsi_eigs.asDiagonal();
		EigenMatrix3 dpsi2_L; 
		dpsi2_L = dpsi2_eigs.asDiagonal();

		Matrix3333 dPBdF; 
		EigenMatrix3 QTdQ;
		QTdQ.setZero();
		EigenMatrix3 delta_Q;
		EigenMatrix3 delta_L;
		delta_L.setZero();

		// compute dPBdF directly
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				// set dW
				EigenMatrix3 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				// construct dQ and dL
				EigenMatrix3 QT_dW_Q = 0.5 * Q.transpose() * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR) * Q;
				for (unsigned int row = 0; row!= 3; row++)
				{
					for (unsigned int col = row+1; col != 3; col++)
					{
						ScalarType lcol_minus_lrow = eigs(col) - eigs(row);
						if (std::abs(lcol_minus_lrow) < EPSILON)
						{
							lcol_minus_lrow = (lcol_minus_lrow>0)?EPSILON:-EPSILON;
						}
						QTdQ(row, col) = QT_dW_Q(row, col)/lcol_minus_lrow;
						QTdQ(col, row) = -QTdQ(row, col);
					}
				}
				delta_Q = Q*QTdQ;
				delta_L(0,0) = QT_dW_Q(0,0);
				delta_L(1,1) = QT_dW_Q(1,1);
				delta_L(2,2) = QT_dW_Q(2,2);

				//// sanity check
				//EigenMatrix3 temp2 = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal()*QTdQ.transpose();
				//EigenMatrix3 diff2 = temp2 - QT_dW_Q;
				//EigenMatrix3 temp = delta_Q * eigs.asDiagonal()*Q.transpose() + Q*delta_L*Q.transpose() + Q*eigs.asDiagonal()*delta_Q.transpose();// temp should be dW
				//EigenMatrix3 diff = temp-0.5 * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR);

				dPBdF(i, j) = m_ExR * (delta_Q*dpsi_L*Q.transpose() + Q*dpsi_L*delta_Q.transpose() + Q*dpsi2_L*delta_L*Q.transpose());
			}
		}

		//Matrix3333 I;
		//I.SetIdentity();
		//Matrix3333 dWdF;
		//for (unsigned int i = 0; i < 3; i++)
		//{
		//	for (unsigned int j = 0; j < 3; j++)
		//	{
		//		dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
		//	}
		//}
		//Matrix3333 dPBdW; // PB = tensor of barrier function
		//dPBdW.SetZero();
		//// upper triangle
		//for (unsigned int i = 0; i < 3; i++)
		//{
		//	for (unsigned int j = i; j < 3; j++)
		//	{
		//		// set dW
		//		EigenMatrix3 delta_W;
		//		delta_W.setZero();
		//		if (i == j)
		//		{
		//			delta_W(i, j) = 1;
		//		}
		//		else
		//		{
		//			delta_W(i, j) = 0.5;
		//			delta_W(j, i) = 0.5;
		//		}

		//		// construct dQ and dL
		//		EigenMatrix3 QTdWQ = Q.transpose() * delta_W * Q;
		//		for (unsigned int row = 0; row!= 3; row++)
		//		{
		//			for (unsigned int col = row+1; col != 3; col++)
		//			{
		//				ScalarType lcol_minus_lrow = eigs(col) - eigs(row);
		//				if (std::abs(lcol_minus_lrow) < EPSILON)
		//				{
		//					lcol_minus_lrow = (lcol_minus_lrow>0)?EPSILON:-EPSILON;
		//				}
		//				QTdQ(row, col) = QTdWQ(row, col)/lcol_minus_lrow;
		//				QTdQ(col, row) = -QTdQ(row, col);
		//			}
		//		}
		//		delta_Q = Q*QTdQ;
		//		delta_L(0,0) = QTdWQ(0,0);
		//		delta_L(1,1) = QTdWQ(1,1);
		//		delta_L(2,2) = QTdWQ(2,2);

		//		dPBdW(i, j) = m_ExR * (delta_Q*dpsi_L*Q.transpose() + Q*dpsi_L*delta_Q.transpose() + Q*dpsi2_L*delta_L*Q.transpose());
		//		if (i != j)
		//		{
		//			dPBdW(j, i) = dPBdW(i, j);
		//		}
		//	}
		//}
		////// lower triangle
		////for (unsigned int i = 0; i < 3; i++)
		////{
		////	for (unsigned int j = 0; j < i; j++)
		////	{
		////		assert((dPBdW(j,i)-dPBdW(j,i).transpose()).squaredNorm() < EPSILON);
		////		dPBdW(i, j) = dPBdW(j, i);
		////	}
		////}

		//// construct dPBdF
		//for (unsigned int i = 0; i < 3; i++)
		//{
		//	for (unsigned int j = 0; j < 3; j++)
		//	{
		//		dPBdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
		//	}
		//}

		dPdF = dPdF + dPBdF;
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}
void TetElementConstraint::EvaluateHessianPenaltyMethodFD(\
	const VectorX& x, \
	std::vector<SparseMatrixTriplet>& hessian_triplets, \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradientPenaltyMethod(x, gradient, d_psi, multiplier);

	EigenVector3 dt[3];
	dt[0] = EigenVector3(1e-6, 0, 0);
	dt[1] = EigenVector3(0, 1e-6, 0);
	dt[2] = EigenVector3(0, 0, 1e-6);
	VectorX x_plus_t;

	EigenMatrix3 H_blocks[4][4];
	// finite difference p0, x
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenVector3 temp[3];
			for (unsigned int dim = 0; dim < 3; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<3,1>(m_p[j]*3,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradientPenaltyMethod(x_plus_t, gradient_prime, d_psi, multiplier);
				temp[dim] = (gradient_prime.block<3,1>(m_p[i]*3,0) - gradient.block<3,1>(m_p[i]*3,0)) * 1e6;
			}

			H_blocks[i][j].block<3, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<3, 1>(0, 1) = temp[1];
			H_blocks[i][j].block<3, 1>(0, 2) = temp[2];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}

bool TetElementConstraint::HasZeroPenaltyEnergy(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType))
{
	EigenMatrix3 F; 
	getDeformationGradient(x, F);
	EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
	EigenVector3 lambdas = W.eigenvalues().real();

	// if energy == zero
	if (psi(lambdas(0), 1) <= 0 && psi(lambdas(1), 1) <= 0 && psi(lambdas(2), 1) <= 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void TetElementConstraint::EvaluateEnergyGradientHessianPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType),  \
	ScalarType (*d_psi)(ScalarType, ScalarType),  \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier)
{
	m_penalty_energy = 0.0;
	m_penalty_gradient.setZero();
	m_penalty_hessian_triplets.clear();

	// preparation
	EigenMatrix3 F; 
	getDeformationGradient(x, F);
	EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
	Eigen::EigenSolver<EigenMatrix3> evd_solver(W);
	EigenMatrix3 Q = evd_solver.eigenvectors().real();
	EigenVector3 eigs = evd_solver.eigenvalues().real();
	EigenVector3 dpsi_eigs;
	dpsi_eigs(0) = d_psi(eigs(0), multiplier);
	dpsi_eigs(1) = d_psi(eigs(1), multiplier);
	dpsi_eigs(2) = d_psi(eigs(2), multiplier);
	EigenVector3 dpsi2_eigs;
	dpsi2_eigs(0) = d2_psi(eigs(0), multiplier);
	dpsi2_eigs(1) = d2_psi(eigs(1), multiplier);
	dpsi2_eigs(2) = d2_psi(eigs(2), multiplier);
	EigenMatrix3 dpsi_L; 
	dpsi_L = dpsi_eigs.asDiagonal();
	EigenMatrix3 dpsi2_L; 
	dpsi2_L = dpsi2_eigs.asDiagonal();

	EigenMatrix3 P; // stress tensor
	P.setZero();
	Matrix3333 dPdF; // differentiate 
	dPdF.SetZero();
	Matrix3333 I_tensor;
	I_tensor.SetIdentity();

	// assembling
	if ((*p_mu) > 0) // elastic term
	{
		// energy
		m_penalty_energy += (*p_mu)*(F-m_ExR).squaredNorm();

		// gradient
		P += 2*(*p_mu)*(F-m_ExR);

		// hessian
		dPdF = dPdF + I_tensor * (2*(*p_mu));
	}
	if ((*p_lambda) > 0) // poisson term
	{
		// energy
		EigenMatrix3 I; I.setIdentity();
		m_penalty_energy += 0.5*(*p_lambda)*std::pow((m_ExR.transpose()*F-I).trace(), 2);

		// gradient
		P += (*p_lambda)*(m_ExR.transpose()*F-I).trace()*m_ExR;

		// hessian
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}
	if (*p_enable_non_inversion)
	{
		// energy
		m_penalty_energy += psi(eigs(0), multiplier) + psi(eigs(1), multiplier) + psi(eigs(2), multiplier);

		// gradient
		P += m_ExR * (Q * dpsi_L * Q.transpose());

		// hessian
		Matrix3333 dWdF;
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I_tensor(i,j) + I_tensor(j, i) * m_ExR);
			}
		}

		Matrix3333 dPBdF; 
		EigenMatrix3 QTdQ;
		QTdQ.setZero();
		EigenMatrix3 delta_Q;
		EigenMatrix3 delta_L;
		delta_L.setZero();

		// compute dPBdF directly
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				// set dW
				EigenMatrix3 delta_F;
				delta_F.setZero();
				delta_F(i, j) = 1;

				// construct dQ and dL
				EigenMatrix3 QT_dW_Q = 0.5 * Q.transpose() * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR) * Q;
				for (unsigned int row = 0; row!= 3; row++)
				{
					for (unsigned int col = row+1; col != 3; col++)
					{
						ScalarType lcol_minus_lrow = eigs(col) - eigs(row);
						if (std::abs(lcol_minus_lrow) < EPSILON)
						{
							lcol_minus_lrow = (lcol_minus_lrow>0)?EPSILON:-EPSILON;
						}
						QTdQ(row, col) = QT_dW_Q(row, col)/lcol_minus_lrow;
						QTdQ(col, row) = -QTdQ(row, col);
					}
				}
				delta_Q = Q*QTdQ;
				delta_L(0,0) = QT_dW_Q(0,0);
				delta_L(1,1) = QT_dW_Q(1,1);
				delta_L(2,2) = QT_dW_Q(2,2);

				//// sanity check
				//EigenMatrix3 temp2 = QTdQ*eigs.asDiagonal() + delta_L + eigs.asDiagonal()*QTdQ.transpose();
				//EigenMatrix3 diff2 = temp2 - QT_dW_Q;
				//EigenMatrix3 temp = delta_Q * eigs.asDiagonal()*Q.transpose() + Q*delta_L*Q.transpose() + Q*eigs.asDiagonal()*delta_Q.transpose();// temp should be dW
				//EigenMatrix3 diff = temp-0.5 * (m_ExR.transpose()*delta_F+delta_F.transpose()*m_ExR);

				dPBdF(i, j) = m_ExR * (delta_Q*dpsi_L*Q.transpose() + Q*dpsi_L*delta_Q.transpose() + Q*dpsi2_L*delta_L*Q.transpose());
			}
		}

		dPdF = dPdF + dPBdF;
	}

	// final touch
	// energy
	m_penalty_energy *= m_volume;
	// gradient
	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();
	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 1);
	g[3] = -g[0]-g[1]-g[2];
	for (unsigned int i = 0; i < 4; i++)
	{
		m_penalty_gradient.block<3, 1>(3*i, 0) += g[i];
	}
	// Hessian
	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					m_penalty_hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}
ScalarType TetElementConstraint::RetrieveEnergy()
{
	return m_penalty_energy;
}
void TetElementConstraint::RetrieveGradient(VectorX& gradient)
{
	for (unsigned int i = 0; i < 4; i++)
	{
		gradient.block<3, 1>(3*m_p[i], 0) += m_penalty_gradient.block<3,1>(3*i, 0);
	}
}
void TetElementConstraint::RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	for (unsigned int i = 0; i < m_penalty_hessian_triplets.size(); ++i)
	{
		hessian_triplets.push_back(m_penalty_hessian_triplets[i]);
	}
}

ScalarType TetElementConstraint::EvaluateEnergyNoConstraint(const VectorX& x, ScalarType multiplier, ScalarType regularization_mu)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*multiplier*(F-m_ExR).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*multiplier*std::pow((m_ExR.transpose()*F-I).trace(), 2);
	}

	energy *= m_volume;

	return energy;
}
void  TetElementConstraint::EvaluateGradientNoConstraint(const VectorX& x, VectorX& gradient, ScalarType multiplier, ScalarType regularization_mu)
{
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*multiplier*(F-m_ExR);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		P += (*p_lambda)*multiplier*(m_ExR.transpose()*F-I).trace()*m_ExR;
	}

	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	for (unsigned int i = 0; i < 4; i++)
	{
		gradient.block<3, 1>(3*m_p[i], 0) += g[i];
	}
}
void  TetElementConstraint::EvaluateHessianNoConstraint(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier, ScalarType regularization_mu)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix3333 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix3333 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu)*multiplier);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*multiplier*m_ExR, m_ExR);
		dPdF = dPdF + dP2dF;
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}

void TetElementConstraint::EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index)
{
	Eigen::Matrix<ScalarType, 3, 4> IND;
	IND.block<3, 3>(0, 0) = EigenMatrix3::Identity();
	IND.block<3, 1>(0, 3) = EigenVector3(-1, -1, -1);

	Eigen::Matrix<ScalarType, 3, 4> G = m_Dm_inv.transpose() * IND;

	for (unsigned int i = 0; i < 3; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			ScalarType coeff = G(i, j);

			g_triplets.push_back(SparseMatrixTriplet(index+i*3+0, 3*m_p[j]+0, coeff));
			g_triplets.push_back(SparseMatrixTriplet(index+i*3+1, 3*m_p[j]+1, coeff));
			g_triplets.push_back(SparseMatrixTriplet(index+i*3+2, 3*m_p[j]+2, coeff));
		}
	}

	index += 9;
}

ScalarType TetElementConstraint::EvaluateEnergyNoninversionConstraintCore(const VectorX& x)
{
	ScalarType energy;

	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		energy =  W.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		energy = F.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix3 W = 0.5 * (m_R.transpose() * F + F.transpose() * m_R);

		energy = W.determinant();
	}

	return energy;
}
void TetElementConstraint::EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_block)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		P += (m_ExR)* (W.determinant() * W.inverse());
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		P += F.determinant() * F.inverse().transpose();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix3 W = 0.5 * (m_R.transpose()*F + F.transpose()*m_R);

		P += (m_R)* (W.determinant() * W.inverse());
	}

	EigenMatrix3 H = P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	gradient_block.resize(12);
	for (unsigned int i = 0; i < 4; i++)
	{
		gradient_block.block<3, 1>(3*i, 0) = g[i];
	}
}
void TetElementConstraint::EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_block)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	Matrix3333 dPdF;
	dPdF.SetZero();

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		ScalarType detW = W.determinant();

		EigenMatrix3 Winv = W.inverse();

		//if (detW < 1e-20)
		//{
		//	//std::cout << "element_id: " << m_element_id << "detW: " << detW << std::endl;
		//}
		ScalarType OneOverDetW = 1/detW;

		EigenMatrix3 detWWinv = detW * Winv;

		Matrix3333 I;
		I.SetIdentity();
		Matrix3333 dWdF;
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dWdF(i, j) = 0.5 * (m_ExR.transpose() * I(i,j) + I(j, i) * m_ExR);
			}
		}

		Matrix3333 dPBdW; // PB = tensor of barrier function
		dPBdW.SetZero();

		// constructing block by block:
		// detW
		directProduct(dPBdW, detWWinv, detWWinv);
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				EigenMatrix3 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j) -= detWWinv * dWdWij * detWWinv;
			}
		}
		dPBdW = dPBdW * OneOverDetW;
		// multiply the exrotation term
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdW(i, j) = m_ExR * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		ScalarType detF = F.determinant();
		EigenMatrix3 Finv = F.inverse();
		EigenMatrix3 FinvT = Finv.transpose();
		//if (detF < 1e-20)
		//{
		//	std::cout << "element_id: " << m_element_id << "detF: " << detF << std::endl;
		//}
		ScalarType OneOverDetF = 1/detF;
		EigenMatrix3 detFFinv = detF * Finv;
		EigenMatrix3 detFFinvT = detF * FinvT;

		// constructing block by block:
		// detW
		directProduct(dPdF, detFFinvT, detFFinvT);
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				EigenMatrix3 dFdFij; dFdFij.setZero();
				dFdFij(i, j) = 1;

				dPdF(i, j) -= (detFFinv * dFdFij * detFFinv).transpose();
			}
		}
		dPdF = dPdF * OneOverDetF;
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix3 W = 0.5 * (m_R.transpose()*F + F.transpose()*m_R);

		ScalarType detW = W.determinant();

		EigenMatrix3 Winv = W.inverse();

		//if (detW < 1e-20)
		//{
		//	//std::cout << "element_id: " << m_element_id << "detW: " << detW << std::endl;
		//}
		ScalarType OneOverDetW = 1/detW;

		EigenMatrix3 detWWinv = detW * Winv;

		Matrix3333 I;
		I.SetIdentity();
		Matrix3333 dWdF;
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dWdF(i, j) = 0.5 * (m_R.transpose() * I(i,j) + I(j, i) * m_R);
			}
		}

		Matrix3333 dPBdW; // PB = tensor of barrier function
		dPBdW.SetZero();

		// constructing block by block:
		// detW
		directProduct(dPBdW, detWWinv, detWWinv);
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				EigenMatrix3 dWdWij; dWdWij.setZero();
				dWdWij(i, j) = 1;

				dPBdW(i, j) -= detWWinv * dWdWij * detWWinv;
			}
		}
		dPBdW = dPBdW * OneOverDetW;
		// multiply the exrotation term
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPBdW(i, j) = m_R * dPBdW(i, j);
			}
		}

		// construct dPBdF
		for (unsigned int i = 0; i < 3; i++)
		{
			for (unsigned int j = 0; j < 3; j++)
			{
				dPdF(i, j) = dPBdW.FrobeniusProduct(dWdF(i, j));
			}
		}
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()));

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	hessian_block.resize(12, 12);
	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			hessian_block.block<3, 3>(i*3, j*3) = H_blocks[i][j];
		}
	}
}

void TetElementConstraint::EvaluateEnergyNoninversionConstraintCore(const VectorX& x, ScalarType& W11, ScalarType& detW22, ScalarType& detW)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	if (m_noninversion_criteria == NC_DETERMINANT_W)
	{
		EigenMatrix3 W = 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);

		W11 = W(0,0);
		detW22 = W.block<2,2>(0,0).determinant();
		detW = W.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F)
	{
		W11 = 0;
		detW22 = 0;
		detW = F.determinant();
	}
	else if (m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		EigenMatrix3 W = 0.5 * (m_R.transpose() * F + F.transpose() * m_R);

		W11 = W(0,0);
		detW22 = W.block<2,2>(0,0).determinant();
		detW = W.determinant();
	}
}

ScalarType TetElementConstraint::EvaluateEnergyNoConstraintCore(const VectorX& x, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	ScalarType energy = 0.0;
	if ((*p_mu) > 0) // elastic term
	{
		energy += (*p_mu)*multiplier*(F-target_term).squaredNorm();
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		energy += 0.5*(*p_lambda)*multiplier*std::pow((target_term.transpose()*F-I).trace(), 2);
	}

	energy *= m_volume;

	return energy;
}
void TetElementConstraint::EvaluateGradientNoConstraintCore(const VectorX& x, VectorX& gradient_block, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	EigenMatrix3 P; // stress tensor
	P.setZero();

	if ((*p_mu) > 0) // elastic term
	{
		P += 2*(*p_mu)*multiplier*(F-target_term);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		EigenMatrix3 I; I.setIdentity();
		P += (*p_lambda)*multiplier*(target_term.transpose()*F-I).trace()*target_term;
	}

	EigenMatrix3 H = m_volume * P * m_Dm_inv.transpose();

	EigenVector3 g[4];
	g[0] = H.block<3,1>(0, 0);
	g[1] = H.block<3,1>(0, 1);
	g[2] = H.block<3,1>(0, 2);
	g[3] = -g[0]-g[1]-g[2];

	gradient_block.resize(12);
	for (unsigned int i = 0; i < 4; i++)
	{
		gradient_block.block<3, 1>(3*i, 0) = g[i];
	}
}
void TetElementConstraint::EvaluateHessianNoConstraintCore(const VectorX& x, EigenMatrix& hessian_block, ScalarType multiplier)
{
	// get deformation gradient
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	EigenMatrix3 target_term;
	switch(m_energy_type)
	{
	case ENERGY_TYPE_COROT:
		target_term = m_R;
		break;
	case ENERGY_TYPE_EXROT:
		target_term = m_ExR;
		break;
	}

	Matrix3333 dPdF;
	dPdF.SetZero();

	if ((*p_mu) > 0) // elastic term
	{
		Matrix3333 I;
		I.SetIdentity();
		dPdF = dPdF + I * (2*(*p_mu)*multiplier);
	}
	if ((*p_lambda) > 0) // poisson term
	{
		Matrix3333 dP2dF;
		directProduct(dP2dF, (*p_lambda)*multiplier*target_term, target_term);
		dPdF = dPdF + dP2dF;
	}

	Matrix3333 dH = dPdF * ((m_Dm_inv.transpose()) * m_volume);

	EigenMatrix3 H_blocks[4][4];

	// i == 0 to 2 case 
	for (unsigned int i = 0; i < 3; i++)
	{
		// j == 0 to 2 case
		for (unsigned int j = 0; j < 3; j++)
		{
			EigenVector3 v0 = dH(0, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v1 = dH(1, j)*m_Dm_inv.transpose().block<3, 1>(0, i);
			EigenVector3 v2 = dH(2, j)*m_Dm_inv.transpose().block<3, 1>(0, i);

			H_blocks[i][j].block<3, 1>(0, 0) = v0;
			H_blocks[i][j].block<3, 1>(0, 1) = v1;
			H_blocks[i][j].block<3, 1>(0, 2) = v2;
		}
		// j == 3 case
		H_blocks[i][3] = -H_blocks[i][0]-H_blocks[i][1]-H_blocks[i][2];
	}
	// i == 3 case
	for (unsigned int j = 0; j < 4; j++)
	{
		H_blocks[3][j] = -H_blocks[0][j]-H_blocks[1][j]-H_blocks[2][j];
	}

	hessian_block.resize(12, 12);
	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			hessian_block.block<3, 3>(i*3, j*3) = H_blocks[i][j];
		}
	}
}

ScalarType TetElementConstraint::EvaluateEnergyGeneralBarrier(const VectorX& x, ScalarType (*psi)(ScalarType), ScalarType multiplier, bool throwable, ScalarType regularization_mu)
{
	ScalarType objective_energy = EvaluateEnergyNoConstraintCore(x, multiplier);

	ScalarType W11, detW22, detW;
	EvaluateEnergyNoninversionConstraintCore(x, W11, detW22, detW);

	if (detW < 0)
		throw EXCEPTION_LOG_OF_NEGATIVE;
	if (m_noninversion_criteria == NC_DETERMINANT_W || m_noninversion_criteria == NC_DETERMINANT_F_UPDATE_ROTATION)
	{
		if (W11 < 0 || detW22 < 0)
		{
			throw EXCEPTION_LOG_OF_NEGATIVE;
		}
	}

	ScalarType barrier_energy = m_volume*(*p_gamma)*psi(detW);

	return objective_energy + barrier_energy;
}
void TetElementConstraint::EvaluateGradientGeneralBarrier(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier, ScalarType regularization_mu)
{
	VectorX objective_gradient;
	EvaluateGradientNoConstraintCore(x, objective_gradient, multiplier);

	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	VectorX barrier_gradient = m_volume*(*p_gamma)*d_psi(EvaluateEnergyNoninversionConstraintCore(x))*detW_gradient;

	VectorX gradient_block = objective_gradient+barrier_gradient;
	for (unsigned int i = 0; i != 4; i ++)
	{
		gradient.block<3,1>(3*m_p[i], 0) += gradient_block.block<3,1>(3*i, 0);
	}
}
void TetElementConstraint::EvaluateHessianGeneralBarrier(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier, ScalarType regularization_mu)
{
	EigenMatrix objective_hessian;
	EvaluateHessianNoConstraintCore(x, objective_hessian, multiplier);

	ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);
	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	EigenMatrix detW_hessian;
	EvaluateHessianNoninversionConstraintCore(x, detW_hessian);
	EigenMatrix barrier_hessian = m_volume*(*p_gamma)*(d2_psi(detW)*detW_gradient*detW_gradient.transpose()+d_psi(detW)*detW_hessian);

	//// DEBUG Finite difference version of hessian
	//EigenMatrix det_hessian_FD;
	//EvaluateHessianNoninversionConstraintCoreFD(x, det_hessian_FD);
	//// DEBUG for positive definiteness of hessian
	//VectorX ev1 = objective_hessian.eigenvalues().real();
	//VectorX evdetW = detW_hessian.eigenvalues().real();
	//VectorX ev2 = barrier_hessian.eigenvalues().real();

	EigenMatrix hessian_block = objective_hessian + barrier_hessian;

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, hessian_block(i*3+row, j*3+col)));
				}
			}
		}
	}
}

// primal dual general barrier
void TetElementConstraint::EvaluateGradientPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier)
{
	ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);

	VectorX objective_gradient;
	EvaluateGradientNoConstraintCore(x, objective_gradient);

	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	VectorX barrier_gradient = m_volume * detW_gradient * lambda(m_element_id);

	// dE/dx part
	VectorX gradient_block = objective_gradient-barrier_gradient;
	for (unsigned int i = 0; i != 4; i ++)
	{
		gradient.block<3,1>(3*m_p[i], 0) += gradient_block.block<3,1>(3*i, 0);
	}

	// dE/dlambda part
	gradient(x.size() + m_element_id) += detW * lambda(m_element_id) - 1/multiplier;
}
void TetElementConstraint::EvaluateHessianPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier)
{
	//EvaluateHessianPrimalDualGeneralBarrierFD(x, lambda, hessian_triplets, d_psi, d2_psi, multiplier);
	//return;

	//std::vector<SparseMatrixTriplet> ht = hessian_triplets;
	//EvaluateHessianPrimalDualGeneralBarrierFD(x, lambda, ht, d_psi, d2_psi, multiplier);

	EigenMatrix objective_hessian;
	EvaluateHessianNoConstraintCore(x, objective_hessian);

	ScalarType detW = EvaluateEnergyNoninversionConstraintCore(x);
	VectorX detW_gradient;
	EvaluateGradientNoninversionConstraintCore(x, detW_gradient);
	EigenMatrix detW_hessian;
	EvaluateHessianNoninversionConstraintCore(x, detW_hessian);
	EigenMatrix barrier_hessian = m_volume * detW_hessian*lambda(m_element_id);

	EigenMatrix hessian_block = objective_hessian - barrier_hessian;

	// d2L/dx2 set to triplets 
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, hessian_block(i*3+row, j*3+col)));
				}
			}
		}
	}

	// d2L/dxdlambda bottom and right part
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int row = 0; row < 3; row++)
		{
			// right
			hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3 + row, x.size()+m_element_id, -m_volume * detW_gradient(i*3+row)));
			// bottm
			hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, m_p[i]*3 + row, lambda(m_element_id)*detW_gradient(i*3+row)));
		}
	}

	VectorX x_plus_dx = x;
	VectorX problem_row(12);
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int dim = 0; dim != 3; dim ++)
		{
			x_plus_dx = x;
			x_plus_dx(m_p[i]*3+dim) += 1e-6;

			ScalarType detW_x_plus_dx = EvaluateEnergyNoninversionConstraintCore(x_plus_dx);
			problem_row(i*3+dim) = lambda(m_element_id)*(detW_x_plus_dx-detW)*1e6;
		}
	}

	// d2L/dlambda2
	hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, x.size()+m_element_id, detW));
}
void TetElementConstraint::EvaluateHessianPrimalDualGeneralBarrierFD(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier)
{
	VectorX x_plus_dx = x;
	VectorX lambda_plus_dlambda = lambda;
	VectorX g(x.size()+lambda.size());
	VectorX g_prime(x.size()+lambda.size());
	g.setZero();
	EvaluateGradientPrimalDualGeneralBarrier(x, lambda, g, d_psi, multiplier);

	for (unsigned int i = 0; i != 4; i ++)
	{
		for (unsigned int dim = 0; dim != 3; dim ++)
		{
			x_plus_dx = x;
			x_plus_dx(m_p[i]*3+dim) += 1e-6;

			g_prime.setZero();
			EvaluateGradientPrimalDualGeneralBarrier(x_plus_dx, lambda, g_prime, d_psi, multiplier);
			VectorX diff = (g_prime-g)*1e6;

			for (unsigned int j = 0; j!=4; j++)
			{
				for (unsigned int dimj = 0; dimj!=3; dimj++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[j]*3+dimj, m_p[i]*3+dim, diff(m_p[j]*3+dimj)));
				}
			}
			hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, m_p[i]*3+dim, diff(x.size()+m_element_id)));
		}
	}

	lambda_plus_dlambda = lambda;
	lambda_plus_dlambda(m_element_id) += 1e-6;
	g_prime.setZero();
	EvaluateGradientPrimalDualGeneralBarrier(x, lambda_plus_dlambda, g_prime, d_psi, multiplier);
	VectorX diff = (g_prime-g)*1e6;

	for (unsigned int j = 0; j!=4; j++)
	{
		for (unsigned int dimj = 0; dimj!=3; dimj++)
		{
			hessian_triplets.push_back(SparseMatrixTriplet(m_p[j]*3+dimj, x.size()+m_element_id, diff(m_p[j]*3+dimj)));
		}
	}
	hessian_triplets.push_back(SparseMatrixTriplet(x.size()+m_element_id, x.size()+m_element_id, diff(x.size()+m_element_id)));
}

#pragma region tet_debugging
void  TetElementConstraint::EvaluateGradientFiniteDifference(const VectorX& x, VectorX& gradient)
{
	ScalarType energy = 0;
	ScalarType energy_prime = 0;
	energy = EvaluateEnergy(x);

	EigenVector3 dt[3];
	dt[0] = EigenVector3(LARGER_EPSILON, 0, 0);
	dt[1] = EigenVector3(0, LARGER_EPSILON, 0);
	dt[2] = EigenVector3(0, 0, LARGER_EPSILON);
	VectorX x_plus_t;

	for (unsigned int i = 0; i < 4; i++)
	{
		ScalarType temp[3];
		for (unsigned int dim = 0; dim < 3; dim++)
		{
			x_plus_t = x;
			x_plus_t.block<3,1>(m_p[i]*3,0) += dt[dim];
			energy_prime = EvaluateEnergy(x_plus_t);
			temp[dim] = (energy_prime - energy) * 1e6;
		}
		gradient(m_p[i]*3,0) += temp[0];
		gradient(m_p[i]*3+1,0) += temp[1];
		gradient(m_p[i]*3+2,0) += temp[2];
	}
}
void  TetElementConstraint::EvaluateHessianFiniteDifference(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets)
{
	unsigned int size = x.size();
	VectorX gradient(size); 
	VectorX gradient_prime(size); 
	gradient.setZero();
	EvaluateGradient(x, gradient);

	EigenVector3 dt[3];
	dt[0] = EigenVector3(LARGER_EPSILON, 0, 0);
	dt[1] = EigenVector3(0, LARGER_EPSILON, 0);
	dt[2] = EigenVector3(0, 0, LARGER_EPSILON);
	VectorX x_plus_t;

	EigenMatrix3 H_blocks[4][4];
	// finite difference p0, x
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenVector3 temp[3];
			for (unsigned int dim = 0; dim < 3; dim++)
			{
				x_plus_t = x;
				x_plus_t.block<3,1>(m_p[j]*3,0) += dt[dim];
				gradient_prime.setZero();
				EvaluateGradient(x_plus_t, gradient_prime);
				temp[dim] = (gradient_prime.block<3,1>(m_p[i]*3,0) - gradient.block<3,1>(m_p[i]*3,0)) * 1e6;
			}

			H_blocks[i][j].block<3, 1>(0, 0) = temp[0];
			H_blocks[i][j].block<3, 1>(0, 1) = temp[1];
			H_blocks[i][j].block<3, 1>(0, 2) = temp[2];
		}
	}

	// set to triplets
	for (unsigned int i = 0; i < 4; i++)
	{
		for (unsigned int j = 0; j < 4; j++)
		{
			EigenMatrix3& block = H_blocks[i][j];
			for (unsigned int row = 0; row < 3; row ++)
			{
				for (unsigned int col = 0; col < 3; col ++)
				{
					hessian_triplets.push_back(SparseMatrixTriplet(m_p[i]*3+row, m_p[j]*3+col, block(row, col)));
				}
			}
		}
	}
}
EigenMatrix3 TetElementConstraint::GetW(const VectorX& x)
{
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	return 0.5 * (m_ExR.transpose()*F + F.transpose()*m_ExR);
}
EigenMatrix3 TetElementConstraint::GetF(const VectorX& x)
{
	EigenMatrix3 F; 
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;

	return F;
}
#pragma endregion
void TetElementConstraint::getEdgeMatrix(const VectorX& x, EigenMatrix3& D)
{
	D.block<3, 1>(0, 0) = x.block<3, 1>(3*m_p[0], 0) - x.block<3, 1>(3*m_p[3], 0);
	D.block<3, 1>(0, 1) = x.block<3, 1>(3*m_p[1], 0) - x.block<3, 1>(3*m_p[3], 0);
	D.block<3, 1>(0, 2) = x.block<3, 1>(3*m_p[2], 0) - x.block<3, 1>(3*m_p[3], 0);
}
void TetElementConstraint::getDeformationGradient(const VectorX& x, EigenMatrix3& F)
{
	EigenMatrix3 Ds;
	getEdgeMatrix(x, Ds);
	F = Ds * m_Dm_inv;
}