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 _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.
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];
}
}
}
}
