670 lines
22 KiB
C++
670 lines
22 KiB
C++
// Copyright 2013 - Christian Schüller 2013, schuellc@inf.ethz.ch
|
|
// Interactive Geometry Lab - ETH Zurich
|
|
|
|
#include "LIMSolver3D.h"
|
|
#include "TetrahedronMesh.h"
|
|
|
|
LIMSolver3D::LIMSolver3D()
|
|
{
|
|
dim = 3;
|
|
mesh = NULL;
|
|
}
|
|
|
|
LIMSolver3D::~LIMSolver3D()
|
|
{
|
|
}
|
|
|
|
void LIMSolver3D::Init(DeformableMesh* mesh)
|
|
{
|
|
Init(static_cast<TetrahedronMesh*>(mesh));
|
|
}
|
|
|
|
void LIMSolver3D::Init(TetrahedronMesh* mesh)
|
|
{
|
|
this->mesh = mesh;
|
|
LIMSolver::Init(mesh);
|
|
}
|
|
|
|
void LIMSolver3D::prepareNMProblemData()
|
|
{
|
|
std::vector<Eigen::Triplet<double> > triplets;
|
|
const int numNodes = mesh->InitalVertices->rows();
|
|
const int numTets = mesh->Tetrahedra->rows();
|
|
|
|
TetrahedronVertexIdx.resize(12,numTets);
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Vector4i nodes = mesh->Tetrahedra->row(t);
|
|
|
|
// create tets vertex indicies
|
|
for(int v=0;v<4;v++)
|
|
{
|
|
for(int i=0;i<3;i++)
|
|
{
|
|
TetrahedronVertexIdx(v*3+i,t) = nodes[v]*3+i;
|
|
}
|
|
}
|
|
|
|
for(int r=0;r<12;r++)
|
|
{
|
|
for(int c=r;c<12;c++)
|
|
{
|
|
int row = TetrahedronVertexIdx(r,t);
|
|
int col = TetrahedronVertexIdx(c,t);
|
|
|
|
// std::sort for upper triangule matrix
|
|
if(row > col)
|
|
{
|
|
int temp = col;
|
|
col = row;
|
|
row = temp;
|
|
}
|
|
|
|
triplets.push_back(Eigen::Triplet<double>(row,col,1));
|
|
}
|
|
}
|
|
}
|
|
|
|
// get positional constraint matrix structure of problem
|
|
for (int k=0;k<linearConstraintsMatrix2.outerSize();++k)
|
|
{
|
|
for (Eigen::SparseMatrix<double>::InnerIterator it(linearConstraintsMatrix2,k);it;++it)
|
|
{
|
|
int row = it.row();
|
|
int col = it.col();
|
|
|
|
// std::sort for upper triangule matrix
|
|
if(row < col)
|
|
triplets.push_back(Eigen::Triplet<double>(row,col,1));
|
|
}
|
|
}
|
|
|
|
// get hessian matrix structure of problem
|
|
std::vector<int> hessRowIdx;
|
|
std::vector<int> hessColIdx;
|
|
prepareProblemData(hessRowIdx,hessColIdx);
|
|
assert(hessRowIdx.size() == hessColIdx.size());
|
|
|
|
for(int i=0;i<hessRowIdx.size();i++)
|
|
{
|
|
// std::sort for upper triangle matrix
|
|
if(hessRowIdx[i] <= hessColIdx[i])
|
|
triplets.push_back(Eigen::Triplet<double>(hessRowIdx[i],hessColIdx[i],1));
|
|
}
|
|
hessian.setFromTriplets(triplets.begin(),triplets.end());
|
|
numNonZeroHessian = hessian.nonZeros();
|
|
|
|
// Init vertex indices link vector
|
|
VertexPositionIndices.resize(numVariables);
|
|
for(int n=0;n<numNodes;n++)
|
|
{
|
|
for(int i=0;i<3;i++)
|
|
{
|
|
VertexPositionIndices[n*3+i] = n+numNodes*i;
|
|
}
|
|
}
|
|
|
|
initialNodes.resize(numVariables);
|
|
for(int n=0;n<numVariables;n++)
|
|
{
|
|
initialNodes[n] = mesh->InitalVertices->coeff(VertexPositionIndices[n]);
|
|
}
|
|
|
|
// set link indices of non zero elements of constraint hessian matrix
|
|
problemHessianCoeffs.resize(1,hessRowIdx.size());
|
|
denseHessianCoeffs.resize(78,numTets);
|
|
diagHessianCoeffs.resize(numVariables);
|
|
posConstraintsHessianCoeffs.resize(linearConstraintsMatrix2.nonZeros());
|
|
|
|
for(int i=0;i<hessRowIdx.size();i++)
|
|
{
|
|
if(hessRowIdx[i] <= hessColIdx[i])
|
|
problemHessianCoeffs[i] = &hessian.coeffRef(hessRowIdx[i],hessColIdx[i]);
|
|
}
|
|
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
int numElem = 0;
|
|
for(int r=0;r<12;r++)
|
|
{
|
|
for(int c=r;c<12;c++)
|
|
{
|
|
int row = TetrahedronVertexIdx(r,t);
|
|
int col = TetrahedronVertexIdx(c,t);
|
|
|
|
// std::sort for upper triangule matrix
|
|
if(row > col)
|
|
{
|
|
int temp = col;
|
|
col = row;
|
|
row = temp;
|
|
}
|
|
|
|
denseHessianCoeffs(numElem,t) = &hessian.coeffRef(row,col);
|
|
|
|
if(col == row)
|
|
diagHessianCoeffs[row] = denseHessianCoeffs(numElem,t);
|
|
|
|
numElem++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// non-flip constraints
|
|
nonFlipHessianCoeffs.resize(36,numTets);
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
for(int i=0;i<36;i++)
|
|
{
|
|
int row = TetrahedronVertexIdx(NonFlipHessian3DIdx[i][0],t);
|
|
int col = TetrahedronVertexIdx(NonFlipHessian3DIdx[i][1],t);
|
|
|
|
// std::sort for upper triangule matrix
|
|
if(row > col)
|
|
{
|
|
int temp = col;
|
|
col = row;
|
|
row = temp;
|
|
}
|
|
|
|
nonFlipHessianCoeffs(i,t) = &hessian.coeffRef(row,col);
|
|
}
|
|
}
|
|
|
|
// get positional constraint hessian entries
|
|
int count = 0;
|
|
for (int k=0;k<linearConstraintsMatrix2.outerSize();++k)
|
|
{
|
|
for (Eigen::SparseMatrix<double>::InnerIterator it(linearConstraintsMatrix2,k);it;++it)
|
|
{
|
|
int row = it.row();
|
|
int col = it.col();
|
|
|
|
// std::sort for upper triangule matrix
|
|
if(row <= col)
|
|
posConstraintsHessianCoeffs[count++] = &hessian.coeffRef(row,col);
|
|
}
|
|
}
|
|
}
|
|
|
|
void LIMSolver3D::computeRestPoseFunctionParameters()
|
|
{
|
|
const int numTets = mesh->Tetrahedra->rows();
|
|
|
|
initalSize.resize(numTets);
|
|
barrierParam1.resize(numTets);
|
|
barrierParam2.resize(numTets);
|
|
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
|
|
|
|
Eigen::Vector3d A(initialNodes[indices[0]],initialNodes[indices[1]],initialNodes[indices[2]]);
|
|
Eigen::Vector3d B(initialNodes[indices[3]],initialNodes[indices[4]],initialNodes[indices[5]]);
|
|
Eigen::Vector3d C(initialNodes[indices[6]],initialNodes[indices[7]],initialNodes[indices[8]]);
|
|
Eigen::Vector3d D(initialNodes[indices[9]],initialNodes[indices[10]],initialNodes[indices[11]]);
|
|
|
|
Eigen::Vector3d a = A-D;
|
|
Eigen::Vector3d b = B-D;
|
|
Eigen::Vector3d c = C-D;
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
|
|
initalSize[t] = detEPS;
|
|
|
|
double quot = detEPS*CompensationExp*pow(det,CompensationExp-1);
|
|
|
|
// must be multiplied by beta later
|
|
barrierParam1[t] = 1/quot;
|
|
barrierParam2[t] = log(detEPS) - pow(det,CompensationExp)/quot;
|
|
}
|
|
}
|
|
|
|
double LIMSolver3D::computeNMFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
|
|
{
|
|
const int numTets = mesh->Tetrahedra->rows();
|
|
|
|
double dObj;
|
|
|
|
// position constraints
|
|
double diff = (*dmesh->ConstraintMatrix * x - subStepConstraints).squaredNorm();
|
|
|
|
// non-flip constraint energy
|
|
double ic = 0.0;
|
|
if(EnableBarriers)
|
|
{
|
|
if(EnableLogBarriers)
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::Vector3d a(x[TetrahedronVertexIdx.coeff(0,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(1,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(2,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
Eigen::Vector3d b(x[TetrahedronVertexIdx.coeff(3,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(4,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(5,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
Eigen::Vector3d c(x[TetrahedronVertexIdx.coeff(6,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(7,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(8,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
if(detEPS > 0.0)
|
|
{
|
|
ic += -log(detEPS); // barrier function
|
|
if(EnableBarrierCompensation) ic += barrierParam1[t]*pow((double)det,CompensationExp)+barrierParam2[t]; // barrier compensation function
|
|
}
|
|
else
|
|
{
|
|
ic = std::numeric_limits<double>::infinity();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::Vector3d a(x[TetrahedronVertexIdx.coeff(0,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(1,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(2,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
Eigen::Vector3d b(x[TetrahedronVertexIdx.coeff(3,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(4,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(5,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
Eigen::Vector3d c(x[TetrahedronVertexIdx.coeff(6,t)]-x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(7,t)]-x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(8,t)]-x[TetrahedronVertexIdx.coeff(11,t)]);
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
|
|
double minDet = initalSize[t]*Gamma;
|
|
if(detEPS < minDet)
|
|
{
|
|
double coeffA = 1/pow(minDet,3);
|
|
double coeffB = -3/pow(minDet,2);
|
|
double coeffC = 3/minDet;
|
|
|
|
double detEPS2 = detEPS*detEPS;
|
|
double detEPS3 = detEPS2*detEPS;
|
|
|
|
if(detEPS > 0.0)
|
|
{
|
|
ic += 1.0/(coeffA*detEPS3 + coeffB*detEPS2 + coeffC*detEPS) - 1.0; // barrier function
|
|
}
|
|
else
|
|
{
|
|
ic = std::numeric_limits<double>::infinity();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if(ic == std::numeric_limits<double>::infinity())
|
|
dObj = std::numeric_limits<double>::infinity();
|
|
else
|
|
{
|
|
CurrentPositionalSubStepEnergy = Alpha*diff;
|
|
CurrentConstraintEnergy = Beta*ic;
|
|
CurrentDeformationEnergy = computeFunction(x);
|
|
dObj = CurrentPositionalSubStepEnergy + CurrentConstraintEnergy + CurrentDeformationEnergy;
|
|
}
|
|
|
|
return dObj ;
|
|
}
|
|
|
|
void LIMSolver3D::computeNMGradient(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, Eigen::Matrix<double,Eigen::Dynamic,1>& grad)
|
|
{
|
|
const int numTets = mesh->Tetrahedra->rows();
|
|
|
|
grad.setZero();
|
|
|
|
// call subclass function
|
|
computeGradient(x,grad);
|
|
|
|
// position constraints
|
|
Eigen::Matrix<double,Eigen::Dynamic,1> temp1 = linearConstraintsMatrix2 * x;
|
|
Eigen::Matrix<double,Eigen::Dynamic,1> temp2 = subStepConstraints.transpose() * *dmesh->ConstraintMatrix;
|
|
grad += 2.0*Alpha*(temp1 - temp2);
|
|
|
|
// non-flip constraints
|
|
if(EnableBarriers)
|
|
{
|
|
if(EnableLogBarriers)
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
|
|
|
|
Eigen::Vector3d A(x[indices[0]],x[indices[1]],x[indices[2]]);
|
|
Eigen::Vector3d B(x[indices[3]],x[indices[4]],x[indices[5]]);
|
|
Eigen::Vector3d C(x[indices[6]],x[indices[7]],x[indices[8]]);
|
|
Eigen::Vector3d D(x[indices[9]],x[indices[10]],x[indices[11]]);
|
|
|
|
Eigen::Vector3d a = A-D;
|
|
Eigen::Vector3d b = B-D;
|
|
Eigen::Vector3d c = C-D;
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
|
|
double term = -1/detEPS; // barrier function
|
|
if(detEPS > 0)
|
|
{
|
|
term += barrierParam1[t]*CompensationExp*pow((double)det,CompensationExp-1); // barrier compensation function - barrier function
|
|
term *= Beta;
|
|
}
|
|
else
|
|
term = std::numeric_limits<double>::infinity();
|
|
|
|
|
|
// partial derivatives in respect to a
|
|
grad[indices[0]] += term*(c[1]* b[2] - c[2]* b[1]);
|
|
grad[indices[1]] += term*(c[2]* b[0] - c[0]* b[2]);
|
|
grad[indices[2]] += term*(c[0]* b[1] - c[1]* b[0]);
|
|
|
|
// partial derivatives in respect to b
|
|
grad[indices[3]] += term*(a[1]* c[2] + a[2]*-c[1]);
|
|
grad[indices[4]] += term*(a[0]*-c[2] + a[2]* c[0]);
|
|
grad[indices[5]] += term*(a[0]* c[1] + a[1]*-c[0]);
|
|
|
|
// partial derivatives in respect to c
|
|
grad[indices[6]] += term*(a[1]*-b[2] + a[2]* b[1]);
|
|
grad[indices[7]] += term*(a[0]* b[2] + a[2]*-b[0]);
|
|
grad[indices[8]] += term*(a[0]*-b[1] + a[1]* b[0]);
|
|
|
|
// partial derivatives in respect to d
|
|
grad[indices[9]] += term*(-c[1]*b[2] + c[2]* b[1] + a[1]*(-C[2]+B[2]) + a[2]*(-B[1]+C[1]));
|
|
grad[indices[10]] += term*(a[0]*(-B[2]+C[2]) - c[2]*b[0] + c[0]*b[2] + a[2]*(-C[0]+B[0]));
|
|
grad[indices[11]] += term*(a[0]*(-C[1]+B[1]) + a[1]*(-B[0]+C[0]) - c[0]*b[1] + c[1]*b[0]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
|
|
|
|
Eigen::Vector3d A(x[indices[0]],x[indices[1]],x[indices[2]]);
|
|
Eigen::Vector3d B(x[indices[3]],x[indices[4]],x[indices[5]]);
|
|
Eigen::Vector3d C(x[indices[6]],x[indices[7]],x[indices[8]]);
|
|
Eigen::Vector3d D(x[indices[9]],x[indices[10]],x[indices[11]]);
|
|
|
|
Eigen::Vector3d a = A-D;
|
|
Eigen::Vector3d b = B-D;
|
|
Eigen::Vector3d c = C-D;
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
|
|
double minDet = initalSize[t]*Gamma;
|
|
if(detEPS < minDet)
|
|
{
|
|
double coeffA = 1/pow(minDet,3);
|
|
double coeffB = -3/pow(minDet,2);
|
|
double coeffC = 3/minDet;
|
|
|
|
double detEPS2 = detEPS*detEPS;
|
|
double detEPS3 = detEPS2*detEPS;
|
|
|
|
double term = -Beta*(3*coeffA*detEPS2 + 2*coeffB*detEPS + coeffC) / pow(coeffA*detEPS3 + coeffB*detEPS2 + coeffC*detEPS,2);
|
|
|
|
// partial derivatives in respect to a
|
|
grad[indices[0]] += term*(c[1]* b[2] - c[2]* b[1]);
|
|
grad[indices[1]] += term*(c[2]* b[0] - c[0]* b[2]);
|
|
grad[indices[2]] += term*(c[0]* b[1] - c[1]* b[0]);
|
|
|
|
// partial derivatives in respect to b
|
|
grad[indices[3]] += term*(a[1]* c[2] + a[2]*-c[1]);
|
|
grad[indices[4]] += term*(a[0]*-c[2] + a[2]* c[0]);
|
|
grad[indices[5]] += term*(a[0]* c[1] + a[1]*-c[0]);
|
|
|
|
// partial derivatives in respect to c
|
|
grad[indices[6]] += term*(a[1]*-b[2] + a[2]* b[1]);
|
|
grad[indices[7]] += term*(a[0]* b[2] + a[2]*-b[0]);
|
|
grad[indices[8]] += term*(a[0]*-b[1] + a[1]* b[0]);
|
|
|
|
// partial derivatives in respect to d
|
|
grad[indices[9]] += term*(-c[1]*b[2] + c[2]* b[1] + a[1]*(-C[2]+B[2]) + a[2]*(-B[1]+C[1]));
|
|
grad[indices[10]] += term*(a[0]*(-B[2]+C[2]) - c[2]*b[0] + c[0]*b[2] + a[2]*(-C[0]+B[0]));
|
|
grad[indices[11]] += term*(a[0]*(-C[1]+B[1]) + a[1]*(-B[0]+C[0]) - c[0]*b[1] + c[1]*b[0]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void LIMSolver3D::computeNMHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
|
|
{
|
|
const int numTets = mesh->Tetrahedra->rows();
|
|
|
|
hessian *= 0;
|
|
|
|
// compute problem function hessian
|
|
computeHessian(x,problemHessianCoeffs);
|
|
|
|
// position constraints: 2*alpha
|
|
int count = 0;
|
|
for (int k=0;k<linearConstraintsMatrix2.outerSize();++k)
|
|
{
|
|
for (Eigen::SparseMatrix<double>::InnerIterator it(linearConstraintsMatrix2,k);it;++it)
|
|
{
|
|
if(it.row() <= it.col())
|
|
*posConstraintsHessianCoeffs[count++] += 2*Alpha*it.value();
|
|
}
|
|
}
|
|
|
|
// non-flip constraints
|
|
if(EnableBarriers)
|
|
{
|
|
if(EnableLogBarriers)
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
|
|
|
|
Eigen::Vector3d A(x[indices[0]],x[indices[1]],x[indices[2]]);
|
|
Eigen::Vector3d B(x[indices[3]],x[indices[4]],x[indices[5]]);
|
|
Eigen::Vector3d C(x[indices[6]],x[indices[7]],x[indices[8]]);
|
|
Eigen::Vector3d D(x[indices[9]],x[indices[10]],x[indices[11]]);
|
|
|
|
Eigen::Vector3d a = A-D;
|
|
Eigen::Vector3d b = B-D;
|
|
Eigen::Vector3d c = C-D;
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
double detEPSInv = 1/detEPS;
|
|
double term = -detEPSInv;
|
|
if(detEPS > 0)
|
|
{
|
|
if(EnableBarrierCompensation) term += barrierParam1[t]*CompensationExp*pow((double)det,CompensationExp-1); // barrier compensation function - barrier function
|
|
term *= Beta;
|
|
}
|
|
else
|
|
term = std::numeric_limits<double>::infinity();
|
|
|
|
Eigen::Matrix<double*,Eigen::Dynamic,1> elems = nonFlipHessianCoeffs.col(t);
|
|
|
|
*elems[0] += term * ( c[2]);
|
|
*elems[1] += term * (-c[1]);
|
|
*elems[2] += term * (-c[2]);
|
|
*elems[3] += term * ( c[0]);
|
|
*elems[4] += term * ( c[1]);
|
|
*elems[5] += term * (-c[0]);
|
|
*elems[6] += term * (-b[2]);
|
|
*elems[7] += term * ( b[1]);
|
|
*elems[8] += term * ( a[2]);
|
|
*elems[9] += term * (-a[1]);
|
|
*elems[10] += term * ( b[2]);
|
|
*elems[11] += term * (-b[0]);
|
|
*elems[12] += term * (-a[2]);
|
|
*elems[13] += term * ( a[0]);
|
|
*elems[14] += term * (-b[1]);
|
|
*elems[15] += term * ( b[0]);
|
|
*elems[16] += term * ( a[1]);
|
|
*elems[17] += term * (-a[0]);
|
|
*elems[18] += term * (-C[2]+B[2]);
|
|
*elems[19] += term * (-B[1]+C[1]);
|
|
*elems[20] += term * ( C[2]-A[2]);
|
|
*elems[21] += term * ( A[1]-C[1]);
|
|
*elems[22] += term * (-B[2]+A[2]);
|
|
*elems[23] += term * (-A[1]+B[1]);
|
|
*elems[24] += term * (-B[2]+C[2]);
|
|
*elems[25] += term * (-C[0]+B[0]);
|
|
*elems[26] += term * (-C[2]+A[2]);
|
|
*elems[27] += term * (-A[0]+C[0]);
|
|
*elems[28] += term * ( B[2]-A[2]);
|
|
*elems[29] += term * ( A[0]-B[0]);
|
|
*elems[30] += term * (-C[1]+B[1]);
|
|
*elems[31] += term * (-B[0]+C[0]);
|
|
*elems[32] += term * ( C[1]-A[1]);
|
|
*elems[33] += term * ( A[0]-C[0]);
|
|
*elems[34] += term * (-B[1]+A[1]);
|
|
*elems[35] += term * (-A[0]+B[0]);
|
|
|
|
Eigen::Matrix<double,Eigen::Dynamic,1> g(12);
|
|
|
|
// partial derivatives in respect to a
|
|
g[0] = (c[1]* b[2] - c[2]* b[1]);
|
|
g[1] = (c[2]* b[0] - c[0]* b[2]);
|
|
g[2] = (c[0]* b[1] - c[1]* b[0]);
|
|
|
|
// partial derivatives in respect to b
|
|
g[3] = (a[1]* c[2] + a[2]*-c[1]);
|
|
g[4] = (a[0]*-c[2] + a[2]* c[0]);
|
|
g[5] = (a[0]* c[1] + a[1]*-c[0]);
|
|
|
|
// partial derivatives in respect to c
|
|
g[6] = (a[1]*-b[2] + a[2]* b[1]);
|
|
g[7] = (a[0]* b[2] + a[2]*-b[0]);
|
|
g[8] = (a[0]*-b[1] + a[1]* b[0]);
|
|
|
|
// partial derivatives in respect to d
|
|
g[9] = (-c[1]*b[2] + c[2]* b[1] + a[1]*(-C[2]+B[2]) + a[2]*(-B[1]+C[1]));
|
|
g[10] = (a[0]*(-B[2]+C[2]) - c[2]*b[0] + c[0]*b[2] + a[2]*(-C[0]+B[0]));
|
|
g[11] = (a[0]*(-C[1]+B[1]) + a[1]*(-B[0]+C[0]) - c[0]*b[1] + c[1]*b[0]);
|
|
|
|
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> g2(12,12);
|
|
g2 = g*g.transpose();
|
|
g2 *= Beta*(barrierParam1[t]*CompensationExp*(CompensationExp-1)*pow((double)det,CompensationExp-2) + detEPSInv*detEPSInv); // barrier compensation function - barrier function
|
|
|
|
int numElem = 0;
|
|
for(int r=0;r<12;r++)
|
|
{
|
|
for(int c=r;c<12;c++)
|
|
{
|
|
*denseHessianCoeffs(numElem++,t) += g2(r,c);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(int t=0;t<numTets;t++)
|
|
{
|
|
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
|
|
|
|
Eigen::Vector3d A(x[indices[0]],x[indices[1]],x[indices[2]]);
|
|
Eigen::Vector3d B(x[indices[3]],x[indices[4]],x[indices[5]]);
|
|
Eigen::Vector3d C(x[indices[6]],x[indices[7]],x[indices[8]]);
|
|
Eigen::Vector3d D(x[indices[9]],x[indices[10]],x[indices[11]]);
|
|
|
|
Eigen::Vector3d a = A-D;
|
|
Eigen::Vector3d b = B-D;
|
|
Eigen::Vector3d c = C-D;
|
|
|
|
double det = a.dot(c.cross(b));
|
|
double detEPS = det-mesh->EPS3;
|
|
|
|
double minDet = initalSize[t]*Gamma;
|
|
if(detEPS < minDet)
|
|
{
|
|
double coeffA = 1/pow(minDet,3);
|
|
double coeffB = -3/pow(minDet,2);
|
|
double coeffC = 3/minDet;
|
|
|
|
double detEPS2 = detEPS*detEPS;
|
|
double detEPS3 = detEPS2*detEPS;
|
|
|
|
double divTerm = (coeffA*detEPS3 + coeffB*detEPS2 + coeffC*detEPS);
|
|
double divTerm2 = divTerm*divTerm;
|
|
double divTerm3 = divTerm2*divTerm;
|
|
|
|
double factTerm = (3*coeffA*detEPS2 + 2*coeffB*detEPS + coeffC);
|
|
|
|
// barrier function
|
|
double term = -Beta*factTerm/divTerm2;
|
|
|
|
Eigen::Matrix<double*,Eigen::Dynamic,1> elems = nonFlipHessianCoeffs.col(t);
|
|
|
|
*elems[0] += term * ( c[2]);
|
|
*elems[1] += term * (-c[1]);
|
|
*elems[2] += term * (-c[2]);
|
|
*elems[3] += term * ( c[0]);
|
|
*elems[4] += term * ( c[1]);
|
|
*elems[5] += term * (-c[0]);
|
|
*elems[6] += term * (-b[2]);
|
|
*elems[7] += term * ( b[1]);
|
|
*elems[8] += term * ( a[2]);
|
|
*elems[9] += term * (-a[1]);
|
|
*elems[10] += term * ( b[2]);
|
|
*elems[11] += term * (-b[0]);
|
|
*elems[12] += term * (-a[2]);
|
|
*elems[13] += term * ( a[0]);
|
|
*elems[14] += term * (-b[1]);
|
|
*elems[15] += term * ( b[0]);
|
|
*elems[16] += term * ( a[1]);
|
|
*elems[17] += term * (-a[0]);
|
|
*elems[18] += term * (-C[2]+B[2]);
|
|
*elems[19] += term * (-B[1]+C[1]);
|
|
*elems[20] += term * ( C[2]-A[2]);
|
|
*elems[21] += term * ( A[1]-C[1]);
|
|
*elems[22] += term * (-B[2]+A[2]);
|
|
*elems[23] += term * (-A[1]+B[1]);
|
|
*elems[24] += term * (-B[2]+C[2]);
|
|
*elems[25] += term * (-C[0]+B[0]);
|
|
*elems[26] += term * (-C[2]+A[2]);
|
|
*elems[27] += term * (-A[0]+C[0]);
|
|
*elems[28] += term * ( B[2]-A[2]);
|
|
*elems[29] += term * ( A[0]-B[0]);
|
|
*elems[30] += term * (-C[1]+B[1]);
|
|
*elems[31] += term * (-B[0]+C[0]);
|
|
*elems[32] += term * ( C[1]-A[1]);
|
|
*elems[33] += term * ( A[0]-C[0]);
|
|
*elems[34] += term * (-B[1]+A[1]);
|
|
*elems[35] += term * (-A[0]+B[0]);
|
|
|
|
Eigen::Matrix<double,Eigen::Dynamic,1> g(12);
|
|
|
|
// partial derivatives in respect to a
|
|
g[0] = (c[1]* b[2] - c[2]* b[1]);
|
|
g[1] = (c[2]* b[0] - c[0]* b[2]);
|
|
g[2] = (c[0]* b[1] - c[1]* b[0]);
|
|
|
|
// partial derivatives in respect to b
|
|
g[3] = (a[1]* c[2] + a[2]*-c[1]);
|
|
g[4] = (a[0]*-c[2] + a[2]* c[0]);
|
|
g[5] = (a[0]* c[1] + a[1]*-c[0]);
|
|
|
|
// partial derivatives in respect to c
|
|
g[6] = (a[1]*-b[2] + a[2]* b[1]);
|
|
g[7] = (a[0]* b[2] + a[2]*-b[0]);
|
|
g[8] = (a[0]*-b[1] + a[1]* b[0]);
|
|
|
|
// partial derivatives in respect to d
|
|
g[9] = (-c[1]*b[2] + c[2]* b[1] + a[1]*(-C[2]+B[2]) + a[2]*(-B[1]+C[1]));
|
|
g[10] = (a[0]*(-B[2]+C[2]) - c[2]*b[0] + c[0]*b[2] + a[2]*(-C[0]+B[0]));
|
|
g[11] = (a[0]*(-C[1]+B[1]) + a[1]*(-B[0]+C[0]) - c[0]*b[1] + c[1]*b[0]);
|
|
|
|
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> g2(12,12);
|
|
g2 = g*g.transpose();
|
|
|
|
// barrier function
|
|
term = Beta*(2*factTerm*factTerm - divTerm*(6*coeffA*detEPS + 2*coeffB)) / divTerm3;
|
|
g2 *= term;
|
|
|
|
int numElem = 0;
|
|
for(int r=0;r<12;r++)
|
|
{
|
|
for(int c=r;c<12;c++)
|
|
{
|
|
*denseHessianCoeffs(numElem++,t) += g2(r,c);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |