inline static void f( INTEGER * N, INTEGER * NRHS, double * A, INTEGER * LDA, INTEGER * IPIV, double * B, INTEGER * LDB, INTEGER * INFO) { DGESV(N, NRHS, A, LDA, IPIV, B, LDB, INFO ); }
int myLu(LinearSystemProblem* problem, double *z , SolverOptions* options) { /* Checks inputs */ if (problem == NULL || z == NULL) numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)"); /* Output info. : 0: ok - >0: problem (depends on solver) */ int info = -1; int n = problem->size; int n2 = n * n; double * Maux = 0; int * ipiv = 0; if (options && options->dWork) Maux = options->dWork; else Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double)); if (options && options->iWork) ipiv = options->iWork; else ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int)); int LAinfo = 0; //displayLS(problem); assert(problem->M->matrix0); assert(problem->q); memcpy(Maux, problem->M->matrix0, n2 * sizeof(double)); // memcpy(z,problem->q,n*sizeof(double)); for (int ii = 0; ii < n; ii++) z[ii] = -problem->q[ii]; DGESV(n, 1, Maux, n, ipiv, z, n, &LAinfo); if (!LAinfo) { info = 0; } else { printf("Equality_driver:: LU factorization failed:\n"); } //printf("LinearSystem_driver: computeError of LinearSystem : %e\n",LinearSystemComputeError(problem,z)); if (!(options && options->dWork)) free(Maux); if (!(options && options->iWork)) free(ipiv); return info; }
int nonSmoothNewtonNeigh(int n, double* z, NewtonFunctionPtr* phi, NewtonFunctionPtr* jacobianPhi, int* iparam, double* dparam) { int itermax = iparam[0]; // maximum number of iterations allowed int iterMaxWithSameZ = itermax / 4; int niter = 0; // current iteration number double tolerance = dparam[0]; /* double coef; */ sFphi = phi; sFjacobianPhi = jacobianPhi; // verbose=1; if (verbose > 0) { printf(" ============= Starting of Newton process =============\n"); printf(" - tolerance: %14.7e\n - maximum number of iterations: %i\n", tolerance, itermax); } int incx = 1; /* int n2 = n*n; */ int infoDGESV; /** merit function and its jacobian */ double psi_z; /** The algorithm is alg 4.1 of the paper of Kanzow and Kleinmichel, "A new class of semismooth Newton-type methods for nonlinear complementarity problems", in Computational Optimization and Applications, 11, 227-251 (1998). We try to keep the same notations */ double rho = 1e-8; double descentCondition, criterion, norm_jacobian_psi_z, normPhi_z; double p = 2.1; double terminationCriterion = 1; double norm; int findNewZ, i, j, NbLookingForANewZ; /* int naux=0; */ double aux = 0; /* double aux1=0; */ int ii; int resls = 1; /* char c; */ /* double * oldz; */ /* oldz=(double*)malloc(n*sizeof(double));*/ NbLookingForANewZ = 0; /** Iterations ... */ while ((niter < itermax) && (terminationCriterion > tolerance)) { scmp++; ++niter; /** Computes phi and its jacobian */ if (sZsol) { for (ii = 0; ii < sN; ii++) szzaux[ii] = sZsol[ii] - z[ii]; printf("dist zzsol %.32e.\n", cblas_dnrm2(n, szzaux, 1)); } (*sFphi)(n, z, sphi_z, 0); (*sFjacobianPhi)(n, z, sjacobianPhi_z, 1); /* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */ cblas_dgemv(CblasColMajor,CblasTrans, n, n, 1.0, sjacobianPhi_z, n, sphi_z, incx, 0.0, sgrad_psi_z, incx); norm_jacobian_psi_z = cblas_dnrm2(n, sgrad_psi_z, 1); /* Computes norm2(phi) */ normPhi_z = cblas_dnrm2(n, sphi_z, 1); /* Computes merit function */ psi_z = 0.5 * normPhi_z * normPhi_z; if (normPhi_z < tolerance) { /*it is the solution*/ terminationCriterion = tolerance / 2.0; break; } if (verbose > 0) { printf("Non Smooth Newton, iteration number %i, norm grad psi= %14.7e , psi = %14.7e, normPhi = %e .\n", niter, norm_jacobian_psi_z, psi_z, normPhi_z); printf(" -----------------------------------------------------------------------\n"); } NbLookingForANewZ++; if (niter > 2) { if (10 * norm_jacobian_psi_z < tolerance || !resls || NbLookingForANewZ > iterMaxWithSameZ) { NbLookingForANewZ = 0; resls = 1; /* if (NbLookingForANewZ % 10 ==1 && 0){ printf("Try NonMonotomnelineSearch\n"); cblas_dcopy(n,sgrad_psi_z,1,sdir_descent,1); cblas_dscal( n , -1.0 ,sdir_descent,incx); NonMonotomnelineSearch( z, phi, 10); continue; } */ /* FOR DEBUG ONLY*/ if (sZsol) { printf("begin plot prev dir\n"); plotMerit(z, 0, 0); printf("end\n"); /* gets(&c);*/ (*sFphi)(n, sZsol, szaux, 0); printf("value psi(zsol)=%e\n", cblas_dnrm2(n, szaux, 1)); cblas_dcopy(n, sZsol, incx, szaux, incx); cblas_daxpy(n , -1 , z , 1 , szaux , 1); printf("dist to sol %e \n", cblas_dnrm2(n, szaux, 1)); for (ii = 0; ii < n; ii++) sdir_descent[ii] = sZsol[ii] - z[ii]; aux = norm; norm = 1; printf("begin plot zzsol dir\n"); plotMerit(z, 0, 0); printf("end\n"); /* gets(&c);*/ norm = aux; } printf("looking for a new Z...\n"); /*may be a local minimal*/ /*find a gradiant going out of this cul-de-sac.*/ norm = n / 2; findNewZ = 0; for (j = 0; j < 20; j++) { for (i = 0; i < n; i++) { if (sZsol) { /* FOR DEBUG ONLY*/ (*sFphi)(n, sZsol, sphi_zaux, 0); norm = cblas_dnrm2(n, sphi_zaux, 1); printf("Norm of the sol %e.\n", norm); for (ii = 0; ii < n; ii++) sdir_descent[ii] = sZsol[ii] - z[ii]; norm = 1; } else { for (ii = 0; ii < n; ii++) { sdir_descent[ii] = 1.0 * rand(); } cblas_dscal(n, 1 / cblas_dnrm2(n, sdir_descent, 1), sdir_descent, incx); cblas_dscal(n, norm, sdir_descent, incx); } cblas_dcopy(n, z, incx, szaux, incx); // cblas_dscal(n,0.0,zaux,incx); /* zaux = z + dir */ cblas_daxpy(n , norm , sdir_descent , 1 , szaux , 1); /* Computes the jacobian of the merit function, jacobian_psi_zaux = transpose(jacobianPhi_zaux).phi_zaux */ (*sFphi)(n, szaux, sphi_zaux, 0); (*sFjacobianPhi)(n, szaux, sjacobianPhi_zaux, 1); /* FOR DEBUG ONLY*/ if (sZsol) { aux = cblas_dnrm2(n, sphi_zaux, 1); printf("Norm of the sol is now %e.\n", aux); for (ii = 0; ii < n; ii++) printf("zsol %e zaux %e \n", sZsol[ii], szaux[ii]); } cblas_dgemv(CblasColMajor, CblasTrans, n, n, 1.0, sjacobianPhi_zaux, n, sphi_zaux, incx, 0.0, sgrad_psi_zaux, incx); cblas_dcopy(n, szaux, 1, szzaux, 1); cblas_daxpy(n , -1 , z , incx , szzaux , incx); /*zzaux must be a descente direction.*/ /*ie jacobian_psi_zaux.zzaux <0 printf("jacobian_psi_zaux : \n");*/ /*cblas_dcopy(n,sdir,incx,sdir_descent,incx); plotMerit(z, phi);*/ aux = cblas_ddot(n, sgrad_psi_zaux, 1, szzaux, 1); /* aux1 = cblas_dnrm2(n,szzaux,1); aux1 = cblas_dnrm2(n,sgrad_psi_zaux,1);*/ aux = aux / (cblas_dnrm2(n, szzaux, 1) * cblas_dnrm2(n, sgrad_psi_zaux, 1)); /* printf("aux: %e\n",aux);*/ if (aux < 0.1 * (j + 1)) { //zaux is the new point. findNewZ = 1; cblas_dcopy(n, szaux, incx, z, incx); break; } } if (findNewZ) break; if (j == 10) { norm = n / 2; } else if (j > 10) norm = -2 * norm; else norm = -norm / 2.0; } if (! findNewZ) { printf("failed to find a new z\n"); /* exit(1);*/ continue; } else continue; } } /* Stops if the termination criterion is satisfied */ terminationCriterion = norm_jacobian_psi_z; /* if(terminationCriterion < tolerance){ break; }*/ /* Search direction calculation Find a solution dk of jacobianPhiMatrix.d = -phiVector. dk is saved in phiVector. */ cblas_dscal(n , -1.0 , sphi_z, incx); DGESV(n, 1, sjacobianPhi_z, n, sipiv, sphi_z, n, &infoDGESV); if (infoDGESV) { printf("DGEV error %d.\n", infoDGESV); } cblas_dcopy(n, sphi_z, 1, sdir_descent, 1); criterion = cblas_dnrm2(n, sdir_descent, 1); /* printf("norm dir descent %e\n",criterion);*/ /*printf("begin plot descent dir\n"); plotMerit(z, phi); printf("end\n"); gets(&c);*/ /*printf("begin plot zzsol dir\n"); plotMeritToZsol(z,phi); printf("end\n"); gets(&c);*/ /* norm = cblas_dnrm2(n,sdir_descent,1); printf("norm desc %e \n",norm); cblas_dscal( n , 1/norm , sdir_descent, 1); */ /* descentCondition = jacobian_psi.dk */ descentCondition = cblas_ddot(n, sgrad_psi_z, 1, sdir_descent, 1); /* Criterion to be satisfied: error < -rho*norm(dk)^p */ criterion = -rho * pow(criterion, p); /* printf("ddddddd %d\n",scmp); if (scmp>100){ displayMat(sjacobianPhi_z,n,n,n); exit(1); }*/ // if ((infoDGESV != 0 || descentCondition > criterion) && 0) // { // printf("no a desc dir, get grad psy\n"); /* dk = - jacobian_psi (remind that dk is saved in phi_z) */ // cblas_dcopy(n, sgrad_psi_z, 1, sdir_descent, 1); // cblas_dscal(n , -1.0 , sdir_descent, incx); /*DEBUG ONLY*/ /*printf("begin plot new descent dir\n"); plotMerit(z); printf("end\n"); gets(&c);*/ // } /* coef=fabs(norm_jacobian_psi_z*norm_jacobian_psi_z/descentCondition); if (coef <1){ cblas_dscal(n,coef,sdir_descent,incx); printf("coef %e norm dir descent is now %e\n",coef,cblas_dnrm2(n,sdir_descent,1)); }*/ /* Step-3 Line search: computes z_k+1 */ /*linesearch_Armijo(n,z,sdir_descent,psi_z, descentCondition, phi);*/ /* if (niter == 10){ printf("begin plot new descent dir\n"); plotMerit(z); printf("end\n"); gets(&c); }*/ /* memcpy(oldz,z,n*sizeof(double));*/ resls = linesearch2_Armijo(n, z, psi_z, descentCondition); if (!resls && niter > 1) { /* displayMat(sjacobianPhi_z,n,n,n); printf("begin plot new descent dir\n"); plotMerit(oldz,psi_z, descentCondition); printf("end\n"); gets(&c);*/ } /* lineSearch_Wolfe(z, descentCondition, phi,jacobianPhi);*/ /* if (niter>3){ printf("angle between prev dir %e.\n",acos(cblas_ddot(n, sdir_descent, 1, sPrevDirDescent, 1)/(cblas_dnrm2(n,sdir_descent,1)*cblas_dnrm2(n,sPrevDirDescent,1)))); }*/ cblas_dcopy(n, sdir_descent, 1, sPrevDirDescent, 1); /* for (j=20;j<32;j++){ if (z[j]<0) z[j]=0; }*/ /* if( 1 || verbose>0) { printf("Non Smooth Newton, iteration number %i, error grad equal to %14.7e , psi value is %14.7e .\n",niter, terminationCriterion,psi_z); printf(" -----------------------------------------------------------------------\n"); }*/ } /* Total number of iterations */ iparam[1] = niter; /* Final error */ dparam[1] = terminationCriterion; /** Free memory*/ if (verbose > 0) { if (dparam[1] > tolerance) printf("Non Smooth Newton warning: no convergence after %i iterations\n" , niter); else printf("Non Smooth Newton: convergence after %i iterations\n" , niter); printf(" The residue is : %e \n", dparam[1]); } /* free(oldz);*/ if (dparam[1] > tolerance) return 1; else return 0; }
int nonSmoothNewton(int n, double* z, NewtonFunctionPtr* phi, NewtonFunctionPtr* jacobianPhi, int* iparam, double* dparam) { if (phi == NULL || jacobianPhi == NULL) { fprintf(stderr, "NonSmoothNewton error: phi or its jacobian function = NULL pointer.\n"); exit(EXIT_FAILURE); } int itermax = iparam[0]; // maximum number of iterations allowed int niter = 0; // current iteration number double tolerance = dparam[0]; if (verbose > 0) { printf(" ============= Starting of Newton process =============\n"); printf(" - tolerance: %14.7e\n - maximum number of iterations: %i\n", tolerance, itermax); } int incx = 1; int n2 = n * n; int infoDGESV; /* Memory allocation for phi and its jacobian */ double * phiVector = (double*)malloc(n * sizeof(*phiVector)); double *jacobianPhiMatrix = (double*)malloc(n2 * sizeof(*jacobianPhiMatrix)); /** merit function and its jacobian */ double psi; double *jacobian_psi = (double*)malloc(n * sizeof(*jacobian_psi)); int* ipiv = (int *)malloc(n * sizeof(*ipiv)); if (phiVector == NULL || jacobianPhiMatrix == NULL || jacobian_psi == NULL || ipiv == NULL) { fprintf(stderr, "NonSmoothNewton, memory allocation failed.\n"); exit(EXIT_FAILURE); } /** The algorithm is alg 4.1 of the paper of Kanzow and Kleinmichel, "A new class of semismooth Newton-type methods for nonlinear complementarity problems", in Computational Optimization and Applications, 11, 227-251 (1998). We try to keep the same notations */ double rho = 1e-8; double descentCondition, criterion, norm_jacobian_psi, normPhi; double p = 2.1; double terminationCriterion = 1; if (jacobian_psi == NULL) { fprintf(stderr, "NonSmoothNewton, memory allocation failed for jacobian_psi.\n"); exit(EXIT_FAILURE); } /** Iterations ... */ while ((niter < itermax) && (terminationCriterion > tolerance)) { ++niter; /** Computes phi and its jacobian */ (*phi)(n, z, phiVector, 0); (*jacobianPhi)(n, z, jacobianPhiMatrix, 1); /* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */ cblas_dgemv(CblasColMajor,CblasTrans, n, n, 1.0, jacobianPhiMatrix, n, phiVector, incx, 0.0, jacobian_psi, incx); norm_jacobian_psi = cblas_dnrm2(n, jacobian_psi, 1); /* Computes norm2(phi) */ normPhi = cblas_dnrm2(n, phiVector, 1); /* Computes merit function */ psi = 0.5 * normPhi * normPhi; /* Stops if the termination criterion is satisfied */ terminationCriterion = norm_jacobian_psi; if (terminationCriterion < tolerance) break; /* Search direction calculation Find a solution dk of jacobianPhiMatrix.d = -phiVector. dk is saved in phiVector. */ cblas_dscal(n , -1.0 , phiVector, incx); DGESV(n, 1, jacobianPhiMatrix, n, ipiv, phiVector, n, &infoDGESV); /* descentCondition = jacobian_psi.dk */ descentCondition = cblas_ddot(n, jacobian_psi, 1, phiVector, 1); /* Criterion to be satisfied: error < -rho*norm(dk)^p */ criterion = cblas_dnrm2(n, phiVector, 1); criterion = -rho * pow(criterion, p); if (infoDGESV != 0 || descentCondition > criterion) { /* dk = - jacobian_psi (remind that dk is saved in phiVector) */ cblas_dcopy(n, jacobian_psi, 1, phiVector, 1); cblas_dscal(n , -1.0 , phiVector, incx); } /* Step-3 Line search: computes z_k+1 */ linesearch_Armijo(n, z, phiVector, psi, descentCondition, phi); if (verbose > 0) { printf("Non Smooth Newton, iteration number %i, error equal to %14.7e .\n", niter, terminationCriterion); printf(" -----------------------------------------------------------------------\n"); } } /* Total number of iterations */ iparam[1] = niter; /* Final error */ dparam[1] = terminationCriterion; /** Free memory*/ free(phiVector); free(jacobianPhiMatrix); free(jacobian_psi); free(ipiv); if (verbose > 0) { if (dparam[1] > tolerance) printf("Non Smooth Newton warning: no convergence after %i iterations\n" , niter); else printf("Non Smooth Newton: convergence after %i iterations\n" , niter); printf(" The residue is : %e \n", dparam[1]); } if (dparam[1] > tolerance) return 1; else return 0; }
int ImplicitNewmarkDense::DoTimestep() { int numIter = 0; double error0 = 0; // error after the first step double errorQuotient; // store current amplitudes and set initial guesses for qaccel, qvel // note: these guesses will later be overriden; they are only used to construct the right-hand-side vector (multiplication with M and C) for(int i=0; i<r; i++) { q_1[i] = q[i]; qvel_1[i] = qvel[i]; qaccel_1[i] = qaccel[i]; qaccel[i] = alpha1 * (q[i] - q_1[i]) - alpha2 * qvel_1[i] - alpha3 * qaccel_1[i]; qvel[i] = alpha4 * (q[i] - q_1[i]) + alpha5 * qvel_1[i] + alpha6 * qaccel_1[i]; } do { int i; PerformanceCounter counterForceAssemblyTime; reducedForceModel->GetForceAndMatrix(q, internalForces, tangentStiffnessMatrix); counterForceAssemblyTime.StopCounter(); forceAssemblyTime = counterForceAssemblyTime.GetElapsedTime(); // scale internal forces for(i=0; i<r; i++) internalForces[i] *= internalForceScalingFactor; /* printf("internalForceScalingFactor = %G\n", internalForceScalingFactor); printf("q:\n"); for(int i=0; i<r; i++) printf("%G ", q[i]); printf("\n"); printf("Internal forces:\n"); for(int i=0; i<r; i++) printf("%G ", internalForces[i]); printf("\n"); */ for(i=0; i<r2; i++) tangentStiffnessMatrix[i] *= internalForceScalingFactor; for(i=0; i<r2; i++) tangentStiffnessMatrix[i] += tangentStiffnessMatrixOffset[i]; /* printf("Tangent stiffness matrix:\n"); for(int i=0; i<r; i++) { for(int j=0; j<r; j++) printf("%.15f ", tangentStiffnessMatrix[r * j + i]); printf("\n"); } printf("Tangent stiffness matrix offset:\n"); for(int i=0; i<r; i++) { for(int j=0; j<r; j++) printf("%.15f ", tangentStiffnessMatrixOffset[r * j + i]); printf("\n"); } printf("----\n"); */ //WriteMatrixToDisk_("Kr", r, r, tangentStiffnessMatrix); //WriteMatrixToDisk_("Mr", r, r, massMatrix); //exit(1); memset(qresidual, 0, sizeof(double) * r); if (useStaticSolver) { // no operation } else { // build effective stiffness: add mass matrix and damping matrix to tangentStiffnessMatrix for(i=0; i<r2; i++) { dampingMatrix[i] = dampingMassCoef * massMatrix[i] + dampingStiffnessCoef * tangentStiffnessMatrix[i]; tangentStiffnessMatrix[i] += alpha4 * dampingMatrix[i]; //tangentStiffnessMatrix[i] += alpha3 * massMatrix[i] + gamma * alpha1 * dampingMatrix[i]; // static Rayleigh damping // add mass matrix to the effective stiffness matrix tangentStiffnessMatrix[i] += alpha1 * massMatrix[i]; } // compute force residual, store it into aux variable qresidual // qresidual = M * qaccel + C * qvel - externalForces + internalForces // M * qaccel cblas_dgemv(CblasColMajor,CblasNoTrans, r,r,1.0,massMatrix,r,qaccel,1,0.0,qresidual,1); // += C * qvel cblas_dgemv(CblasColMajor,CblasNoTrans, r,r,1.0,dampingMatrix,r,qvel,1,1.0,qresidual,1); } // add externalForces, internalForces for(i=0; i<r; i++) { qresidual[i] += internalForces[i] - externalForces[i]; qresidual[i] *= -1; qdelta[i] = qresidual[i]; } /* printf("internalForceScalingFactor = %G\n", internalForceScalingFactor); printf("internal forces:\n"); for(int i=0; i<r; i++) printf("%G ", internalForces[i]); printf("\n"); printf("external forces:\n"); for(int i=0; i<r; i++) printf("%G ", externalForces[i]); printf("\n"); printf("mass matrix:\n"); for(int i=0; i<r*r; i++) printf("%G ", massMatrix[i]); printf("\n"); printf("damping matrix:\n"); for(int i=0; i<r*r; i++) printf("%G ", dampingMatrix[i]); printf("\n"); printf("effective stiffness matrix:\n"); for(int i=0; i<r*r; i++) printf("%G ", tangentStiffnessMatrix[i]); printf("\n"); printf("matrix rhs:\n"); for(int i=0; i<r; i++) printf("%G ", qdelta[i]); printf("\n"); */ double error = 0; for(i=0; i<r; i++) error += qresidual[i] * qresidual[i]; // on the first iteration, compute initial error if (numIter == 0) { error0 = error; errorQuotient = 1.0; } else { // rel error wrt to initial error before performing this iteration errorQuotient = error / error0; } if ((errorQuotient < epsilon * epsilon) || (error == 0)) { break; } // solve (effective stiffness) * qdelta = qresidual PerformanceCounter counterSystemSolveTime; //counterSystemSolveTime.StartCounter(); // it starts automatically in constructor switch (solver) { case generalMatrixSolver: { INTEGER N = r; INTEGER NRHS = 1; double * A = tangentStiffnessMatrix; INTEGER LDA = r; double * B = qdelta; INTEGER LDB = r; INTEGER INFO; #ifdef __APPLE__ #define DGESV dgesv_ #else #define DGESV dgesv #endif DGESV ( &N, &NRHS, A, &LDA, IPIV->GetBuf(), B, &LDB, &INFO ); if (INFO != 0) { printf("Error: Gaussian elimination solver returned non-zero exit status %d.\n",(int)INFO); return 1; } } break; case symmetricMatrixSolver: { // call dsysv ( uplo, n, nrhs, a, lda, ipiv, b, ldb, work, lwork, info) #ifdef __APPLE__ #define DSYSV dsysv_ #else #define DSYSV dsysv #endif char uplo = 'U'; INTEGER nrhs = 1; INTEGER info; INTEGER R = r; INTEGER symmetricSolver_lworkI = symmetricSolver_lwork; DSYSV ( &uplo, &R, &nrhs, tangentStiffnessMatrix, &R, IPIV->GetBuf(), qdelta, &R, symmetricSolver_work, &symmetricSolver_lworkI, &info); if (info != 0) { printf("Error: Symmetric indefinite solver returned non-zero exit status %d.\n",(int)info); return 1; } } break; case positiveDefiniteMatrixSolver: { // call dposv ( uplo, n, nrhs, a, lda, b, ldb, info) #ifdef __APPLE__ #define DPOSV dposv_ #else #define DPOSV dposv #endif char uplo = 'U'; INTEGER nrhs = 1; INTEGER info = 0; INTEGER R = r; DPOSV ( &uplo, &R, &nrhs, tangentStiffnessMatrix, &R, qdelta, &R, &info); if (info != 0) { printf("Error: Positive-definite Cholesky solver returned non-zero exit status %d.\n",(int)info); return 1; } } break; default: printf("Error: reduced integration solver not specified.\n"); return 1; break; } counterSystemSolveTime.StopCounter(); systemSolveTime = counterSystemSolveTime.GetElapsedTime(); /* printf("qdelta:\n"); for(int i=0; i<r; i++) printf("%G ", qdelta[i]); printf("\n"); */ // update state for(i=0; i<r; i++) { q[i] += qdelta[i]; qaccel[i] = alpha1 * (q[i] - q_1[i]) - alpha2 * qvel_1[i] - alpha3 * qaccel_1[i]; qvel[i] = alpha4 * (q[i] - q_1[i]) + alpha5 * qvel_1[i] + alpha6 * qaccel_1[i]; } numIter++; } while (numIter < maxIterations); /* printf("Num iterations performed: %d (maxIterations=%d)\n", numIter, maxIterations); if ((numIter >= maxIterations) && (maxIterations > 1)) { printf("Warning: method did not converge in max number of iterations.\n"); } */ return 0; }
int NM_gesv(NumericsMatrix* A, double *b) { assert(A->size0 == A->size1); int info = 1; switch (A->storageType) { case NM_DENSE: { assert(A->matrix0); DGESV(A->size0, 1, A->matrix0, A->size0, NM_iWork(A, A->size0), b, A->size0, &info); break; } case NM_SPARSE_BLOCK: /* sparse block -> triplet -> csc */ case NM_SPARSE: { switch (NM_linearSolverParams(A)->solver) { case NS_CS_LUSOL: info = !cs_lusol(1, NM_csc(A), b, DBL_EPSILON); break; #ifdef WITH_MUMPS case NS_MUMPS: { /* the mumps instance is initialized (call with job=-1) */ DMUMPS_STRUC_C* mumps_id = NM_MUMPS_id(A); mumps_id->rhs = b; mumps_id->job = 6; /* compute the solution */ dmumps_c(mumps_id); /* clean the mumps instance */ mumps_id->job = -2; dmumps_c(mumps_id); info = mumps_id->info[0]; if (info > 0) { if (verbose > 0) { printf("NM_gesv: MUMPS fails : info(1)=%d, info(2)=%d\n", info, mumps_id->info[1]); } } if (verbose > 1) { printf("MUMPS : condition number %g\n", mumps_id->rinfog[9]); printf("MUMPS : component wise scaled residual %g\n", mumps_id->rinfog[6]); printf("MUMPS : \n"); } /* Here we free mumps_id ... */ free(NM_linearSolverParams(A)->solver_data); NM_linearSolverParams(A)->solver_data = NULL; break; } #endif default: { fprintf(stderr, "NM_gesv: unknown sparse linearsolver : %d\n", NM_linearSolverParams(A)->solver); exit(EXIT_FAILURE); } } break; } default: assert (0 && "NM_gesv unknown storageType"); } /* some time we cannot find a solution to a linear system, and its fine, for * instance with the minFBLSA. Therefore, we should not check here for * problems, but the calling function has to check the return code.*/ // CHECK_RETURN(info); return info; }
int Matrix::Solve(const Matrix &b, Matrix &x) const { int n = numRows; int nrhs = x.numCols; #ifdef _G3DEBUG if (numRows != numCols) { opserr << "Matrix::Solve(B,X) - the matrix of dimensions [" << numRows << " " << numCols << "] is not square\n"; return -1; } if (n != x.numRows) { opserr << "Matrix::Solve(B,X) - #rows of X, " << x.numRows << " is not same as the matrices: " << numRows << endln; return -2; } if (n != b.numRows) { opserr << "Matrix::Solve(B,X) - #rows of B, " << b.numRows << " is not same as the matrices: " << numRows << endln; return -2; } if (x.numCols != b.numCols) { opserr << "Matrix::Solve(B,X) - #cols of B, " << b.numCols << " , is not same as that of X, b " << x.numCols << endln; return -3; } #endif // check work area can hold all the data if (dataSize > sizeDoubleWork) { if (matrixWork != 0) { delete [] matrixWork; } matrixWork = new double[dataSize]; sizeDoubleWork = dataSize; if (matrixWork == 0) { opserr << "WARNING: Matrix::Solve() - out of memory creating work area's\n"; sizeDoubleWork = 0; return -3; } } // check work area can hold all the data if (n > sizeIntWork) { if (intWork != 0) { delete [] intWork; } intWork = new int[n]; sizeIntWork = n; if (intWork == 0) { opserr << "WARNING: Matrix::Solve() - out of memory creating work area's\n"; sizeIntWork = 0; return -3; } } x = b; // copy the data int i; for (i=0; i<dataSize; i++) matrixWork[i] = data[i]; int ldA = n; int ldB = n; int info; double *Aptr = matrixWork; double *Xptr = x.data; int *iPIV = intWork; info = -1; #ifdef _WIN32 #ifndef _DLL DGESV(&n,&nrhs,Aptr,&ldA,iPIV,Xptr,&ldB,&info); #endif #ifdef _DLL opserr << "Matrix::Solve - not implemented in dll\n"; return -1; #endif #else dgesv_(&n,&nrhs,Aptr,&ldA,iPIV,Xptr,&ldB,&info); /* // further correction if required double Bptr[n*n]; for (int i=0; i<n*n; i++) Bptr[i] = b.data[i]; double *origData = data; double Ferr[n]; double Berr[n]; double newWork[3*n]; int newIwork[n]; dgerfs_("N",&n,&n,origData,&ldA,Aptr,&n,iPIV,Bptr,&ldB,Xptr,&ldB, Ferr, Berr, newWork, newIwork, &info); */ #endif return info; }
int Matrix::Solve(const Vector &b, Vector &x) const { int n = numRows; #ifdef _G3DEBUG if (numRows != numCols) { opserr << "Matrix::Solve(b,x) - the matrix of dimensions " << numRows << ", " << numCols << " is not square " << endln; return -1; } if (n != x.Size()) { opserr << "Matrix::Solve(b,x) - dimension of x, " << numRows << "is not same as matrix " << x.Size() << endln; return -2; } if (n != b.Size()) { opserr << "Matrix::Solve(b,x) - dimension of x, " << numRows << "is not same as matrix " << b.Size() << endln; return -2; } #endif // check work area can hold all the data if (dataSize > sizeDoubleWork) { if (matrixWork != 0) { delete [] matrixWork; } matrixWork = new double[dataSize]; sizeDoubleWork = dataSize; if (matrixWork == 0) { opserr << "WARNING: Matrix::Solve() - out of memory creating work area's\n"; sizeDoubleWork = 0; return -3; } } // check work area can hold all the data if (n > sizeIntWork) { if (intWork != 0) { delete [] intWork; } intWork = new int[n]; sizeIntWork = n; if (intWork == 0) { opserr << "WARNING: Matrix::Solve() - out of memory creating work area's\n"; sizeIntWork = 0; return -3; } } // copy the data int i; for (i=0; i<dataSize; i++) matrixWork[i] = data[i]; // set x equal to b x = b; int nrhs = 1; int ldA = n; int ldB = n; int info; double *Aptr = matrixWork; double *Xptr = x.theData; int *iPIV = intWork; #ifdef _WIN32 #ifndef _DLL DGESV(&n,&nrhs,Aptr,&ldA,iPIV,Xptr,&ldB,&info); #endif #ifdef _DLL opserr << "Matrix::Solve - not implemented in dll\n"; return -1; #endif #else dgesv_(&n,&nrhs,Aptr,&ldA,iPIV,Xptr,&ldB,&info); #endif return 0; }
void n_svm(double *A, double *w, double *gamma, DOC *d_p, DOC *d_n){ int i,j,k,m,n,*im,in,jn,nn,iter; double alpha, *H, *Q, xx,current_xx; // double **a, **v, *star, *hu, *u; double *star, *hu, *u; int ldu,ldvt,lwork,lda,ldb,info,*ipiv,emm,enn,rsh=1; double *s,*ut,*vt,*work, *HH; char ch1='N'; //str[99], // // m=num_tot; lda=NM_MAX; ldb=lda; n=num_SVs_tot; im = new int [m+4]; for(i=0;i<(m+4);i++)im[i]=i*m; // // create H & Q matrices // H = new double [(m+1)*(m+1)]; Q = new double [(m+1)*(m+1)]; nn=n+1; for(i=0;i<num_p;i++){ k=i*nn; in=i*n; for(j=0;j<n;j++)H[k+j]=A[in+j]; H[k+n]=-1.0; } for(i=0;i<num_n;i++){ k=(i+num_p)*nn; in=(i+num_p)*n; for(j=0;j<n;j++)H[k+j]=-A[in+j]; H[k+n]=1.0; } for(i=0;i<m;i++){ in=i*nn; for(j=0;j<m;j++){ if(i==j) { Q[im[i]+j] = 1.0 / nu; if ( i<num_p ) Q[im[i]+j] = Q[im[i]+j] / J_nu; } else Q[im[i]+j]=0.0; jn=j*nn; for(k=0;k<nn;k++) Q[im[i]+j]=Q[im[i]+j]+H[in+k]*H[jn+k]; // printf("%8.4lf",Q[im[i]+j]); } // printf("\n"); } lwork = 8*lda; HH = (double *)malloc((lda*m)*sizeof(double)); hu = (double *)malloc(ldb*sizeof(double)); ipiv = (int *)malloc(lda*sizeof(int)); s = (double *)malloc((lda)*sizeof(double)); ut = (double *)malloc(sizeof(double)); vt = (double *)malloc(sizeof(double)); work = (double *)malloc((lwork)*sizeof(double)); // a=H', a=UWV; w is svd(a); after call svdcmp a was changed to U; // output the H' to file and run SVD program by system command // then then reading the norm(H',2) back from SVD runbig results emm=nn; enn=m; /* if ( (fpw=fopen("Matrix_H__T.dat","wt")) != NULL ) { fprintf(fpw, "%d , %d\n", nn,m); */ for(i=0;i<nn;i++) for(j=0;j<m;j++) { // fprintf(fpw,"%26.16lf\n",H[j*nn+i]); HH[i+j*lda]=H[j*nn+i]; } /* fclose(fpw); } */ if(debug){ #ifdef WIN32 printf("Printing time before svd the %d * %d matrix: ",nn,m); fflush(stdout); system("date /T & time /T"); #else printf("Printing time before svd the %d * %d matrix: ",nn,m); fflush(stdout); system("date"); #endif } delete[] H; ldu = 1; ldvt = 1; #ifdef WIN32 DGESVD(&ch1,&ch1, &emm, &enn, HH, &lda, s, ut, &ldu, vt, &ldvt, work, &lwork,&info); #else dgesvd_(&ch1,&ch1, &emm, &enn, HH, &lda, s, ut, &ldu, vt, &ldvt, work, &lwork,&info); #endif alpha = s[0] ; if (debug){ #ifdef WIN32 printf("Norm(H^^T)=%20.16lf\nPrinting time after svd : ",alpha); fflush(stdout); system("date /T & time /T"); #else printf("Norm(H^^T)=%20.16lf\nPrinting time after svd : ",alpha); fflush(stdout); system("date"); #endif } /* fpw=fopen("svd_sss","wt"); for(i=0;i<nn;i++)fprintf(fpw,"%26.16lf\n",s[i]); fclose(fpw); */ free(s); free(ut); free(vt); free(work); star = new double[1+m]; u = new double[1+m]; // hu = new double[1+m]; /* system("SVD_lapack_C Matrix_H__T.dat SVD_of_H__T.dat"); if ( (fpr=fopen("SVD_of_H__T.dat","rt")) != NULL ) { fscanf(fpr,"%lf",&alpha); fclose(fpr); } */ alpha = alpha*alpha*1.1+1.1/nu; // in matlab: hu=-max(((Q*u-e)-alpha*u),0)+Q*u-e; // at this step, hu = [-1](1:m) column vector; for(i=0;i<m;i++){ // a[i][1] =-1.0; u[i] = 0.0; // hu[i]=a[i][1]; hu[i] = - 1.0; } xx = vector_norm(m,hu); // printf("m= %d | hu[0]=%lf | norm(hu)=%lf\n",m,hu[0],xx); iter=1; current_xx=1.0e99; while( (xx>stop_criteria) && ( fabs(current_xx-xx) >= stop_criteria ) ){ current_xx = xx; for(i=0;i<m;i++){ star[i]=(Q[im[i]+i]-alpha)*u[i]-1; for(j=0;j<i;j++)star[i]=star[i]+Q[im[i]+j]*u[j]; for(j=i+1;j<m;j++)star[i]=star[i]+Q[im[i]+j]*u[j]; if(star[i]>TINY) star[i]=1.0; else star[i]=0.0; } /* if(iter<10)sprintf(str,"Matrix_A_and_B_0%d.dat",iter); else sprintf(str,"Matrix_A_and_B_%d.dat",iter); fpw=fopen(str,"wt") ; fprintf(fpw, "%d\n", m); */ for(i=0;i<m;i++){ // for(j=1;j<=m;j++)a[i][j]=Q[im[i-1]+j-1]*(1-star[i]); for(j=0;j<m;j++)w[j]=Q[im[i]+j]*(1-star[i]); // a[i][i]=a[i][i]+alpha*star[i]; w[i]=w[i]+alpha*star[i]; for(j=0;j<m;j++){ // fprintf(fpw,"%26.16lf\n",w[j]); HH[i+j*lda] = w[j] ; } } /* for ( i=0;i<m;i++ ){ for(j=0;j<m;j++){ fprintf(fpw,"%20.14lf\n",HH[i+j*lda]); } } for(i=0;i<m;i++){ fprintf(fpw,"%20.14lf\n",hu[i]); } fclose(fpw); */ if (debug){ #ifdef WIN32 printf("\nPrinting time before solving the %d-D linear equation: ",m); fflush(stdout); system("date /T & time /T"); #else printf("\nPrinting time before solving the %d-D linear equation: ",m); fflush(stdout); system("date"); #endif } // system("SOL_lin_eq_lapack_C Matrix_A_and_B.dat X_of_AX_eq_B.dat"); enn=m; rsh=1; #ifdef WIN32 DGESV( &enn, &rsh, HH, &lda, ipiv, hu, &ldb, &info); #else dgesv_( &enn, &rsh, HH, &lda, ipiv, hu, &ldb, &info); #endif /* fpw=fopen("xxxx","wt"); for(i=0;i<m;i++)fprintf(fpw,"%26.16lf\n",hu[i]); fclose(fpw); if ( (fpr=fopen("X_of_AX_eq_B.dat","rt")) != NULL ) { for(i=0;i<m;i++) fscanf(fpr,"%lf",&hu[i]); fclose(fpr); } */ if (debug){ #ifdef WIN32 printf("Printing time after solving equation: "); fflush(stdout); system("date /T & time /T"); #else printf("Printing time after solving equation: "); fflush(stdout); system("date"); #endif } for(i=0;i<m;i++) u[i]=u[i]-hu[i]; for(i=0;i<m;i++){ hu[i]=-1.0; for(j=0;j<m;j++) hu[i]=hu[i]+u[j]*Q[im[i]+j]; xx=hu[i]-u[i]*alpha; if(xx<TINY)xx=0.0; hu[i]=hu[i]-xx; // a[i][1]=hu[i]; // printf("%10.4lf\n",hu[i]); } xx = vector_norm(m,hu); printf("iteration %d : xx = %1.2le\n",iter,xx); fflush(stdout); iter++; } printf(" iteration done !\n"); fflush(stdout); gamma[1]=0.0; for(i=0;i<num_p;i++) gamma[1]=gamma[1]+u[i]; for(i=num_p;i<m;i++){ u[i]=-u[i]; gamma[1]=gamma[1]+u[i]; } gamma[1]=-gamma[1]; for(i=0;i<n;i++){ w[i]=0.0; for(j=0;j<m;j++) w[i]=w[i]+A[j*n+i]*u[j]; // printf("%lf\n",w[i]); } // Classification for the traing set k=0; for(i=0;i<num_p;i++){ xx=-gamma[1];; for(j=0;j<num_SVs_tot;j++)xx=xx+w[j]*A[i*num_SVs_tot+j]; if(xx>0.0)k++; } printf("Total %d of %d positive were classified correctly\n",k,num_p);fflush(stdout); /* nn=0; fpw=fopen("tmp.de.pre","wt"); for(i=0;i<num_n;i++){ xx=-gamma[1];; for(j=0;j<num_SVs_tot;j++)xx=xx+w[j]*A[(i+num_p)*num_SVs_tot+j]; strcpy(str,d_n[i].nam); if(str[6]=='.') str[6]='\0'; else str[4]='\0'; fprintf(fpw,"%d %s %lf\n",d_n[i].ind,str,xx); if(xx<0.0)nn++; } printf("Total %d of %d negative were classified correctly\n",nn,num_n);fflush(stdout); fclose(fpw); */ nn=0; for(i=0;i<num_n;i++){ xx=-gamma[1];; for(j=0;j<num_SVs_tot;j++)xx=xx+w[j]*A[(i+num_p)*num_SVs_tot+j]; if(xx<0.0)nn++; } printf("Total %d of %d negative were classified correctly\n",nn,num_n);fflush(stdout); nn=nn+k; xx = (double) nn / (double) num_tot; xx=xx*100.0; printf("Total classification rate %d / %d (%6.2lf %% )\n",nn,num_tot,xx); fflush(stdout); delete[] star; free(hu); delete[] u; delete[] Q; free(HH); // free(bb); free(ipiv); }