inline static INTEGER f( INTEGER m, double * A,
INTEGER * IPIV, double * WORK,
INTEGER LWORK)
{
INTEGER M = m;
INTEGER N = m;
INTEGER LDA = m;
INTEGER INFO;
DGETRF (&M, &N, A, &LDA, IPIV, &INFO);
if (INFO != 0)
{
printf("Warning: LAPACK routine DGETRF returned non-zero exit status %d.\n",(int)INFO);
}
//SUBROUTINE DGETRI( N, A, LDA, IPIV, WORK, LWORK, INFO );
DGETRI(&M, A, &LDA, IPIV, WORK, &LWORK, &INFO);
if (INFO != 0)
{
printf("Warning: LAPACK routine DGETRI returned non-zero exit status %d.\n",(int)INFO);
}
return INFO;
}
int CentralDifferencesDense::UpdateLU()
{
if (r == 0)
return 0;
reducedForceModel->GetTangentStiffnessMatrix(q, stiffnessMatrix);
// construct damping matrix
for(int i=0; i<r2; i++)
dampingMatrix[i] = dampingMassCoef * massMatrix[i] + dampingStiffnessCoef * stiffnessMatrix[i];
// do LU decomposition
for(int i=0; i<r2; i++)
LUFactor[i] = massMatrix[i] + 0.5 * timestep * dampingMatrix[i];
INTEGER M = r;
INTEGER N = r;
double * A = LUFactor;
INTEGER LDA = r;
INTEGER INFO;
DGETRF(&M, &N, A, &LDA, IPIV->GetBuf(), &INFO);
if (INFO != 0)
{
printf("Warning: LAPACK routine DGETRF returned non-zero exit status %d.\n",(int)INFO);
return INFO;
}
return 0;
}
/* Main program */
int main() {
/* Locals */
int n = N, lda = LDA, info;
/* Local arrays */
int ipiv[N];
double a[LDA*N] = {
6.80, -2.11, 5.66, 5.97, 8.23,
-6.05, -3.30, 5.36, -4.44, 1.08,
-0.45, 2.58, -2.70, 0.27, 9.04,
8.32, 2.71, 4.35, -7.17, 2.14,
-9.67, -5.14, -7.26, 6.08, -6.87
};
double b[LDA*N];
cblas_dcopy (LDA*N, a, 1, b, 1);
/* Executable statements */
printf( "LAPACKE_dgetr (column-major, high-level) Example Program Results\n" );
/* Solve the equations A*X = B */
DGETRF(n, n, a, lda, ipiv, &info );
if( info > 0 ) {
printf( "DGETRF failed.\n" );
exit( 1 );
}
DGETRI(n, a, lda, ipiv, &info );
if( info > 0 ) {
printf( "DGETRI failed.\n" );
exit( 1 );
}
double tol = 1e-9;
double c[LDA*N];
cblas_dgemm(CblasColMajor, CblasNoTrans,CblasNoTrans, N, N, N, 1.0, a, N, b, N, 0.0, c, N);
for(int i=0;i<N;++i)
for(int j = 0; j<N; ++j)
if(i==j)
{if (fabs(c[i+N*j]-1.0)>tol) exit(1);}
else
{if (fabs(c[i+N*j]) > tol) exit(1);}
print_matrix("Id?", N,N,b,N);
exit( 0 );
}
int extractLCP(NumericsMatrix* MGlobal, double *z , int *indic, int *indicop, double *submatlcp , double *submatlcpop,
int *ipiv , int *sizesublcp , int *sizesublcpop)
{
if (MGlobal == NULL || z == NULL)
numerics_error("extractLCP", "Null input for one arg (problem, z, ...)");
int info;
/* double epsdiag = DBL_EPSILON;*/
/* Extract data from problem */
if (MGlobal->storageType == 1)
numerics_error("extractLCP", "Not yet implemented for sparse storage");
double * M = MGlobal->matrix0;
int sizelcp = MGlobal->size0;
if (M == NULL)
numerics_error("extractLCP", "Null input matrix M");
/* workspace = (double*)malloc(sizelcp * sizeof(double)); */
/* printf("recalcul_submat\n");*/
/* indic = set of indices for which z[i] is positive */
/* indicop = set of indices for which z[i] is null */
/* test z[i] sign */
int i, j = 0, k = 0;
for (i = 0; i < sizelcp; i++)
{
if (z[i] > w[i]) /* if (z[i] >= epsdiag)*/
{
indic[j] = i;
j++;
}
else
{
indicop[k] = i;
k++;
}
}
/* size of the sub-matrix that corresponds to indic */
*sizesublcp = j;
/* size of the sub-matrix that corresponds to indicop */
*sizesublcpop = k;
/* If indic is non-empty, copy corresponding M sub-matrix into submatlcp */
if (*sizesublcp != 0)
{
for (j = 0; j < *sizesublcp; j++)
{
for (i = 0; i < *sizesublcp; i++)
submatlcp[(j * (*sizesublcp)) + i] = M[(indic[j] * sizelcp) + indic[i]];
}
/* LU factorization and inverse in place for submatlcp */
DGETRF(*sizesublcp, *sizesublcp, submatlcp, *sizesublcp, ipiv, info);
if (info != 0)
{
numerics_warning("extractLCP", "LU factorization failed") ;
return 1;
}
DGETRI(*sizesublcp, submatlcp, *sizesublcp, ipiv , info);
if (info != 0)
{
numerics_warning("extractLCP", "LU inversion failed");
return 1;
}
/* if indicop is not empty, copy corresponding M sub-matrix into submatlcpop */
if (*sizesublcpop != 0)
{
for (j = 0; j < *sizesublcp; j++)
{
for (i = 0; i < *sizesublcpop; i++)
submatlcpop[(j * (*sizesublcpop)) + i] = vec[(indic[j] * sizelcp) + indicop[i]];
}
}
}
return 0;
}
int avi_caoferris(AffineVariationalInequalities* problem, double *z, double *w, SolverOptions* options)
{
unsigned n = problem->size;
assert(n > 0);
unsigned nrows = problem->poly->size_ineq;
assert(nrows - n > 0);
unsigned n_I = nrows - n; /* Number of inactive constraints */
/* Create the data problem */
LinearComplementarityProblem lcplike_pb;
lcplike_pb.size = nrows;
NumericsMatrix num_mat;
fillNumericsMatrix(&num_mat, NM_DENSE, nrows, nrows, calloc(nrows*nrows, sizeof(double)));
lcplike_pb.M = &num_mat;
lcplike_pb.q = (double *)calloc(nrows, sizeof(double));
double* a_bar = (double *)malloc(nrows*sizeof(double));
double* B_A_T = (double*)malloc(n*n*sizeof(double));
double* copyA = (double*)malloc(n*n*sizeof(double));
double* B_I_T = (double*)malloc(n*(n_I)*sizeof(double));
double* d_vec = (double *)malloc(nrows*sizeof(double));
int* basis = (int *)malloc((2*nrows+1)*sizeof(int));
siconos_find_vertex(problem->poly, n, basis);
DEBUG_PRINT_VEC_INT(basis, nrows+1);
const double* H = problem->poly->H;
const double* K = problem->poly->K;
/* Set of active constraints */
unsigned* A = (unsigned*)malloc(n*sizeof(unsigned));
int* active_constraints = &basis[nrows+1];
/* set active_constraints to 1 at the beginning */
memset(active_constraints, -1, nrows*sizeof(int));
DEBUG_PRINT_VEC_INT(active_constraints, nrows);
unsigned indx_B_I_T = 0;
for (unsigned i = 1; i <= nrows; ++i)
{
assert((unsigned)abs(basis[i]) > nrows); /* we don't want slack variable here */
int indx = abs(basis[i]) - nrows - 1 - n;
if (indx >= 0)
{
/* this is an inactive constraint */
assert(indx_B_I_T < n_I);
assert((unsigned)indx < nrows);
cblas_dcopy(n, &H[indx], nrows, &B_I_T[indx_B_I_T*n], 1); /* form B_I_T */
active_constraints[indx] = 0; /* desactivate the constraint */
lcplike_pb.q[n+indx_B_I_T] = -K[indx]; /* partial construction of q[n:nrows] as -K_I */
indx_B_I_T++;
}
}
DEBUG_PRINT_VEC_INT(active_constraints, nrows);
unsigned indx_B_A_T = 0;
for (unsigned i = 0; i < nrows; ++i)
{
if (active_constraints[i] == -1)
{
assert(indx_B_A_T < n);
A[indx_B_A_T] = i+1; /* note which constraints is active */
cblas_dcopy(n, &H[i], nrows, &B_A_T[indx_B_A_T*n], 1); /* form B_A_T */
d_vec[indx_B_A_T] = K[i]; /* save K_A */
indx_B_A_T++;
}
}
assert(indx_B_A_T == n && "there were not enough active constraints");
DEBUG_PRINT_VEC_STR("K_A", d_vec, n);
cblas_dcopy(n*n, problem->M->matrix0, 1, copyA, 1);
DEBUG_PRINT_MAT(B_A_T, n, n);
DEBUG_PRINT_MAT(B_I_T, n, n_I);
/* get LU for B_A_T */
int* ipiv = basis;
int infoLAPACK = 0;
/* LU factorisation of B_A_T */
DGETRF(n, n, B_A_T, n, ipiv, &infoLAPACK);
assert(infoLAPACK <= 0 && "avi_caoferris :: info from DGETRF > 0, this should not append !\n");
/* compute B_A_T^{-1}B_I_T */
DGETRS(LA_NOTRANS, n, n_I, B_A_T, n, ipiv, B_I_T, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = B_I_T is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}B_I_T\n");
DEBUG_PRINT_MAT(B_I_T, n, n_I);
/* Compute B_A_T^{-1} A */
DGETRS(LA_NOTRANS, n, n, B_A_T, n, ipiv, copyA, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = A is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}A\n");
DEBUG_PRINT_MAT(copyA, n, n);
/* do some precomputation for \bar{q}: B_A_T^{-1}q_{AVI} */
cblas_dcopy_msan(n, problem->q, 1, a_bar, 1);
DGETRS(LA_NOTRANS, n, 1, B_A_T, n, ipiv, a_bar, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = a_bar is not zero!\n");
DEBUG_PRINT_VEC_STR("B_A_T{-1}q_{AVI}", a_bar, n);
/* Do the transpose of B_A_T^{-1} A */
double* basepointer = &num_mat.matrix0[nrows*nrows - n*n];
for (unsigned i = 0; i < n; ++i) cblas_dcopy(n, ©A[i*n], 1, &basepointer[i], n);
/* Compute B_A_T^{-1}(B_A_T^{-1}M)_T */
DGETRS(LA_NOTRANS, n, n, B_A_T, n, ipiv, basepointer, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = (B_A_T^{-1}M)_T is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}(B_A_T^{-1}M)_T\n");
DEBUG_PRINT_MAT(basepointer, n, n);
for (unsigned i = 0; i < n; ++i) cblas_dcopy(n, &basepointer[n*i], 1, ©A[i], n);
DEBUG_PRINT_VEC_STR("b_I =: q[n:nrows]", (&lcplike_pb.q[n]), n_I);
/* partial construction of q: q[n:nrows] += (B_A_T^{-1}*B_I_T)_T K_A */
cblas_dgemv(CblasColMajor, CblasTrans, n_I, n, 1.0, B_I_T, n_I, d_vec, 1, 1.0, &lcplike_pb.q[n], 1);
DEBUG_PRINT_VEC_STR("final q[n:nrows] as b_I + B_I B_A^{-1}b_A", (&lcplike_pb.q[n]), n_I);
/* Compute B_A_T^{-1} M B_A^{-1} K_A
* We have to set CblasTrans since we still have a transpose */
/* XXX It looks like we could have 2 here, but not it does not work w/ it. Investigate why -- xhub */
cblas_dgemv(CblasColMajor, CblasTrans, n, n, 1.0, basepointer, n, d_vec, 1, 0.0, lcplike_pb.q, 1);
DEBUG_PRINT_VEC_STR("B_A_T^{-1} M B_A^{-1} K_A =: q[0:n]", lcplike_pb.q, n);
/* q[0:n] = 2 B_A_T^{-1} A B_A^{-1}b_A + B_A_T{-1} q_{AVI} */
/* XXX about the + or -: we do not follow the convention of Cao & Ferris */
cblas_daxpy(n, 1.0, a_bar, 1, lcplike_pb.q, 1);
DEBUG_PRINT("final q\n");
DEBUG_PRINT_VEC(lcplike_pb.q, nrows);
/* q is now ready, let's deal with M */
/* set some pointers to sub-matrices */
double* upper_left_mat = num_mat.matrix0;
double* upper_right_mat = &num_mat.matrix0[n*nrows];
double* lower_left_mat = &num_mat.matrix0[n];
double* lower_right_mat = &upper_right_mat[n];
/* copy the B_A_T^{-1} B_I_T (twice) and set the lower-left part to 0*/
for (unsigned i = 0, j = 0, k = 0; i < n_I; ++i, j += n_I, k += nrows)
{
cblas_dcopy(n, ©A[n*i], 1, &upper_right_mat[k], 1);/* copy into the right location B_A_T^{-1} M B_A^{-1} */
cblas_dcopy(n_I, &B_I_T[j], 1, &upper_left_mat[k], 1); /* copy B_A_T^{-1}*B_I_T to the upper-right block */
cblas_dscal(n, -1.0, &upper_left_mat[k], 1); /* take the opposite of the matrix */
cblas_dcopy(n_I, &B_I_T[j], 1, &lower_right_mat[i], nrows); /* copy B_IB_A^{-1} to the lower-left block */
memset(&lower_left_mat[k], 0, sizeof(double)*(n_I)); /* set the lower-left block to 0 */
}
DEBUG_PRINT_MAT(num_mat.matrix0, nrows, nrows);
/* Matrix M is now ready */
/* Save K_A */
double* K_A = a_bar;
cblas_dcopy(n, d_vec, 1, K_A, 1);
DEBUG_PRINT_VEC(K_A, n);
/* We put -1 because we directly copy it in stage 3 */
for (unsigned int i = 0; i < n; ++i) d_vec[i] = -1.0;
memset(&d_vec[n], 0, n_I*sizeof(double));
DEBUG_PRINT_VEC_INT_STR("Active set", A, n);
double* u_vec = (double *)calloc(nrows, sizeof(double));
double* s_vec = (double *)calloc(nrows, sizeof(double));
/* Call directly the 3rd stage
* Here w is used as u and z as s in the AVI */
int info = avi_caoferris_stage3(&lcplike_pb, u_vec, s_vec, d_vec, n, A, options);
/* Update z */
/* XXX why no w ? */
DEBUG_PRINT_VEC_INT(A, n);
for (unsigned i = 0; i < n; ++i) z[i] = s_vec[A[i]-1] + K_A[i];
DEBUG_PRINT_VEC_STR("s_A + K_A", z, n);
DGETRS(LA_TRANS, n, 1, B_A_T, n, ipiv, z, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A X = s_A + K_A is not zero!\n");
DEBUG_PRINT_VEC_STR("solution z", z, n);
/* free allocated stuff */
free(u_vec);
free(s_vec);
free(A);
free(basis);
free(d_vec);
free(B_I_T);
free(copyA);
free(B_A_T);
freeNumericsMatrix(lcplike_pb.M);
free(lcplike_pb.q);
free(a_bar);
return info;
}
void globalFrictionContact3D_nsgs(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double *globalVelocity, int* info, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
int n = problem->M->size0;
int m = 3 * nc;
NumericsMatrix* M = problem->M;
NumericsMatrix* H = problem->H;
double* q = problem->q;
double* b = problem->b;
double* mu = problem->mu;
/* Maximum number of iterations */
int itermax = iparam[0];
/* Tolerance */
double tolerance = dparam[0];
/* Check for trivial case */
*info = checkTrivialCaseGlobal(n, q, velocity, reaction, globalVelocity, options);
if (*info == 0)
return;
SolverGlobalPtr local_solver = NULL;
FreeSolverGlobalPtr freeSolver = NULL;
ComputeErrorGlobalPtr computeError = NULL;
/* Connect local solver */
initializeGlobalLocalSolver(n, &local_solver, &freeSolver, &computeError, M, q, mu, iparam);
/***** NSGS Iterations *****/
int iter = 0; /* Current iteration number */
double error = 1.; /* Current error */
int hasNotConverged = 1;
int contact; /* Number of the current row of blocks in M */
SparseBlockStructuredMatrix *Htrans = (SparseBlockStructuredMatrix*)malloc(sizeof(SparseBlockStructuredMatrix));
if (H->storageType != M->storageType)
{
// if(verbose==1)
fprintf(stderr, "Numerics, GlobalFrictionContact3D_nsgs. H->storageType != M->storageType :This case is not taken into account.\n");
exit(EXIT_FAILURE);
}
else if (M->storageType == 1)
{
inverseDiagSBM(M->matrix1);
Global_MisInverse = 1;
transposeSBM(H->matrix1, Htrans);
}
else if (M->storageType == 0)
{
/* Assume that M is not already LU */
int infoDGETRF = -1;
Global_ipiv = (int *)malloc(n * sizeof(int));
assert(!Global_MisLU);
DGETRF(n, n, M->matrix0, n, Global_ipiv, &infoDGETRF);
Global_MisLU = 1;
assert(!infoDGETRF);
}
else
{
fprintf(stderr, "Numerics, GlobalFrictionContactProblem_nsgs failed M->storageType not compatible.\n");
exit(EXIT_FAILURE);
}
dparam[0] = dparam[2]; // set the tolerance for the local solver
double* qtmp = (double*)malloc(n * sizeof(double));
for (int i = 0; i < n; i++) qtmp[i] = 0.0;
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
/* Solve the first part with the current reaction */
/* qtmp <--q */
cblas_dcopy(n, q, 1, qtmp, 1);
double alpha = 1.0;
double beta = 1.0;
/*qtmp = H reaction +qtmp */
prodNumericsMatrix(m, n, alpha, H, reaction , beta, qtmp);
if (M->storageType == 1)
{
beta = 0.0;
assert(Global_MisInverse);
/* globalVelocity = M^-1 qtmp */
prodNumericsMatrix(n, n, alpha, M, qtmp , beta, globalVelocity);
}
else if (M->storageType == 0)
{
int infoDGETRS = -1;
cblas_dcopy(n, qtmp, 1, globalVelocity, 1);
assert(Global_MisLU);
DGETRS(LA_NOTRANS, n, 1, M->matrix0, n, Global_ipiv, globalVelocity , n, &infoDGETRS);
assert(!infoDGETRS);
}
/* Compute current local velocity */
/* velocity <--b */
cblas_dcopy(m, b, 1, velocity, 1);
if (H->storageType == 1)
{
/* velocity <-- H^T globalVelocity + velocity*/
beta = 1.0;
prodSBM(n, m, alpha, Htrans, globalVelocity , beta, velocity);
}
else if (H->storageType == 0)
{
cblas_dgemv(CblasColMajor,CblasTrans, n, m, 1.0, H->matrix0 , n, globalVelocity , 1, 1.0, velocity, 1);
}
/* Loop through the contact points */
for (contact = 0 ; contact < nc ; ++contact)
{
/* (*local_solver)(contact,n,reaction,iparam,dparam); */
int pos = contact * 3;
double normUT = sqrt(velocity[pos + 1] * velocity[pos + 1] + velocity[pos + 2] * velocity[pos + 2]);
double an = 1.0;
reaction[pos] -= an * (velocity[pos] + mu[contact] * normUT);
reaction[pos + 1] -= an * velocity[pos + 1];
reaction[pos + 2] -= an * velocity[pos + 2];
projectionOnCone(&reaction[pos], mu[contact]);
}
/* int k; */
/* printf("\n"); */
/* for (k = 0 ; k < m; k++) printf("velocity[%i] = %12.8e \t \t reaction[%i] = %12.8e \n ", k, velocity[k], k , reaction[k]); */
/* for (k = 0 ; k < n; k++) printf("globalVelocity[%i] = %12.8e \t \n ", k, globalVelocity[k]); */
/* printf("\n"); */
/* **** Criterium convergence **** */
(*computeError)(problem, reaction , velocity, globalVelocity, tolerance, &error);
if (verbose > 0)
printf("----------------------------------- FC3D - NSGS - Iteration %i Error = %14.7e\n", iter, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
if (H->storageType == 1)
{
freeSBM(Htrans);
}
free(Htrans);
free(qtmp);
/* free(Global_ipiv); */
dparam[0] = tolerance;
dparam[1] = error;
/***** Free memory *****/
(*freeSolver)(problem);
}
void GETRF<double>(const int m, const int n , double* A,
const int ld, int* ipiv, int& info)
{
DGETRF(&m, &n, A, &ld, ipiv, &info);
}
int
Matrix::Invert(Matrix &theInverse) const
{
int n = numRows;
#ifdef _G3DEBUG
if (numRows != numCols) {
opserr << "Matrix::Solve(B,X) - the matrix of dimensions [" << numRows << "," << numCols << "] is not square\n";
return -1;
}
if (n != theInverse.numRows) {
opserr << "Matrix::Solve(B,X) - #rows of X, " << numRows<< ", is not same as matrix " << theInverse.numRows << 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
theInverse = *this;
for (int i=0; i<dataSize; i++)
matrixWork[i] = data[i];
int ldA = n;
int info;
double *Wptr = matrixWork;
double *Aptr = theInverse.data;
int workSize = sizeDoubleWork;
int *iPIV = intWork;
#ifdef _WIN32
#ifndef _DLL
DGETRF(&n,&n,Aptr,&ldA,iPIV,&info);
#endif
#ifdef _DLL
opserr << "Matrix::Solve - not implemented in dll\n";
return -1;
#endif
if (info != 0)
return info;
#ifndef _DLL
DGETRI(&n,Aptr,&ldA,iPIV,Wptr,&workSize,&info);
#endif
#ifdef _DLL
opserr << "Matrix::Solve - not implemented in dll\n";
return -1;
#endif
#else
dgetrf_(&n,&n,Aptr,&ldA,iPIV,&info);
if (info != 0)
return info;
dgetri_(&n,Aptr,&ldA,iPIV,Wptr,&workSize,&info);
#endif
return info;
}
