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; }