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
}
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 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::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
}
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
}
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
}