Example #1
0
File: lcp.cpp Project: 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
}
Example #2
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
}