int main ()
{
float *v1 = (float *) malloc (N * sizeof (float));
float *v2 = (float *) malloc (N * sizeof (float));
float p1, p2;
init (v1, v2, N);
p1 = dotprod_ref (v1, v2, N);
p2 = dotprod (v1, v2, N);
check (p1, p2);
free (v1);
free (v2);
return 0;
}
static void
irt_rpf_mdim_grm_rawprob(const double *spec,
const double *param, const double *th,
double *out)
{
int numDims = spec[RPF_ISpecDims];
const int numOutcomes = spec[RPF_ISpecOutcomes];
const double dprod = dotprod(param, th, numDims);
const double *kat = param + (int) spec[RPF_ISpecDims];
out[0] = 1;
for (int kx=0; kx < numOutcomes-1; kx++) {
double athb = -(dprod + kat[kx]);
if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
double tmp = 1 / (1 + exp(athb));
out[kx+1] = tmp;
}
out[numOutcomes] = 0;
}
static void
irt_rpf_mdim_grm_prob(const double *spec,
const double *param, const double *th,
double *out)
{
const int numDims = spec[RPF_ISpecDims];
const int numOutcomes = spec[RPF_ISpecOutcomes];
const double *slope = param;
const double dprod = dotprod(slope, th, numDims);
const double *kat = param + (int) spec[RPF_ISpecDims];
double athb = -(dprod + kat[0]);
if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
double tmp = 1 / (1 + exp(athb));
out[0] = 1-tmp;
out[1] = tmp;
for (int kx=2; kx < numOutcomes; kx++) {
if (1e-6 + kat[kx-1] >= kat[kx-2]) {
for (int ky=0; ky < numOutcomes; ky++) {
out[ky] = nan("I");
}
return;
}
double athb = -(dprod + kat[kx-1]);
if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
double tmp = 1 / (1 + exp(athb));
out[kx-1] -= tmp;
out[kx] = tmp;
}
for (int kx=0; kx < numOutcomes; kx++) {
if (out[kx] <= 0) {
_grm_fix_crazy_stuff(spec, numOutcomes, out);
return;
}
}
}
static void
irt_rpf_mdim_drm_prob2(const double *spec,
const double *param, const double *th,
double *out1, double *out2)
{
int numDims = spec[RPF_ISpecDims];
double dprod = dotprod(param, th, numDims);
double diff = param[numDims];
double athb = -(dprod + diff);
if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
double tmp = 1 / (1 + exp(athb));
out1[0] = 1-tmp;
out1[1] = tmp;
if (numDims) {
const double gg = antilogit(param[numDims+1]);
const double uu = antilogit(param[numDims+2]);
tmp = gg + (uu-gg) * tmp;
}
out2[0] = 1-tmp;
out2[1] = tmp;
}
int main(void){
double *a;
double *b;
a = (double *) malloc(N * sizeof(double));
b = (double *) malloc(N * sizeof(double));
double *p = a;
double *q = b;
while(p<(a+N)){
*(p) = 1.0;
*(q) = 1.0;
p++;
q++;
}
double c = dotprod(a, b, N);
printf("%.2f\n", c);
free(a);
free(b);
return 0;
}
Need ompcore(double D[], double x[], double DtX[], double XtX[], double G[], mwSize n, mwSize m, mwSize L,
int T, double eps, int gamma_mode, int profile, double msg_delta, int erroromp)
{
profdata pd;
/* mxArray *Gamma;*/
mwIndex i, j, signum, pos, *ind, *gammaIr, *gammaJc, gamma_count;
mwSize allocated_coefs, allocated_cols;
int DtX_specified, XtX_specified, batchomp, standardomp, *selected_atoms,*times_atoms ;
double *alpha, *r, *Lchol, *c, *Gsub, *Dsub, sum, *gammaPr, *tempvec1, *tempvec2;
double eps2, resnorm, delta, deltaprev, secs_remain;
int mins_remain, hrs_remain;
clock_t lastprint_time, starttime;
Need my;
/*** status flags ***/
DtX_specified = (DtX!=0); /* indicates whether D'*x was provided */
XtX_specified = (XtX!=0); /* indicates whether sum(x.*x) was provided */
standardomp = (G==0); /* batch-omp or standard omp are selected depending on availability of G */
batchomp = !standardomp;
/*** allocate output matrix ***/
if (gamma_mode == FULL_GAMMA) {
/* allocate full matrix of size m X L */
Gamma = mxCreateDoubleMatrix(m, L, mxREAL);
gammaPr = mxGetPr(Gamma);
gammaIr = 0;
gammaJc = 0;
}
else {
/* allocate sparse matrix with room for allocated_coefs nonzeros */
/* for error-omp, begin with L*sqrt(n)/2 allocated nonzeros, otherwise allocate L*T nonzeros */
allocated_coefs = erroromp ? (mwSize)(ceil(L*sqrt((double)n)/2.0) + 1.01) : L*T;
Gamma = mxCreateSparse(m, L, allocated_coefs, mxREAL);
gammaPr = mxGetPr(Gamma);
gammaIr = mxGetIr(Gamma);
gammaJc = mxGetJc(Gamma);
gamma_count = 0;
gammaJc[0] = 0;
}
/*** helper arrays ***/
alpha = (double*)mxMalloc(m*sizeof(double)); /* contains D'*residual */
ind = (mwIndex*)mxMalloc(n*sizeof(mwIndex)); /* indices of selected atoms */
selected_atoms = (int*)mxMalloc(m*sizeof(int)); /* binary array with 1's for selected atoms */
times_atoms = (int*)mxMalloc(m*sizeof(int));
c = (double*)mxMalloc(n*sizeof(double)); /* orthogonal projection result */
/* current number of columns in Dsub / Gsub / Lchol */
allocated_cols = erroromp ? (mwSize)(ceil(sqrt((double)n)/2.0) + 1.01) : T;
/* Cholesky decomposition of D_I'*D_I */
Lchol = (double*)mxMalloc(n*allocated_cols*sizeof(double));
/* temporary vectors for various computations */
tempvec1 = (double*)mxMalloc(m*sizeof(double));
tempvec2 = (double*)mxMalloc(m*sizeof(double));
if (batchomp) {
/* matrix containing G(:,ind) - the columns of G corresponding to the selected atoms, in order of selection */
Gsub = (double*)mxMalloc(m*allocated_cols*sizeof(double));
}
else {
/* matrix containing D(:,ind) - the selected atoms from D, in order of selection */
Dsub = (double*)mxMalloc(n*allocated_cols*sizeof(double));
/* stores the residual */
r = (double*)mxMalloc(n*sizeof(double));
}
if (!DtX_specified) {
/* contains D'*x for the current signal */
DtX = (double*)mxMalloc(m*sizeof(double));
}
/*** initializations for error omp ***/
if (erroromp) {
eps2 = eps*eps; /* compute eps^2 */
if (T<0 || T>n) { /* unspecified max atom num - set max atoms to n */
T = n;
}
}
/*** initialize timers ***/
initprofdata(&pd); /* initialize profiling counters */
starttime = clock(); /* record starting time for eta computations */
lastprint_time = starttime; /* time of last status display */
/********************** perform omp for each signal **********************/
for (signum=0; signum<L; ++signum) {
/* initialize residual norm and deltaprev for error-omp */
if (erroromp) {
if (XtX_specified) {
resnorm = XtX[signum];
}
else {
resnorm = dotprod(x+n*signum, x+n*signum, n);
addproftime(&pd, XtX_TIME);
}
deltaprev = 0; /* delta tracks the value of gamma'*G*gamma */
}
else {
/* ignore residual norm stopping criterion */
eps2 = 0;
resnorm = 1;
}
if (resnorm>eps2 && T>0) {
/* compute DtX */
if (!DtX_specified) {
matT_vec(1, D, x+n*signum, DtX, n, m);
addproftime(&pd, DtX_TIME);
}
/* initialize alpha := DtX */
memcpy(alpha, DtX + m*signum*DtX_specified, m*sizeof(double));
/* mark all atoms as unselected */
for (i=0; i<m; ++i) {
selected_atoms[i] = 0;
}
for (i=0; i<m; ++i) {
times_atoms[i] = 0;
}
}
/* main loop */
i=0;
while (resnorm>eps2 && i<T) {
/* index of next atom */
pos = maxabs(alpha, m);
addproftime(&pd, MAXABS_TIME);
/* stop criterion: selected same atom twice, or inner product too small */
if (selected_atoms[pos] || alpha[pos]*alpha[pos]<1e-14) {
break;
}
/* mark selected atom */
ind[i] = pos;
selected_atoms[pos] = 1;
times_atoms[pos]++;
/* matrix reallocation */
if (erroromp && i>=allocated_cols) {
allocated_cols = (mwSize)(ceil(allocated_cols*MAT_INC_FACTOR) + 1.01);
Lchol = (double*)mxRealloc(Lchol,n*allocated_cols*sizeof(double));
batchomp ? (Gsub = (double*)mxRealloc(Gsub,m*allocated_cols*sizeof(double))) :
(Dsub = (double*)mxRealloc(Dsub,n*allocated_cols*sizeof(double))) ;
}
/* append column to Gsub or Dsub */
if (batchomp) {
memcpy(Gsub+i*m, G+pos*m, m*sizeof(double));
}
else {
memcpy(Dsub+i*n, D+pos*n, n*sizeof(double));
}
/*** Cholesky update ***/
if (i==0) {
*Lchol = 1;
}
else {
/* incremental Cholesky decomposition: compute next row of Lchol */
if (standardomp) {
matT_vec(1, Dsub, D+n*pos, tempvec1, n, i); /* compute tempvec1 := Dsub'*d where d is new atom */
addproftime(&pd, DtD_TIME);
}
else {
vec_assign(tempvec1, Gsub+i*m, ind, i); /* extract tempvec1 := Gsub(ind,i) */
}
backsubst('L', Lchol, tempvec1, tempvec2, n, i); /* compute tempvec2 = Lchol \ tempvec1 */
for (j=0; j<i; ++j) { /* write tempvec2 to end of Lchol */
Lchol[j*n+i] = tempvec2[j];
}
/* compute Lchol(i,i) */
sum = 0;
for (j=0; j<i; ++j) { /* compute sum of squares of last row without Lchol(i,i) */
sum += SQR(Lchol[j*n+i]);
}
if ( (1-sum) <= 1e-14 ) { /* Lchol(i,i) is zero => selected atoms are dependent */
break;
}
Lchol[i*n+i] = sqrt(1-sum);
}
addproftime(&pd, LCHOL_TIME);
i++;
/* perform orthogonal projection and compute sparse coefficients */
vec_assign(tempvec1, DtX + m*signum*DtX_specified, ind, i); /* extract tempvec1 = DtX(ind) */
cholsolve('L', Lchol, tempvec1, c, n, i); /* solve LL'c = tempvec1 for c */
addproftime(&pd, COMPCOEF_TIME);
/* update alpha = D'*residual */
if (standardomp) {
mat_vec(-1, Dsub, c, r, n, i); /* compute r := -Dsub*c */
vec_sum(1, x+n*signum, r, n); /* compute r := x+r */
/*memcpy(r, x+n*signum, n*sizeof(double)); /* assign r := x */
/*mat_vec1(-1, Dsub, c, 1, r, n, i); /* compute r := r-Dsub*c */
addproftime(&pd, COMPRES_TIME);
matT_vec(1, D, r, alpha, n, m); /* compute alpha := D'*r */
addproftime(&pd, DtR_TIME);
/* update residual norm */
if (erroromp) {
resnorm = dotprod(r, r, n);
addproftime(&pd, UPDATE_RESNORM_TIME);
}
}
else {
mat_vec(1, Gsub, c, tempvec1, m, i); /* compute tempvec1 := Gsub*c */
memcpy(alpha, DtX + m*signum*DtX_specified, m*sizeof(double)); /* set alpha = D'*x */
vec_sum(-1, tempvec1, alpha, m); /* compute alpha := alpha - tempvec1 */
addproftime(&pd, UPDATE_DtR_TIME);
/* update residual norm */
if (erroromp) {
vec_assign(tempvec2, tempvec1, ind, i); /* assign tempvec2 := tempvec1(ind) */
delta = dotprod(c,tempvec2,i); /* compute c'*tempvec2 */
resnorm = resnorm - delta + deltaprev; /* residual norm update */
deltaprev = delta;
addproftime(&pd, UPDATE_RESNORM_TIME);
}
}
}
/*** generate output vector gamma ***/
if (gamma_mode == FULL_GAMMA) { /* write the coefs in c to their correct positions in gamma */
for (j=0; j<i; ++j) {
gammaPr[m*signum + ind[j]] = c[j];
}
}
else {
/* sort the coefs by index before writing them to gamma */
quicksort(ind,c,i);
addproftime(&pd, INDEXSORT_TIME);
/* gamma is full - reallocate */
if (gamma_count+i >= allocated_coefs) {
while(gamma_count+i >= allocated_coefs) {
allocated_coefs = (mwSize)(ceil(GAMMA_INC_FACTOR*allocated_coefs) + 1.01);
}
mxSetNzmax(Gamma, allocated_coefs);
mxSetPr(Gamma, mxRealloc(gammaPr, allocated_coefs*sizeof(double)));
mxSetIr(Gamma, mxRealloc(gammaIr, allocated_coefs*sizeof(mwIndex)));
gammaPr = mxGetPr(Gamma);
gammaIr = mxGetIr(Gamma);
}
/* append coefs to gamma and update the indices */
for (j=0; j<i; ++j) {
gammaPr[gamma_count] = c[j];
gammaIr[gamma_count] = ind[j];
gamma_count++;
}
gammaJc[signum+1] = gammaJc[signum] + i;
}
/*** display status messages ***/
if (msg_delta>0 && (clock()-lastprint_time)/(double)CLOCKS_PER_SEC >= msg_delta)
{
lastprint_time = clock();
/* estimated remainig time */
secs2hms( ((L-signum-1)/(double)(signum+1)) * ((lastprint_time-starttime)/(double)CLOCKS_PER_SEC) ,
&hrs_remain, &mins_remain, &secs_remain);
mexPrintf("omp: signal %d / %d, estimated remaining time: %02d:%02d:%05.2f\n",
signum+1, L, hrs_remain, mins_remain, secs_remain);
mexEvalString("drawnow;");
}
}
/* end omp */
/*** print final messages ***/
if (msg_delta>0) {
mexPrintf("omp: signal %d / %d\n", signum, L);
}
if (profile) {
printprofinfo(&pd, erroromp, batchomp, L);
}
/* free memory */
if (!DtX_specified) {
mxFree(DtX);
}
if (standardomp) {
mxFree(r);
mxFree(Dsub);
}
else {
mxFree(Gsub);
}
mxFree(tempvec2);
mxFree(tempvec1);
mxFree(Lchol);
mxFree(c);
mxFree(selected_atoms);
mxFree(ind);
mxFree(alpha);
my.qGamma=Gamma;
my.qtimes__atoms=times__atoms;
/*return Gamma;*/
return my;
}
double compute_delta(double jd, double moon[])
{
double earth[3];
get_earth_helio_coordsv (jd, earth);
double jupiter[3];
get_jupiter_helio_coordsv(jd, jupiter);
double moonl[3];
double moond[3];
double Avector[3];
double Cvector[3];
int j = 0;
for (j = 0; j < 3; j++) {
moond[j] = jupiter[j] + moon[j];
Avector[j] = moond[j] - earth[j];
Cvector[j] = jupiter[j] - earth[j];
}
// double theta = acos(dotprod(Avector, Cvector) / (mag(Avector) * mag(Cvector)));
/* double Cross[3]; */
/* Cross[0] = Cvector[1] * Avector[2] - Cvector[2] * Avector[1]; */
/* Cross[1] = Cvector[2] * Avector[0] - Cvector[0] * Avector[2]; */
/* Cross[2] = Cvector[0] * Avector[1] - Cvector[1] * Avector[0]; */
// double y = mag(Avector) * sin(theta);
// printf("Z: %10.10f\n", Cvector[0] * Avector[1] - Cvector[1] * Avector[0]);
double moonv[3];
crossprod(Cvector, Avector, moonv);
double planev[3] = {0.0, 0.0, 1.0};
// crossprod(earth, jupiter, planev);
double inplanev[3];
crossprod(planev, Avector, inplanev);
double inplanevunit[3];
unitv(inplanev, inplanevunit);
double y = dotprod(inplanevunit, moon);
double z = moonv[2];
moonv[2] = 0; // this makes us work in a plane
// double y = mag(moonv);
/* double A = mag(Avector); // sqrt(pow(earth[0] - moond[0], 2) + pow(earth[1] - moond[1], 2) + pow(earth[2] - moond[2], 2)); */
/* double B = mag(moon); // sqrt(pow(moon[0], 2) + pow(moon[1], 2) + pow(moon[2], 2)); */
/* double C = mag(Cvector); // sqrt(pow(earth[0] - jupiter[0], 2) + pow(earth[1] - jupiter[1], 2) + pow(earth[2] - jupiter[2], 2)); */
/* double y = sqrtl(powl(A,2) - (powl( ( powl(C,2) + powl(A,2) - powl(B,2) ) / (2*C) , 2 ) ) ); */
/* double z = Cvector[0] * Avector[1] - Cvector[1] * Avector[0]; */
if (z < 0) {
// y = -1.0 * y;
}
return (double)y;
}
static void
irt_rpf_nominal_deriv1(const double *spec,
const double *param,
const double *where,
const double *weight, double *out)
{
int nfact = spec[RPF_ISpecDims];
int ncat = spec[RPF_ISpecOutcomes];
double aTheta = dotprod(param, where, nfact);
double aTheta2 = aTheta * aTheta;
Eigen::VectorXd num(ncat);
Eigen::VectorXd ak(ncat);
_nominal_rawprob2(spec, param, where, aTheta, ak.data(), num.data());
Eigen::VectorXd P(ncat);
Eigen::VectorXd P2(ncat);
Eigen::VectorXd P3(ncat);
Eigen::VectorXd ak2(ncat);
Eigen::VectorXd dat_num(ncat);
double numsum = 0;
double numakD = 0;
double numak2D2 = 0;
Eigen::VectorXd numakDTheta_numsum(nfact);
for (int kx=0; kx < ncat; kx++) {
ak2[kx] = ak[kx] * ak[kx];
dat_num[kx] = weight[kx]/num[kx];
numsum += num[kx];
numakD += num[kx] * ak[kx];
numak2D2 += num[kx] * ak2[kx];
}
double numsum2 = numsum * numsum;
for (int kx=0; kx < ncat; kx++) {
P[kx] = num[kx]/numsum;
P2[kx] = P[kx] * P[kx];
P3[kx] = P2[kx] * P[kx];
}
double sumNumak = dotprod(num.data(), ak.data(), ncat);
for (int fx=0; fx < nfact; fx++) {
numakDTheta_numsum[fx] = sumNumak * where[fx] / numsum;
}
for (int jx = 0; jx < nfact; jx++) {
double tmpvec = 0;
for(int i = 0; i < ncat; i++) {
tmpvec += dat_num[i] * (ak[i] * where[jx] * P[i] -
P[i] * numakDTheta_numsum[jx]) * numsum;
}
out[jx] -= tmpvec;
}
int dkoffset;
if (nfact == 0) {
dkoffset = 0;
} else {
dkoffset = ncat - 1;
}
for(int i = 1; i < ncat; i++) {
if (nfact) {
double offterm = makeOffterm(weight, P[i], aTheta, ncat, i);
double tmpvec = dat_num[i] * (aTheta * P[i] - P2[i] * aTheta) * numsum - offterm;
out[nfact + i - 1] -= tmpvec;
}
double offterm2 = makeOffterm(weight, P[i], 1, ncat, i);
double tmpvec2 = dat_num[i] * (P[i] - P2[i]) * numsum - offterm2;
out[nfact + dkoffset + i - 1] -= tmpvec2;
}
int hessbase = nfact + (ncat-1) + dkoffset;
int d2ind = 0;
//a's
for (int j = 0; j < nfact; j++) {
for (int k = 0; k <= j; k++) {
double tmpvec = 0;
for (int i = 0; i < ncat; i++) {
tmpvec += dat_num[i] * (ak2[i] * where[j] * where[k] * P[i] -
ak[i] * where[j] * P[i] * numakDTheta_numsum[k] -
ak[i] * where[k] * P[i] * numakDTheta_numsum[j] +
2 * P[i] * numakD * where[j] * numakD * where[k] / numsum2 -
P[i] * numak2D2 * where[j] * where[k] / numsum) * numsum -
dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
numsum * ak[i] * where[k] +
dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
numakD * where[k];
}
out[hessbase + d2ind++] -= tmpvec;
}
}
//a's with ak and d
for(int k = 1; k < ncat; k++){
int akrow = hessbase + (nfact+k)*(nfact+k-1)/2;
int dkrow = hessbase + (nfact+ncat+k-1)*(nfact+ncat+k-2)/2;
for(int j = 0; j < nfact; j++){
double tmpvec = 0;
double tmpvec2 = 0;
for(int i = 0; i < ncat; i++){
if(i == k){
tmpvec += dat_num[i] * (ak[i]*where[j] * aTheta*P[i] -
aTheta*P[i]*numakDTheta_numsum[j] +
where[j]*P[i] - 2*ak[i]*where[j]*aTheta*P2[i] +
2*aTheta*P2[i]*numakDTheta_numsum[j] -
where[j]*P2[i])*numsum -
dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*numsum*ak[i]*where[j] +
dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*(numakD*where[j]);
tmpvec2 += dat_num[i]*(ak[i]*where[j]*P[i] -
2*ak[i]*where[j]*P2[i] -
P[i]*numakDTheta_numsum[j] +
2*P2[i]*numakDTheta_numsum[j])*numsum -
dat_num[i]*(P[i] - P2[i])*numsum*ak[i]*where[j] +
dat_num[i]*(P[i] - P2[i])*(numakD*where[j]);
} else {
tmpvec += -weight[i]*ak[k]*aTheta*where[j]*P[k] +
weight[i]*P[k]*aTheta*numakDTheta_numsum[j] -
weight[i]*P[k]*where[j];
tmpvec2 += -weight[i]*ak[k]*where[j]*P[k] +
weight[i]*P[k]*numakDTheta_numsum[j];
}
}
out[akrow + j] -= tmpvec;
out[dkrow + j] -= tmpvec2;
}
}
//ak's and d's
for(int j = 1; j < ncat; j++){
int akrow = hessbase + (nfact+j)*(nfact+j-1)/2;
int dkrow = hessbase + (nfact+dkoffset+j)*(nfact+dkoffset+j-1)/2;
double tmpvec = makeOffterm(weight, P2[j], aTheta2, ncat, j);
double tmpvec2 = makeOffterm(weight, P[j], aTheta2, ncat, j);
double offterm = tmpvec - tmpvec2;
tmpvec = makeOffterm(weight, P2[j], 1, ncat, j);
tmpvec2 = makeOffterm(weight, P[j], 1, ncat, j);
double offterm2 = tmpvec - tmpvec2;
if (nfact) {
out[akrow + nfact + j - 1] -=
(dat_num[j]*(aTheta2*P[j] - 3*aTheta2*P2[j] +
2*aTheta2*P3[j])*numsum - weight[j]/num[j] *
(aTheta*P[j] - aTheta*P2[j])*numsum*aTheta + weight[j] *
(aTheta*P[j] - aTheta*P2[j])*aTheta + offterm);
}
out[dkrow + nfact + dkoffset + j - 1] -=
(dat_num[j]*(P[j] - 3*P2[j] + 2*P3[j])*numsum - weight[j]/num[j] *
(P[j] - P2[j])*numsum + weight[j] *
(P[j] - P2[j]) + offterm2);
for(int i = 1; i < ncat; i++) {
if(j > i) {
if (nfact) {
offterm = makeOffterm2(weight, P[j], P[i], aTheta2, ncat, i);
tmpvec = dat_num[i] * (-aTheta2*P[i]*P[j] + 2*P2[i] *aTheta2*P[j])*numsum +
dat_num[i] * (aTheta*P[i] - P2[i] * aTheta)*aTheta*num[j]+offterm;
out[akrow + nfact + i - 1] -= tmpvec;
}
offterm2 = makeOffterm2(weight, P[j], P[i], 1, ncat, i);
tmpvec2 = dat_num[i] * (-P[i]*P[j] + 2*P2[i] *P[j]) * numsum +
dat_num[i] * (P[i] - P2[i]) * num[j] + offterm2;
out[dkrow + nfact + dkoffset + i - 1] -= tmpvec2;
}
if (nfact == 0) continue;
if (abs(j-i) == 0) {
tmpvec = makeOffterm(weight, P2[i], aTheta, ncat, i);
tmpvec2 = makeOffterm(weight, P[i], aTheta, ncat, i);
offterm = tmpvec - tmpvec2;
tmpvec = dat_num[i]*(aTheta*P[i] - 3*aTheta*P2[i] +
2*aTheta*P3[i]) * numsum - dat_num[i] *
(aTheta*P[i] - aTheta*P2[i])*numsum + weight[i] *
(P[i] - P2[i])*aTheta + offterm;
out[dkrow + nfact + i - 1] -= tmpvec;
} else {
offterm = makeOffterm2(weight, P[j], P[i], aTheta, ncat, i);
tmpvec = dat_num[i] * (-aTheta*P[i]*P[j] + 2*P2[i] *aTheta*P[j]) * numsum +
dat_num[i] * (P[i] - P2[i]) * aTheta * num[j] + offterm;
out[dkrow + nfact + i - 1] -= tmpvec;
}
}
}
}
void sf_ccgstep( bool forget /* restart flag */,
int nx /* model size */,
int ny /* data size */,
sf_complex* x /* current model [nx] */,
const sf_complex* g /* gradient [nx] */,
sf_complex* rr /* data residual [ny] */,
const sf_complex* gg /* conjugate gradient [ny] */)
/*< Step of Claerbout's conjugate-gradient iteration for complex operators.
The data residual is rr = A x - dat
>*/
{
sf_double_complex sds, gdg, gds, sdg, determ, gdr, sdr, alfa, beta;
int i;
if (Allocated == false) {
Allocated = forget = true;
S = sf_complexalloc (nx);
Ss = sf_complexalloc (ny);
}
if (forget) {
for (i = 0; i < nx; i++) {
S[i] = sf_cmplx(0.0,0.0);
}
for (i = 0; i < ny; i++) {
Ss[i] = sf_cmplx(0.0,0.0);
}
beta = sf_dcmplx(0.0,0.0);
if (cblas_scnrm2(ny,gg,1) <= 0.) return;
#ifdef SF_HAS_COMPLEX_H
alfa = - dotprod( ny, gg, rr) / dotprod(ny, gg, gg);
#else
alfa = sf_dcneg(sf_dcdiv(dotprod( ny, gg, rr),dotprod(ny, gg, gg)));
#endif
} else {
/* search plane by solving 2-by-2
G . (R - G*alfa - S*beta) = 0
S . (R - G*alfa - S*beta) = 0 */
gdg = dotprod( ny, gg, gg);
sds = dotprod( ny, Ss, Ss);
gds = dotprod( ny, gg, Ss);
sdg = dotprod( ny, Ss, gg);
if (cabs(gdg) == 0. || cabs(sds) == 0.) return;
#ifdef SF_HAS_COMPLEX_H
determ = 1.0 - (gds/gdg)*(gds/sds);
if (creal(determ) > EPSILON) {
determ *= gdg * sds;
} else {
determ = gdg * sds * EPSILON;
}
gdr = - dotprod( ny, gg, rr);
sdr = - dotprod( ny, Ss, rr);
alfa = ( sds * gdr - gds * sdr ) / determ;
beta = (-sdg * gdr + gdg * sdr ) / determ;
#else
determ = sf_dcneg(sf_dcmul(sf_dcdiv(gds,gdg),
sf_dcdiv(gds,sds)));
determ.r += 1.0;
if (creal(determ) > EPSILON) {
determ = sf_dcmul(determ,sf_dcmul(gdg,sds));
} else {
determ = sf_dcmul(gdg,sf_dcrmul(sds,EPSILON));
}
gdr = sf_dcneg(dotprod( ny, gg, rr));
sdr = sf_dcneg(dotprod( ny, Ss, rr));
alfa = sf_dcdiv(sf_dcsub(sf_dcmul(sds,gdr),
sf_dcmul(gds,sdr)),determ);
beta = sf_dcdiv(sf_dcsub(sf_dcmul(gdg,sdr),
sf_dcmul(sdg,gdr)),determ);
#endif
}
for (i = 0; i < nx; i++) {
#ifdef SF_HAS_COMPLEX_H
S[i] = alfa * g[i] + beta * S[i];
x[i] += S[i];
#else
S[i] = sf_cadd(sf_dccmul(alfa,g[i]),
sf_dccmul(beta,S[i]));
x[i] = sf_cadd(x[i],S[i]);
#endif
}
for (i = 0; i < ny; i++) {
#ifdef SF_HAS_COMPLEX_H
Ss[i] = alfa * gg[i] + beta * Ss[i];
rr[i] += Ss[i];
#else
Ss[i] = sf_cadd(sf_dccmul(alfa,gg[i]),
sf_dccmul(beta,Ss[i]));
rr[i] = sf_cadd(rr[i],Ss[i]);
#endif
}
}
/* Create a new bubble. */
void *
glb_bubble_new(glb_data *d, GLfloat x, GLfloat y, GLfloat z, GLfloat scale,
GLfloat y_incr, GLfloat scale_incr)
{
int i, j;
/* GLfloat axes [glb_config.nr_nudge_axes][3]; */
GLfloat axes[5][3]; /* HARD CODED for SunCC */
int nr_vertices;
glb_vertex *vertices = glb_sphere_get_vertices(d, &nr_vertices);
bubble *b = (bubble *) malloc(sizeof *b);
if (b == 0)
return 0;
if (glb_config.bubble_colour[0] == -1.0) {
b->color[0] = ((float) (NRAND(100)) / 100.0);
b->color[1] = ((float) (NRAND(100)) / 100.0);
b->color[2] = ((float) (NRAND(100)) / 100.0);
} else {
b->color[0] = glb_config.bubble_colour[0];
b->color[1] = glb_config.bubble_colour[1];
b->color[2] = glb_config.bubble_colour[2];
}
b->color[3] = glb_config.bubble_colour[3];
b->contributions = (GLfloat *) malloc(sizeof (GLfloat) * nr_vertices *
glb_config.nr_nudge_axes);
if (b->contributions == 0) {
(void) free((void *) b);
return 0;
}
b->nudge_angle = (GLfloat *) malloc(sizeof (GLfloat) * glb_config.nr_nudge_axes);
if (b->nudge_angle == 0) {
(void) free((void *) b->contributions);
(void) free((void *) b);
return 0;
}
b->nudge_angle_incr = (GLfloat *) malloc(sizeof (GLfloat) * glb_config.nr_nudge_axes);
if (b->nudge_angle_incr == 0) {
(void) free((void *) b->nudge_angle);
(void) free((void *) b->contributions);
(void) free((void *) b);
return 0;
}
/* Initialize primitive elements. */
b->x = x;
b->y = y;
b->z = z;
b->scale = scale;
b->y_incr = y_incr;
b->scale_incr = scale_incr;
b->rotx = b->roty = b->rotz = 0;
b->rotx_incr = glb_drand() * glb_config.rotation_factor * 2
- glb_config.rotation_factor;
b->roty_incr = glb_drand() * glb_config.rotation_factor * 2
- glb_config.rotation_factor;
b->rotz_incr = glb_drand() * glb_config.rotation_factor * 2
- glb_config.rotation_factor;
/* Initialize the nudge angle arrays. */
for (i = 0; i < glb_config.nr_nudge_axes; ++i) {
b->nudge_angle[i] = 0;
b->nudge_angle_incr[i] = glb_drand() * glb_config.nudge_angle_factor;
}
/* Choose some random nudge axes. */
for (i = 0; i < glb_config.nr_nudge_axes; ++i) {
axes[i][0] = glb_drand() * 2 - 1;
axes[i][1] = glb_drand() * 2 - 1;
axes[i][2] = glb_drand() * 2 - 1;
normalize(axes[i]);
}
/* Calculate the contribution that each nudge axis has on each vertex. */
for (i = 0; i < nr_vertices; ++i)
for (j = 0; j < glb_config.nr_nudge_axes; ++j)
b->contributions[i * glb_config.nr_nudge_axes + j]
= max(0, dotprod(vertices[i], axes[j]));
return (void *) b;
}
void my_raytrace(int mousex, int mousey)
{
double modelViewMatrix[16];
double projMatrix[16];
float heldSta[3];
float heldDir[3];
int viewport[4];
int foundIntersection = 0;
int hit = 0;
int i;
double clickPoint[3];
GLfloat clickedPoint[4];
GLfloat intersectionPoint[3];
float closestPoint[3];
float rayStart[3];
float rayDirection[3];
OBJECT *cur;
OBJECT *temp;
double norm;
// first we need to get the modelview matrix, the projection matrix, and the viewport
glGetDoublev(GL_MODELVIEW_MATRIX, modelViewMatrix);
glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);
glGetIntegerv(GL_VIEWPORT, viewport);
mousey = viewport[3]-mousey;
// gluUnProject with a Z value of 1 will find the point on the far clipping plane
// corresponding the the mouse click. This is not the same as the vector
// representing the click.
gluUnProject(mousex, mousey, 1.0, modelViewMatrix, projMatrix, viewport, &clickPoint[0], &clickPoint[1], &clickPoint[2]);
// Now we need a vector representing the click. It should start at the camera
// position. We can subtract the click point, we will get the vector
/* code for finding direction vector, set rayStart and rayDirection */
rayStart[0] = my_cam.pos[0];
rayStart[1] = my_cam.pos[1];
rayStart[2] = my_cam.pos[2];
rayDirection[0] = clickPoint[0]-rayStart[0];
rayDirection[1] = clickPoint[1]-rayStart[1];
rayDirection[2] = clickPoint[2]-rayStart[2];
norm = sqrt(rayDirection[0]*rayDirection[0]+rayDirection[1]*rayDirection[1]+rayDirection[2]*rayDirection[2]);
rayDirection[0] = rayDirection[0]/norm;
rayDirection[1] = rayDirection[1]/norm;
rayDirection[2] = rayDirection[2]/norm;
heldDir[0] = rayDirection[0];
heldDir[1] = rayDirection[1];
heldDir[2] = rayDirection[2];
heldSta[0] = rayStart[0];
heldSta[1] = rayStart[1];
heldSta[2] = rayStart[2];
lineStart[0]=rayStart[0];
lineStart[1]=rayStart[1];
lineStart[2]=rayStart[2];
lineEnd[0]=clickPoint[0];
lineEnd[1]=clickPoint[1];
lineEnd[2]=clickPoint[2];
// now go through the shapes and see if there is a hit
for (i=0; i<num_objects; i++)
{
cur = my_objects + i;
hit = 0;
rayDirection[0] = heldDir[0];
rayDirection[1] = heldDir[1];
rayDirection[2] = heldDir[2];
rayStart[0] = heldSta[0];
rayStart[1] = heldSta[1];
rayStart[2] = heldSta[2];
switch (cur->sid)
{
case 3:
//translation
clickedPoint[0] = rayStart[0];
clickedPoint[1] = rayStart[1];
clickedPoint[2] =rayStart[2];
clickedPoint[3] =1;
set_T(-cur->translate[0],-cur->translate[1],-cur->translate[2]);
matrix_mult(T,clickedPoint);
rayStart[0] = result[0];
rayStart[1] = result[1];
rayStart[2] = result[2];
//rotation point
//RzT
clickedPoint[0] = rayStart[0];
clickedPoint[1] = rayStart[1];
clickedPoint[2] =rayStart[2];
clickedPoint[3] =1;
set_RzT(cur->rotate[2]);
matrix_mult(RzT,clickedPoint);
rayStart[0] = result[0];
rayStart[1] = result[1];
rayStart[2] = result[2];
//RyT
clickedPoint[0] = rayStart[0];
clickedPoint[1] = rayStart[1];
clickedPoint[2] =rayStart[2];
clickedPoint[3] =1;
set_RyT(cur->rotate[1]);
matrix_mult(RyT,clickedPoint);
rayStart[0] = result[0];
rayStart[1] = result[1];
rayStart[2] = result[2];
//RxT
clickedPoint[0] = rayStart[0];
clickedPoint[1] = rayStart[1];
clickedPoint[2] =rayStart[2];
clickedPoint[3] =1;
set_RxT(cur->rotate[0]);
matrix_mult(RxT,clickedPoint);
rayStart[0] = result[0];
rayStart[1] = result[1];
rayStart[2] = result[2];
//scaling point
clickedPoint[0] = rayStart[0];
clickedPoint[1] = rayStart[1];
clickedPoint[2] =rayStart[2];
clickedPoint[3] =1;
set_S(1/cur->scale[0],1/cur->scale[1],1/cur->scale[2]);
matrix_mult(S,clickedPoint);
rayStart[0] = result[0];
rayStart[1] = result[1];
rayStart[2] = result[2];
//rotation vector
//RzT
clickedPoint[0] = rayDirection[0];
clickedPoint[1] = rayDirection[1];
clickedPoint[2] =rayDirection[2];
clickedPoint[3] =1;
set_RzT(cur->rotate[2]);
matrix_mult(RzT,clickedPoint);
rayDirection[0] = result[0];
rayDirection[1] = result[1];
rayDirection[2] = result[2];
//RyT
clickedPoint[0] = rayDirection[0];
clickedPoint[1] = rayDirection[1];
clickedPoint[2] =rayDirection[2];
clickedPoint[3] =1;
set_RyT(cur->rotate[1]);
matrix_mult(RyT,clickedPoint);
rayDirection[0] = result[0];
rayDirection[1] = result[1];
rayDirection[2] = result[2];
//RxT
clickedPoint[0] = rayDirection[0];
clickedPoint[1] = rayDirection[1];
clickedPoint[2] =rayDirection[2];
clickedPoint[3] =1;
set_RxT(cur->rotate[0]);
matrix_mult(RxT,clickedPoint);
rayDirection[0] = result[0];
rayDirection[1] = result[1];
rayDirection[2] = result[2];
//scaling vector
clickedPoint[0] = rayDirection[0];
clickedPoint[1] = rayDirection[1];
clickedPoint[2] =rayDirection[2];
clickedPoint[3] =1;
set_S(1/cur->scale[0],1/cur->scale[1],1/cur->scale[2]);
matrix_mult(S,clickedPoint);
rayDirection[0] = result[0];
rayDirection[1] = result[1];
rayDirection[2] = result[2];
hit = my_raytrace_sph(cur, rayStart, rayDirection, intersectionPoint);
//scale intersect
clickedPoint[0] = intersectionPoint[0];
clickedPoint[1] = intersectionPoint[1];
clickedPoint[2] = intersectionPoint[2];
clickedPoint[3] = 1;
set_S(cur->scale[0],cur->scale[1],cur->scale[2]);
matrix_mult(S,clickedPoint);
intersectionPoint[0]= result[0];
intersectionPoint[1]=result[1];
intersectionPoint[2]=result[2];
//rotate intersect
//Rx
clickedPoint[0] = intersectionPoint[0];
clickedPoint[1] = intersectionPoint[1];
clickedPoint[2] =intersectionPoint[2];
clickedPoint[3] =1;
set_Rx(cur->rotate[0]);
matrix_mult(Rx,clickedPoint);
intersectionPoint[0] = result[0];
intersectionPoint[1] = result[1];
intersectionPoint[2] = result[2];
//Ry
clickedPoint[0] = intersectionPoint[0];
clickedPoint[1] = intersectionPoint[1];
clickedPoint[2] =intersectionPoint[2];
clickedPoint[3] =1;
set_Ry(cur->rotate[1]);
matrix_mult(Ry,clickedPoint);
intersectionPoint[0] = result[0];
intersectionPoint[1] = result[1];
intersectionPoint[2] = result[2];
//Rz
clickedPoint[0] = intersectionPoint[0];
clickedPoint[1] = intersectionPoint[1];
clickedPoint[2] =intersectionPoint[2];
clickedPoint[3] =1;
set_Rz(cur->rotate[2]);
matrix_mult(Rz,clickedPoint);
intersectionPoint[0] = result[0];
intersectionPoint[1] = result[1];
intersectionPoint[2] = result[2];
//translate intersect
clickedPoint[0] = intersectionPoint[0];
clickedPoint[1] = intersectionPoint[1];
clickedPoint[2] = intersectionPoint[2];
clickedPoint[3] = 1;
set_T(cur->translate[0],cur->translate[1],cur->translate[2]);
matrix_mult(T,clickedPoint);
intersectionPoint[0]= result[0];
intersectionPoint[1]=result[1];
intersectionPoint[2]=result[2];
break;
default:
break;
}
// found intersection
if (hit)
{
if (foundIntersection)
{
if(intersectionPoint[2]>closestPoint[2]){
closestPoint[0] = intersectionPoint[0];
closestPoint[1] = intersectionPoint[1];
closestPoint[2] = intersectionPoint[2];
printf("found closer collision\n");
}
else{
printf("found another collision, but it was not closer\n");
}
temp=cur;
}
else
{
closestPoint[0] = intersectionPoint[0];
closestPoint[1] = intersectionPoint[1];
closestPoint[2] = intersectionPoint[2];
temp= cur;
}
foundIntersection = 1;
}
}
if (foundIntersection)
{
LITE *pl;
printf("Intersected with object %s at (%f, %f, %f)\n", "object_name", closestPoint[0], closestPoint[1], closestPoint[2]);
illum_light_b=0;
illum_light_g=0;
illum_light_r=0;
float L_vec[4];
float temp_center[4];
float N_vec[4];
L_vec[0]=0;
L_vec[1]=0;
L_vec[2]=0;
L_vec[3]=0;
N_vec[0]=0;
N_vec[1]=0;
N_vec[2]=0;
N_vec[3]=0;
illu_r=0;
illu_g=0;
illu_b=0;
//go through lights
for(i=1;i<num_lights+1;i++){
pl = my_lights+i;
//calculate normal
N_vec[0] = closestPoint[0]-temp->translate[0];
N_vec[1] = closestPoint[1]-temp->translate[1];
N_vec[2] = closestPoint[2]-temp->translate[2];
N_vec[3] = 0;
//normalize normal
normalize(N_vec);
//calc light dir
L_vec[0]=pl->pos[0] - closestPoint[0];
L_vec[1]=pl->pos[1] - closestPoint[1];
L_vec[2]=pl->pos[2] - closestPoint[2];
L_vec[3]=0;
//normalize light
normalize(L_vec);
//calculate light illumination
illum_light_r+=pl->diff[0]*(k_d*temp->diff[0]*dotprod(N_vec,L_vec));
illum_light_g+=pl->diff[1]*(k_d*temp->diff[1]*dotprod(N_vec,L_vec));
illum_light_b+=pl->diff[2]*(k_d*temp->diff[2]*dotprod(N_vec,L_vec));
}
//calculate entire rgb illumination
illu_r=k_a*temp->amb[0]+illum_light_r;
illu_g=k_a*temp->amb[1]+illum_light_g;
illu_b =k_a*temp->amb[2]+illum_light_b;
}
}
void LBR4p_invdyn(double TAU[][7], const double* input1, const double* input2, const double* input3){
/* declare variables */
double inertia_row1[7][1];
double inertia_row2[7][1];
double inertia_row3[7][1];
double inertia_row4[7][1];
double inertia_row5[7][1];
double inertia_row6[7][1];
double inertia_row7[7][1];
double coriolis_row1[7][1];
double coriolis_row2[7][1];
double coriolis_row3[7][1];
double coriolis_row4[7][1];
double coriolis_row5[7][1];
double coriolis_row6[7][1];
double coriolis_row7[7][1];
double gravload[7][1];
double friction[7][1];
/* call the computational routines */
LBR4p_gravload(gravload, input1);
LBR4p_friction(friction, input2);
/* rowwise routines */
LBR4p_inertia_row_1(inertia_row1, input1);
LBR4p_inertia_row_2(inertia_row2, input1);
LBR4p_inertia_row_3(inertia_row3, input1);
LBR4p_inertia_row_4(inertia_row4, input1);
LBR4p_inertia_row_5(inertia_row5, input1);
LBR4p_inertia_row_6(inertia_row6, input1);
LBR4p_inertia_row_7(inertia_row7, input1);
LBR4p_coriolis_row_1(coriolis_row1, input1, input2);
LBR4p_coriolis_row_2(coriolis_row2, input1, input2);
LBR4p_coriolis_row_3(coriolis_row3, input1, input2);
LBR4p_coriolis_row_4(coriolis_row4, input1, input2);
LBR4p_coriolis_row_5(coriolis_row5, input1, input2);
LBR4p_coriolis_row_6(coriolis_row6, input1, input2);
LBR4p_coriolis_row_7(coriolis_row7, input1, input2);
/* fill output vector */
TAU[0][0] = dotprod(inertia_row1, input3, 7) /* inertia */
+ dotprod(coriolis_row1, input2, 7) /* coriolis */
+ gravload[0][0]
- friction[0][0];
TAU[0][1] = dotprod(inertia_row2, input3, 7) /* inertia */
+ dotprod(coriolis_row2, input2, 7) /* coriolis */
+ gravload[1][0]
- friction[1][0];
TAU[0][2] = dotprod(inertia_row3, input3, 7) /* inertia */
+ dotprod(coriolis_row3, input2, 7) /* coriolis */
+ gravload[2][0]
- friction[2][0];
TAU[0][3] = dotprod(inertia_row4, input3, 7) /* inertia */
+ dotprod(coriolis_row4, input2, 7) /* coriolis */
+ gravload[3][0]
- friction[3][0];
TAU[0][4] = dotprod(inertia_row5, input3, 7) /* inertia */
+ dotprod(coriolis_row5, input2, 7) /* coriolis */
+ gravload[4][0]
- friction[4][0];
TAU[0][5] = dotprod(inertia_row6, input3, 7) /* inertia */
+ dotprod(coriolis_row6, input2, 7) /* coriolis */
+ gravload[5][0]
- friction[5][0];
TAU[0][6] = dotprod(inertia_row7, input3, 7) /* inertia */
+ dotprod(coriolis_row7, input2, 7) /* coriolis */
+ gravload[6][0]
- friction[6][0];
}
double solvoptweights(unsigned short n,
double x[],
double fun(),
void grad(),
double options[],
double func(),
void gradc()
)
{
double min(double, double);
double max(double, double);
/*solvoptweights returns the optimum function value.
Arguments to the function:
n is the space dimension,
x is an n-vector, the coordinates of the starting point
at a call to the function and the optimizer at a regular return,
fun is the entry name of an external function which computes the value
of the objective function 'fun' at a point x.
synopsis: double fun(double x[])
grad is the entry name of an external function which computes the gradient
vector of the objective function 'fun' at a point x.
synopsis: void grad(double x[],double g[])
options is a vector of optional parameters:
options[0]= H, where sign(H)=-1 resp. sign(H)=+1 means minimize
resp. maximize FUN (valid only for unconstrained problem)
and H itself is a factor for the initial trial step size
(options[0]=-1.e0 by default),
options[1]= relative error for the argument
in terms of the max-norm (1.e-4 by default),
options[2]= relative error for the function value (1.e-6 by default),
options[3]= limit for the number of iterations (15000 by default),
options[4]= control of the display of intermediate results and
error resp. warning messages (default value is -1,
i.e., no intermediate output but error and warning
messages),
options[5]= admissible maximal residual for a set of constraints
(options[5]=1.e-8 by default),
options[6]= the coefficient of space dilation (2.5 by default),
options[7]= the lower bound for the stepsize used for the finite
difference approximation of gradients (1.e-11 by default).
(@ ... changes should be done with care)
Returned optional values:
options[8], the number of iterations, if positive,
or an abnormal stop code, if negative (see manual for more),
-1: allocation error,
-2: improper space dimension,
-3: <fun> returns an improper value,
-4: <grad> returns a zero or improper vector at the starting point,
-5: <func> returns an improper value,
-6: <gradc> returns an improper vector,
-7: function is unbounded,
-8: gradient is zero, but stopping criteria are not fulfilled,
-9: iterations limit exceeded,
-11: Premature stop is possible,
-12: Result may not provide the true optimum,
-13: Result may be inaccurate in view of a point.
-14: Result may be inaccurate in view of a function value,
options[9] , the number of objective function evaluations,
options[10], the number of gradient evaluations,
____________________________________________________________________________*/
double default_options[11]=
{-1.0,1.e-4,1.e-6,15000.,-1.0,1.e-8,2.5,1.e-11,0.0,0.0,0.0};
void null_entry(); //void apprgrdn();
unsigned short app;
unsigned short /*FsbPnt, FsbPnt1, */termflag, stopf;
unsigned short stopping, dispwarn, /*Reset,*/ ksm,knan/*,obj*/;
unsigned short kstore, knorms, k, kcheck, numelem;
long ajp,ajpp;
unsigned short ld, mxtc, termx, limxterm, nzero, krerun;
unsigned short kflat, stepvanish, i,j,ni,ii, kd,kj,kc,ip;
unsigned short iterlimit, kg,k1,k2/*, kless*/;
short dispdata, warnno;
double nsteps[3]={0.0,0.0,0.0}, kk, nx;
double gnorms[10]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
double ajb,ajs, des, dq,du20,du10,du03;
double n_float/*, inteps*/;
double low_bound, ZeroGrad, y;
double lowxbound, lowfbound, detfr, detxr, grbnd;
double f,/*fp,fp1,fc,*/f1,f2,fm,fopt,frec,fst /*,fp_rate*/;
double gamma,w,wdef,h1,h,hp;
double dx,ng,/*ngc,*/nng,ngt,nrmz,ng1,d,dd, laststep;
double zero=0.,one=1.,two=2.,three=3.,four=4.,
five=5.,/*six=6.,*/seven=7.,eight=8.,nine=9.,ten=10.,
hundr=100.,infty=1.e100,epsnorm=1.e-15,epsnorm2=1.e-30,
powerm12=1.e-12;
double *B; /* space transformation matrix (allocatable) */
/* allocatable working arrays: */
double *g,*g0,*g1,*gt,*gc,*z,*x1,*xopt,*xrec,*grec,*xx,*deltax;
unsigned short *idx;
char *endwarn;
double integraltol;
double integral;
double mean(double *,int);
double dotprod(double *, double *, int);
/* silly initialisations */
endwarn = NULL;
kd = 0;
/* Check the dimension: */
if (n<2)
{ Rprintf (errmes); Rprintf (error2);
options[8]=-one;
return(zero);
} n_float=n;
/* Allocate the memory for working arrays: */
B=(double *)calloc(n*n,sizeof(double));
g=(double *)calloc(n,sizeof(double));
g0=(double *)calloc(n,sizeof(double));
g1=(double *)calloc(n,sizeof(double));
gt=(double *)calloc(n,sizeof(double));
gc=(double *)calloc(n,sizeof(double));
z=(double *)calloc(n,sizeof(double));
x1=(double *)calloc(n,sizeof(double));
xopt=(double *)calloc(n,sizeof(double));
xrec=(double *)calloc(n,sizeof(double));
grec=(double *)calloc(n,sizeof(double));
xx=(double *)calloc(n,sizeof(double));
deltax=(double *)calloc(n,sizeof(double));
idx=(unsigned short *)calloc(n,sizeof(unsigned short));
if (B==NULL ||g==NULL ||g0==NULL ||g1==NULL ||gt==NULL ||
gc==NULL ||z==NULL ||x1==NULL ||xopt==NULL||xrec==NULL ||
grec==NULL||xx==NULL||deltax==NULL||idx==NULL)
{
Rprintf (allocerrstr);
options[8]=-one;
return(zero);
}
/* ANALIZE THE ARGUMENTS PASSED
User-supplied gradients: */
if (grad==null_entry) app=1; else app=0;
/* options: */
for (i=0;i<=7;i++)
{ if (options[i]==zero) options[i]=default_options[i];
else if (i==1 || i==2 || i==5)
{ options[i]=max(options[i],powerm12);
options[i]=min(options[i],one);
if (i==1) options[i]=max(options[i],options[8]*hundr);
}
else if (i==6) options[6]=max(options[i],1.5e0);
}
for (i=8;i<=10;i++) options[i]=zero;
iterlimit= (unsigned short) options[3];
/* integral tol */
integraltol = options[5];
/* Set h1 = -1: we are minimizing :*/
h1=-one;
/* Multiplier for the matrix for the inverse of the space dilation: */
wdef=one/options[6]-one;
/* Iterations counter: */
k=0;
/* Gamma control : */
ajb=one+0.1/(n_float*n_float);
ajp=20; ajpp=ajp; ajs=1.15e0; knorms=0;
/* Display control : */
if (options[4]<=zero)
{ dispdata=0;
if (options[4]<=-one+0.1) dispwarn=0; else dispwarn=1;
}
else { dispdata=(short)(floor(options[4]+0.1)); dispwarn=1; }
ld=dispdata;
/* Stepsize control : */
dq=5.1; /* Step divider (at f_{i+1}>gamma*f_{i}) */
du20=two; du10=1.5; du03=1.05; /* Step multipliers */
kstore=3;
//if (app) des=6.3; /* Desired number of steps per 1-D search */
/*else*/ des=3.3; /* Same for the case of analytical grads. */
mxtc=3; /* Number of trial cycles (wall detect) */
termx=0; limxterm=50; /* Counter and limit for x-criterion */
/*ddx=max(1.e-11,options[7]); stepsize for gradient approximation */
low_bound=-one+1.e-4; /* Lower bound cosine to detect a ravine */
ZeroGrad=n_float*1.e-16; /* Lower bound for a gradient norm */
nzero=0; /* Zero-gradient events counter */
lowxbound=max(options[1],1.e-3); /* Low bound for the variables */
lowfbound=options[2]*options[2]; /* Lower bound for function values */
krerun=0; /* Re-run events counter */
detfr=options[2]*hundr; /* Relative error for f/f_{record} */
detxr=options[1]*ten; /* Relative error for norm(x)/norm(x_{record})*/
warnno=0; /* the number of a warn.mess. to end with */
kflat=0; /* counter for points of flatness */
stepvanish=0; /* counter for vanished steps */
stopf=0; /* last-check flag */
/* End of setting constants */
/* End of the preamble */
/* COMPUTE THE OBJECTIVE FUNCTION (first time): */
f=fun(x);
integral = f+dotprod(x,weights,truepoints);
options[9]+=one;
if (fabs(f)>=infty)
{ if (dispwarn) { Rprintf (errmes); Rprintf (error32); Rprintf (error6); }
options[8]=-three; goto endrun;
}
for (i=0;i<n;i++) xrec[i]=x[i]; frec=f; /* record the point */
/* COMPUTE THE GRADIENT (first time): */
grad(x,g); options[10]+=one;
ng=zero; for (i=0;i<n;i++) ng+=g[i]*g[i]; ng=sqrt(ng);
if (ng>=infty)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error42); Rprintf(error6); }
options[8]=-four; goto endrun;
}
else if (ng<ZeroGrad)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error43); Rprintf(error6); }
options[8]=-four; goto endrun;
}
for (i=0;i<n;i++) grec[i]=g[i]; nng=ng;
/* INITIAL STEPSIZE : */
d=zero; for (i=0;i<n;i++) { if (d<fabs(x[i])) d=fabs(x[i]); }
h=h1*sqrt(options[1])*d; /* smallest possible stepsize */
/* if (fabs(options[0])!=one)
h=h1*max(fabs(options[0]),fabs(h)); *//* user-supplied stepsize */
/* else */
h=h1*max(one/log(ng+1.1),fabs(h)); /* calculated stepsize */
/*--------------------------------------------------------------------
RESETTING LOOP */
while (1)
{ kcheck=0; /* checkpoint counter */
kg=0; /* stepsizes stored */
kj=0; /* ravine jump counter */
for(i=0;i<n;i++)
{ for(j=0;j<n;j++) B[i*n+j]=zero; B[i*n+i]=one; g1[i]=g[i];
}
fst=f; dx=zero;
/*-----------------------------------------------------------------
MAIN ITERATIONS */
while (1)
{ k+=1; kcheck+=1; laststep=dx;
/* ADJUST GAMMA : */
gamma=one+max(pow(ajb,(ajp-kcheck)*n),two*options[2]);
gamma=min(gamma,pow(ajs,max(one,log10(nng+one))));
/* Gradient in the transformed space (gt) : */
ngt=zero; ng1=zero; dd=zero;
for (i=0;i<n;i++)
{ d=zero; for (j=0;j<n;j++) d+=B[j+i*n]*g[j];
gt[i]=d; dd+=d*g1[i]; ngt+=d*d; ng1+=g1[i]*g1[i];
}
ngt=sqrt(ngt); ng1=sqrt(ng1); dd/=ngt*ng1;
w=wdef;
/* JUMPING OVER A RAVINE */
if (dd<low_bound)
{ if (kj==2) for(i=0;i<n;i++) xx[i]=x[i];
if (kj==0) kd=4;
kj+=1; w=-0.9; h*=two;
if (kj>2*kd)
{ kd+=1; warnno=1; endwarn=endwarn1;
for(i=0;i<n;i++)
{ if (fabs(x[i]-xx[i])<epsnorm*fabs(x[i]))
{ if (dispwarn) { Rprintf(wrnmes); Rprintf(warn08); }
}
}
}
}
else kj=0;
/* DILATION : */
nrmz=zero; for(i=0;i<n;i++) { z[i]=gt[i]-g1[i]; nrmz+=z[i]*z[i]; }
nrmz=sqrt(nrmz);
if (nrmz>epsnorm*ngt)
{ for(i=0;i<n;i++) z[i]/=nrmz;
d=zero; for (i=0;i<n;i++) d+=z[i]*gt[i];
ng1=zero; d*=w;
for (i=0;i<n;i++)
/* Make a space transformation: g1=gt+w*(z*gt')*z: */
{ dd=zero; g1[i]=gt[i]+d*z[i]; ng1+=g1[i]*g1[i];
for (j=0;j<n;j++) dd+=B[j*n+i]*z[j]; dd*=w;
/* New inverse matrix: B = B ( I + (1/alpha -1)zz' ) */
for (j=0;j<n;j++) B[j*n+i]+=dd*z[j];
}
ng1=sqrt(ng1);
}
else { for (i=0;i<n;i++) z[i]=zero; nrmz=zero; }
for (i=0;i<n;i++) gt[i]=g1[i]/ng1;
/* Gradient in the non-transformed space: g0 = B' * gt */
for (i=0;i<n;i++)
{ d=zero; for (j=0;j<n;j++) d+=B[j*n+i]*gt[j];
g0[i]=d;
}
/* CHECK FOR THE NEED OF RESETTING */
if (kcheck>1)
{ numelem=0;
for(i=0;i<n;i++)
{ if (fabs(g[i])>ZeroGrad) { idx[numelem]=i; numelem+=1; }
}
if (numelem>0)
{ grbnd=epsnorm*(numelem*numelem); ii=0;
for(i=0;i<numelem;i++)
{ j=idx[i]; if (fabs(g1[j])<=fabs(g[j])*grbnd) ii+=1;
}
if (ii==n || nrmz==zero)
{ if (dispwarn) { Rprintf(wrnmes); Rprintf(warn20); }
if (fabs(fst-f)<fabs(f)*.01) ajp-=10*n;
else ajp=ajpp;
h=h1*dx/three; k=k-1; break;
}
}
}
/* STORE THE CURRENT VALUES AND SET THE COUNTERS FOR 1-D SEARCH */
for (i=0;i<n;i++) xopt[i]=x[i]; hp=h; fopt=f; k1=0; k2=0;
ksm=0; kc=0; knan=0;
/* 1-D SEARCH */
while (1)
{ for (i=0;i<n;i++) x1[i]=x[i]; f1=f;
if (f1<zero) dd=-one; else dd=one;
/* Next point: */
for (i=0;i<n;i++) x[i]+=hp*g0[i];
ii=0; for (i=0;i<n;i++)
{ if (fabs(x[i]-x1[i])<fabs(x[i])*epsnorm) ii+=1;
}
/* COMPUTE THE FUNCTION VALUE AT A POINT: */
f=fun(x);
integral = f +dotprod(x,weights,truepoints);
options[9]+=one;
if (h1*f>=infty)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error5); }
options[8]=-seven; goto endrun;
}
/* No function value at a point : */
if (fabs(f)>=infty)
{ if (dispwarn) { Rprintf(wrnmes); Rprintf(error32); }
if (ksm || kc>=mxtc) { options[8]=-three; goto endrun; }
else
{ k2+=1; k1=0; hp/=dq; for(i=0;i<n;i++) x[i]=x1[i];
f=f1; knan=1;
}
}
/* STEP SIZE IS ZERO TO THE EXTENT OF EPSNORM */
else if (ii==n)
{ stepvanish+=1;
/*if (stepvanish>=5) */
if (stepvanish>=20)
{ if (dispwarn) { Rprintf(termwarn1); Rprintf("step size 0\n"); Rprintf(endwarn4); }
options[8]=-14.; goto endrun;
}
else
{ for(i=0;i<n;i++) x[i]=x1[i];
f=f1; hp*=ten; ksm=1;
}
}
/* USE A SMALLER STEP: */
else if (h1*f<h1*pow(gamma,dd)*f1)
{ if (ksm) break;
k2+=1; k1=0; hp/=dq; for (i=0;i<n;i++) x[i]=x1[i]; f=f1;
if (kc>=mxtc) break;
}
/* 1-D OPTIMIZER IS LEFT BEHIND */
else
{ if (h1*f<=h1*f1) break;
/* USE LARGER STEP */
k1+=1; if (k2>0) kc+=1; k2=0;
if (k1>=20) hp*=du20;
else if (k1>=10) hp*=du10;
else if (k1>= 3) hp*=du03;
}
}
/* ------------------------ End of 1-D search ------------------ */
/* ADJUST THE TRIAL STEP SIZE : */
dx=zero; for (i=0;i<n;i++) dx+=(xopt[i]-x[i])*(xopt[i]-x[i]); dx=sqrt(dx);
if (kg<kstore) kg+=1;
if (kg>=2) for (i=kg-1;i>0;i--) nsteps[i]=nsteps[i-1];
d=zero; for (i=0;i<n;i++) d+=g0[i]*g0[i]; d=sqrt(d);
nsteps[0]=dx/(fabs(h)*d);
kk=zero; d=zero;
for (i=1;i<=kg;i++) { dd=kg-i+1; d+=dd; kk+=nsteps[i-1]*dd; }
kk/=d;
if (kk>des)
{ if (kg==1) h*=kk-des+one;
else h*=sqrt(kk-des+one);
}
else if (kk<des) h*=sqrt(kk/des);
if (ksm) stepvanish+=1;
/* COMPUTE THE GRADIENT : */
grad(x,g); options[10]+=one;
ng=zero; for(i=0;i<n;i++) ng+=g[i]*g[i]; ng=sqrt(ng);
if (ng>=infty)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error42); }
options[8]=-four; goto endrun;
}
else if (ng<ZeroGrad)
{ if (dispwarn) { Rprintf(wrnmes); Rprintf(warn1); }
ng=ZeroGrad;
}
/* new record */
if (h1*f>h1*frec)
{ frec=f; for(i=0;i<n;i++) { xrec[i]=x[i]; grec[i]=g[i]; }
}
/* average gradient norm */
if (ng>ZeroGrad)
{ if (knorms<10) knorms+=1;
if (knorms>=2) { for(i=knorms-1;i>0;i--) gnorms[i]=gnorms[i-1]; }
gnorms[0]=ng;
nng=one; for(i=0;i<knorms;i++) nng*=gnorms[i];
nng=pow(nng,one/knorms);
}
/* Norm of X: */
nx=zero; for(i=0;i<n;i++) nx+=x[i]*x[i]; nx=sqrt(nx);
/*-----------------------------------------------------------------
DISPLAY THE CURRENT VALUES: */
if (k==ld)
{ Rprintf (
"\nIter # ... Function Val ... Step Value ... Integral ... Grad Norm"
"\n%6i %12.6g %12.6g %12.6g %12.5g",k,f,dx,integral,ng);
ld+=dispdata;
}
/*-----------------------------------------------------------------
CHECK THE STOPPING CRITERIA: */
termflag=1;
if(kcheck<=5 || (kcheck<=12 && ng>one)) { termflag=0;}
if(kc>=mxtc || knan) { termflag=0;}
if(fabs(integral-1) >= integraltol) termflag = 0; /* ARGUMENT : */
if (termflag)
{
ii=0; stopping=1;
for(i=0;i<n;i++)
{ if (fabs(x[i])>=lowxbound)
{ idx[ii]=i; ii+=1;
if (fabs(xopt[i]-x[i])>options[1]*fabs(x[i])) { stopping=0;}
}
}
if (ii==0 || stopping)
{ stopping=1; termx+=1;
d=zero; for(i=0;i<n;i++) d+=(x[i]-xrec[i])*(x[i]-xrec[i]); d=sqrt(d);
/* FUNCTION : */
if(fabs(f-frec)>detfr*fabs(f) &&
fabs(f-fopt)>=options[2]*fabs(f) &&
krerun<=3)
{
stopping=0;
if (ii>0)
{ for(i=0;i<ii;i++)
{ j=idx[i];
if (fabs(xrec[j]-x[j])>detxr*fabs(x[j]))
{ stopping=1; break;
}
}
}
if (stopping)
{
if (dispwarn) { Rprintf(wrnmes); Rprintf(warn09); }
ng=zero;
for(i=0;i<n;i++)
{ x[i]=xrec[i]; g[i]=grec[i]; ng+=g[i]*g[i];
}
ng=sqrt(ng);
f=frec; krerun+=1;
h=h1*max(dx,detxr*nx)/krerun;
warnno=2; endwarn=endwarn2; break;
}
else {h*=ten; }
}
else if(/*fabs(integral - 1.0) <= integraltol &&*/ (fabs(f-fopt)<=options[2]*fabs(f) ||
fabs(f)<=lowfbound ||
(fabs(f-fopt)<=options[2] && termx>=limxterm )))
{ if (stopf)
{ if (dx<=laststep)
{ if (warnno==1 && ng<sqrt(options[2])) warnno=0;
if (!app)
{ for(i=0;i<n;i++)
{ if (fabs(g[i])<=epsnorm2)
{ warnno=3; endwarn=endwarn3; break;
}
}
}
// ending
if (warnno!=0)
{ options[8]=-warnno-ten;
if (dispwarn)
{ Rprintf(termwarn1); Rprintf(endwarn);
}
}
else { options[8]=k; if (dispwarn) Rprintf(termwarn0); }
goto endrun;
}
}
else stopf=1;
}
else if (dx<powerm12*max(nx,one) && termx>=limxterm )
{ options[8]=-14.;
if (dispwarn)
{ Rprintf(termwarn1); Rprintf(endwarn4);
}
f=frec; for(i=0;i<n;i++) x[i]=xrec[i];
goto endrun;
}
} /* stopping */
} /* termflag */
/* ITERATIONS LIMIT */
if (k==iterlimit)
{ options[8]=-nine;
if (dispwarn) { Rprintf(wrnmes); Rprintf(warn4); }
goto endrun;
}
/* ------------ end of the check ---------------- */
/* ZERO GRADIENT : */
{ if (ng<=ZeroGrad)
{ nzero+=1;
if (dispwarn) { Rprintf(wrnmes); Rprintf(warn1); }
if (nzero>=3) { options[8]=-eight; goto endrun; }
for(i=0;i<n;i++) g0[i]*=-h/two;
for(i=1;i<=10;i++)
{ for(j=0;j<n;j++) x[j]+=g0[j];
f=fun(x);
integral=f+dotprod(x,weights,truepoints);
options[9]+=one;
if (fabs(f)>=infty)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error32); }
options[8]=-three; goto endrun;
}
grad(x,g); options[10]+=one;
ng=zero; for(j=0;j<n;j++) ng+=g[j]*g[j]; ng=sqrt(ng);
if (ng>=infty)
{ if (dispwarn) { Rprintf(errmes); Rprintf(error42); }
options[8]=-four; goto endrun;
}
if (ng>ZeroGrad) break;
}
if (ng<=ZeroGrad)
{ if (dispwarn) { Rprintf(termwarn1); Rprintf(warn1); }
options[8]=-eight; goto endrun;
}
h=h1*dx; break;
}
}
/* FUNCTION IS FLAT AT THE POINT : */
if (fabs(f-fopt)<fabs(fopt)*options[2] &&
kcheck>5 && ng<one )
{ ni=0;
for(i=0;i<n;i++) { if (fabs(g[i])<=epsnorm2) { idx[ni]=i; ni+=1; } }
if (ni>=1 && ni<=n/2 && kflat<=3)
{ kflat+=1;
if (dispwarn) { Rprintf(wrnmes); Rprintf(warn31); }
warnno=1; endwarn=endwarn1;
for(i=0;i<n;i++) x1[i]=x[i]; fm=f;
for(i=0;i<ni;i++)
{ j=idx[i]; f2=fm; y=x[j];
if (y==zero) x1[j]=one;
else if (fabs(y)<one)
{ if (y<0) x1[j]=-one; else x1[j]=one;
}
else x1[j]=y;
for(ip=1;ip<=20;i++)
{ x1[j]/=1.15; f1=fun(x1); options[9]+=one;
if (fabs(f1)<infty)
{ if (h1*f1>h1*fm) { y=x1[j]; fm=f1; }
else if (h1*f2>h1*f1) break;
else if (f2==f1) x1[j]/=1.5;
f2=f1;
}
} x1[j]=y;
}
if (h1*fm>h1*f)
{
grad(x1,gt); options[10]+=one;
ngt=zero; for(i=0;i<n;i++) ngt+=gt[i]*gt[i];
if (ngt>epsnorm2 || ngt<infty)
{ if (dispwarn) Rprintf(warn32);
for(i=0;i<n;i++) { x[i]=x1[i]; g[i]=gt[i]; }
ng=ngt; f=fm; h=h1*dx/three; options[2]/=five; break;
} /* regular gradient */
} /* a better value has been found */
} /* function is flat */
} /* pre-conditions are fulfilled */
} /* end of the iteration cycle */
} /* end of the resetting cycle */
endrun:
/* do a subgradient if required */
/* if (options[0] !=0)
{
for (i=1; i<options[0]; i++)
{
subgradeffw(x,g);
ng=zero; for(j=0;j<n;j++) ng+=g[j]*g[j]; ng=sqrt(ng);
for (j=0; j<n; j++)
{
x[j]+=h*g[j]/(i*ng);
}
}
}*/
/* deallocate working arrays: */
free(idx); free(deltax); free(xx); free(grec); free(xrec); free(xopt);
free(x1); free(z); free(gc); free(gt); free(g1); free(g0); free(g);
free(B);
return(f);
}
int solver(int m,int n,int nz,int *iA, int *kA,
double *A, double *b, double *c, double f,
double *x, double *y, double *w, double *z)
{
double *dx, *dw, *dy, *dz; /* step directions */
double *fx, *fy, *gx, *gy;
double phi, psi, dphi, dpsi;
double *rho, *sigma, normr, norms; /* infeasibilites */
double *D, *E; /* diagonal matrices */
double gamma, beta, delta, mu, theta; /* parameters */
double *At; /* arrays for A^T */
int *iAt, *kAt;
int i,j,iter,v=1,status=5;
double primal_obj, dual_obj;
/*******************************************************************
* Allocate memory for arrays.
*******************************************************************/
MALLOC( dx, n, double );
MALLOC( dw, m, double );
MALLOC( dy, m, double );
MALLOC( dz, n, double );
MALLOC( rho, m, double );
MALLOC( sigma, n, double );
MALLOC( D, n, double );
MALLOC( E, m, double );
MALLOC( fx, n, double );
MALLOC( fy, m, double );
MALLOC( gx, n, double );
MALLOC( gy, m, double );
MALLOC( At, nz, double );
MALLOC( iAt, nz, int );
MALLOC( kAt, m+1, int );
/****************************************************************
* Initialization. *
****************************************************************/
for (j=0; j<n; j++) {
x[j] = 1.0;
z[j] = 1.0;
}
for (i=0; i<m; i++) {
w[i] = 1.0;
y[i] = 1.0;
}
phi = 1.0;
psi = 1.0;
atnum(m,n,kA,iA,A,kAt,iAt,At);
/****************************************************************
* Display Banner.
****************************************************************/
printf ("m = %d,n = %d,nz = %d\n",m,n,nz);
printf(
"--------------------------------------------------------------------------\n"
" | Primal | Dual | |\n"
" Iter | Obj Value Infeas | Obj Value Infeas | mu |\n"
"- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \n"
);
fflush(stdout);
/****************************************************************
* Iteration.
****************************************************************/
beta = 0.80;
delta = 2*(1-beta);
for (iter=0; iter<MAX_ITER; iter++) {
/*************************************************************
* STEP 1: Compute mu.
*************************************************************/
mu = (dotprod(z,x,n)+dotprod(w,y,m)+phi*psi) / (n+m+1);
/*************************************************************
* STEP 1: Compute primal and dual objective function values.
*************************************************************/
primal_obj = dotprod(c,x,n);
dual_obj = dotprod(b,y,m);
/*************************************************************
* STEP 2: Check stopping rule.
*************************************************************/
if ( mu < EPS ) {
if ( phi > EPS ) {
status = 0;
break; /* OPTIMAL */
}
else
if ( dual_obj < 0.0) {
status = 2;
break; /* PRIMAL INFEASIBLE */
}
else
if ( primal_obj > 0.0) {
status = 4;
break; /* DUAL INFEASIBLE */
}
else
{
status = 7; /* NUMERICAL PROBLEM */
break;
}
}
/*************************************************************
* STEP 3: Compute infeasibilities.
*************************************************************/
smx(m,n,A,kA,iA,x,rho);
for (i=0; i<m; i++) {
rho[i] = rho[i] - b[i]*phi + w[i];
}
normr = sqrt( dotprod(rho,rho,m) )/phi;
for (i=0; i<m; i++) {
rho[i] = -(1-delta)*rho[i] + w[i] - delta*mu/y[i];
}
smx(n,m,At,kAt,iAt,y,sigma);
for (j=0; j<n; j++) {
sigma[j] = -sigma[j] + c[j]*phi + z[j];
}
norms = sqrt( dotprod(sigma,sigma,n) )/phi;
for (j=0; j<n; j++) {
sigma[j] = -(1-delta)*sigma[j] + z[j] - delta*mu/x[j];
}
gamma = -(1-delta)*(dual_obj - primal_obj + psi) + psi - delta*mu/phi;
/*************************************************************
* Print statistics.
*************************************************************/
printf("%8d %14.7e %8.1e %14.7e %8.1e %8.1e \n",
iter, high(primal_obj/phi+f), high(normr),
high(dual_obj/phi+f), high(norms), high(mu) );
fflush(stdout);
/*************************************************************
* STEP 4: Compute step directions.
*************************************************************/
for (j=0; j<n; j++) { D[j] = z[j]/x[j]; }
for (i=0; i<m; i++) { E[i] = w[i]/y[i]; }
ldltfac(n, m, kAt, iAt, At, E, D, kA, iA, A, v);
for (j=0; j<n; j++) { fx[j] = -sigma[j]; }
for (i=0; i<m; i++) { fy[i] = rho[i]; }
forwardbackward(E, D, fy, fx);
for (j=0; j<n; j++) { gx[j] = -c[j]; }
for (i=0; i<m; i++) { gy[i] = -b[i]; }
forwardbackward(E, D, gy, gx);
dphi = (dotprod(c,fx,n)-dotprod(b,fy,m)+gamma)/
(dotprod(c,gx,n)-dotprod(b,gy,m)-psi/phi);
for (j=0; j<n; j++) { dx[j] = fx[j] - gx[j]*dphi; }
for (i=0; i<m; i++) { dy[i] = fy[i] - gy[i]*dphi; }
for (j=0; j<n; j++) { dz[j] = delta*mu/x[j] - z[j] - D[j]*dx[j]; }
for (i=0; i<m; i++) { dw[i] = delta*mu/y[i] - w[i] - E[i]*dy[i]; }
dpsi = delta*mu/phi - psi - (psi/phi)*dphi;
/*************************************************************
* STEP 5: Compute step length.
*************************************************************/
theta = 1.0;
for (j=0; j<n; j++) {
theta
=
MIN(theta, linesearch(x[j],z[j],dx[j],dz[j],beta,delta,mu));
}
for (i=0; i<m; i++) {
theta
=
MIN(theta,linesearch(y[i],w[i],dy[i],dw[i],beta,delta,mu));
}
theta = MIN(theta,linesearch(phi,psi,dphi,dpsi,beta,delta,mu));
/*
if (theta < 4*beta/(n+m+1)) {
printf("ratio = %10.3e \n", theta*(n+m+1)/(4*beta));
status = 7;
break;
}
*/
if (theta < 1.0) theta *= 0.9999;
/*************************************************************
* STEP 6: Step to new point
*************************************************************/
for (j=0; j<n; j++) {
x[j] = x[j] + theta*dx[j];
z[j] = z[j] + theta*dz[j];
}
for (i=0; i<m; i++) {
y[i] = y[i] + theta*dy[i];
w[i] = w[i] + theta*dw[i];
}
phi = phi + theta*dphi;
psi = psi + theta*dpsi;
}
for (j=0; j<n; j++) {
x[j] /= phi;
z[j] /= phi;
}
for (i=0; i<m; i++) {
y[i] /= phi;
w[i] /= phi;
}
/****************************************************************
* Free work space *
****************************************************************/
FREE( w );
FREE( z );
FREE( dx );
FREE( dw );
FREE( dy );
FREE( dz );
FREE( rho );
FREE( sigma );
FREE( D );
FREE( E );
FREE( fx );
FREE( fy );
FREE( gx );
FREE( gy );
FREE( At );
FREE( iAt );
FREE( kAt );
return status;
} /* End of solver */
