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 }
void _dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip) { dAASSERT (L && d && b && n > 0 && nskip >= n); dSolveL1 (L,b,n,nskip); dVectorScale (b,d,n); dSolveL1T (L,b,n,nskip); }
void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) { // the `Dell' and `ell' that are computed here are saved. if index i is // later added to the factorization then they can be reused. // // @@@ question: do we need to solve for entire delta_x??? yes, but // only if an x goes below 0 during the step. int j; if (nC > 0) { dReal *aptr = AROW(i); # ifdef NUB_OPTIMIZATIONS // if nub>0, initial part of aptr[] is guaranteed 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]; if (!only_transfer) { for (j=0; j<nC; j++) tmp[j] = ell[j]; dSolveL1T (L,tmp,nC,nskip); if (dir > 0) { for (j=0; j<nC; j++) a[C[j]] = -tmp[j]; } else { for (j=0; j<nC; j++) a[C[j]] = tmp[j]; } } } }
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 }
void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) { // the `Dell' and `ell' that are computed here are saved. if index i is // later added to the factorization then they can be reused. // // @@@ question: do we need to solve for entire delta_x??? yes, but // only if an x goes below 0 during the step. if (m_nC > 0) { { dReal *Dell = m_Dell; int *C = m_C; dReal *aptr = AROW(i); # ifdef NUB_OPTIMIZATIONS // if nub>0, initial part of aptr[] is guaranteed 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); { dReal *ell = m_ell, *Dell = m_Dell, *d = m_d; const int nC = m_nC; for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j]; } if (!only_transfer) { dReal *tmp = m_tmp, *ell = m_ell; { const int nC = m_nC; for (int j=0; j<nC; ++j) tmp[j] = ell[j]; } dSolveL1T (m_L,tmp,m_nC,m_nskip); if (dir > 0) { int *C = m_C; dReal *tmp = m_tmp; const int nC = m_nC; for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j]; } else { int *C = m_C; dReal *tmp = m_tmp; const int nC = m_nC; for (int j=0; j<nC; ++j) a[C[j]] = tmp[j]; } } } }