numerics/src/LCP/lcp_psor.c
/*\warning omega is not explicitely used. must be completed    */
\warning omega is not explicitely used. must be completed
\ warning omega is not explicitely used . must be completed
void lcp_psor(LinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options)
{
  double * M = problem->M->matrix0;
  double * q = problem->q;
  int n = problem->size;
  int incx = 1, incy = 1;
  int incxn;
  int i, iter;
  double qs, err;
  double *ww, *diag;
  int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
  double tol = options->dparam[SICONOS_DPARAM_TOL];
  double omega = options->dparam[SICONOS_LCP_DPARAM_RHO]; 
  printf("Warning : omega %f is not used !!!!!\n", omega);
  incxn = n;
  options->iparam[SICONOS_IPARAM_ITER_DONE] = 0;
  options->dparam[SICONOS_DPARAM_RESIDU] = 0.0;
  ww   = (double*)malloc(n * sizeof(double));
  diag = (double*)malloc(n * sizeof(double));
  qs = cblas_dnrm2(n, q, incx);
  if(verbose > 0) printf("\n ||q||= %g \n", qs);
  if(qs <= DBL_EPSILON)
  {
    for(i = 0 ; i < n ; ++i)
    {
      w[i] = 0.;
      z[i] = 0.;
    }
    free(ww);
    free(diag);
    *info = 0;
    return;
  }
  for(i = 0 ; i < n ; ++i)
  {
    ww[i] = 0.;
  }
  cblas_dcopy(n, q, incx, w, incy);
  for(i = 0 ; i < n ; ++i)
  {
    if(fabs(M[i * n + i]) < DBL_EPSILON)
    {
      if(verbose > 0)
      {
        printf(" Warning negative diagonal term \n");
        printf(" The local problem cannot be solved \n");
      }
      *info = 2;
      free(diag);
      free(ww);
      return;
    }
    else diag[i] = 1.0 / M[i * n + i];
  }
  iter = 0;
  err  = 1.;
  qs   = -1.0;
  cblas_dcopy(n, q, incx, w, incy);
  while((iter < itermax) && (err > tol))
  {
    ++iter;
    cblas_dcopy(n, w, incx, ww, incy);       
    cblas_dcopy(n, q, incx, w, incy);        
    for(i = 0 ; i < n ; ++i)
    {
      z[i] = 0.0;
      z[i] = fmax(0.0, -(q[i] + cblas_ddot(n, &M[i], incxn, z, incy)) * diag[i]);
    }
    lcp_compute_error(problem, z, w, tol, &err);
  }
  options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
  options->dparam[SICONOS_DPARAM_RESIDU] = err;
  if(err > tol)
  {
    printf("Siconos/Numerics: lcp_psor: No convergence of PSOR after %d iterations\n", iter);
    printf("Siconos/Numerics: lcp_psor: The residue is : %g \n", err);
    *info = 1;
  }
  else
  {
    if(verbose > 0)
    {
      printf("Siconos/Numerics: lcp_psor: Convergence of PSOR after %d iterations\n", iter);
      printf("Siconos/Numerics: lcp_psor: The residue is : %g \n", err);
    }
    *info = 0;
  }
  free(ww);
  free(diag);
  return;
}
void lcp_psor ( LinearComplementarityProblem * problem , double * z , double * w , int * info , SolverOptions * options ) { double * M = problem -> M -> matrix0 ; double * q = problem -> q ; int n = problem -> size ; int incx = 1 , incy = 1 ; int incxn ; int i , iter ; double qs , err ; double * ww , * diag ; int itermax = options -> iparam [ SICONOS_IPARAM_MAX_ITER ] ; double tol = options -> dparam [ SICONOS_DPARAM_TOL ] ; double omega = options -> dparam [ SICONOS_LCP_DPARAM_RHO ] ; printf ( " \n " , omega ) ; incxn = n ; options -> iparam [ SICONOS_IPARAM_ITER_DONE ] = 0 ; options -> dparam [ SICONOS_DPARAM_RESIDU ] = 0.0 ; ww = ( double * ) malloc ( n * sizeof ( double ) ) ; diag = ( double * ) malloc ( n * sizeof ( double ) ) ; qs = cblas_dnrm2 ( n , q , incx ) ; if ( verbose > 0 ) printf ( " \n \n " , qs ) ; if ( qs <= DBL_EPSILON ) { for ( i = 0 ; i < n ; ++ i ) { w [ i ] = 0. ; z [ i ] = 0. ; } free ( ww ) ; free ( diag ) ; * info = 0 ; return ; } for ( i = 0 ; i < n ; ++ i ) { ww [ i ] = 0. ; } cblas_dcopy ( n , q , incx , w , incy ) ; for ( i = 0 ; i < n ; ++ i ) { if ( fabs ( M [ i * n + i ] ) < DBL_EPSILON ) { if ( verbose > 0 ) { printf ( " \n " ) ; printf ( " \n " ) ; } * info = 2 ; free ( diag ) ; free ( ww ) ; return ; } else diag [ i ] = 1.0 / M [ i * n + i ] ; } iter = 0 ; err = 1. ; qs = -1.0 ; cblas_dcopy ( n , q , incx , w , incy ) ; while ( ( iter < itermax ) && ( err > tol ) ) { ++ iter ; cblas_dcopy ( n , w , incx , ww , incy ) ; cblas_dcopy ( n , q , incx , w , incy ) ; for ( i = 0 ; i < n ; ++ i ) { z [ i ] = 0.0 ; z [ i ] = fmax ( 0.0 , - ( q [ i ] + cblas_ddot ( n , & M [ i ] , incxn , z , incy ) ) * diag [ i ] ) ; } lcp_compute_error ( problem , z , w , tol , & err ) ; } options -> iparam [ SICONOS_IPARAM_ITER_DONE ] = iter ; options -> dparam [ SICONOS_DPARAM_RESIDU ] = err ; if ( err > tol ) { printf ( " \n " , iter ) ; printf ( " \n " , err ) ; * info = 1 ; } else { if ( verbose > 0 ) { printf ( " \n " , iter ) ; printf ( " \n " , err ) ; } * info = 0 ; } free ( ww ) ; free ( diag ) ; return ; }
\warning omega is not explicitely used.
\ warning omega is not explicitely used .
/* matrix M/vector q of the lcp */ /* size of the LCP */ // Not yet used /* Initialize output */ /* Allocation */ /* Check for non trivial case */ // Note FP : den never used ... //if (qs > DBL_EPSILON) den = 1.0 / qs; //else /* Intialization of w and z */ /*if(initmethod == 0) {*/ /* dcopy_( (integer *)&n , q , (integer *)&incx , w , (integer *)&incy );*/ /*dcopy_( (integer *)&n , ww , (integer *)&incx , z , (integer *)&incy );*/ /*}    else if(initmethod == 0) { */ /* Preparation of the diagonal of the inverse matrix */ /*start iterations*/ /* w --> ww */ /* q --> w */ /*      zi = -( q[i] + ddot_( (integer *)&n , &M[i] , (integer *)&incxn , z , (integer *)&incy ))*diag[i]; */ /*       if( zi < 0 ) z[i] = 0.0;  */ /*       else z[i] = zi; */ /* **** Criterium convergence **** */ /* **** ********************* **** */