Esempio n. 1
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
}
Esempio n. 2
0
File: lcp.cpp Progetto: 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
}
Esempio 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
}
Esempio n. 4
0
File: lcp.cpp Progetto: 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
}
Esempio 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
}
Esempio 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
}
Esempio n. 7
0
File: lcp.cpp Progetto: dartsim/dart
dLCP::dLCP (int _n, int _nskip, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
            dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
            dReal *_Dell, dReal *_ell, dReal *_tmp,
            bool *_state, int *_findex, int *_p, int *_C, dReal **Arows):
  m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
# ifdef ROWPTRS
  m_A(Arows),
#else
  m_A(_Adata),
#endif
  m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
  m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
  m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
{
  {
    dSetZero (m_x,m_n);
  }

  {
# ifdef ROWPTRS
    // make matrix row pointers
    dReal *aptr = _Adata;
    ATYPE A = m_A;
    const int n = m_n, nskip = m_nskip;
    for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
# endif
  }

  {
    int *p = m_p;
    const int n = m_n;
    for (int k=0; k<n; ++k) p[k]=k;		// initially unpermuted
  }

  /*
  // for testing, we can do some random swaps in the area i > nub
  {
    const int n = m_n;
    const int nub = m_nub;
    if (nub < n) {
    for (int k=0; k<100; k++) {
      int i1,i2;
      do {
        i1 = dRandInt(n-nub)+nub;
        i2 = dRandInt(n-nub)+nub;
      }
      while (i1 > i2); 
      //printf ("--> %d %d\n",i1,i2);
      swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0);
    }
  }
  */

  // permute the problem so that *all* the unbounded variables are at the
  // start, i.e. look for unbounded variables not included in `nub'. we can
  // potentially push up `nub' this way and get a bigger initial factorization.
  // note that when we swap rows/cols here we must not just swap row pointers,
  // as the initial factorization relies on the data being all in one chunk.
  // variables that have findex >= 0 are *not* considered to be unbounded even
  // if lo=-inf and hi=inf - this is because these limits may change during the
  // solution process.

  {
    int *findex = m_findex;
    dReal *lo = m_lo, *hi = m_hi;
    const int n = m_n;
    for (int k = m_nub; k<n; ++k) {
      if (findex && findex[k] >= 0) continue;
      if (lo[k]==-dInfinity && hi[k]==dInfinity) {
        swapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0);
        m_nub++;
      }
    }
  }

  // if there are unbounded variables at the start, factorize A up to that
  // point and solve for x. this puts all indexes 0..nub-1 into C.
  if (m_nub > 0) {
    const int nub = m_nub;
    {
      dReal *Lrow = m_L;
      const int nskip = m_nskip;
      for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,AROW(j),(j+1)*sizeof(dReal));
    }
    dFactorLDLT (m_L,m_d,nub,m_nskip);
    memcpy (m_x,m_b,nub*sizeof(dReal));
    dSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
    dSetZero (m_w,nub);
    {
      int *C = m_C;
      for (int k=0; k<nub; ++k) C[k] = k;
    }
    m_nC = nub;
  }

  // permute the indexes > nub such that all findex variables are at the end
  if (m_findex) {
    const int nub = m_nub;
    int *findex = m_findex;
    int num_at_end = 0;
    for (int k=m_n-1; k >= nub; k--) {
      if (findex[k] >= 0) {
        swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1);
        num_at_end++;
      }
    }
  }

  // print info about indexes
  /*
  {
    const int n = m_n;
    const int nub = m_nub;
    for (int k=0; k<n; k++) {
      if (k<nub) printf ("C");
      else if (m_lo[k]==-dInfinity && m_hi[k]==dInfinity) printf ("c");
      else printf (".");
    }
    printf ("\n");
  }
  */
}
Esempio n. 8
0
dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
	    dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
	    dReal *_Dell, dReal *_ell, dReal *_tmp,
	    int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
{
  n = _n;
  nub = _nub;
  Adata = _Adata;
  A = 0;
  x = _x;
  b = _b;
  w = _w;
  lo = _lo;
  hi = _hi;
  L = _L;
  d = _d;
  Dell = _Dell;
  ell = _ell;
  tmp = _tmp;
  state = _state;
  findex = _findex;
  p = _p;
  C = _C;
  nskip = dPAD(n);
  dSetZero (x,n);

  int k;

# ifdef ROWPTRS
  // make matrix row pointers
  A = Arows;
  for (k=0; k<n; k++) A[k] = Adata + k*nskip;
# else
  A = Adata;
# endif

  nC = 0;
  nN = 0;
  for (k=0; k<n; k++) p[k]=k;		// initially unpermuted

  /*
  // for testing, we can do some random swaps in the area i > nub
  if (nub < n) {
    for (k=0; k<100; k++) {
      int i1,i2;
      do {
	i1 = dRandInt(n-nub)+nub;
	i2 = dRandInt(n-nub)+nub;
      }
      while (i1 > i2); 
      //printf ("--> %d %d\n",i1,i2);
      swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0);
    }
  }
  */

  // permute the problem so that *all* the unbounded variables are at the
  // start, i.e. look for unbounded variables not included in `nub'. we can
  // potentially push up `nub' this way and get a bigger initial factorization.
  // note that when we swap rows/cols here we must not just swap row pointers,
  // as the initial factorization relies on the data being all in one chunk.
  // variables that have findex >= 0 are *not* considered to be unbounded even
  // if lo=-inf and hi=inf - this is because these limits may change during the
  // solution process.

  for (k=nub; k<n; k++) {
    if (findex && findex[k] >= 0) continue;
    if (lo[k]==-dInfinity && hi[k]==dInfinity) {
      swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);
      nub++;
    }
  }

  // if there are unbounded variables at the start, factorize A up to that
  // point and solve for x. this puts all indexes 0..nub-1 into C.
  if (nub > 0) {
    for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));
    dFactorLDLT (L,d,nub,nskip);
    memcpy (x,b,nub*sizeof(dReal));
    dSolveLDLT (L,d,x,nub,nskip);
    dSetZero (w,nub);
    for (k=0; k<nub; k++) C[k] = k;
    nC = nub;
  }

  // permute the indexes > nub such that all findex variables are at the end
  if (findex) {
    int num_at_end = 0;
    for (k=n-1; k >= nub; k--) {
      if (findex[k] >= 0) {
	swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);
	num_at_end++;
      }
    }
  }

  // print info about indexes
  /*
  for (k=0; k<n; k++) {
    if (k<nub) printf ("C");
    else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");
    else printf (".");
  }
  printf ("\n");
  */
}