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