コード例 #1
0
int rATL_dpotrfL(int N, double *A,int lda)
{
  double *An, *Ar;
  int Nleft, Nright, ierr;

  if (N > 4) {
    Nleft = N >> 1;
#if 0
    int nb = ilaenv_f77(&IONE, "DPOTRF", "L", &N,
		     &IMONE,&IONE, &IMONE, strlen("DPOTRF"), strlen("L"));
    if (Nleft > nb<<1) Nleft = (Nleft/nb)*nb;
#endif
#if 0
    if (Nleft > 64) {
	Nleft = 64;
    }
#endif
    Nright = N - Nleft;
    ierr = rATL_dpotrfL(Nleft, A,lda);
    if (!ierr) {
      Ar = A + Nleft;
      An = Ar + lda * Nleft;
      dtrsm_f77 ((char *)"R",(char *)"L",(char *)"T",(char *)"N",
		 &Nright,&Nleft,&DONE,A,&lda,
		 Ar, &lda, strlen("R"),strlen("L"),
		 strlen("T"),strlen("N"));
      dsyrk_f77 ((char *)"L",(char *)"N",&Nright,&Nleft,&DMONE,
		 Ar, &lda, &DONE,An,&lda,strlen("L"),strlen("N"));
      ierr = rATL_dpotrfL(Nright, An,lda);
      if (ierr) return(ierr+Nleft);
    }
    else return(ierr);
  }
コード例 #2
0
ファイル: cpodes_lapack.c プロジェクト: AyMaN-GhOsT/simbody
/*
 * cpLapackDenseProjSetup does the setup operations for the dense 
 * linear solver.
 * It calls the Jacobian evaluation routine and, depending on ftype,
 * it performs various factorizations.
 */
static int cpLapackDenseProjSetup(CPodeMem cp_mem, N_Vector y, N_Vector cy,
                                  N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1)
{
  int ier;
  CPDlsProjMem cpdlsP_mem;
  realtype *col_i, rim1, ri;
  int i, j, nd, one = 1;
  int retval;

  realtype coef_1 = ONE, coef_0 = ZERO;

  cpdlsP_mem = (CPDlsProjMem) lmemP;

  nd = ny-nc;

  /* Call Jacobian function (G will contain the Jacobian transposed) */
  retval = djacP(nc, ny, tn, y, cy, G, JP_data, c_tmp1, c_tmp2);
  njeP++;
  if (retval < 0) {
    cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackDenseProjSetup", MSGD_JACFUNC_FAILED);
    return(-1);
  } else if (retval > 0) {
    return(1);
  }

  /* Save Jacobian before factorization for possible use by lmultP */
  dcopy_f77(&(G->ldata), G->data, &one, savedG->data, &one);

  /* Factorize G, depending on ftype */
  switch (ftype) {

  case CPDIRECT_LU:

    /* 
     * LU factorization of G^T with partial pivoting
     *    P*G^T = | U1^T | * L^T
     *            | U2^T |
     * After factorization, P is encoded in pivotsP and
     * G^T is overwritten with U1 (nc by nc unit upper triangular), 
     * U2 ( nc by ny-nc rectangular), and L (nc by nc lower triangular).
     * Return ier if factorization failed. 
     */
    dgetrf_f77(&ny, &nc, G->data, &ny, pivotsP, &ier);
    if (ier != 0) return(ier);

    /* 
     * Build S = U1^{-1} * U2 (in place, S overwrites U2) 
     * For each row j of G, j = nc,...,ny-1, perform
     * a backward substitution (row version).
     * After this step, G^T contains U1, S, and L.
     */
    for (j=nc; j<ny; j++)
      dtrsv_f77("L", "T", "U", &nc, G->data, &ny, (G->data + j), &ny, 1, 1, 1);

    /*   
     * Build K = D1 + S^T * D2 * S 
     * S^T is stored in g_mat[nc...ny-1][0...nc]
     * Compute and store only the lower triangular part of K.
     */
    if (pnorm == CP_PROJ_L2NORM) {
      dsyrk_f77("L", "N", &nd, &nc, &coef_1, (G->data + nc), &ny, &coef_0, K->data, &nd, 1, 1);
      LapackDenseAddI(K);
    } else {
      cplLUcomputeKD(cp_mem, s_tmp1);
    }

    /*
     * Perform Cholesky decomposition of K: K = C*C^T
     * After factorization, the lower triangular part of K contains C.
     * Return ier if factorization failed. 
     */
    dpotrf_f77("L", &nd, K->data, &nd, &ier, 1);
    if (ier != 0) return(ier);

    break;

  case CPDIRECT_QR:

    /* 
     * QR factorization of G^T: G^T = Q*R
     * After factorization, the upper triangular part of G^T 
     * contains the matrix R. The lower trapezoidal part of
     * G^T, together with the array beta, encodes the orthonormal
     * columns of Q as elementary reflectors.
     */
    dgeqrf_f77(&ny, &nc, G->data, &ny, beta, wrk, &len_wrk, &ier);
    if (ier != 0) return(ier);

    /* If projecting in WRMS norm */
    if (pnorm == CP_PROJ_ERRNORM) {
      /* Build K = Q^T * D^(-1) * Q */
      cplQRcomputeKD(cp_mem, s_tmp1);
      /* Perform Cholesky decomposition of K */
      dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
      if (ier != 0) return(ier);
    }

    break;

  case CPDIRECT_QRP:

    /* 
     * QR with pivoting factorization of G^T: G^T * P = Q * R.
     * After factorization, the upper triangular part of G^T 
     * contains the matrix R. The lower trapezoidal part of
     * G^T, together with the array beta, encodes the orthonormal
     * columns of Q as elementary reflectors.
     * The pivots are stored in 'pivotsP'.
     */
    for (i=0; i<nc; i++) pivotsP[i] = 0;
    dgeqp3_f77(&ny, &nc, G->data, &ny, pivotsP, beta, wrk, &len_wrk, &ier);
    if (ier != 0) return(ier);

    /*
     * Determine the number of independent constraints.
     * After the QR factorization, the diagonal elements of R should 
     * be in decreasing order of their absolute values.
     */
    rim1 = ABS(G->data[0]);
    for (i=1, nr=1; i<nc; i++, nr++) {
      col_i = G->cols[i];
      ri = ABS(col_i[i]);
      if (ri < 100*uround) break;
      if (ri/rim1 < RPowerR(uround, THREE/FOUR)) break;
    }

    /* If projecting in WRMS norm */
    if (pnorm == CP_PROJ_ERRNORM) {
      /* Build K = Q^T * D^(-1) * Q */
      cplQRcomputeKD(cp_mem, s_tmp1);
      /* Perform Cholesky decomposition of K */
      dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
      if (ier != 0) return(ier);
    }

    break;

  case CPDIRECT_SC:

    /* 
     * Build K = G*D^(-1)*G^T
     * G^T is stored in g_mat[0...ny-1][0...nc]
     * Compute and store only the lower triangular part of K.
     */
    if (pnorm == CP_PROJ_L2NORM) {
      dsyrk_f77("L", "T", &nc, &ny, &coef_1, G->data, &ny, &coef_0, K->data, &nc, 1, 1);
    } else {
      cplSCcomputeKD(cp_mem, s_tmp1);
    }

    /* 
     * Perform Cholesky decomposition of K: K = C*C^T
     * After factorization, the lower triangular part of K contains C.
     * Return 1 if factorization failed. 
     */
    dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
    if (ier != 0) return(ier);

    break;

  }

  return(0);
}