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; }
int CentralDifferencesDense::DoTimestep() { if (r == 0) return 0; // the reduced force interpolation PerformanceCounter counterForceAssemblyTime; reducedForceModel->GetInternalForce(q,internalForces); counterForceAssemblyTime.StopCounter(); forceAssemblyTime = counterForceAssemblyTime.GetElapsedTime(); if (plasticfq != NULL) { SetTotalForces(internalForces); for(int i=0; i<r; i++) internalForces[i] -= plasticfq[i]; } PerformanceCounter counterSystemSolveTime; for (int i=0; i<r; i++) internalForces[i] *= internalForceScalingFactor; if (tangentialDampingMode) UpdateLU(); // update equation is: // // (massMatrix + dt / 2 * dampingMatrix) * q(t+1) = (dt)^2 * (fr - Rr(q(t))) + dt/2 * dampingMatrix * q(t-1) + massMatrix * (2q(t) - q(t-1)) // LU decomposition of massMatrix + dt / 2 * Dr is available in L,U // fr = U^T * f are the reduced external forces // Rr is the vector of reduced internal forces // update equation follows from Newton's law // Mu'' = -Cu' - F_int + F_ext // Mu'' + Cu' + R(u) = F_ext // R(u) = F_int // here, F_int is the external loading force necessary to sustain a certain deformation // it is opposite to the internal forces acting on the body in a given deformation state // compute rhs = (dt)^2 * (fr - Rr(q(t))) + dt/2 * dampingMatrix * q(t-1) + massMatrix * (2q(t) - q(t-1)) // first, compute rhs = massMatrix * (2*q - q_1) for (int i=0; i<r; i++) { rhs[i] = 0; for (int j=0; j<r; j++) rhs[i] += massMatrix[ELT(r,i,j)] * (2 * q[j] - q_1[j]); } // rhs += dt / 2 * dampingMatrix * q_{n-1} for (int i=0; i<r; i++) for (int j=0; j<r; j++) rhs[i] += timestep / 2 * dampingMatrix[ELT(r,i,j)] * q_1[j]; // rhs += dt * dt * (fr - Rr(q(t))) for (int i=0; i<r; i++) rhs[i] += timestep * timestep * (externalForces[i] - internalForces[i]); // now rhs contains the correct values // solve (M~ + dt/2 D~) * qnew = rhs // use data from the previously computed LU decomposition char trans='N'; INTEGER nrhs = 1; INTEGER INFO; INTEGER R = r; DGETRS (&trans,&R,&nrhs,LUFactor,&R,IPIV->GetBuf(),rhs,&R,&INFO); if (INFO != 0) { printf("Error: DGETRS returned a non-zero exit code %d.\n", (int)INFO); return INFO; } // the solution qnew is now in rhs // update velocity // and update previous and current positions for (int i=0; i<r; i++) { qvel[i] = (rhs[i] - q[i]) / timestep; q_1[i] = q[i]; q[i] = rhs[i]; } ProcessPlasticDeformations(); counterSystemSolveTime.StopCounter(); systemSolveTime = counterSystemSolveTime.GetElapsedTime(); return 0; }
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); }