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);
// }
}
