void GetVhat(double *u,
double *q,
sdpdat *sdt)
{
int i,n;
double ese,lamda,dl,rtmp,*r,*y,*dy,*uv;
chfac *cf,*mf;
symat *cy;
syoff *st;
n =sdt->nrow;
lamda=sdt->lamda;
dl =sdt->dl;
cf =sdt->sf;
mf =sdt->mf;
y =sdt->y;
dy =sdt->dy;
r =sdt->rw+n;
uv =sdt->rw+5*n;
cy =sdt->cy;
st =sdt->st;
/*
* r=L*u;
*/
GetUhat(mf,u,r);
/*
* r=S^-1*u
*/
ChlSolve(cf,u,r);
dCopy(n,r,u);
if (sdt->ptyp==EquCut||sdt->ptyp==UquCut)
{
rtmp=0.0;
ese =0.0;
for (i=0; i<n; i++)
{
rtmp+=r[i];
ese +=q[i];
}
ese=1.0-lamda*ese;
rtmp=lamda*rtmp/ese;
for (i=0; i<n; i++)
u[i]=r[i]+rtmp*q[i];
}
} /* GetVhat */
bool CubicSplineInterpolation::caltridiagonalMatrices(
cv::Mat_<double> &input_a,
cv::Mat_<double> &input_b,
cv::Mat_<double> &input_c,
cv::Mat_<double> &input_d,
cv::Mat_<double> &output_x )
{
int rows = input_a.rows;
int cols = input_a.cols;
if ( ( rows == 1 && cols > rows ) ||
(cols == 1 && rows > cols ) )
{
const int count = ( rows > cols ? rows : cols ) - 1;
output_x = cv::Mat_<double>::zeros(rows, cols);
cv::Mat_<double> cCopy, dCopy;
input_c.copyTo(cCopy);
input_d.copyTo(dCopy);
if ( input_b(0) != 0 )
{
cCopy(0) /= input_b(0);
dCopy(0) /= input_b(0);
}
else
{
return false;
}
for ( int i=1; i < count; i++ )
{
double temp = input_b(i) - input_a(i) * cCopy(i-1);
if ( temp == 0.0 )
{
return false;
}
cCopy(i) /= temp;
dCopy(i) = ( dCopy(i) - dCopy(i-1)*input_a(i) ) / temp;
}
output_x(count) = dCopy(count);
for ( int i=count-2; i > 0; i-- )
{
output_x(i) = dCopy(i) - cCopy(i)*output_x(i+1);
}
return true;
}
else
{
return false;
}
}
void GetLhat(sdpdat *sdt)
{
int i,j,k,n,itmp=0;
double lamda,dl,rtmp,*q,*y,*dy,*uv;
chfac *cf,*mf;
array *cj,*sj;
symat *cy;
syoff *st;
n=sdt->nrow;
if ( sdt->ptyp!=UquCut)
{
sdt->mf=NULL;
CfcFree(&sdat->mf);
SyoFree(&sdat->st);
/* Reassign L factor for rank reduction */
sdt->mf=sdt->mf=CfcAlloc(n,"mf, GetLhat");
copyChl(sdt->sf,sdt->mf);
}
lamda=sdt->lamda;
dl =sdt->dl;
cf =sdt->sf;
mf =sdt->mf;
y =sdt->y;
dy =sdt->dy;
q =sdt->rw;
uv =sdt->rw+5*n;
cy =sdt->cy;
st =sdt->st;
/*
* find L such that LL^T=S+Diag(dy)+dl*e*e^T
*/
if (sdt->ptyp==UquCut)
{
rtmp=dl-lamda;
for (j=0; j<n; j++)
mf->diag[j]=cy->diag[j]+(dy[j]-y[j])+rtmp;
for (j=0; j<n; j++)
{
for (i=j+1; i<n; i++)
q[i]=rtmp;
cj=cy->roff+j;
for (k=0; k<cj->nn0; k++)
q[cj->ja[k]]+=cj->an[k];
sj=st->roff+j;
for (k=0; k<sj->nn0; k++)
sj->an[k]=q[sj->ja[k]];
}
}
else
{
for (j=0; j<n; j++)
{
mf->diag[mf->invp[j]]=cy->diag[j]+(dy[j]-y[j]);
}
dCopy(sdt->sf->unnz,uv,mf->uval);
if (sdt->ptyp==DvdCut)
{
rtmp=dl-lamda;
mf->diag[mf->invp[sdt->is]]+=rtmp;
mf->diag[mf->invp[sdt->it]]+=rtmp;
mf->uval[mf->upst] +=rtmp;
}
}
/* If return is not 0 */
if( sdt->ptyp != EquCut )
{
if (CfcOk!=ChlFact(mf,sdt->iw,q,true))
{
ExitProc(SysError,NULL);
}
}
else ChlFact(mf,sdt->iw,q,false);
mf->NegDiag =-1;
if( sdt->ptyp == EquCut )
{
for(j=0;j<n;j++)
{
if( mf->diag[mf->invp[j]]<0 ) /* Why invp? */
{
if( mf->NegDiag <0 )
{
mf->NegDiag = mf->invp[j];
itmp++;
}
else mf->NegDiag =-2;
}
}
}
if(mf->NegDiag<-1)
{
printf("\n Error in ecut rank reduction.");
printf("\n More than one Negative number in diagonal.");
printf("\n There are %d negitives.",itmp);
ShutDown();
}
} /* GetLhat */
void ChlSolveBackwardPrivate(chfac *sf,
double x[],
double b[])
{
/* Note: x: input, or left hand side b: output, or solution */
int i,s,t,sze,f,l,*ls,
*subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub,
*ujbeg=sf->ujbeg,*uhead=sf->uhead;
double x1,x2,*l1,*l2,rtemp1,
*diag=sf->diag,*uval=sf->uval;
if (sf->nsnds) {
s = sf->nsnds - 1;
f = subg[s];
l = subg[s+1];
dCopy(l-f,x+f,b+f);
SolBward(l-f,diag+f,uval,uhead+f,b+f);
s = sf->nsnds-1;
for(; s>=1; --s) {
f = subg[s-1];
l = subg[s];
i = l;
for(; i>1+f; --i) {
-- i;
ls = usub+ujbeg[i];
l1 = uval+uhead[i-1]+1;
l2 = uval+uhead[i ]+0;
sze = ujsze[i];
x1 = 0.0;
x2 = 0.0;
for(t=0; t<sze; ++t) {
rtemp1 = b[ls[t]];
x1 += l1[t]*rtemp1;
x2 += l2[t]*rtemp1;
}
b[i] = x[i ] - x2 / diag[i];
b[i-1] = x[i-1] - (x1 + uval[uhead[i-1]]*b[i]) / diag[i-1];
}
for(; i>f;) {
-- i;
l1 = uval+uhead[i];
ls = usub+ujbeg[i];
sze = ujsze[i];
x1 = 0.0;
for(t=0; t<sze; ++t)
x1+= l1[t]*b[ls[t]];
b[i] = x[i] - x1/diag[i];
}
}
}
}
