static void solve_velocity_interior(Element_List **V, Bsystem **Vbsys){ register int i; int N,nbl,rows,id,info; int *bw = Vbsys[0]->Gmat->bwidth_c; int eDIM = V[0]->flist[0]->dim(); double *hj; int **ipiv = Vbsys[0]->Gmat->cipiv; double **invc = Vbsys[0]->Gmat-> invc; double **binvc = Vbsys[0]->Gmat->binvc; double **invcd = Vbsys[0]->Gmat->invcd; double ***dbinvc = Vbsys[0]->Gmat->dbinvc; Element *E,*P; for(i = 0; i < eDIM; ++i) for(E=V[i]->fhead;E;E=E->next){ N = E->Nmodes - E->Nbmodes; E->state = 't'; if(!N) continue; id = E->geom->id; nbl = E->Nbmodes; hj = E->vert->hj + nbl; P = V[eDIM]->flist[E->id]; rows = P->Nmodes; if(Vbsys[0]->lambda->wave){ /* dbinvc only store dbi in this formulation */ dgemv('N', N, rows, -1., dbinvc[i][id], N, P->vert->hj, 1, 1.0,hj,1); dgetrs('T',N,rows,invc[id],N,ipiv[id],dbinvc[i][id],N,info); if(N > 3*bw[id]) dgbtrs('N', N, bw[id]-1,bw[id]-1, 1, invc[id], 3*bw[id]-2, ipiv[id], hj, N, info); else dgetrs('N', N, 1, invc[id], N, ipiv[id], hj, N, info); dgemv('N', N, nbl , -1., invcd [id], N, E->vert->hj, 1, 1.0,hj,1); } else{ if(N > 2*bw[id]) dpbtrs('L', N, bw[id]-1, 1, invc[id], bw[id], hj, N, info); else dpptrs('L', N, 1, invc[id], hj, N, info); dgemv('N', N, nbl , -1., binvc [id], N, E->vert->hj, 1, 1.0,hj,1); dgemv('N', N, rows, -1., dbinvc[i][id], N, P->vert->hj, 1, 1.0,hj,1); } } }
static void solve_pressure(Element_List **V, double *rhs, Bsystem **Vbsys) { register int i; int eDIM = V[0]->flist[0]->dim(); int info,N,nbl,nblv,id,*bmap; Bsystem *B = Vbsys[0], *PB = Vbsys[eDIM]; Element *E; double *hj,*tmp; double *sc = B->signchange; if(eDIM == 2) tmp = dvector(0,8*LGmax); else tmp = dvector(0,18*(LGmax-1)*(LGmax-1)); /* back solve for pressure */ for(E=V[eDIM]->fhead;E;E=E->next) { N = E->Nmodes - 1; id = E->geom->id; hj = E->vert->hj + 1; E->state = 't'; /* solve interior and negate */ if(PB->lambda->wave) { dgetrs('N', N, 1, PB->Gmat->invc[id], N, PB->Gmat->cipiv[id],hj, N, info); } else { dpptrs('L', N, 1, PB->Gmat->invc[id], hj, N, info); dneg(N,hj,1); } bmap = PB->bmap[E->id]; nblv = V[0]->flist[E->id]->Nbmodes; nbl = eDIM*nblv+1; for(i = 0; i < nbl; ++i) tmp[i] = rhs[bmap[i]]; for(i = 0; i < eDIM; ++i) dvmul(nblv,sc,1,tmp+nblv*i,1,tmp+nblv*i,1); if(PB->lambda->wave) dgemv('N', N, nbl,-1.0, PB->Gmat->invcd[id], N, tmp, 1, 1.,hj,1); else dgemv('N', N, nbl,-1.0, PB->Gmat->binvc[id], N, tmp, 1, 1.,hj,1); sc += nblv; } free(tmp); }
/** Compute y = alpha*A*x + beta*y or y = alpha*A'*x + beta*y, * depending whether "trans" is 111 or 112. * Matrix "A" must be dense. The value of "order" is 101 if A is * in row major order, or 102 if in column major order. * (Values for "order" and "trans" correspond to Intel MKL definitions.) */ void KNITRO_EXPORT KTR_dgemv (const char order, const char trans, const int m, const int n, const double alpha, const double * const A, const int lda, const double * const x, const int incx, const double beta, double * const y, const int incy) { char codeTranspose; double * newA; int i, j, k; if ((n <= 0) || (m <= 0)) return; if (trans == 111) codeTranspose = 'N'; else if (trans == 112) codeTranspose = 'T'; else /*---- NO WAY TO REPORT THIS ERROR. */ return; if (order == 102) { /*---- A IS IN COLUMN MAJOR ORDER, AS EXPECTED BY ACML. */ dgemv (codeTranspose, m, n, alpha, A, lda, x, incx, beta, y, incy); return; } else if (order != 101) /*---- NO WAY TO REPORT THIS ERROR. */ return; /*---- MUST CHANGE A FROM ROW MAJOR ORDER TO COLUMN MAJOR. */ newA = (double *) malloc (m * lda * sizeof (double)); k = 0; for (j = 0; j < n; j++) for (i = 0; i < m; i++) { newA[k] = A[(lda * i) + j]; k++; } dgemv (codeTranspose, m, n, alpha, newA, m, x, incx, beta, y, incy); free (newA); return; }
double* cmblkl_select_cls(cmblkl *llkl,lklbs* self) { double* cls; int cli,off,lff,l; double one; double *wl, *wl0; int inc; int *offset_cl,*ell; int ndim,xdim; one=1; if (llkl->wl==NULL) { wl0 = &one; inc = 0; } else { wl0 = llkl->wl; inc = 1; } offset_cl = llkl->offset_cl; ndim = llkl->ndim; xdim = llkl->xdim; ell = llkl->ell; if (ndim == self->tot_cl && llkl->wl==NULL && xdim==0 && llkl->unit==1) { cls = self->cl_theo; } else { cls = self->cl_select; for(cli=0;cli<6;cli++) { off = offset_cl[cli]; lff = self->offset_lmax[cli]; wl = wl0; if (off!=-1) { for(l=0;l<llkl->nell;l++) { cls[off+l] = self->cl_theo[lff+ell[l]] * (*wl) * llkl->unit; //_DEBUGHERE_("off %d l %d cl_t[%d]=%g cl_u[%d]=%g wl=%g",off,l,llkl->ell[l],self->cl_theo[lff+llkl->ell[l]],cls[off+l],*wl) wl += inc; } } } } if (llkl->bins!=NULL) { char trans; int npar; double done,dzero; int one; trans='T'; npar = llkl->nbins; one = 1; done = 1.; dzero = 0; llkl->pls[0]=10; //_DEBUGHERE_("cls[0]=%g pls[0]=%g bins[0]=%g",cls[0],llkl->pls[0],llkl->bins[0]); dgemv(&trans, &ndim, &npar, &done, llkl->bins, &ndim, cls, &one, &dzero, llkl->pls, &one); //_DEBUGHERE_("cls[0]=%g pls[0]=%g ",cls[0],llkl->pls[0]); return llkl->pls; } return cls; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { if (nlhs != 1 || nrhs != 6) { mexErrMsgTxt("mxMultATx: wrong I/O.\n" "Usage: \n" "y = mxMultATx(alpha, A, m, n, x, beta);\n" "y := alpha A' * x + beta * y \n" "A [m x n (double)] \n" "x [m x 1 (double)] \n"); } char *chn = "N"; char *cht = "T"; double alpha = mxGetScalar(prhs[0]); double* A = (double*)mxGetData(prhs[1]); double m = mxGetScalar(prhs[2]); long int M = (int)m; double n = mxGetScalar(prhs[3]); long int N = (int)n; double* x = (double*)mxGetData(prhs[4]); double beta = mxGetScalar(prhs[5]); long int lda = M; long int incx = 1; long int incy = 1; plhs[0] = mxCreateNumericMatrix(N, 1, mxDOUBLE_CLASS, mxREAL); double* y = (double*)mxGetData(plhs[0]); dgemv(cht, &M, &N, &alpha, A, &lda, x, &incx, &beta, y, &incy); }
/* compute the slope vector dy for the transient equation * dy + cy = p. useful in the transient solver */ void slope_fn_block(block_model_t *model, double *y, double *p, double *dy) { /* shortcuts */ int n = model->n_nodes; double **c = model->c; /* for our equation, dy = p - cy */ #if (MATHACCEL == MA_INTEL || MATHACCEL == MA_APPLE) /* dy = p */ cblas_dcopy(n, p, 1, dy, 1); /* dy = dy - c*y = p - c*y */ cblas_dgemv(CblasRowMajor, CblasNoTrans, n, n, -1, c[0], n, y, 1, 1, dy, 1); #elif (MATHACCEL == MA_AMD || MATHACCEL == MA_SUN) /* dy = p */ dcopy(n, p, 1, dy, 1); /* dy = dy - c*y = p - c*y */ dgemv('T', n, n, -1, c[0], n, y, 1, 1, dy, 1); #else int i; double *t = dvector(n); matvectmult(t, c, y, n); for (i = 0; i < n; i++) dy[i] = p[i]-t[i]; free_dvector(t); #endif }
void MxVdispl(Vector y, const Matrix A, const Vector x, double alpha, double beta, int ydispl) { char trans='N'; dgemv(&trans, &A->rows, &A->cols, &alpha, A->data[0], &A->rows, x->data, &x->stride, &beta, y->data+ydispl*y->stride, &y->stride); }
void patternRecognitionPCA::reconstructFromPCA(double *& newRedSample, double *& outputSample, int pcaDim) { if(pcaDim > outputDim) return; else if(pcaDim == outputDim) return reconstructFromPCA(newRedSample, outputSample); else { int i, j; char transY = 'T'; ptrdiff_t inc = 1; ptrdiff_t m = (ptrdiff_t) pcaDim; ptrdiff_t n = (ptrdiff_t) inputDim; double alpha = 1.0; double beta = 0.0; double * smallReductionMatrix = new double[inputDim * pcaDim]; for(i = 0; i < inputDim; i++) { for(j = 0; j < pcaDim; j++) smallReductionMatrix[i*pcaDim + j] = reductionMatrix[i*outputDim + j]; } //Use the transposed reduction matrix here: //size ( 1 + ( inputDim- 1 ) ) dgemv(&transY, &m, &n, &alpha, smallReductionMatrix, &m, newRedSample, &inc, &beta, outputSample, &inc); for(i = 0; i < inputDim; i++) outputSample[i] = mean[i] + outputSample[i]; // printf(" smallReductionMatrix%f\n", smallReductionMatrix[19347]); // printf(" newRedSample%f\n", newRedSample[pcaDim]); delete [] smallReductionMatrix; } }
void MxV(Vector u, Matrix A, Vector v) { char trans='N'; double onef=1.0; double zerof=0.0; int one=1; dgemv(&trans, &A->rows, &A->cols, &onef, A->data[0], &A->rows, v->data, &one, &zerof, u->data, &one); }
void cblas_dgemv(enum CBLAS_ORDER Order,enum CBLAS_TRANSPOSE transA, mwSignedIndex M, mwSignedIndex N, double alpha, double *A, mwSignedIndex lda, double *B, mwSignedIndex incB, double beta, double *C, mwSignedIndex incC) { char ta[1]; if (transA==111) { *ta='N'; } else { *ta='T'; }; dgemv(ta,&M,&N,&alpha,A,&lda,B,&incB,&beta,C,&incC); }
int parallel_dgemv(int *BLACS_PARAMS, double **M_local,double *V, double *ANS, int first_row, int first_col, int *displs, int *rcvcnt){ double *V_loc, *ANS_loc; V_loc = &V[first_col]; ANS_loc = &ANS[first_row]; dgemv('N',BLACS_PARAMS[11],BLACS_PARAMS[12],1.0,*M_local,BLACS_PARAMS[11],V_loc,1,0.0,ANS_loc,1); //from this point V is not needed - use it as temporary array V_loc = &V[first_row]; //for temorary use MPI_Allreduce(ANS_loc,V_loc,BLACS_PARAMS[11],MPI_DOUBLE,MPI_SUM,MPI_COMM_ROW_NEW); return 0; }
static vl::Error gemv(vl::Context& context, char op, ptrdiff_t m, ptrdiff_t n, type alpha, type const * a, ptrdiff_t lda, type const * x, ptrdiff_t incx, type beta, type * y, ptrdiff_t incy) { dgemv(&op, &m, &n, &alpha, (type*)a, &lda, (type*)x, &incx, &beta, y, &incy) ; return vl::vlSuccess ; }
/************************************ Given the factorization LB = U for some B, solve the problem Bx = vec for x Solve using LAPACK functions. ************************************/ void LU_Solve1(PT_Matrix pL, PT_Matrix pUt, double *vec, double *x, ptrdiff_t *info) { ptrdiff_t n, incx=1; char U='U', N='N',T='T'; double alpha=1.0, beta=0.0; n = Matrix_Rows(pL); /* solve using lapack */ /* compute x = L*vec */ dgemv(&T, &n, &n, &alpha, pL->A, &(pL->rows_alloc), vec, &incx, &beta, x, &incx); /* solve U*xnew = x using lapack function that also checks for singularity */ dtrtrs(&U, &N, &N, &n, &incx, pUt->A, &(pUt->rows_alloc), x, &n, info); /* printf("my x=\n"); */ /* Vector_Print_raw(x,n); */ }
void patternRecognitionPCA::reconstructFromPCA(double *& newRedSample, double *& outputSample) { int i; char transY = 'T'; ptrdiff_t inc = 1; ptrdiff_t m = (ptrdiff_t) outputDim; ptrdiff_t n = (ptrdiff_t) inputDim; double alpha = 1.0; double beta = 0.0; //Use the transposed reduction matrix here: // MONICA-y := alpha*reductionMatrix'*newRedSample + beta*y dgemv(&transY, &m, &n, &alpha, reductionMatrix, &m, newRedSample, &inc, &beta, outputSample, &inc); for(i = 0; i < inputDim; i++) outputSample[i] = mean[i] + outputSample[i]; }
void patternRecognitionPCA::projectToPCA(double *& newSample, double *& reducedSample) { int i; char transN = 'N'; ptrdiff_t n = (ptrdiff_t) inputDim; ptrdiff_t m = (ptrdiff_t) outputDim; double alpha = 1.0; ptrdiff_t incx = 1; double beta = 0.0; double * modifiedSample = new double[inputDim]; for(i = 0; i < inputDim; i++) modifiedSample[i] = newSample[i] - mean[i]; //Use the reduction matrix to add a sample to the PCA space: dgemv(&transN, &m, &n, &alpha, reductionMatrix, &m, modifiedSample, &incx, &beta, reducedSample, &incx); delete [] modifiedSample; }
void MxV(Vector y, const Matrix A, const Vector x, double alpha, double beta) { char trans='N'; int one=1; #ifdef HAVE_MPI Vector temp = createVector(A->rows); copyVector(y, x); #else Vector temp=y; #endif dgemv(&trans, &A->rows, &A->cols, &alpha, A->data[0], &A->rows, x->data, &x->stride, &beta, temp->data, &one); #ifdef HAVE_MPI for (int i=0;i<x->comm_size;++i) { MPI_Reduce(temp->data+x->displ[i], y->data, x->sizes[i], MPI_DOUBLE, MPI_SUM, i, *x->comm); } freeVector(temp); #endif }
END_TEST START_TEST( using_MKL_DGEMV ) { // y:= alpha*a*x + beta*y PetscErrorCode ierr; char trans = 'n'; int m = 3, n = 4; double alpha = one; double *a; PetscMalloc( m*n*sizeof(double), &a); for( int i = 0; i < m*n; i++) a[i] = i+1; int lda = m; double *x; PetscMalloc( n*sizeof(double), &x); // for(int i = 0; i < n; i++) x[0] = one; x[1] = zero; x[2] = zero; x[3] = zero; int incx = 1; double beta = zero; double *y; PetscMalloc( n*sizeof(double), &y); for(int i = 0; i < n; i++) y[i] = zero; int incy = 1; dgemv(&trans,&m,&n,&alpha,a,&lda,x,&incx,&beta,y,&incy); for( int i = 0; i < n; i++ ) printf("y: %f\n", y[i]); PetscFree(a); PetscFree(x); PetscFree(y); }
// ---------------------------------------------------------------------- int Dense3d::evaluate(const DblNumVec& srcDen, DblNumVec& trgVal) { //----------------------------------- iA(srcDen.m()==srcDOF()*(*_srcPos).n()); iA(trgVal.m()==trgDOF()*(*_trgPos).n()); int dim = this->dim(); int srcDOF = this->srcDOF(); int trgDOF = this->trgDOF(); /* Number of sources */ int numSrc = (*_srcPos).n(); /* Number of targets */ int numTrg = (*_trgPos).n(); DblNumMat inter(trgDOF, numSrc*srcDOF); for(int i=0; i<numTrg; i++) { DblNumMat onePosMat(dim, 1, false, (*_trgPos).clmdata(i)); DblNumVec oneValVec(trgDOF, false, trgVal.data()+trgDOF*i); iC( _knl.kernel((*_srcPos), (*_srcNor), onePosMat, inter) ); iC( dgemv(1.0, inter, srcDen, 0.0, oneValVec) ); } return 0; }
void patternRecognitionPCA::projectToPCA(double *& newSample, double *& reducedSample, int pcaDim) { if(pcaDim > outputDim) return; else if(pcaDim == outputDim) return projectToPCA(newSample, reducedSample); else { int i, j; double * modifiedSample = new double[inputDim]; char transN = 'N'; ptrdiff_t n = (ptrdiff_t) inputDim; ptrdiff_t m = (ptrdiff_t) pcaDim; double alpha = 1.0; ptrdiff_t incx = 1; double beta = 0.0; double * smallReductionMatrix = new double[inputDim * pcaDim]; for(i = 0; i < inputDim; i++) { for(j = 0; j < pcaDim; j++) smallReductionMatrix[i*pcaDim + j] = reductionMatrix[i*outputDim + j]; } for(i = 0; i < inputDim; i++) modifiedSample[i] = newSample[i] - mean[i]; //Use the reduction matrix to add a sample to the PCA space: //y := alpha*smallReductionMatrix*inc + beta*y. dgemv(&transN, &m, &n, &alpha, smallReductionMatrix, &m, modifiedSample, &incx, &beta, reducedSample, &incx); delete [] modifiedSample; delete [] smallReductionMatrix; } }
void vHRedLinearLogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps+1; double Fitparams[numfit]; double *EFAC; double EQUAD, redamp, redalpha; int pcount=0; // printf("here1\n"); for(int p=0;p<ndim;p++){ // printf("param %i %g %g\n",p,((MNStruct *)context)->Dpriors[p][0],((MNStruct *)context)->Dpriors[p][1]); Cube[p]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[p]+((MNStruct *)context)->Dpriors[p][0]; } // printf("here1.5\n"); for(int p=0;p < numfit; p++){ Fitparams[p]=Cube[p]; pcount++; // printf("param: %i %g \n",p,Fitparams[p]); } if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double[1]; EFAC[0]=1; // } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double[1]; EFAC[0]=Cube[pcount]; pcount++; } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double[((MNStruct *)context)->numFitEFAC]; for(int p=0;p< ((MNStruct *)context)->numFitEFAC; p++){ EFAC[p]=Cube[pcount]; pcount++; } } if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=0; // printf("EQUAD: %g \n",EQUAD); } else{ EQUAD=pow(10.0,2*Cube[pcount]); pcount++; // printf("E: %g %g \n",EQUAD,EFAC[0]); } redamp=Cube[pcount]; pcount++; redalpha=Cube[pcount]; pcount++; double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; double *Diffvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Diffvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } double secday=24*60*60; double LongestPeriod=1.0/pow(10.0,-5); double flo=1.0/LongestPeriod; double modelalpha=redalpha; double gwamp=pow(10.0,redamp); double gwampsquared=gwamp*gwamp*(pow((365.25*secday),2)/(12*M_PI*M_PI))*(pow(365.25,(1-modelalpha)))/(pow(flo,(modelalpha-1))); double timdiff=0; double covconst=gsl_sf_gamma(1-modelalpha)*sin(0.5*M_PI*modelalpha); // printf("constants: %g %g \n",gwampsquared,covconst); double **CovMatrix = new double*[((MNStruct *)context)->pulse->nobs]; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs;o1++)CovMatrix[o1]=new double[((MNStruct *)context)->pulse->nobs]; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ for(int o2=0;o2<((MNStruct *)context)->pulse->nobs; o2++){ timdiff=((MNStruct *)context)->pulse->obsn[o1].bat-((MNStruct *)context)->pulse->obsn[o2].bat; double tau=2.0*M_PI*fabs(timdiff); double covsum=0; for(int k=0; k <=10; k++){ covsum=covsum+pow(-1.0,k)*(pow(flo*tau,2*k))/(iter_factorial(2*k)*(2*k+1-modelalpha)); } CovMatrix[o1][o2]=gwampsquared*(covconst*pow((flo*tau),(modelalpha-1)) - covsum); // printf("%i %i %g %g %g\n",o1,o2,CovMatrix[o1][o2],fabs(timdiff),covsum); if(o1==o2){ CovMatrix[o1][o2] += pow(((((MNStruct *)context)->pulse->obsn[o1].toaErr)*pow(10.0,-6))*EFAC[((MNStruct *)context)->sysFlags[o1]],2) + EQUAD; } } } double covdet=0; double *WorkDiffvec = new double[((MNStruct *)context)->pulse->nobs]; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ WorkDiffvec[o1]=Diffvec[o1]; } dpotrf(CovMatrix, ((MNStruct *)context)->pulse->nobs, covdet); dpotrs(CovMatrix, WorkDiffvec, ((MNStruct *)context)->pulse->nobs); double Chisq=0; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ Chisq += Diffvec[o1]*WorkDiffvec[o1]; } if(isnan(covdet) || isinf(covdet) || isnan(Chisq) || isinf(Chisq)){ lnew=-pow(10.0,200); // printf("red amp and alpha %g %g\n",redamp,redalpha); // printf("Like: %g %g %g \n",lnew,Chisq,covdet); } else{ lnew = -0.5*(((MNStruct *)context)->pulse->nobs*log(2*M_PI) + covdet + Chisq); // printf("red amp and alpha %g %g\n",redamp,redalpha); } // endClock = clock(); // // printf("Finishing off: time taken = %.2f (s)\n",(endClock-startClock)/(float)CLOCKS_PER_SEC); delete[] EFAC; for(int o=0;o<((MNStruct *)context)->pulse->nobs;o++){delete[] CovMatrix[o];} delete[] CovMatrix; delete[] WorkDiffvec; delete[] Diffvec; delete[] Fitvec; printf("Like: %g %g %g \n",lnew,Chisq,covdet); }
void LRedMarginLinearLogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps+1; double Fitparams[numfit]; double *EFAC; double EQUAD; int pcount=0; // printf("here1\n"); for(int p=0;p<ndim;p++){ // printf("param %i %g %g\n",p,((MNStruct *)context)->Dpriors[p][0],((MNStruct *)context)->Dpriors[p][1]); Cube[p]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[p]+((MNStruct *)context)->Dpriors[p][0]; } // printf("here1.5\n"); for(int p=0;p < numfit; p++){ Fitparams[p]=Cube[p]; pcount++; // printf("param: %i %g \n",p,Fitparams[p]); } if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double[1]; EFAC[0]=1; // } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double[1]; EFAC[0]=Cube[pcount]; pcount++; } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double[((MNStruct *)context)->numFitEFAC]; for(int p=0;p< ((MNStruct *)context)->numFitEFAC; p++){ EFAC[p]=Cube[pcount]; pcount++; } } if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=0; // printf("EQUAD: %g \n",EQUAD); } else{ EQUAD=pow(10.0,2*Cube[pcount]); pcount++; // printf("EQUAD: %g %g %g %i \n",EQUAD,EQUADPrior[0],EQUADPrior[1],((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps + ((MNStruct *)context)->numFitEFAC); } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; double *Diffvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Diffvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } int FitCoeff=2*(((MNStruct *)context)->numFitRedCoeff); double *powercoeff=new double[FitCoeff]; double *Noise=new double[((MNStruct *)context)->pulse->nobs]; double *GDiffvec=new double[((MNStruct *)context)->Gsize]; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Noise[o]=pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))*EFAC[((MNStruct *)context)->sysFlags[o]],2) + EQUAD; } double **NG = new double*[((MNStruct *)context)->pulse->nobs]; for (int k=0; k<((MNStruct *)context)->pulse->nobs; k++) NG[k] = new double[((MNStruct *)context)->Gsize]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ for(int j=0;j<((MNStruct *)context)->Gsize; j++){ NG[i][j]=((MNStruct *)context)->GMatrix[i][j]*Noise[i]; } } double** GG = new double*[((MNStruct *)context)->Gsize]; for (int k=0; k<((MNStruct *)context)->Gsize; k++) GG[k] = new double[((MNStruct *)context)->Gsize]; dgemm(((MNStruct *)context)->GMatrix, NG,GG,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize, 'T','N'); double tdet=0; dpotrf(GG, ((MNStruct *)context)->Gsize, tdet); dpotri(GG,((MNStruct *)context)->Gsize); dgemm(((MNStruct *)context)->GMatrix, GG,NG,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize, ((MNStruct *)context)->Gsize, ((MNStruct *)context)->Gsize, 'N','N'); double **GNG = new double*[((MNStruct *)context)->pulse->nobs]; for (int k=0; k<((MNStruct *)context)->pulse->nobs; k++) GNG[k] = new double[((MNStruct *)context)->pulse->nobs]; dgemm(NG, ((MNStruct *)context)->GMatrix, GNG,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize, ((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize, 'N','T'); double timelike=0; for(int o1=0; o1<((MNStruct *)context)->pulse->nobs; o1++){ for(int o2=0;o2<((MNStruct *)context)->pulse->nobs; o2++){ timelike=timelike+Diffvec[o1]*GNG[o1][o2]*Diffvec[o2]; } } double *NFd = new double[FitCoeff]; double **FMatrix=new double*[((MNStruct *)context)->pulse->nobs]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ FMatrix[i]=new double[FitCoeff]; } double **NF=new double*[((MNStruct *)context)->pulse->nobs]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ NF[i]=new double[FitCoeff]; } double **FNF=new double*[FitCoeff]; for(int i=0;i<FitCoeff;i++){ FNF[i]=new double[FitCoeff]; } double start,end; int go=0; for (int i=0;i<((MNStruct *)context)->pulse->nobs;i++) { if (((MNStruct *)context)->pulse->obsn[i].deleted==0) { if (go==0) { go = 1; start = (double)((MNStruct *)context)->pulse->obsn[i].bat; end = start; } else { if (start > (double)((MNStruct *)context)->pulse->obsn[i].bat) start = (double)((MNStruct *)context)->pulse->obsn[i].bat; if (end < (double)((MNStruct *)context)->pulse->obsn[i].bat) end = (double)((MNStruct *)context)->pulse->obsn[i].bat; } } } // printf("Total time span = %.6f days = %.6f years\n",end-start,(end-start)/365.25); double maxtspan=end-start; double freqdet=0; for (int i=0; i<FitCoeff/2; i++){ int pnum=pcount; double pc=Cube[pcount]; powercoeff[i]=pow(10.0,pc)/(maxtspan*24*60*60);///(365.25*24*60*60)/4; powercoeff[i+FitCoeff/2]=powercoeff[i]; freqdet=freqdet+2*log(powercoeff[i]); pcount++; } int coeffsize=FitCoeff/2; std::vector<double>freqs(FitCoeff/2); for(int i=0;i<FitCoeff/2;i++){ freqs[i]=double(i+1)/maxtspan; } for(int i=0;i<FitCoeff/2;i++){ for(int k=0;k<((MNStruct *)context)->pulse->nobs;k++){ double time=(double)((MNStruct *)context)->pulse->obsn[k].bat; //- (double)((MNStruct *)context)->pulse->param[param_pepoch].val[0] - maxtspan/2; FMatrix[k][i]=cos(2*M_PI*freqs[i]*time); // printf("cos %i %i %g \n",i,k,time); } } for(int i=0;i<FitCoeff/2;i++){ for(int k=0;k<((MNStruct *)context)->pulse->nobs;k++){ double time=(double)((MNStruct *)context)->pulse->obsn[k].bat; //- (double)((MNStruct *)context)->pulse->param[param_pepoch].val[0] - maxtspan/2; FMatrix[k][i+FitCoeff/2]=sin(2*M_PI*freqs[i]*time); // printf("sin %i %i %g \n",i+FitCoeff/2,k,time); } } dgemm(GNG, FMatrix , NF, ((MNStruct *)context)->pulse->nobs,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->pulse->nobs, FitCoeff, 'N', 'N'); dgemm(FMatrix, NF , FNF, ((MNStruct *)context)->pulse->nobs, FitCoeff, ((MNStruct *)context)->pulse->nobs, FitCoeff, 'T', 'N'); dgemv(NF,Diffvec,NFd,((MNStruct *)context)->pulse->nobs,FitCoeff,'T'); double **PPFM=new double*[FitCoeff]; for(int i=0;i<FitCoeff;i++){ PPFM[i]=new double[FitCoeff]; for(int j=0;j<FitCoeff;j++){ PPFM[i][j]=0; } } for(int c1=0; c1<FitCoeff; c1++){ PPFM[c1][c1]=1.0/powercoeff[c1]; } for(int j=0;j<FitCoeff;j++){ for(int k=0;k<FitCoeff;k++){ PPFM[j][k]=PPFM[j][k]+FNF[j][k]; } } double jointdet=0; dpotrf(PPFM, FitCoeff, jointdet); dpotri(PPFM,FitCoeff); double freqlike=0; for(int i=0;i<FitCoeff;i++){ for(int j=0;j<FitCoeff;j++){ // printf("%i %i %g %g\n",i,j,NFd[i],PPFM[i][j]); freqlike=freqlike+NFd[i]*PPFM[i][j]*NFd[j]; } } lnew=-0.5*(tdet+jointdet+freqdet+timelike-freqlike); if(isnan(lnew) || isinf(lnew)){ lnew=-pow(10.0,200); // printf("red amp and alpha %g %g\n",redamp,redalpha); // printf("Like: %g %g %g \n",lnew,Chisq,covdet); } delete[] EFAC; delete[] powercoeff; delete[] NFd; for (int j = 0; j < FitCoeff; j++){ delete[]PPFM[j]; } delete[]PPFM; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[]NF[j]; } delete[]NF; for (int j = 0; j < FitCoeff; j++){ delete[]FNF[j]; } delete[]FNF; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[]FMatrix[j]; } delete[]FMatrix; delete[] Noise; delete[] Diffvec; delete[] Fitvec; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[] NG[j]; } delete[] NG; for (int j = 0; j < ((MNStruct *)context)->Gsize; j++){ delete[]GG[j]; } delete[] GG; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[] GNG[j]; } delete[] GNG; // if(isinf(lnew) || isinf(jointdet) || isinf(tdet) || isinf(freqdet) || isinf(timelike) || isinf(freqlike)){ // printf("Chisq: %g %g %g %g %g %g \n",lnew,jointdet,tdet,freqdet,timelike,freqlike); // } }
void WhiteMarginLinearLogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps+1; double Fitparams[numfit]; double *EFAC; double EQUAD; int pcount=0; // printf("here1\n"); for(int p=0;p<ndim;p++){ // printf("param %i %g %g\n",p,((MNStruct *)context)->Dpriors[p][0],((MNStruct *)context)->Dpriors[p][1]); Cube[p]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[p]+((MNStruct *)context)->Dpriors[p][0]; } // printf("here1.5\n"); for(int p=0;p < numfit; p++){ Fitparams[p]=Cube[p]; pcount++; // printf("param: %i %g \n",p,Fitparams[p]); } // printf("here3\n"); if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double[1]; EFAC[0]=1; // } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double[1]; EFAC[0]=Cube[pcount]; pcount++; } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double[((MNStruct *)context)->numFitEFAC]; for(int p=0;p< ((MNStruct *)context)->numFitEFAC; p++){ EFAC[p]=Cube[pcount]; pcount++; } } // printf("here4\n"); if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=0; // printf("E: %g %g\n",EFAC[0],EQUAD); } else{ EQUAD=pow(10.0,2*Cube[pcount]); pcount++; } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; double *Diffvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Diffvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } double *Noise=new double[((MNStruct *)context)->pulse->nobs]; double *GDiffvec=new double[((MNStruct *)context)->Gsize]; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Noise[o]=pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))*EFAC[((MNStruct *)context)->sysFlags[o]],2) + EQUAD; } double **NG = new double*[((MNStruct *)context)->pulse->nobs]; for (int k=0; k<((MNStruct *)context)->pulse->nobs; k++) NG[k] = new double[((MNStruct *)context)->Gsize]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ for(int j=0;j<((MNStruct *)context)->Gsize; j++){ NG[i][j]=((MNStruct *)context)->GMatrix[i][j]*Noise[i]; } } double** GG = new double*[((MNStruct *)context)->Gsize]; for (int k=0; k<((MNStruct *)context)->Gsize; k++) GG[k] = new double[((MNStruct *)context)->Gsize]; dgemm(((MNStruct *)context)->GMatrix, NG,GG,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize,((MNStruct *)context)->pulse->nobs, ((MNStruct *)context)->Gsize, 'T','N'); dgemv(((MNStruct *)context)->GMatrix,Diffvec,GDiffvec,((MNStruct *)context)->pulse->nobs,((MNStruct *)context)->Gsize,'T'); double detN=0; double *WorkGDiffvec = new double[((MNStruct *)context)->Gsize]; for(int o1=0;o1<((MNStruct *)context)->Gsize; o1++){ WorkGDiffvec[o1]=GDiffvec[o1]; } dpotrf(GG, ((MNStruct *)context)->Gsize, detN); dpotrs(GG, WorkGDiffvec, ((MNStruct *)context)->Gsize); double Chisq=0; for(int o1=0;o1<((MNStruct *)context)->Gsize; o1++){ Chisq += GDiffvec[o1]*WorkGDiffvec[o1]; } if(isnan(detN) || isinf(detN) || isnan(Chisq) || isinf(Chisq)){ lnew=-pow(10.0,200); } else{ lnew = -0.5*(((MNStruct *)context)->pulse->nobs*log(2*M_PI) + detN + Chisq); } //printf("lnew: %g %g %g \n", lnew, detN, Chisq); delete[] EFAC; delete[] Fitvec; delete[] Diffvec; delete[] GDiffvec; delete[] WorkGDiffvec; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[] NG[j]; } delete[] NG; for (int j = 0; j < ((MNStruct *)context)->Gsize; j++){ delete[]GG[j]; } delete[] GG; }
void WhiteLinearLogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps+1; double Fitparams[numfit]; double *EFAC; double EQUAD; int pcount=0; //printf("here1\n"); for(int p=0;p<ndim;p++){ // printf("param %i %g %g\n",p,((MNStruct *)context)->Dpriors[p][0],((MNStruct *)context)->Dpriors[p][1]); Cube[p]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[p]+((MNStruct *)context)->Dpriors[p][0]; } //printf("here1.5\n"); for(int p=0;p < numfit; p++){ Fitparams[p]=Cube[p]; pcount++; // printf("param: %i %g \n",p,Fitparams[p]); } //printf("here3\n"); if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double[1]; EFAC[0]=1; // } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double[1]; EFAC[0]=Cube[pcount]; pcount++; } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double[((MNStruct *)context)->numFitEFAC]; for(int p=0;p< ((MNStruct *)context)->numFitEFAC; p++){ EFAC[p]=Cube[pcount]; pcount++; } } //printf("here4\n"); if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=0; // printf("E: %g %g\n",EFAC[0],EQUAD); } else{ EQUAD=pow(10.0,2*Cube[pcount]); pcount++; } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; double *Diffvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Diffvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } double Chisq=0; double noiseval=0; double detN=0; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ noiseval=pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))*EFAC[((MNStruct *)context)->sysFlags[o]],2) + EQUAD; Chisq += pow((Diffvec[o]),2)/noiseval; detN += log(noiseval); } if(isnan(detN) || isinf(detN) || isnan(Chisq) || isinf(Chisq)){ lnew=-pow(10.0,200); } else{ lnew = -0.5*(((MNStruct *)context)->pulse->nobs*log(2*M_PI) + detN + Chisq); } delete[] EFAC; delete[] Fitvec; delete[] Diffvec; //printf("%g %g %g \n",lnew,Chisq,detN); }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { int k; long i,j,nVars,nSamples,one=1; double gi,Hi,alphaOld,c,d,UB,LB,scaling,scaling2; char trans='N'; mwIndex *jc, *ir; if (nrhs < 4) mexErrMsgTxt("At least 4 arguments are needed: {alpha,yXt,lambda,jVals[,w,yxxy]}"); double *alpha = mxGetPr(prhs[0]); double *yXt = mxGetPr(prhs[1]); double lambda = mxGetScalar(prhs[2]); int *iVals = (int*)mxGetPr(prhs[3]); /* Compute Sizes */ nSamples = mxGetN(prhs[1]); nVars = mxGetM(prhs[1]); int maxIter = mxGetM(prhs[3]); /* Basic input checking */ if (nSamples != mxGetM(prhs[0]) || 1 != mxGetN(prhs[0])) mexErrMsgTxt("alpha should be a column vector, with length equal to the number of columns of yXt"); if (!mxIsClass(prhs[3],"int32") || 1 != mxGetN(prhs[3])) mexErrMsgTxt("iVals must be an int32 column vector"); int sparse = 0; if (mxIsSparse(prhs[1])) { sparse = 1; jc = mxGetJc(prhs[1]); ir = mxGetIr(prhs[1]); } lambda = lambda*nSamples; /* Initialize w */ double *w; if (nrhs >= 5) { w = mxGetPr(prhs[4]); if (nVars != mxGetM(prhs[4]) || 1 != mxGetN(prhs[4])) mexErrMsgTxt("w should be a column vector, with the same number of rows as yXt"); } else { w = mxCalloc(nVars,sizeof(double)); if (sparse) { for(i = 0;i < nSamples;i++) { if (sparse) { for(j = jc[i];j < jc[i+1];j++) { w[ir[j]] -= yXt[j]*alpha[i]/lambda; } } } } else { scaling = -1/lambda; scaling2 = 0; dgemv(&trans,&nVars,&nSamples,&scaling,yXt,&nVars,alpha,&one,&scaling2,w,&one); } } /* Compute yxxy */ double *yxxy; if (nrhs >= 6) { yxxy = mxGetPr(prhs[5]); if (nSamples != mxGetM(prhs[5]) || 1 != mxGetN(prhs[5])) mexErrMsgTxt("yxxy should be a column vector, with length equal to the number of columns in yXt"); } else { yxxy = mxCalloc(nSamples,sizeof(double)); for(i = 0;i < nSamples;i++) { if (sparse) { for(j = jc[i];j < jc[i+1];j++) { yxxy[i] += yXt[j]*yXt[j]; } } else { yxxy[i] = ddot(&nVars,&yXt[nVars*i],&one,&yXt[nVars*i],&one); } } } for(k=0;k<maxIter;k++) { /* Select next sample to update */ i = iVals[k]-1; alphaOld = alpha[i]; if(alphaOld == 0 || alphaOld == 1) alpha[i] = 0.5; LB = 0.0; UB = 1.0; c = (alphaOld/lambda)*yxxy[i]; if (sparse) { for(j = jc[i];j < jc[i+1];j++) { c += w[ir[j]]*yXt[j]; } } else c += ddot(&nVars,w,&one,&yXt[nVars*i],&one); d = yxxy[i]/lambda; gi = log(alpha[i]) - log(1-alpha[i]) - c + alpha[i]*d; while (fabs(gi) > 1e-8 && UB-LB > 1e-15) { Hi = 1/alpha[i] + 1/(1-alpha[i]) + d; alpha[i] -= gi/Hi; if (alpha[i] <= LB || alpha[i] >= UB) alpha[i] = (LB+UB)/2; gi = log(alpha[i]) - log(1-alpha[i]) - c + alpha[i]*d; if (gi > 0) UB = alpha[i]; else LB = alpha[i]; } if (sparse) { for(j = jc[i];j < jc[i+1];j++) { w[ir[j]] += yXt[j]*(alphaOld - alpha[i])/lambda; } } else { scaling = (alphaOld - alpha[i])/lambda; daxpy(&nVars,&scaling,&yXt[i*nVars],&one,w,&one); } } if (nrhs < 5) mxFree(w); if (nrhs < 6) mxFree(yxxy); }
int iterateDynamics(std::vector<Robot *> robotVec, std::vector<DynamicBody *> bodyVec, DynamicParameters *dp) { double h = dp->timeStep; bool useContactEps = dp->useContactEps; static double Jcg_tmp[9],Jcg_B[9],Jcg_N[9],Jcg_N_inv[9],R_N_B[9]; static double db0=0.0,tmp3[3]; static mat3 Rot; static int info; World *myWorld; KinematicChain *chain; int numBodies = bodyVec.size(),errCode = 0; int numRobots = robotVec.size(); int numJoints=0; int numDOF=0; int bn,cn,i,j; int Mrows,Dcols,Arows,Hrows,Hcols,Nurows,Nucols; int numDOFLimits=0; std::list<Contact *> contactList; std::list<Contact *> objContactList; std::list<Contact *>::iterator cp; // unsigned long dmark = dmalloc_mark(); double *ql = new double[7*numBodies]; double *qnew = new double[7*numBodies]; double *vl = new double[6*numBodies]; double *vlnew = new double[6*numBodies]; double *M = new double[(6*numBodies)*(6*numBodies)]; double *M_i = new double[(6*numBodies)*(6*numBodies)]; double *fext = new double[6*numBodies]; // LCP matrix double *A; // LCP vectors double *g,*lambda; double *predLambda = NULL; //used for debugging the prediction of LCP basis // main matrices for contact constraints double *H; // main matrices for joint constraints double *Nu; // main vector for contact constraints double *k; // main vectors for joint constraints double *eps; // intermediate matrices for contact constraints double *HtM_i,*v1; // intermediate matrices for contact constraints double *v2; // intermediate matrices for case of both joint and contact constraints double *NutM_i,*NutM_iNu,*INVNutM_iNu,*INVNutM_iNuNut; double *INVNutM_iNuNutM_i,*INVNutM_iNuNutM_iH; // intermediate vectors for case of both joint and contact constraints double *NutM_ikminuseps,*INVNutM_iNuNutM_ikminuseps; double *currq,*currM; Mrows = 6*numBodies; myWorld = bodyVec[0]->getWorld(); std::map<Body*, int> islandIndices; for (i=0;i<myWorld->getNumBodies();i++) { islandIndices.insert( std::pair<Body*, int>(myWorld->getBody(i), -1) ); } for (i=0;i<numBodies;i++) { islandIndices[ bodyVec[i] ] = i; } // count the joints and DOF, and the joint coupling constraints int numCouplingConstraints = 0; for (i=0;i<numRobots;i++) { numDOF += robotVec[i]->getNumDOF(); for (j=0;j<robotVec[i]->getNumChains();j++) { chain = robotVec[i]->getChain(j); numJoints += chain->getNumJoints(); } for (j=0;j<robotVec[i]->getNumDOF();j++) { numCouplingConstraints += robotVec[i]->getDOF(j)->getNumCouplingConstraints(); numDOFLimits += robotVec[i]->getDOF(j)->getNumLimitConstraints(); } } DBGP("Dynamics time step: " << h); DBGP("numJoints: " << numJoints); // count the total number of joints and contacts int numContacts = 0; int numTotalFrictionEdges = 0; int numDynJointConstraints=0; for (bn=0;bn<numBodies;bn++) { //count joints if (bodyVec[bn]->getDynJoint()) { int numCon = bodyVec[bn]->getDynJoint()->getNumConstraints(); numDynJointConstraints += numCon; DBGP(bodyVec[bn]->getName().latin1() << ": " << numCon << " constraints"); } //count contacts objContactList = bodyVec[bn]->getContacts(); for (cp=objContactList.begin();cp!=objContactList.end();cp++) { // check if the mate of this contact is already in the contact list if (std::find(contactList.begin(),contactList.end(),(*cp)->getMate()) == contactList.end()) { numContacts++; numTotalFrictionEdges += (*cp)->numFrictionEdges; contactList.push_back(*cp); } } } DBGP("Num contacts: " << numContacts); DBGP("Num friction edges: " << numTotalFrictionEdges); DBGP("Num dynjoint: " << numDynJointConstraints); // zero out matrices dcopy(Mrows*Mrows,&db0,0,M,1); dcopy(Mrows*Mrows,&db0,0,M_i,1); dcopy(Mrows,&db0,0,fext,1); //allocate the joint constraint matrices if (numJoints) { Nurows = Mrows; Nucols = numDynJointConstraints + numCouplingConstraints; DBGP("Nucols: " << Nucols); Nu = new double[Nurows * Nucols]; dcopy(Nurows*Nucols,&db0,0,Nu,1); eps = new double[Nucols]; dcopy(Nucols,&db0,0,eps,1); Arows = Mrows+Nucols; } // allocate the LCP matrix if (numContacts || numDOFLimits) { Dcols = numTotalFrictionEdges; DBGP("numContacts " << numContacts); DBGP("Dcols " << Dcols); DBGP("numDOFLimits " << numDOFLimits); Hrows = Mrows; Hcols = Dcols + 2*numContacts + numDOFLimits; H = new double[Hrows * Hcols]; dcopy(Hrows*Hcols,&db0,0,H,1); v1 = new double[Hrows * Hcols]; v2 = new double[Hrows]; dcopy(Hrows*Hcols,&db0,0,v1,1); dcopy(Hrows,&db0,0,v2,1); k = new double[Mrows]; //holds mass*previous velocity and external impulses Arows = Hcols; lambda = new double[Arows]; // the LCP solution } else { Dcols = 0; } // allocate the constraint matrix if (numJoints || numContacts) { A = new double[Arows*Arows]; g = new double[Arows]; dcopy(Arows*Arows,&db0,0,A,1); dcopy(Arows,&db0,0,g,1); } // compute mass matrix and external forces for (bn=0;bn<numBodies;bn++) { memcpy(vl+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double)); memcpy(vlnew+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double)); memcpy(ql+7*bn,bodyVec[bn]->getPos(),7*sizeof(double)); memcpy(qnew+7*bn,bodyVec[bn]->getPos(),7*sizeof(double)); currq = qnew + 7*bn; Quaternion tmpQuat(currq[3],currq[4],currq[5],currq[6]); tmpQuat.ToRotationMatrix(Rot); // The rotation matrix returned by ToRotationMatrix is expressed as // a graphics style rot matrix (new axes are in rows), the R_N_B matrix // is a robotics style rot matrix (new axes in columns) R_N_B[0] = Rot[0]; R_N_B[3] = Rot[1]; R_N_B[6] = Rot[2]; R_N_B[1] = Rot[3]; R_N_B[4] = Rot[4]; R_N_B[7] = Rot[5]; R_N_B[2] = Rot[6]; R_N_B[5] = Rot[7]; R_N_B[8] = Rot[8]; // Jcg_N = R_N_B * Jcg_B * R_N_B'; // where Jcg_B is inertia matrix in body coords // Jcg_N is inertia matrix in world coords ? memcpy(Jcg_B,bodyVec[bn]->getInertia(),9*sizeof(double)); //multiply by mass dscal(9, bodyVec[bn]->getMass(), Jcg_B, 1); dgemm("N","N",3,3,3,1.0,R_N_B,3,Jcg_B,3,0.0,Jcg_tmp,3); dgemm("N","T",3,3,3,1.0,Jcg_tmp,3,R_N_B,3,0.0,Jcg_N,3); if ((info = invertMatrix(3,Jcg_N,Jcg_N_inv))) { printf("In iterateDynamics, inertia matrix inversion failed (info is %d)\n",info); fprintf(stderr,"%f %f %f\n",Jcg_B[0], Jcg_B[1], Jcg_B[2]); fprintf(stderr,"%f %f %f\n",Jcg_B[3], Jcg_B[4], Jcg_B[5]); fprintf(stderr,"%f %f %f\n",Jcg_B[6], Jcg_B[7], Jcg_B[8]); fprintf(stderr,"Body is %s\n",bodyVec[bn]->getName().latin1()); } currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M currM[0] = bodyVec[bn]->getMass(); currM[6*numBodies+1] = bodyVec[bn]->getMass(); currM[12*numBodies+2] = bodyVec[bn]->getMass(); fillMatrixBlock(Jcg_N,3,3,3,5,5,currM,Mrows); currM = M_i+((6*bn)*Mrows + bn*6);//point to correct block of M_i currM[0] = 1.0/bodyVec[bn]->getMass(); currM[Mrows+1] = 1.0/bodyVec[bn]->getMass(); currM[2*Mrows+2] = 1.0/bodyVec[bn]->getMass(); fillMatrixBlock(Jcg_N_inv,3,3,3,5,5,currM,Mrows); // compute external wrench // fext = [ 0 0 -9810.0*mass -[ang_vel_N x (Jcg_N * ang_vel_N)] ] //based on this, it would appear that graspit force units are N*1.0e6 fext[6*bn+2] = -9810.0 * bodyVec[bn]->getMass() * dp->gravityMultiplier; // force of gravity // fext[6*bn+2] = 0; // NO force of gravity dgemv("N",3,3,1.0,Jcg_N,3,&vl[6*bn+3],1,0.0,tmp3,1); // inertial moments fext[6*bn+3] = - (vl[6*bn+4]*tmp3[2] - vl[6*bn+5]*tmp3[1]); fext[6*bn+4] = - (vl[6*bn+5]*tmp3[0] - vl[6*bn+3]*tmp3[2]); fext[6*bn+5] = - (vl[6*bn+3]*tmp3[1] - vl[6*bn+4]*tmp3[0]); double ForcesToBodyFrame[36]; transf invBody = bodyVec[bn]->getTran().inverse(); vec3 invBodyTransl = invBody.translation(); buildForceTransform(invBody,invBodyTransl,ForcesToBodyFrame); DBGP("fext initial: "); DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) ); // add any other wrenches that have accumulated on the body daxpy(6,1.0,bodyVec[bn]->getExtWrenchAcc(),1,&fext[6*bn],1); DBGP("fext with accumulated wrench: "); DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) ); if (numContacts||numDOFLimits) { // k = Mv_l + hfext currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M dgemv("N",6,6,1.0,currM,Mrows,vl+6*bn,1,0.0,k+6*bn,1); } } if (numJoints) { int ncn = 0; int hcn = 0; for (i=0;i<numBodies;i++) { if (bodyVec[i]->getDynJoint()) bodyVec[i]->getDynJoint()-> buildConstraints(Nu,eps,numBodies,islandIndices,ncn); } for (i=0;i<numRobots;i++) { robotVec[i]->buildDOFLimitConstraints(islandIndices,numBodies,H,g,hcn); robotVec[i]->buildDOFCouplingConstraints(islandIndices,numBodies,Nu,eps,ncn); } for (i=0;i<Nucols;i++) { eps[i] *= ERP/h; } for (i=0; i<hcn; i++) { g[i] *= ERP/h; } } // add contacts to the LCP if (!contactList.empty()) { DBGP("processing contacts"); double Ftform_N_C[36]; // A is square double *Wn = &H[numDOFLimits*Hrows]; double *D = &H[(numDOFLimits+numContacts)*Hrows]; double *E = &A[(numDOFLimits+numContacts+Dcols)*Arows + numDOFLimits+numContacts]; double *negET = &A[(numDOFLimits+numContacts)*Arows + numDOFLimits+numContacts+Dcols]; double *MU = &A[numDOFLimits*Arows + numDOFLimits+numContacts+Dcols]; double *contactEps = &g[numDOFLimits]; int frictionEdgesCount = 0; for (cp=contactList.begin(),cn=0; cp!=contactList.end(); cp++,cn++){ //DBGP("contact " << cn); transf cf = (*cp)->getContactFrame() * (*cp)->getBody1Tran(); transf cf2 = (*cp)->getMate()->getContactFrame() * (*cp)->getBody2Tran(); DBGP("CONTACT DISTANCE: " << (cf.translation() - cf2.translation()).len()); if (useContactEps) { contactEps[cn] = MIN(0.0,-ERP/h * (Contact::THRESHOLD/2.0 - (cf.translation() - cf2.translation()).len())); } DBGP(" EPS: " << contactEps[cn]); vec3 normal(cf.affine().element(2,0), cf.affine().element(2,1), cf.affine().element(2,2)); // find which body is this contact from for (bn=0;bn<numBodies;bn++) if ((*cp)->getBody1() == bodyVec[bn]) break; if (bn<numBodies) { //????? this doesn't seem correct vec3 radius = cf.translation() - ( bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN ); // radius = radius / 1000.0; // convert to meters vec3 RcrossN = radius * normal; DBGP("body1 normal: " << normal); DBGP("body1 radius: " << radius); Wn[cn*Hrows+6*bn] = normal.x(); Wn[cn*Hrows+6*bn+1] = normal.y(); Wn[cn*Hrows+6*bn+2] = normal.z(); Wn[cn*Hrows+6*bn+3] = RcrossN.x(); Wn[cn*Hrows+6*bn+4] = RcrossN.y(); Wn[cn*Hrows+6*bn+5] = RcrossN.z(); vec3 bodyOrigin = bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN; buildForceTransform(cf,bodyOrigin,Ftform_N_C); /* dgemm("N","N", 6,Contact::numFrictionEdges,6, 1.0,Ftform_N_C,6, Contact::frictionEdges,6, 0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows); */ dgemm("N","N", 6,(*cp)->numFrictionEdges,6, //m, n, k 1.0,Ftform_N_C,6, //alfa, A, lda (*cp)->frictionEdges,6, //B, ldb 0.0,&D[ frictionEdgesCount*Hrows+6*bn],Hrows); //beta, C, ldc } //find the other body for(bn=0;bn<numBodies;bn++) if ((*cp)->getBody2() == bodyVec[bn]) break; if (bn<numBodies) { //normal = vec3(cf2.affine().element(2,0), cf2.affine().element(2,1),cf2.affine().element(2,2)); normal = -normal; //vec3 radius = cf2.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN); vec3 radius = cf.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN); vec3 RcrossN = radius * normal; DBGP("body2 normal: " << normal); DBGP("body2 radius: " << radius); Wn[cn*Hrows+6*bn] = normal.x(); Wn[cn*Hrows+6*bn+1] = normal.y(); Wn[cn*Hrows+6*bn+2] = normal.z(); Wn[cn*Hrows+6*bn+3] = RcrossN.x(); Wn[cn*Hrows+6*bn+4] = RcrossN.y(); Wn[cn*Hrows+6*bn+5] = RcrossN.z(); vec3 bodyOrigin = bodyVec[bn]->getCoG()*(*cp)->getBody2Tran() - position::ORIGIN; buildForceTransform(cf,bodyOrigin,Ftform_N_C); //buildForceTransform(cf2,bodyOrigin,Ftform_N_C); /* dgemm("N","N",6,Contact::numFrictionEdges,6,-1.0,Ftform_N_C,6, Contact::frictionEdges,6, 0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows);*/ //original graspit had a -1.0 here in front of Ftform_N_C dgemm("N","N", 6,(*cp)->numFrictionEdges,6, -1.0,Ftform_N_C,6, (*cp)->frictionEdges,6, 0.0,&D[ frictionEdgesCount*Hrows+6*bn ],Hrows); } //for (i=cn*Contact::numFrictionEdges; i<(cn+1)*Contact::numFrictionEdges; i++) { for (i=frictionEdgesCount; i<frictionEdgesCount+(*cp)->numFrictionEdges; i++) { E[cn*Arows+i] = 1.0; negET[i*Arows+cn] = -1.0; } MU[cn*Arows + cn] = (*cp)->getCof(); frictionEdgesCount += (*cp)->numFrictionEdges; } } if (numContacts || numDOFLimits) daxpy(Mrows,h,fext,1,k,1); if (numJoints && (numContacts || numDOFLimits)) { // Cnu1 = INV(Nu'M_iNu)Nu'M_iH // Cnu2 = INV(Nu'M_iNu)(Nu'M_ik-eps) // v1 = -NuCnu1 // v2 = -NuCnu2 NutM_i = new double[Nucols*Mrows]; NutM_iNu = new double[Nucols*Nucols]; INVNutM_iNu = new double[Nucols*Nucols]; INVNutM_iNuNut = new double[Nucols*Nurows]; INVNutM_iNuNutM_i = new double[Nucols*Mrows]; INVNutM_iNuNutM_iH = new double[Nucols*Hcols]; NutM_ikminuseps = new double[Nucols]; INVNutM_iNuNutM_ikminuseps = new double[Nucols]; dgemm("T","N",Nucols,Mrows,Mrows,1.0,Nu,Nurows,M_i,Mrows, 0.0,NutM_i,Nucols); dgemm("N","N",Nucols,Nucols,Mrows,1.0,NutM_i,Nucols,Nu,Nurows, 0.0,NutM_iNu,Nucols); if ((info = invertMatrix(Nucols,NutM_iNu,INVNutM_iNu))) printf("In iterateDynamics, NutM_iNu matrix inversion failed (info is %d)\n",info); dgemm("N","T",Nucols,Nurows,Nucols,1.0,INVNutM_iNu,Nucols,Nu,Nurows, 0.0,INVNutM_iNuNut,Nucols); dgemm("N","N",Nucols,Mrows,Mrows,1.0,INVNutM_iNuNut,Nucols,M_i,Mrows, 0.0,INVNutM_iNuNutM_i,Nucols); dgemm("N","N",Nucols,Hcols,Mrows,1.0,INVNutM_iNuNutM_i,Nucols,H,Hrows, 0.0,INVNutM_iNuNutM_iH,Nucols); dgemm("N","N",Nurows,Hcols,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_iH,Nucols, 0.0,v1,Nurows); dgemv("N",Nucols,Mrows,1.0,NutM_i,Nucols,k,1,0.0,NutM_ikminuseps,1); daxpy(Nucols,-1.0,eps,1,NutM_ikminuseps,1); dgemv("N",Nucols,Nucols,1.0,INVNutM_iNu,Nucols,NutM_ikminuseps,1, 0.0,INVNutM_iNuNutM_ikminuseps,1); dgemv("N",Nurows,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_ikminuseps,1, 0.0,v2,1); } if (numContacts || numDOFLimits) { // in the simple case without joint constraints // A = H'M_iv1 + N // g = H'M_iv2 // where N is already stored in A // v1 is the first term of v_(l+1) and v2 is the second term // v_l+1 = M_i(v1 lambda + v2) = M_i(H lambda + k) // k is (Mv_l + hfext) //add H to v1 //add k to v2 DBGP("k:"); DBGST( disp_mat(stdout,k,1,Mrows,0) ); DBGP("first g:"); DBGST( disp_mat(stdout,g,1,Arows,0) ); daxpy(Mrows*Hcols,1.0,H,1,v1,1); daxpy(Mrows,1.0,k,1,v2,1); // build A and g HtM_i = new double[Hcols*Mrows]; dgemm("T","N",Hcols,Mrows,Hrows,1.0,H,Hrows,M_i,Mrows,0.0,HtM_i,Hcols); dgemm("N","N",Hcols,Hcols,Mrows,1.0,HtM_i,Hcols,v1,Mrows,1.0,A,Arows); // dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,0.0,g,1); dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,1.0,g,1); } int frictionEdgesCount; //debug information; can be removed if (numContacts || numDOFLimits) { bool lemkePredict = false; if (lemkePredict) { //try to use information from previous time steps to guess a good starting basis for Lemke's algorithm assembleLCPPrediction(lambda, Arows, numDOFLimits, &contactList); predLambda = new double[Arows]; // keep a copy of the prediction so we can check it later dcopy(Arows, lambda, 1, predLambda, 1); // fprintf(stderr,"Prediction: \n"); // printLCPBasis(predLambda, Arows, numDOFLimits, numContacts); } // double startTime; // startTime = getTime(); DBGP("g:"); DBGST( for (i=0;i<Arows;i++) printf("%le ",g[i]); );
/*! Given a vector of pointers to dynamic bodies, and the number of bodies in the vector, this routine will move those bodies in the direction of their current velocity for the length of the timestep, \a h. It uses the pre-computed velocities and accelerations computed by iterateDynamics and stored for each body. */ int moveBodies(int numBodies,std::vector<DynamicBody *> bodyVec,double h) { static double V[42]; static double tmp12[12]; static double B[12]; static double R_N_B[9]; static double newPos[7]; static mat3 Rot; int bn; double currq[7]; double currv[6]; int errCode=SUCCESS; for (bn=0;bn<numBodies;bn++) { memcpy(currq,bodyVec[bn]->getPos(),7*sizeof(double)); memcpy(currv,bodyVec[bn]->getVelocity(),6*sizeof(double));; Quaternion tmpQuat(currq[3],currq[4],currq[5],currq[6]); tmpQuat.ToRotationMatrix(Rot); // The rotation matrix returned by ToRotationMatrix is expressed as // a graphics style rot matrix (new axes are in rows), the R_N_B matrix // is a robotics style rot matrix (new axes in columns) R_N_B[0] = Rot[0]; R_N_B[3] = Rot[1]; R_N_B[6] = Rot[2]; R_N_B[1] = Rot[3]; R_N_B[4] = Rot[4]; R_N_B[7] = Rot[5]; R_N_B[2] = Rot[6]; R_N_B[5] = Rot[7]; R_N_B[8] = Rot[8]; // B relates the angular velocity of the body (expressed in // the body frame) to the time derivative of the Euler parameters B[0] = -currq[4]; B[4] = -currq[5]; B[8] = -currq[6]; B[1] = currq[3]; B[5] = -currq[6]; B[9] = currq[5]; B[2] = currq[6]; B[6] = currq[3]; B[10]= -currq[4]; B[3] = -currq[5]; B[7] = currq[4]; B[11]= currq[3]; dscal(12,0.5,B,1); // V is a list of matrices. Each matrix (V_bn) can be multiplied by // body bn's 6x1 velocity vector to get the 7x1 time derivative // of the body's position. // V_bn = [ eye(3,3) zeros(3,3); // zeros(4,3) B*R_N_B' ]; // This list of matrices will be used at the end to compute the new // position from the new velocity dgemm("N","T",4,3,3,1.0,B,4,R_N_B,3,0.0,tmp12,4); V[0] = 1.0; V[8] = 1.0; V[16]= 1.0; fillMatrixBlock(tmp12,4,3,3,6,5,V,7); dcopy(7,currq,1,newPos,1); dgemv("N",7,6,h,V,7,currv,1,1.0,newPos,1); #ifdef GRASPITDBG fprintf(stdout,"object %s new velocity: \n", bodyVec[bn]->getName().latin1()); for (int i=0;i<6;i++) fprintf(stdout,"%le ",currv[i]); printf("\n"); fprintf(stdout,"object %s new position: \n", bodyVec[bn]->getName().latin1()); disp_mat(stdout,newPos,1,7,0); #endif //should we bother to check if the object has moved? (for optimization) if (!bodyVec[bn]->setPos(newPos)) { DBGP("requested position is out of bounds for this object!"); errCode = FAILURE; } } return errCode; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { size_t NN, X_sum, P, D, Lp1; mwSize K; double *X_inds, *X_vals; double *lPhi, *lTheta, *lPsi, *W; mxArray *Km; double *inds, *vals, *num_nz; double eps; if(nlhs != NOUTPUTS) mexErrMsgTxt("Usage: see \"help sample_Xpn_kernel_meat\""); if(nrhs != NINPUTS) mexErrMsgTxt("Usage: see \"help sample_Xpn_kernel_meat\""); // Grab inputs X_inds = mxGetPr(prhs[0]); X_vals = mxGetPr(prhs[1]); lPhi = mxGetPr(prhs[2]); lTheta = mxGetPr(prhs[3]); lPsi = mxGetPr(prhs[4]); W = mxGetPr(prhs[6]); X_sum = mxGetScalar(prhs[7]); Km = (mxArray*)prhs[5]; // Cell array NN = mxGetM(prhs[0]); P = mxGetM(prhs[2]); K = mxGetN(prhs[2]); D = mxGetN(prhs[4]); Lp1 = mxGetM(prhs[6]); eps = mxGetEps(); // Allocate output plhs[0] = mxCreateDoubleMatrix(X_sum, 3, mxREAL); plhs[1] = mxCreateDoubleMatrix(X_sum, 1, mxREAL); plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL); inds = mxGetPr(plhs[0]); vals = mxGetPr(plhs[1]); num_nz = mxGetPr(plhs[2]); // Pre-allocate scratch arrays unsigned int topic_sz = K*sizeof(double); double *zeta = (double*)mxMalloc(topic_sz); double *cump_scr = (double*)mxMalloc(topic_sz); double *mnr = (double*)mxMalloc(topic_sz); // log-thinning activation function, not Phi as in the topics // Change this to just a double* //mxArray *lphi = mxCreateDoubleMatrix(D, K, mxREAL); //mxArray *res; double *A, *lphi; char *chn = "N"; // don't transpose for dgemv double onef = 1.0, zerof = 0.0; // alpha, beta in dgemv size_t onei = 1; // incx in dgemv lphi = mxMalloc(D*K*sizeof(double)); //lphi_ptr = mxGetPr(lphi); //printf("Computing lphi...\n"); //printf(" K: %d\n", K); // Compute phi{k} = p(Znk = 1) for all k here (Each one is a matrix) for (int k = 0; k < K; k++) { //printf("k: %d ", k); A = mxGetPr(mxGetCell(Km, k)); //printf("Before dgemv.."); dgemv(chn, &D, &Lp1, &onef, A, &D, &W[k*Lp1], &onei, &zerof, &lphi[k*D], &onei); //dgemv(chn, &D, &Lp1, &onef, A, &D, &W[k*Lp1], &onei, &zerof, &lphi_ptr[k*D], &onei); //printf("after dgemv\n"); } //printf("Sampling...\n"); // Probit each entry and take log normcdf(lphi, D*K); for (int ii = 0; ii < D*K; ii++) lphi[ii] = log(lphi[ii] + eps); // main loop unsigned int cur_idx = 0; unsigned int noff = X_sum; unsigned int koff = 2*X_sum; for(int ii = 0; ii < NN; ii++) { // Get indices and data (convert to 0-based) unsigned int p = X_inds[ii] - 1; unsigned int n = X_inds[ii + NN] - 1; // These arrays have NN rows unsigned int xx = X_vals[ii]; // Construct probability double zeta_max = DBL_MIN; memset(zeta,0,topic_sz); for(int k = 0; k < K; k++) { // When we say N we mean D // lZnk is NxK and lPsi is KxN so we index them backwards // lphi is NxK and contains the log-thinning probabilities //zeta[k] = lPhi[p + k*P] + lTheta[k] + lZnk[n + k*D] + lPsi[k + n*K]; zeta[k] = lPhi[p + k*P] + lTheta[k] + lphi[n + k*D] + lPsi[k + n*K]; if(zeta[k] > zeta_max) zeta_max = zeta[k]; } for(int k = 0; k < K; k++) { zeta[k] -= zeta_max; zeta[k] = exp(zeta[k]); } double zeta_sum = sum(zeta, K); for(int k = 0; k < K; k++) zeta[k] /= zeta_sum; // for(int k = 0; k < K; k++) // printf("%.2f,", zeta[k]); // printf("\n"); // Split up observed counts into topics and add to output if(xx > 1) { multirnd(mnr, xx, zeta, K, cump_scr, eps); for(int k = 0; k < K; k++) { if(mnr[k] > 0) { inds[cur_idx] = p + 1; inds[cur_idx + noff] = n + 1; inds[cur_idx + koff] = k + 1; // +1 b/c k is index vals[cur_idx] = mnr[k]; cur_idx++; } } } else { // Result of below is 1-based, so don't add 1 to kk below double kk = discrnd(zeta, K, cump_scr, eps); inds[cur_idx] = p + 1; inds[cur_idx + noff] = n + 1; inds[cur_idx + koff] = kk; vals[cur_idx] = 1; cur_idx++; } } // Free scratch memory mxFree(zeta); mxFree(cump_scr); mxFree(mnr); mxFree(lphi); *num_nz = cur_idx; // cur_idx has been incr. 1 beyond already }
void NewLRedMarginGPULogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { clock_t startClock,endClock; double **EFAC; double *EQUAD; int pcount=0; int totdims = ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; totdims += ((MNStruct *)context)->numFitEFAC*((MNStruct *)context)->EPolTerms + ((MNStruct *)context)->numFitEQUAD; totdims +=2*((MNStruct *)context)->incStep; if(((MNStruct *)context)->incRED==2)totdims+=((MNStruct *)context)->numFitRedCoeff; if(((MNStruct *)context)->incDM==2)totdims+=((MNStruct *)context)->numFitDMCoeff; if(((MNStruct *)context)->varyRedCoeff==1)totdims+=2; if(((MNStruct *)context)->varyDMCoeff==1)totdims+=2; if(((MNStruct *)context)->incRED==3)totdims+=2; if(((MNStruct *)context)->incDM==3)totdims+=2; if(((MNStruct *)context)->yearlyDM == 1)totdims+=2; if(((MNStruct *)context)->incsinusoid == 1)totdims+=3; if(((MNStruct *)context)->incGWB == 1)totdims+=1; int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; long double LDparams[numfit]; double Fitparams[numfit]; double *Resvec=new double[((MNStruct *)context)->pulse->nobs]; int fitcount=0; for(int p=0;p<totdims;p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ Cube[pcount]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[pcount]+((MNStruct *)context)->Dpriors[p][0]; pcount++; } } pcount=0; if(((MNStruct *)context)->doLinear==0){ for(int p=0;p< ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ LDparams[p]=Cube[fitcount]*(((MNStruct *)context)->LDpriors[p][1]) + (((MNStruct *)context)->LDpriors[p][0]); fitcount++; } else if(((MNStruct *)context)->Dpriors[p][1] == ((MNStruct *)context)->Dpriors[p][0]){ LDparams[p]=((MNStruct *)context)->Dpriors[p][0]*(((MNStruct *)context)->LDpriors[p][1]) + (((MNStruct *)context)->LDpriors[p][0]); } } pcount=0; double phase=(double)LDparams[0]; pcount++; for(int p=1;p<((MNStruct *)context)->numFitTiming;p++){ ((MNStruct *)context)->pulse->param[((MNStruct *)context)->TempoFitNums[p][0]].val[((MNStruct *)context)->TempoFitNums[p][1]] = LDparams[pcount]; pcount++; } for(int p=0;p<((MNStruct *)context)->numFitJumps;p++){ ((MNStruct *)context)->pulse->jumpVal[((MNStruct *)context)->TempoJumpNums[p]]= LDparams[pcount]; pcount++; } fastformBatsAll(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars); /* Form Barycentric arrival times */ formResiduals(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars,1); /* Form residuals */ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].residual+phase; } } else if(((MNStruct *)context)->doLinear==1){ fitcount=0; for(int p=0;p< ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ Fitparams[p]=Cube[fitcount]; fitcount++; } else if(((MNStruct *)context)->Dpriors[p][1] == ((MNStruct *)context)->Dpriors[p][0]){ Fitparams[p]=0; } } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } delete[] Fitvec; } pcount=fitcount; if(((MNStruct *)context)->incStep > 0){ for(int i = 0; i < ((MNStruct *)context)->incStep; i++){ double StepAmp = Cube[pcount]; pcount++; double StepTime = Cube[pcount]; pcount++; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ if(((MNStruct *)context)->pulse->obsn[o1].bat > StepTime){ Resvec[o1] += StepAmp; } } } } ///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////Get White Noise vector/////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////// double equadpriorterm=0; if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=1; } } else{ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=0; } } } } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=Cube[pcount]; } pcount++; } else{ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=pow(10.0,Cube[pcount]); } pcount++; } } } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int p=0;p< ((MNStruct *)context)->systemcount; p++){ EFAC[n-1][p]=Cube[pcount]; pcount++; } } else{ for(int p=0;p< ((MNStruct *)context)->systemcount; p++){ EFAC[n-1][p]=pow(10.0,Cube[pcount]); pcount++; } } } } if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=0; } } else if(((MNStruct *)context)->numFitEQUAD == 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); equadpriorterm+=log(pow(10.0,Cube[pcount])); } pcount++; } else if(((MNStruct *)context)->numFitEQUAD > 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); equadpriorterm+=log(pow(10.0,Cube[pcount])); pcount++; } } double *Noise; double *BATvec; Noise=new double[((MNStruct *)context)->pulse->nobs]; BATvec=new double[((MNStruct *)context)->pulse->nobs]; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ BATvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].bat; } if(((MNStruct *)context)->whitemodel == 0){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ double EFACterm=0; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFACterm=EFACterm + pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))/pow(pow(10.0,-7),n-1),n)*EFAC[n-1][((MNStruct *)context)->sysFlags[o]]; } Noise[o]= 1.0/(pow(EFACterm,2) + EQUAD[((MNStruct *)context)->sysFlags[o]]); } } else if(((MNStruct *)context)->whitemodel == 1){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Noise[o]=1.0/(EFAC[0][((MNStruct *)context)->sysFlags[o]]*EFAC[0][((MNStruct *)context)->sysFlags[o]]*(pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6)),2) + EQUAD[((MNStruct *)context)->sysFlags[o]])); } } ////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////Form the Power Spectrum////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// int FitRedCoeff=2*(((MNStruct *)context)->numFitRedCoeff); int FitDMCoeff=2*(((MNStruct *)context)->numFitDMCoeff); if(((MNStruct *)context)->incFloatDM != 0)FitDMCoeff+=2*((MNStruct *)context)->incFloatDM; if(((MNStruct *)context)->incFloatRed != 0)FitRedCoeff+=2*((MNStruct *)context)->incFloatRed; int totCoeff=0; if(((MNStruct *)context)->incRED != 0)totCoeff+=FitRedCoeff; if(((MNStruct *)context)->incDM != 0)totCoeff+=FitDMCoeff; double *powercoeff=new double[totCoeff]; for(int o=0;o<totCoeff; o++){ powercoeff[o]=0; } double priorterm=0; bool uniformprior=0; double start,end; int go=0; for (int i=0;i<((MNStruct *)context)->pulse->nobs;i++) { if (((MNStruct *)context)->pulse->obsn[i].deleted==0) { if (go==0) { go = 1; start = (double)((MNStruct *)context)->pulse->obsn[i].bat; end = start; } else { if (start > (double)((MNStruct *)context)->pulse->obsn[i].bat) start = (double)((MNStruct *)context)->pulse->obsn[i].bat; if (end < (double)((MNStruct *)context)->pulse->obsn[i].bat) end = (double)((MNStruct *)context)->pulse->obsn[i].bat; } } } double maxtspan=1*(end-start); double *freqs = new double[totCoeff]; double *DMVec=new double[((MNStruct *)context)->pulse->nobs]; double DMKappa = 2.410*pow(10.0,-16); int startpos=0; double freqdet=0; double GWBAmpPrior=0; if(((MNStruct *)context)->incRED==2){ for (int i=0; i<FitRedCoeff/2; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[i]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; powercoeff[i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[i+FitRedCoeff/2]=powercoeff[i]; freqdet=freqdet+2*log(powercoeff[i]); pcount++; } startpos=FitRedCoeff; } else if(((MNStruct *)context)->incRED==3){ freqdet=0; for(int pl = 0; pl < ((MNStruct *)context)->numFitRedPL; pl ++){ double redamp=Cube[pcount]; pcount++; double redindex=Cube[pcount]; pcount++; double Tspan = maxtspan; double f1yr = 1.0/3.16e7; redamp=pow(10.0, redamp); if(uniformprior==1)priorterm+=log(redamp); double Agw=redamp; for (int i=0; i<FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed ; i++){ freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[i]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; double rho = (Agw*Agw/12.0/(M_PI*M_PI))*pow(f1yr,(-3)) * pow(freqs[i]*365.25,(-redindex))/(maxtspan*24*60*60); powercoeff[i]+= rho; powercoeff[i+FitRedCoeff/2]+= rho; } } int coefftovary=0; double amptovary=0.0; if(((MNStruct *)context)->varyRedCoeff==1){ coefftovary=int(pow(10.0,Cube[pcount]))-1; pcount++; amptovary=pow(10.0,Cube[pcount])/(maxtspan*24*60*60); pcount++; powercoeff[coefftovary]=amptovary; powercoeff[coefftovary+FitRedCoeff/2]=amptovary; } double GWBAmp=0; if(((MNStruct *)context)->incGWB==1){ GWBAmp=pow(10.0,Cube[pcount]); pcount++; GWBAmpPrior=log(GWBAmp); double Tspan = maxtspan; double f1yr = 1.0/3.16e7; for (int i=0; i<FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed ; i++){ double rho = (GWBAmp*GWBAmp/12.0/(M_PI*M_PI))*pow(f1yr,(-3)) * pow(freqs[i]*365.25,(-4.333))/(maxtspan*24*60*60); powercoeff[i]+= rho; powercoeff[i+FitRedCoeff/2]+= rho; } } for (int i=0; i<FitRedCoeff/2; i++){ freqdet=freqdet+2*log(powercoeff[i]); } startpos=FitRedCoeff; } if(((MNStruct *)context)->incsinusoid == 1){ double sineamp=pow(10.0,Cube[pcount]); pcount++; double sinephase=Cube[pcount]; pcount++; double sinefreq=pow(10.0,Cube[pcount])/maxtspan; pcount++; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]-= sineamp*sin(2*M_PI*sinefreq*(double)((MNStruct *)context)->pulse->obsn[o].bat + sinephase); } } if(((MNStruct *)context)->incDM==2){ for (int i=0; i<FitDMCoeff/2; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=((MNStruct *)context)->sampleFreq[startpos/2 - ((MNStruct *)context)->incFloatRed+i]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitDMCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); pcount++; } for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ DMVec[o]=1.0/(DMKappa*pow((double)((MNStruct *)context)->pulse->obsn[o].freqSSB,2)); } } else if(((MNStruct *)context)->incDM==3){ for(int pl = 0; pl < ((MNStruct *)context)->numFitDMPL; pl ++){ double DMamp=Cube[pcount]; pcount++; double DMindex=Cube[pcount]; pcount++; double Tspan = maxtspan; double f1yr = 1.0/3.16e7; DMamp=pow(10.0, DMamp); if(uniformprior==1)priorterm+=log(DMamp); for (int i=0; i<FitDMCoeff/2; i++){ freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[startpos/2 - ((MNStruct *)context)->incFloatRed +i]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; double rho = (DMamp*DMamp)*pow(f1yr,(-3)) * pow(freqs[startpos+i]*365.25,(-DMindex))/(maxtspan*24*60*60); powercoeff[startpos+i]+=rho; powercoeff[startpos+i+FitDMCoeff/2]+=rho; } } int coefftovary=0; double amptovary=0.0; if(((MNStruct *)context)->varyDMCoeff==1){ coefftovary=int(pow(10.0,Cube[pcount]))-1; pcount++; amptovary=pow(10.0,Cube[pcount])/(maxtspan*24*60*60); pcount++; powercoeff[startpos+coefftovary]=amptovary; powercoeff[startpos+coefftovary+FitDMCoeff/2]=amptovary; } for (int i=0; i<FitDMCoeff/2; i++){ freqdet=freqdet+2*log(powercoeff[startpos+i]); } for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ DMVec[o]=1.0/(DMKappa*pow((double)((MNStruct *)context)->pulse->obsn[o].freqSSB,2)); } } if(((MNStruct *)context)->yearlyDM == 1){ double yearlyamp=pow(10.0,Cube[pcount]); pcount++; double yearlyphase=Cube[pcount]; pcount++; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]-= yearlyamp*sin((2*M_PI/365.25)*(double)((MNStruct *)context)->pulse->obsn[o].bat + yearlyphase)*DMVec[o]; } } ///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////Get Time domain likelihood////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////// double tdet=0; double timelike=0; for(int o=0; o<((MNStruct *)context)->pulse->nobs; o++){ timelike+=Resvec[o]*Resvec[o]*Noise[o]; tdet -= log(Noise[o]); } ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////get TNDMVec//////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// int TimetoMargin=0; for(int i =0; i < ((MNStruct *)context)->numFitTiming+((MNStruct *)context)->numFitJumps; i++){ if(((MNStruct *)context)->LDpriors[i][2]==1)TimetoMargin++; } double **TNDM=new double*[((MNStruct *)context)->pulse->nobs]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ TNDM[i]=new double[TimetoMargin]; } double *TNDMVec=new double[((MNStruct *)context)->pulse->nobs*TimetoMargin]; if(TimetoMargin != ((MNStruct *)context)->numFitTiming+((MNStruct *)context)->numFitJumps){ getCustomDMatrixLike(context, TNDM); for(int g=0;g<TimetoMargin; g++){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ TNDMVec[g*((MNStruct *)context)->pulse->nobs + o]=TNDM[o][g]; } } } ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////Call GPU Code///////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// double *likevals=new double[2]; int totalsize=TimetoMargin+totCoeff; NewLRedMarginGPUWrapper_(context, TNDMVec, freqs, powercoeff, Resvec, BATvec, DMVec, Noise, ((MNStruct *)context)->pulse->nobs, FitRedCoeff,FitDMCoeff, TimetoMargin, totCoeff, totalsize, ((MNStruct *)context)->incRED,((MNStruct *)context)->incDM, ((MNStruct *)context)->numFitTiming, ((MNStruct *)context)->numFitJumps, likevals); ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////calculate likelihood/////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// double jointdet=likevals[0]; double freqlike=likevals[1]; lnew=-0.5*(tdet+jointdet+freqdet+timelike-freqlike) + equadpriorterm + GWBAmpPrior; if(isnan(lnew) || isinf(lnew)){ lnew=-pow(10.0,20); // printf("red amp and alpha %g %g\n",redamp,redalpha); // printf("Like: %g %g %g \n",lnew,Chisq,covdet); } delete[] DMVec; delete[] EFAC; delete[] EQUAD; delete[] powercoeff; delete[] freqs; delete[] Noise; delete[] Resvec; delete[] BATvec; delete[] likevals; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[]TNDM[j]; } delete[]TNDM; delete[]TNDMVec; //printf("GPUChisq: %.8g %.8g %.8g %.8g %.8g %.8g \n",lnew,jointdet,tdet,freqdet,timelike,freqlike); // if(isinf(lnew) || isinf(jointdet) || isinf(tdet) || isinf(freqdet) || isinf(timelike) || isinf(freqlike)){ //printf("Chisq: %g %g %g %g %g %g \n",lnew,jointdet,tdet,freqdet,timelike,freqlike); // } }
void LRedGPULogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { //printf("hereNM"); clock_t startClock,endClock; double *EFAC; double *EQUAD; int pcount=0; int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; long double LDparams[numfit]; double Fitparams[numfit]; double *Resvec=new double[((MNStruct *)context)->pulse->nobs]; for(int p=0;p<ndim;p++){ Cube[p]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[p]+((MNStruct *)context)->Dpriors[p][0]; } if(((MNStruct *)context)->doLinear==0){ for(int p=0;p< ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; p++){ LDparams[p]=Cube[p]*(((MNStruct *)context)->LDpriors[p][1]) + (((MNStruct *)context)->LDpriors[p][0]); } double phase=(double)LDparams[0]; pcount++; for(int p=1;p<((MNStruct *)context)->numFitTiming;p++){ ((MNStruct *)context)->pulse->param[((MNStruct *)context)->TempoFitNums[p][0]].val[((MNStruct *)context)->TempoFitNums[p][1]] = LDparams[pcount]; pcount++; } for(int p=0;p<((MNStruct *)context)->numFitJumps;p++){ ((MNStruct *)context)->pulse->jumpVal[((MNStruct *)context)->TempoJumpNums[p]]= LDparams[pcount]; pcount++; } if(((MNStruct *)context)->pulse->param[param_dmmodel].fitFlag[0] == 1){ int DMnum=((MNStruct *)context)->pulse[0].dmoffsDMnum; for(int i =0; i < DMnum; i++){ ((MNStruct *)context)->pulse[0].dmoffsDM[i]=Cube[ndim-DMnum+i]; } } fastformBatsAll(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars); /* Form Barycentric arrival times */ formResiduals(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars,1); /* Form residuals */ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].residual+phase; } } else if(((MNStruct *)context)->doLinear==1){ for(int p=0;p < numfit; p++){ Fitparams[p]=Cube[p]; pcount++; } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } delete[] Fitvec; } if(((MNStruct *)context)->incStep > 0){ for(int i = 0; i < ((MNStruct *)context)->incStep; i++){ double StepAmp = Cube[pcount]; pcount++; double StepTime = Cube[pcount]; pcount++; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ double time = (double)((MNStruct *)context)->pulse->obsn[o1].bat; if(time > StepTime){ Resvec[o1] += StepAmp; } } } } if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[o]=1; } } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[o]=Cube[pcount]; } pcount++; } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double[((MNStruct *)context)->systemcount]; for(int p=0;p< ((MNStruct *)context)->systemcount; p++){ EFAC[p]=Cube[pcount]; pcount++; } } if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=0; } } else if(((MNStruct *)context)->numFitEQUAD == 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); } pcount++; } else if(((MNStruct *)context)->numFitEQUAD > 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); pcount++; } } int FitRedCoeff=2*(((MNStruct *)context)->numFitRedCoeff); int FitDMCoeff=2*(((MNStruct *)context)->numFitDMCoeff); int totCoeff=0; if(((MNStruct *)context)->incRED != 0)totCoeff+=FitRedCoeff; if(((MNStruct *)context)->incDM != 0)totCoeff+=FitDMCoeff; double *powercoeff=new double[totCoeff]; for(int o=0;o<totCoeff; o++){ powercoeff[o]=0; } double *WorkNoise=new double[((MNStruct *)context)->pulse->nobs]; double tdet=0; double timelike=0; double *BATvec=new double[((MNStruct *)context)->pulse->nobs]; if(((MNStruct *)context)->whitemodel == 0){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ // printf("Noise %i %g %g %g\n",m1,Noise[m1],EFAC[flagList[m1]],EQUAD); WorkNoise[o]=pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))*EFAC[((MNStruct *)context)->sysFlags[o]],2) + EQUAD[((MNStruct *)context)->sysFlags[o]]; tdet=tdet+log(WorkNoise[o]); WorkNoise[o]=1.0/WorkNoise[o]; timelike=timelike+pow(Resvec[o],2)*WorkNoise[o]; BATvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].bat; } } else if(((MNStruct *)context)->whitemodel == 1){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ // printf("Noise %i %g %g %g\n",m1,Noise[m1],EFAC[flagList[m1]],EQUAD); WorkNoise[o]=EFAC[((MNStruct *)context)->sysFlags[o]]*EFAC[((MNStruct *)context)->sysFlags[o]]*(pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6)),2) + EQUAD[((MNStruct *)context)->sysFlags[o]]); tdet=tdet+log(WorkNoise[o]); WorkNoise[o]=1.0/WorkNoise[o]; timelike=timelike+pow(Resvec[o],2)*WorkNoise[o]; BATvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].bat; } } double *NFd = new double[totCoeff]; double **FNF=new double*[totCoeff]; for(int i=0;i<totCoeff;i++){ FNF[i]=new double[totCoeff]; } double start,end; int go=0; for (int i=0;i<((MNStruct *)context)->pulse->nobs;i++) { if (((MNStruct *)context)->pulse->obsn[i].deleted==0) { if (go==0) { go = 1; start = (double)((MNStruct *)context)->pulse->obsn[i].bat; end = start; } else { if (start > (double)((MNStruct *)context)->pulse->obsn[i].bat) start = (double)((MNStruct *)context)->pulse->obsn[i].bat; if (end < (double)((MNStruct *)context)->pulse->obsn[i].bat) end = (double)((MNStruct *)context)->pulse->obsn[i].bat; } } } double maxtspan=end-start; double *freqs = new double[totCoeff]; double *DMVec=new double[((MNStruct *)context)->pulse->nobs]; double DMKappa = 2.410*pow(10.0,-16); int startpos=0; double freqdet=0; if(((MNStruct *)context)->incRED==2){ if(((MNStruct *)context)->incFloatRed == 0){ for (int i=0; i<FitRedCoeff/2; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[i]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; powercoeff[i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[i+FitRedCoeff/2]=powercoeff[i]; freqdet=freqdet+2*log(powercoeff[i]); pcount++; } } else if(((MNStruct *)context)->incFloatRed >0){ for (int i=0; i<FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed ; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[i]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; powercoeff[i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[i+FitRedCoeff/2]=powercoeff[i]; freqdet=freqdet+2*log(powercoeff[i]); pcount++; } for (int i=FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed; i<FitRedCoeff/2; i++){ //printf("Freq: %g \n", Cube[pcount]); freqs[startpos+i]=Cube[pcount]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; pcount++; int pnum=pcount; double pc=Cube[pcount]; pcount++; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitRedCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); } } startpos=FitRedCoeff; } else if(((MNStruct *)context)->incRED==3){ freqdet=0; for(int pl = 0; pl < ((MNStruct *)context)->numFitRedPL; pl ++){ double redamp=Cube[pcount]; pcount++; double redindex=Cube[pcount]; pcount++; redamp=pow(10.0, redamp); for (int i=0; i<FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed ; i++){ freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[i]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; double PLcomp=redamp*redamp*pow((freqs[i]*365.25),-1.0*redindex)/(maxtspan*24*60*60); powercoeff[i]+= PLcomp; powercoeff[i+FitRedCoeff/2]+= PLcomp; } } for (int i=0; i<FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed ; i++){ freqdet=freqdet+2*log(powercoeff[i]); // printf("%i %g %g \n",i,powercoeff[i], freqdet); } for (int i=FitRedCoeff/2 - ((MNStruct *)context)->incFloatRed; i<FitRedCoeff/2; i++){ // Cube[pcount]=floor(Cube[pcount]); freqs[startpos+i]=Cube[pcount]/maxtspan; freqs[startpos+i+FitRedCoeff/2]=freqs[startpos+i]; pcount++; int pnum=pcount; double pc=Cube[pcount]; pcount++; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitRedCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); } startpos=FitRedCoeff; } // printf("DM\n"); double nlist[((MNStruct *)context)->incFloatDM][2]; if(((MNStruct *)context)->incDM==2){ if(((MNStruct *)context)->incFloatDM == 0){ for (int i=0; i<FitDMCoeff/2; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=((MNStruct *)context)->sampleFreq[startpos/2 - ((MNStruct *)context)->incFloatRed+i]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitDMCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); pcount++; } } else if(((MNStruct *)context)->incFloatDM >0){ for (int i=0; i<FitDMCoeff/2 - ((MNStruct *)context)->incFloatDM ; i++){ int pnum=pcount; double pc=Cube[pcount]; freqs[startpos+i]=((MNStruct *)context)->sampleFreq[startpos/2 - ((MNStruct *)context)->incFloatRed +i]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitDMCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); pcount++; } for (int i=FitDMCoeff/2 - ((MNStruct *)context)->incFloatDM; i<FitDMCoeff/2; i++){ freqs[startpos+i]=Cube[pcount]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; pcount++; int pnum=pcount; double pc=Cube[pcount]; pcount++; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitDMCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); } } for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ DMVec[o]=1.0/(DMKappa*pow((double)((MNStruct *)context)->pulse->obsn[o].freqSSB,2)); } } else if(((MNStruct *)context)->incDM==3){ for(int pl = 0; pl < ((MNStruct *)context)->numFitDMPL; pl ++){ double DMamp=Cube[pcount]; pcount++; double DMindex=Cube[pcount]; pcount++; DMamp=pow(10.0, DMamp); for (int i=0; i<FitDMCoeff/2 - ((MNStruct *)context)->incFloatDM ; i++){ freqs[startpos+i]=(double)((MNStruct *)context)->sampleFreq[startpos/2 - ((MNStruct *)context)->incFloatRed +i]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; double PLcomp=DMamp*DMamp*pow((freqs[startpos+i]*365.25),-1.0*DMindex)/(maxtspan*24*60*60); powercoeff[startpos+i]+=PLcomp; powercoeff[startpos+i+FitDMCoeff/2]+=PLcomp; } } for (int i=0; i<FitDMCoeff/2 - ((MNStruct *)context)->incFloatDM ; i++){ freqdet=freqdet+2*log(powercoeff[startpos+i]); // printf("%i %g %g \n", i, powercoeff[startpos+i], freqdet); } for (int i= FitDMCoeff/2 - ((MNStruct *)context)->incFloatDM ; i<FitDMCoeff/2; i++){ //Cube[pcount]=floor(Cube[pcount]); freqs[startpos+i]=Cube[pcount]/maxtspan; freqs[startpos+i+FitDMCoeff/2]=freqs[startpos+i]; pcount++; int pnum=pcount; double pc=Cube[pcount]; pcount++; powercoeff[startpos+i]=pow(10.0,pc)/(maxtspan*24*60*60); powercoeff[startpos+i+FitDMCoeff/2]=powercoeff[startpos+i]; freqdet=freqdet+2*log(powercoeff[startpos+i]); } for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ DMVec[o]=1.0/(DMKappa*pow((double)((MNStruct *)context)->pulse->obsn[o].freqSSB,2)); } } LRedGPUWrapper_(freqs, Resvec, BATvec, DMVec, WorkNoise, FNF, NFd, ((MNStruct *)context)->pulse->nobs, FitRedCoeff, FitDMCoeff, totCoeff,((MNStruct *)context)->incRED, ((MNStruct *)context)->incDM); double **PPFM=new double*[totCoeff]; for(int i=0;i<totCoeff;i++){ PPFM[i]=new double[totCoeff]; for(int j=0;j<totCoeff;j++){ PPFM[i][j]=0; } } for(int c1=0; c1<totCoeff; c1++){ PPFM[c1][c1]=1.0/powercoeff[c1]; } for(int j=0;j<totCoeff;j++){ for(int k=0;k<totCoeff;k++){ PPFM[j][k]=PPFM[j][k]+FNF[j][k]; } } double jointdet=0; double freqlike=0; double *WorkCoeff = new double[totCoeff]; for(int o1=0;o1<totCoeff; o1++){ WorkCoeff[o1]=NFd[o1]; } int info=0; dpotrfInfo(PPFM, totCoeff, jointdet, info); dpotrs(PPFM, WorkCoeff, totCoeff); for(int j=0;j<totCoeff;j++){ freqlike += NFd[j]*WorkCoeff[j]; } lnew=-0.5*(((double)((MNStruct *)context)->pulse->nobs)*log(2.0*M_PI) + tdet+jointdet+freqdet+timelike-freqlike); if(isnan(lnew) || isinf(lnew)){ lnew=-pow(10.0,200); // printf("red amp and alpha %g %g\n",redamp,redalpha); } //printf("Like: %g %g %g %g %g %g\n",lnew,jointdet,tdet,freqdet,timelike,freqlike); //printf("CPULIKE: %g %g %g %g %g %g \n", lnew, jointdet,tdet,freqdet,timelike,freqlike); delete[] EFAC; delete[] EQUAD; delete[] WorkNoise; delete[] powercoeff; delete[] Resvec; delete[] BATvec; delete[] NFd; delete[] freqs; delete[] DMVec; delete[] WorkCoeff; for (int j = 0; j < totCoeff; j++){ delete[] PPFM[j]; } delete[] PPFM; for (int j = 0; j < totCoeff; j++){ delete[] FNF[j]; } delete[] FNF; }
void WhiteMarginGPULogLike(double *Cube, int &ndim, int &npars, double &lnew, void *context) { clock_t startClock,endClock; double **EFAC; double *EQUAD; int pcount=0; int totdims = ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; totdims += ((MNStruct *)context)->numFitEFAC*((MNStruct *)context)->EPolTerms + ((MNStruct *)context)->numFitEQUAD; totdims +=2*((MNStruct *)context)->incStep; if(((MNStruct *)context)->yearlyDM == 1)totdims+=2; if(((MNStruct *)context)->incsinusoid == 1)totdims+=3; int numfit=((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; long double LDparams[numfit]; double Fitparams[numfit]; double *Resvec=new double[((MNStruct *)context)->pulse->nobs]; int fitcount=0; for(int p=0;p<totdims;p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ Cube[pcount]=(((MNStruct *)context)->Dpriors[p][1]-((MNStruct *)context)->Dpriors[p][0])*Cube[pcount]+((MNStruct *)context)->Dpriors[p][0]; pcount++; } } pcount=0; if(((MNStruct *)context)->doLinear==0){ for(int p=0;p< ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ LDparams[p]=Cube[fitcount]*(((MNStruct *)context)->LDpriors[p][1]) + (((MNStruct *)context)->LDpriors[p][0]); fitcount++; } else if(((MNStruct *)context)->Dpriors[p][1] == ((MNStruct *)context)->Dpriors[p][0]){ LDparams[p]=((MNStruct *)context)->Dpriors[p][0]*(((MNStruct *)context)->LDpriors[p][1]) + (((MNStruct *)context)->LDpriors[p][0]); } } pcount=0; double phase=(double)LDparams[0]; pcount++; for(int p=1;p<((MNStruct *)context)->numFitTiming;p++){ ((MNStruct *)context)->pulse->param[((MNStruct *)context)->TempoFitNums[p][0]].val[((MNStruct *)context)->TempoFitNums[p][1]] = LDparams[pcount]; pcount++; } for(int p=0;p<((MNStruct *)context)->numFitJumps;p++){ ((MNStruct *)context)->pulse->jumpVal[((MNStruct *)context)->TempoJumpNums[p]]= LDparams[pcount]; pcount++; } fastformBatsAll(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars); /* Form Barycentric arrival times */ formResiduals(((MNStruct *)context)->pulse,((MNStruct *)context)->numberpulsars,1); /* Form residuals */ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].residual+phase; } } else if(((MNStruct *)context)->doLinear==1){ fitcount=0; for(int p=0;p< ((MNStruct *)context)->numFitTiming + ((MNStruct *)context)->numFitJumps; p++){ if(((MNStruct *)context)->Dpriors[p][1] != ((MNStruct *)context)->Dpriors[p][0]){ Fitparams[p]=Cube[fitcount]; fitcount++; } else if(((MNStruct *)context)->Dpriors[p][1] == ((MNStruct *)context)->Dpriors[p][0]){ Fitparams[p]=0; } } double *Fitvec=new double[((MNStruct *)context)->pulse->nobs]; dgemv(((MNStruct *)context)->DMatrix,Fitparams,Fitvec,((MNStruct *)context)->pulse->nobs,numfit,'N'); for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]=((MNStruct *)context)->pulse->obsn[o].residual-Fitvec[o]; } delete[] Fitvec; } pcount=fitcount; if(((MNStruct *)context)->incStep > 0){ for(int i = 0; i < ((MNStruct *)context)->incStep; i++){ double StepAmp = Cube[pcount]; pcount++; double StepTime = Cube[pcount]; pcount++; for(int o1=0;o1<((MNStruct *)context)->pulse->nobs; o1++){ if(((MNStruct *)context)->pulse->obsn[o1].bat > StepTime){ Resvec[o1] += StepAmp; } } } } ///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////Get White Noise vector/////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////// double equadpriorterm=0; if(((MNStruct *)context)->numFitEFAC == 0){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=1; } } else{ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=0; } } } } else if(((MNStruct *)context)->numFitEFAC == 1){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=Cube[pcount]; } pcount++; } else{ for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EFAC[n-1][o]=pow(10.0,Cube[pcount]); } pcount++; } } } else if(((MNStruct *)context)->numFitEFAC > 1){ EFAC=new double*[((MNStruct *)context)->EPolTerms]; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFAC[n-1]=new double[((MNStruct *)context)->systemcount]; if(n==1){ for(int p=0;p< ((MNStruct *)context)->systemcount; p++){ EFAC[n-1][p]=Cube[pcount]; pcount++; } } else{ for(int p=0;p< ((MNStruct *)context)->systemcount; p++){ EFAC[n-1][p]=pow(10.0,Cube[pcount]); pcount++; } } } } if(((MNStruct *)context)->numFitEQUAD == 0){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=0; } } else if(((MNStruct *)context)->numFitEQUAD == 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); equadpriorterm+=log(pow(10.0,Cube[pcount])); } pcount++; } else if(((MNStruct *)context)->numFitEQUAD > 1){ EQUAD=new double[((MNStruct *)context)->systemcount]; for(int o=0;o<((MNStruct *)context)->systemcount; o++){ EQUAD[o]=pow(10.0,2*Cube[pcount]); equadpriorterm+=log(pow(10.0,Cube[pcount])); pcount++; } } double *Noise; double *BATvec; Noise=new double[((MNStruct *)context)->pulse->nobs]; BATvec=new double[((MNStruct *)context)->pulse->nobs]; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ BATvec[o]=(double)((MNStruct *)context)->pulse->obsn[o].bat; } if(((MNStruct *)context)->whitemodel == 0){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ double EFACterm=0; for(int n=1; n <=((MNStruct *)context)->EPolTerms; n++){ EFACterm=EFACterm + pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6))/pow(pow(10.0,-7),n-1),n)*EFAC[n-1][((MNStruct *)context)->sysFlags[o]]; } Noise[o]= 1.0/(pow(EFACterm,2) + EQUAD[((MNStruct *)context)->sysFlags[o]]); } } else if(((MNStruct *)context)->whitemodel == 1){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Noise[o]=1.0/(EFAC[0][((MNStruct *)context)->sysFlags[o]]*EFAC[0][((MNStruct *)context)->sysFlags[o]]*(pow(((((MNStruct *)context)->pulse->obsn[o].toaErr)*pow(10.0,-6)),2) + EQUAD[((MNStruct *)context)->sysFlags[o]])); } } if(((MNStruct *)context)->incsinusoid == 1){ double start,end; int go=0; for (int i=0;i<((MNStruct *)context)->pulse->nobs;i++) { if (((MNStruct *)context)->pulse->obsn[i].deleted==0) { if (go==0) { go = 1; start = (double)((MNStruct *)context)->pulse->obsn[i].bat; end = start; } else { if (start > (double)((MNStruct *)context)->pulse->obsn[i].bat) start = (double)((MNStruct *)context)->pulse->obsn[i].bat; if (end < (double)((MNStruct *)context)->pulse->obsn[i].bat) end = (double)((MNStruct *)context)->pulse->obsn[i].bat; } } } double maxtspan=1*(end-start); double sineamp=pow(10.0,Cube[pcount]); pcount++; double sinephase=Cube[pcount]; pcount++; double sinefreq=pow(10.0,Cube[pcount])/maxtspan; pcount++; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]-= sineamp*sin(2*M_PI*sinefreq*(double)((MNStruct *)context)->pulse->obsn[o].bat + sinephase); } } if(((MNStruct *)context)->yearlyDM == 1){ double DMKappa = 2.410*pow(10.0,-16); double yearlyamp=pow(10.0,Cube[pcount]); pcount++; double yearlyphase=Cube[pcount]; pcount++; for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ Resvec[o]-= yearlyamp*sin((2*M_PI/365.25)*(double)((MNStruct *)context)->pulse->obsn[o].bat + yearlyphase)/(DMKappa*pow((double)((MNStruct *)context)->pulse->obsn[o].freqSSB,2)); } } ///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////Get Time domain likelihood////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////// double tdet=0; double timelike=0; for(int o=0; o<((MNStruct *)context)->pulse->nobs; o++){ timelike+=Resvec[o]*Resvec[o]*Noise[o]; tdet -= log(Noise[o]); } ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////get TNDMVec//////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// int TimetoMargin=0; for(int i =0; i < ((MNStruct *)context)->numFitTiming+((MNStruct *)context)->numFitJumps; i++){ if(((MNStruct *)context)->LDpriors[i][2]==1)TimetoMargin++; } double **TNDM=new double*[((MNStruct *)context)->pulse->nobs]; for(int i=0;i<((MNStruct *)context)->pulse->nobs;i++){ TNDM[i]=new double[TimetoMargin]; } double *TNDMVec=new double[((MNStruct *)context)->pulse->nobs*TimetoMargin]; if(TimetoMargin != ((MNStruct *)context)->numFitTiming+((MNStruct *)context)->numFitJumps){ getCustomDMatrixLike(context, TNDM); for(int g=0;g<TimetoMargin; g++){ for(int o=0;o<((MNStruct *)context)->pulse->nobs; o++){ TNDMVec[g*((MNStruct *)context)->pulse->nobs + o]=TNDM[o][g]; } } } ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////Call GPU Code///////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// double *likevals=new double[2]; int totalsize=TimetoMargin; WhiteMarginGPUWrapper_(context, TNDMVec, Resvec, BATvec, Noise, ((MNStruct *)context)->pulse->nobs, TimetoMargin, totalsize, ((MNStruct *)context)->numFitTiming, ((MNStruct *)context)->numFitJumps, likevals); ////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////calculate likelihood/////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////// double jointdet=likevals[0]; double freqlike=likevals[1]; lnew=-0.5*(tdet+jointdet+timelike-freqlike) + equadpriorterm; if(isnan(lnew) || isinf(lnew)){ lnew=-pow(10.0,20); // printf("red amp and alpha %g %g\n",redamp,redalpha); // printf("Like: %g %g %g \n",lnew,Chisq,covdet); } delete[] EFAC; delete[] EQUAD; delete[] Noise; delete[] Resvec; delete[] BATvec; delete[] likevals; for (int j = 0; j < ((MNStruct *)context)->pulse->nobs; j++){ delete[]TNDM[j]; } delete[]TNDM; delete[]TNDMVec; //printf("GPUChisq: %.8g %.8g %.8g %.8g %.8g %.8g \n",lnew,jointdet,tdet,freqdet,timelike,freqlike); // if(isinf(lnew) || isinf(jointdet) || isinf(tdet) || isinf(freqdet) || isinf(timelike) || isinf(freqlike)){ //printf("Chisq: %g %g %g %g %g %g \n",lnew,jointdet,tdet,freqdet,timelike,freqlike); // } }