Ejemplo n.º 1
0
Archivo: lcp.cpp Proyecto: dartsim/dart
void dLCP::transfer_i_to_C (int i)
{
  {
    if (m_nC > 0) {
      // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
      {
        const int nC = m_nC;
        dReal *const Ltgt = m_L + nC*m_nskip, *ell = m_ell;
        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
      }
      const int nC = m_nC;
      m_d[nC] = dRecip (AROW(i)[i] - dDot(m_ell,m_Dell,nC));
    }
    else {
      m_d[0] = dRecip (AROW(i)[i]);
    }

    swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);

    const int nC = m_nC;
    m_C[nC] = nC;
    m_nC = nC + 1; // nC value is outdated after this line
  }

# ifdef DEBUG_LCP
  checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
  if (i < (m_n-1)) checkPermutations (i+1,m_n,m_nC,m_nN,m_p,m_C);
# endif
}
Ejemplo n.º 2
0
void dLCP::transfer_i_from_N_to_C (int i)
{
  int j;
  if (nC > 0) {
    dReal *aptr = AROW(i);
#   ifdef NUB_OPTIMIZATIONS
    // if nub>0, initial part of aptr unpermuted
    for (j=0; j<nub; j++) Dell[j] = aptr[j];
    for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
#   else
    for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
#   endif
    dSolveL1 (L,Dell,nC,nskip);
    for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
    for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
    d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
  }
  else {
    d[0] = dRecip (AROW(i)[i]);
  }
  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
  C[nC] = nC;
  nN--;
  nC++;

  // @@@ TO DO LATER
  // if we just finish here then we'll go back and re-solve for
  // delta_x. but actually we can be more efficient and incrementally
  // update delta_x here. but if we do this, we wont have ell and Dell
  // to use in updating the factorization later.

# ifdef DEBUG_LCP
  checkFactorization (A,L,d,nC,C,nskip);
# endif
}
Ejemplo n.º 3
0
void dLCP::transfer_i_from_N_to_C (int i)
{
  {
    if (m_nC > 0) {
      {
        dReal *const aptr = AROW(i);
        dReal *Dell = m_Dell;
        const int *C = m_C;
#   ifdef NUB_OPTIMIZATIONS
        // if nub>0, initial part of aptr unpermuted
        const int nub = m_nub;
        int j=0;
        for ( ; j<nub; ++j) Dell[j] = aptr[j];
        const int nC = m_nC;
        for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
#   else
        const int nC = m_nC;
        for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
#   endif
      }
      dSolveL1 (m_L,m_Dell,m_nC,m_nskip);
      {
        const int nC = m_nC;
        dReal *const Ltgt = m_L + nC*m_nskip;
        dReal *ell = m_ell, *Dell = m_Dell, *d = m_d;
        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
      }
      const int nC = m_nC;
      dReal Aii_dDot = AROW(i)[i] - dDot(m_ell, m_Dell, nC);
      if(dFabs(Aii_dDot) < 1e-16) {
          Aii_dDot += 1e-6;
      }
      m_d[nC] = dRecip (Aii_dDot);
    }
    else {
        if(dFabs(AROW(i)[i]) < 1e-16) {
            AROW(i)[i] += 1e-6;
        }
        m_d[0] = dRecip (AROW(i)[i]);
    }

    swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);

    const int nC = m_nC;
    m_C[nC] = nC;
    m_nN--;
    m_nC = nC + 1; // nC value is outdated after this line
  }

  // @@@ TO DO LATER
  // if we just finish here then we'll go back and re-solve for
  // delta_x. but actually we can be more efficient and incrementally
  // update delta_x here. but if we do this, we wont have ell and Dell
  // to use in updating the factorization later.

# ifdef DEBUG_LCP
  checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
# endif
}
Ejemplo n.º 4
0
Archivo: lcp.cpp Proyecto: dartsim/dart
void dLCP::transfer_i_from_C_to_N (int i, void *tmpbuf)
{
  {
    int *C = m_C;
    // remove a row/column from the factorization, and adjust the
    // indexes (black magic!)
    int last_idx = -1;
    const int nC = m_nC;
    int j = 0;
    for ( ; j<nC; ++j) {
      if (C[j]==nC-1) {
        last_idx = j;
      }
      if (C[j]==i) {
        dLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,tmpbuf);
        int k;
        if (last_idx == -1) {
          for (k=j+1 ; k<nC; ++k) {
            if (C[k]==nC-1) {
              break;
            }
          }
          dIASSERT (k < nC);
        }
        else {
          k = last_idx;
        }
        C[k] = C[j];
        if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
        break;
      }
    }
    dIASSERT (j < nC);

    swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,i,nC-1,m_nskip,1);

    m_nN++;
    m_nC = nC - 1; // nC value is outdated after this line
  }

# ifdef DEBUG_LCP
  checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
# endif
}
Ejemplo n.º 5
0
void dLCP::transfer_i_to_C (int i)
{
  int j;
  if (nC > 0) {
    // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
    for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
    d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
  }
  else {
    d[0] = dRecip (AROW(i)[i]);
  }
  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
  C[nC] = nC;
  nC++;

# ifdef DEBUG_LCP
  checkFactorization (A,L,d,nC,C,nskip);
  if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
# endif
}
Ejemplo n.º 6
0
void dLCP::transfer_i_from_C_to_N (int i)
{
  // remove a row/column from the factorization, and adjust the
  // indexes (black magic!)
  int j,k;
  for (j=0; j<nC; j++) if (C[j]==i) {
    dLDLTRemove (A,C,L,d,n,nC,j,nskip);
    for (k=0; k<nC; k++) if (C[k]==nC-1) {
      C[k] = C[j];
      if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
      break;
    }
    dIASSERT (k < nC);
    break;
  }
  dIASSERT (j < nC);
  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
  nC--;
  nN++;

# ifdef DEBUG_LCP
  checkFactorization (A,L,d,nC,C,nskip);
# endif
}