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