예제 #1
0
int main(void)
{
  double Hdat[] = {1,  0, -1, 0,
                0,  1, 0, -1};

  NumericsMatrix* H = NM_create_from_data(NM_DENSE, 4, 2, Hdat);

  double K[] = {-2, -3, -7, -8};

  polyhedron P = { SICONOS_SET_POLYHEDRON, 4, 0, H, K, NULL, NULL};

  int basis[11] = {0};

  siconos_find_vertex(&P, 2, basis);

  for(unsigned i = 0; i < 11; ++i) printf("%d ", basis[i]);
  printf("\n");
  return 0;
}
예제 #2
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, &copyA[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, &copyA[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, &copyA[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;
}