void NRICP::determineNonRigidOptimalDeformation()
{
//Find X = (At*A)-1 * At * B
//For current stiffness
GLuint i, four_i;
GLuint v1, v2;
GLuint four_v1, four_v2;
GLuint auxRowIndex;
GLuint sizeRowsMG = 4 * m_templateEdgeCount;
GLuint weight;
//Alpha * M * G
if(m_stiffnessChanged)
{
i = 0;
for(std::set< std::pair<GLuint, GLuint> >::iterator it = m_adjMat->begin(); it != m_adjMat->end(); ++it)
{
four_i = 4 * i; //for all edges
v1 = it->first;
v2 = it->second;
four_v1 = 4 * v1;
four_v2 = 4 * v2;
m_A->coeffRef(four_i, four_v1) = (-1) * m_stiffness;
m_A->coeffRef(four_i + 1, four_v1 + 1) = (-1) * m_stiffness;
m_A->coeffRef(four_i + 2, four_v1 + 2) = (-1) * m_stiffness;
m_A->coeffRef(four_i + 3, four_v1 + 3) = (-1) * m_stiffness * m_gamma;
m_A->coeffRef(four_i, four_v2) = m_stiffness;
m_A->coeffRef(four_i + 1, four_v2 + 1) = m_stiffness;
m_A->coeffRef(four_i + 2, four_v2 + 2) = m_stiffness;
m_A->coeffRef(four_i + 3, four_v2 + 3) = m_stiffness * m_gamma;
i++;
}
m_stiffnessChanged = false;
}
//W * D
for(GLuint i = 0; i < m_templateVertCount; ++i)
{
auxRowIndex = i + sizeRowsMG;
four_i = 4 * i;
weight = (*m_W)(i);
m_A->coeffRef(auxRowIndex, four_i) = m_D->coeff(i, four_i) * weight;
m_A->coeffRef(auxRowIndex, four_i + 1) = m_D->coeff(i, four_i + 1) * weight;
m_A->coeffRef(auxRowIndex, four_i + 2) = m_D->coeff(i, four_i + 2) * weight;
m_A->coeffRef(auxRowIndex, four_i + 3) = m_D->coeff(i, four_i + 3) * weight;
m_B->coeffRef(auxRowIndex, 0) = (*m_U)(i, 0) * weight;
m_B->coeffRef(auxRowIndex, 1) = (*m_U)(i, 1) * weight;
m_B->coeffRef(auxRowIndex, 2) = (*m_U)(i, 2) * weight;
}
solveLinearSystem();
}
// ===================================================
// Methods
// ===================================================
void
OneDFSIBC::applyBC ( const Real& time, const Real& timeStep, const solution_Type& solution,
const fluxPtr_Type& fluxPtr, vectorPtrContainer_Type& rhs )
{
UInt iNode;
( M_bcSide == OneDFSI::left ) ? iNode = 0 : iNode = fluxPtr->physics()->data()->numberOfNodes() - 1;
container2D_Type boundaryU;
boundaryU[0] = (*solution.find ("A")->second) (iNode);
boundaryU[1] = (*solution.find ("Q")->second) (iNode);
// Eigenvalues and eigenvectors of the jacobian diffFlux (= dF/dU = H)
container2D_Type eigenvalues;
container2D_Type leftEigenvector1, leftEigenvector2;
fluxPtr->eigenValuesEigenVectors ( boundaryU[0], boundaryU[1],
eigenvalues, leftEigenvector1, leftEigenvector2, iNode );
std::map<bcLine_Type, container2D_Type> bcMatrix;
bcMatrix[ OneDFSI::first ] = container2D_Type();
bcMatrix[ OneDFSI::second ] = container2D_Type();
container2D_Type bcRHS;
// First line of Matrix and RHS
computeMatrixAndRHS ( time, timeStep, fluxPtr, OneDFSI::first,
leftEigenvector1, leftEigenvector2, iNode, bcMatrix, bcRHS[0] );
// Second line of Matrix and RHS
computeMatrixAndRHS ( time, timeStep, fluxPtr, OneDFSI::second,
leftEigenvector1, leftEigenvector2, iNode, bcMatrix, bcRHS[1] );
container2D_Type bc = solveLinearSystem ( bcMatrix[OneDFSI::first], bcMatrix[OneDFSI::second], bcRHS );
// Set the BC in the RHS
(*rhs[0]) ( iNode ) = bc[0];
(*rhs[1]) ( iNode ) = bc[1];
#ifdef HAVE_LIFEV_DEBUG
debugStream (6311) << "[OneDFSIBC::applyBC] on bcSide " << M_bcSide << " imposing [ A, Q ] = [ " << bc[0] << ", " << bc[1] << " ]\n";
#endif
}
bool Triangle::getBarycentricCoordinates(const Ray& r, real_t& t,real_t mult[3], Vector3 position[3])
{
Matrix3 A;
Vector3 b,sol;
for(int i=0;i<3;i++)
{
A(0,i) = position[0][i] - position[1][i];
A(1,i) = position[0][i] - position[2][i];
A(2,i) = r.d[i];
b[i] = position[0][i] - r.e[i];
}
if(!solveLinearSystem(A, b, sol))
return false;
t = sol[2];
mult[0] = 1-sol[0]-sol[1];
mult[1] = sol[0];
mult[2] = sol[1];
return (sol[0]+sol[1]) <= 1 && sol[0]>=0 && sol[1]>=0;
}
double UpdateSystem( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference){
double time;
//int direction = (int)(myRand()*DIMENSIONS+0.5);
int direction = ADI_direction; ADI_direction = (ADI_direction+1)%DIMENSIONS;
for( time = 0; time+timeStep <= timeDifference; time += timeStep){
#if DIFFUSION_METHODE == EXPLICIT
//fprintf( stderr, "EXPLICIT\n");
// Finite Differences: explicit method
for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
voronoiDiagram->voronoiCells[i]->doxygen = timeStep * (Oxygen_Diffusion * secondDerivationOxygen( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
#if DIFFUSION_REACTION_SEPARAT == FALSE
- voronoiDiagram->voronoiCells[i]->oxygen * GiveMeTheOxygenRate( voronoiDiagram->voronoiCells[i]) // reaction
#endif
);
voronoiDiagram->voronoiCells[i]->dglucose = timeStep * (Glucose_Diffusion * secondDerivationGlucose( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
#if DIFFUSION_REACTION_SEPARAT == FALSE
- voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]) // reaction
#endif
);
}
#else
for( int d=0; d<DIMENSIONS; d++){
//fprintf(stderr, "dir=%i\n", direction);
// Finite Differences: alternating direction implicit (ADI)
double weight = 1.;
//int i = 0;
//int ii = 0;
//int iii = 0;
for( int iset=0; iset<voronoiDiagram->xN[(d==0?1:0)]/*20*/; iset++){
for( int iiset=0; iiset<voronoiDiagram->xN[(d<=1?2:1)]/*20*/; iiset++){
// base index
int index=0;
int index_count = 0;
for( int dd=0; dd<DIMENSIONS; dd++){
if( dd!=direction){
if(index_count==0)
index += (int)( iset*pow( voronoiDiagram->xN[(d==0?1:0)]/*20*/, dd));
else
index += (int)( iiset*pow( voronoiDiagram->xN[(d<=1?2:1)]/*20*/, dd));
index_count++;
}
}
//fprintf(stderr, "setupMatrixADI(o): %i <= (%i,%i)\n", index, iset, iiset);
#if DIFFUSION_METHODE == CRANK_NICHOLSON
setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'o', direction);
//fprintf( stderr, "CRANK_NICHOLSON\n");
#elif DIFFUSION_METHODE == ADI
setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
#elif DIFFUSION_METHODE == IMPLICIT
//fprintf( stderr, "ADI\n");
setupMatrix( voronoiDiagram, index, timeStep, 'o', direction);
#endif
//setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
//fprintf(stderr, "solve\n");
//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
#if DIFFUSION_METHODE == IMPLICIT
if( d!=0)
voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);
else
#endif
voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);
//fprintf(stderr, "setupMatrixADI(g): %i <= (%i,%i)\n", index, iset, iiset);
#if DIFFUSION_METHODE == CRANK_NICHOLSON
setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'g', direction);
//fprintf( stderr, "CRANK_NICHOLSON\n");
#elif DIFFUSION_METHODE == ADI
setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
#elif DIFFUSION_METHODE == IMPLICIT
//fprintf( stderr, "Implicit\n");
setupMatrix( voronoiDiagram, index, timeStep, 'g', direction);
#endif
//setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
#if DIFFUSION_METHODE == IMPLICIT
if( d!=0)
//add differences
voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
else
#endif
// set differences
voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
}
}
#endif
#if DIFFUSION_METHODE == IMPLICIT
if(d+1==DIMENSIONS)
#endif
// update half time step
{//fprintf(stderr, "update half time step\n");
for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
&& voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
&& voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
&& voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
#if DIMENSIONS > 2
&& voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
&& voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
#endif
){
voronoiDiagram->voronoiCells[i]->oxygen += voronoiDiagram->voronoiCells[i]->doxygen;
if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose;
if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
}
}}
// change direction
direction = (direction+1)%DIMENSIONS;
}
#if DIFFUSION_REACTION_SEPARAT == TRUE
// reaction
for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
&& voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
&& voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
&& voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
#if DIMENSIONS > 2
&& voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
&& voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
#endif
){
voronoiDiagram->voronoiCells[i]->oxygen += - timeStep * voronoiDiagram->voronoiCells[i]->oxygen * GiveMeTheOxygenRate( voronoiDiagram->voronoiCells[i]);
if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
voronoiDiagram->voronoiCells[i]->glucose += - timeStep * voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]);
if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
}
}
#endif
#if DIFFUSION_METHODE != EXPLICIT
}
#endif
return timeDifference - time;
}
/*! \fn solve system with Newton-Raphson
*
* \param [in] [n] size of equation
* [eps] tolerance for x
* [h] tolerance for f'
* [k] maximum number of iterations
* [work] work array size of (n*X)
* [f] user provided function
* [data] userdata
* [info]
* [calculate_jacobian] flag which decides whether Jacobian is calculated
* (0) once for the first calculation
* (i) every i steps (=1 means original newton method)
* (-1) never, factorization has to be given in A
*
*/
int _omc_newton(int(*f)(int*, double*, double*, void*, int), DATA_NEWTON* solverData, void* userdata)
{
int i, j, k = 0, l = 0, nrsh = 1;
int *n = &(solverData->n);
double *x = solverData->x;
double *fvec = solverData->fvec;
double *eps = &(solverData->ftol);
double *fdeps = &(solverData->epsfcn);
int * maxfev = &(solverData->maxfev);
double *fjac = solverData->fjac;
double *work = solverData->rwork;
int *iwork = solverData->iwork;
int *info = &(solverData->info);
int calc_jac = 1;
double error_f = 1.0 + *eps, scaledError_f = 1.0 + *eps, delta_x = 1.0 + *eps, delta_f = 1.0 + *eps, delta_x_scaled = 1.0 + *eps, lambda = 1.0;
double current_fvec_enorm, enorm_new;
if(ACTIVE_STREAM(LOG_NLS_V))
{
infoStreamPrint(LOG_NLS_V, 1, "######### Start Newton maxfev: %d #########", (int)*maxfev);
infoStreamPrint(LOG_NLS_V, 1, "x vector");
for(i=0; i<*n; i++)
infoStreamPrint(LOG_NLS_V, 0, "x[%d]: %e ", i, x[i]);
messageClose(LOG_NLS_V);
messageClose(LOG_NLS_V);
}
*info = 1;
/* calculate the function values */
(*f)(n, x, fvec, userdata, 1);
solverData->nfev++;
/* save current fvec in f_old*/
memcpy(solverData->f_old, fvec, *n*sizeof(double));
error_f = current_fvec_enorm = enorm_(n, fvec);
while(error_f > *eps && scaledError_f > *eps && delta_x > *eps && delta_f > *eps && delta_x_scaled > *eps)
{
if(ACTIVE_STREAM(LOG_NLS_V))
{
infoStreamPrint(LOG_NLS_V, 0, "\n**** start Iteration: %d *****", (int) l);
/* Debug output */
infoStreamPrint(LOG_NLS_V, 1, "function values");
for(i=0; i<*n; i++)
infoStreamPrint(LOG_NLS_V, 0, "fvec[%d]: %e ", i, fvec[i]);
messageClose(LOG_NLS_V);
}
/* calculate jacobian if no matrix is given */
if (calc_jac == 1 && solverData->calculate_jacobian >= 0)
{
(*f)(n, x, fvec, userdata, 0);
solverData->factorization = 0;
calc_jac = solverData->calculate_jacobian;
}
else
{
solverData->factorization = 1;
calc_jac--;
}
/* debug output */
if(ACTIVE_STREAM(LOG_NLS_JAC))
{
char buffer[4096];
infoStreamPrint(LOG_NLS_JAC, 1, "jacobian matrix [%dx%d]", (int)*n, (int)*n);
for(i=0; i<solverData->n;i++)
{
buffer[0] = 0;
for(j=0; j<solverData->n; j++)
sprintf(buffer, "%s%10g ", buffer, fjac[i*(*n)+j]);
infoStreamPrint(LOG_NLS_JAC, 0, "%s", buffer);
}
messageClose(LOG_NLS_JAC);
}
if (solveLinearSystem(n, iwork, fvec, fjac, solverData) != 0)
{
*info=-1;
break;
}
else
{
for (i =0; i<*n; i++)
solverData->x_new[i]=x[i]-solverData->x_increment[i];
infoStreamPrint(LOG_NLS_V,1,"x_increment");
for(i=0; i<*n; i++)
infoStreamPrint(LOG_NLS_V, 0, "x_increment[%d] = %e ", i, solverData->x_increment[i]);
messageClose(LOG_NLS_V);
if (solverData->newtonStrategy == NEWTON_DAMPED)
{
damping_heuristic(x, f, current_fvec_enorm, n, fvec, &lambda, &k, solverData, userdata);
}
else if (solverData->newtonStrategy == NEWTON_DAMPED2)
{
damping_heuristic2(0.75, x, f, current_fvec_enorm, n, fvec, &k, solverData, userdata);
}
else if (solverData->newtonStrategy == NEWTON_DAMPED_LS)
{
LineSearch(x, f, current_fvec_enorm, n, fvec, &k, solverData, userdata);
}
else if (solverData->newtonStrategy == NEWTON_DAMPED_BT)
{
Backtracking(x, f, current_fvec_enorm, n, fvec, solverData, userdata);
}
else
{
/* calculate the function values */
(*f)(n, solverData->x_new, fvec, userdata, 1);
solverData->nfev++;
}
calculatingErrors(solverData, &delta_x, &delta_x_scaled, &delta_f, &error_f, &scaledError_f, n, x, fvec);
/* updating x */
memcpy(x, solverData->x_new, *n*sizeof(double));
/* updating f_old */
memcpy(solverData->f_old, fvec, *n*sizeof(double));
current_fvec_enorm = error_f;
/* check if maximum iteration is reached */
if (++l > *maxfev)
{
*info = -1;
warningStreamPrint(LOG_NLS_V, 0, "Warning: maximal number of iteration reached but no root found");
break;
}
}
if(ACTIVE_STREAM(LOG_NLS_V))
{
infoStreamPrint(LOG_NLS_V,1,"x vector");
for(i=0; i<*n; i++)
infoStreamPrint(LOG_NLS_V, 0, "x[%d] = %e ", i, x[i]);
messageClose(LOG_NLS_V);
printErrors(delta_x, delta_x_scaled, delta_f, error_f, scaledError_f, eps);
}
}
solverData->numberOfIterations += l;
solverData->numberOfFunctionEvaluations += solverData->nfev;
return 0;
}
