static PyObject * _Bas_bspdeval(PyObject *self, PyObject *args)
{
int d, mc, nc, n;
npy_intp dim[2];
double u, **ctrlmat, **pntmat;
PyObject *input_ctrl, *input_k;
PyArrayObject *ctrl, *k, *pnt;
if(!PyArg_ParseTuple(args, "iOOdi", &d, &input_ctrl, &input_k, &u, &n))
return NULL;
ctrl = (PyArrayObject *) PyArray_ContiguousFromObject(input_ctrl, PyArray_DOUBLE, 2, 2);
if(ctrl == NULL)
return NULL;
k = (PyArrayObject *) PyArray_ContiguousFromObject(input_k, PyArray_DOUBLE, 1, 1);
if(k == NULL)
return NULL;
mc = ctrl->dimensions[0];
nc = ctrl->dimensions[1];
dim[0] = mc;
dim[1] = n + 1;
pnt = (PyArrayObject *) PyArray_SimpleNew(2, dim, PyArray_DOUBLE);
ctrlmat = vec2mat(ctrl->data, mc, nc);
pntmat = vec2mat(pnt->data, mc, n + 1);
_bspdeval(d, ctrlmat, mc, nc, (double *)k->data, k->dimensions[0], u, n, pntmat);
free(pntmat);
free(ctrlmat);
Py_DECREF(ctrl);
Py_DECREF(k);
return PyArray_Return(pnt);
}
static PyObject * _Bas_bspeval(PyObject *self, PyObject *args)
{
int d, mc, nc, nu, dim[2];
double **ctrlmat, **pntmat;
PyObject *input_ctrl, *input_k, *input_u;
PyArrayObject *ctrl, *k, *u, *pnt;
if(!PyArg_ParseTuple(args, "iOOO", &d, &input_ctrl, &input_k, &input_u))
return NULL;
ctrl = (PyArrayObject *) PyArray_ContiguousFromObject(input_ctrl, PyArray_DOUBLE, 2, 2);
if(ctrl == NULL)
return NULL;
k = (PyArrayObject *) PyArray_ContiguousFromObject(input_k, PyArray_DOUBLE, 1, 1);
if(k == NULL)
return NULL;
u = (PyArrayObject *) PyArray_ContiguousFromObject(input_u, PyArray_DOUBLE, 1, 1);
if(u == NULL)
return NULL;
mc = ctrl->dimensions[0];
nc = ctrl->dimensions[1];
nu = u->dimensions[0];
dim[0] = mc;
dim[1] = nu;
pnt = (PyArrayObject *) PyArray_FromDims(2, dim, PyArray_DOUBLE);
ctrlmat = vec2mat(ctrl->data, mc, nc);
pntmat = vec2mat(pnt->data, mc, nu);
_bspeval(d, ctrlmat, mc, nc, (double *)k->data, k->dimensions[0], (double *)u->data, nu, pntmat);
free(pntmat);
free(ctrlmat);
Py_DECREF(ctrl);
Py_DECREF(k);
Py_DECREF(u);
return PyArray_Return(pnt);
}
void P_nested(vector<double> &P, const vector<double> &par,
const NumericMatrix &Theta, const int &N, const int &nfact, const int &ncat,
const int &correct)
{
NumericVector dummy(1);
const int par_size = par.size();
vector<double> dpar(nfact+3), npar(par_size - nfact - 3, 1.0);
for(int i = 0; i < nfact+3; ++i)
dpar[i] = par[i];
for(int i = nfact+3; i < par_size; ++i)
npar[i - (nfact+3) + nfact] = par[i];
vector<double> Pd(N*2), Pn(N*(ncat-1));
P_dich(Pd, dpar, Theta, dummy, N, nfact);
P_nominal(Pn, npar, Theta, dummy, N, nfact, ncat-1, 0, 0);
NumericMatrix PD = vec2mat(Pd, N, 2);
NumericMatrix PN = vec2mat(Pn, N, ncat-1);
int k = 0, which = 0;
for(int i = 0; i < ncat; ++i){
if((i+1) == correct){
for(int j = 0; j < N; ++j){
P[which] = PD(j,1);
++which;
}
--k;
} else {
for(int j = 0; j < N; ++j){
P[which] = PD(j,0) * PN(j,k);
++which;
}
}
++k;
}
}
static PyObject * _Bas_bspdegelev(PyObject *self, PyObject *args)
{
int d, mc, nc, nk, t, nh;
npy_intp dim[2];
double **ctrlmat, **icmat;
PyObject *input_ctrl, *input_k;
PyArrayObject *ctrl, *k, *ic, *ik;
if(!PyArg_ParseTuple(args, "iOOi", &d, &input_ctrl, &input_k, &t))
return NULL;
ctrl = (PyArrayObject *) PyArray_ContiguousFromObject(input_ctrl, PyArray_DOUBLE, 2, 2);
if(ctrl == NULL)
return NULL;
k = (PyArrayObject *) PyArray_ContiguousFromObject(input_k, PyArray_DOUBLE, 1, 1);
if(k == NULL)
return NULL;
mc = ctrl->dimensions[0];
nc = ctrl->dimensions[1];
nk = k->dimensions[0];
dim[0] = mc;
dim[1] = nc*(t + 1);
ic = (PyArrayObject *) PyArray_SimpleNew(2, dim, PyArray_DOUBLE);
ctrlmat = vec2mat(ctrl->data, mc, nc);
icmat = vec2mat(ic->data, mc, nc*(t + 1));
dim[0] = (t + 1)*nk;
ik = (PyArrayObject *) PyArray_SimpleNew(1, dim, PyArray_DOUBLE);
_bspdegelev(d, ctrlmat, mc, nc, (double *)k->data, nk, t, &nh, icmat, (double *)ik->data);
free(icmat);
free(ctrlmat);
Py_DECREF(ctrl);
Py_DECREF(k);
return Py_BuildValue("(OOi)", (PyObject *)ic, (PyObject *)ik, nh);
}
RcppExport SEXP computeItemTrace(SEXP Rpars, SEXP RTheta, SEXP Ritemloc, SEXP Roffterm)
{
BEGIN_RCPP
const List pars(Rpars);
const NumericMatrix Theta(RTheta);
const NumericMatrix offterm(Roffterm);
const vector<int> itemloc = as< vector<int> >(Ritemloc);
const int J = itemloc.size() - 1;
const int nfact = Theta.ncol();
const int N = Theta.nrow();
vector<double> itemtrace(N * (itemloc[J]-1));
S4 item = pars[0];
NumericMatrix FD = item.slot("fixed.design");
int USEFIXED = 0;
if(FD.nrow() > 2) USEFIXED = 1;
for(int which = 0; which < J; ++which)
_computeItemTrace(itemtrace, Theta, pars, offterm(_, which), itemloc,
which, nfact, N, USEFIXED);
NumericMatrix ret = vec2mat(itemtrace, N, itemloc[J]-1);
return(ret);
END_RCPP
}
//Estep for bfactor
RcppExport SEXP Estepbfactor(SEXP Ritemtrace, SEXP Rprior, SEXP RPriorbetween, SEXP RX,
SEXP Rr, SEXP Rsitems, SEXP RPrior, SEXP Rncores)
{
BEGIN_RCPP
List ret;
const NumericMatrix itemtrace(Ritemtrace);
const vector<double> prior = as< vector<double> >(Rprior);
const vector<double> Priorbetween = as< vector<double> >(RPriorbetween);
const vector<double> Prior = as< vector<double> >(RPrior);
const vector<int> r = as< vector<int> >(Rr);
const int ncores = as<int>(Rncores);
const IntegerMatrix data(RX);
const IntegerMatrix sitems(Rsitems);
const int nitems = data.ncol();
const int npquad = prior.size();
const int nbquad = Priorbetween.size();
const int nquad = nbquad * npquad;
const int npat = r.size();
vector<double> expected(npat);
vector<double> r1vec(nquad*nitems, 0.0);
vector<double> r2vec(nbquad, 0.0);
_Estepbfactor(expected, r1vec, r2vec, itemtrace, prior, Priorbetween, r, ncores,
data, sitems, Prior);
NumericMatrix r1 = vec2mat(r1vec, nquad, nitems);
ret["r1"] = r1;
ret["expected"] = wrap(expected);
ret["r2"] = wrap(r2vec);
return(ret);
END_RCPP
}
static PyObject * _Bas_bspbezdecom(PyObject *self, PyObject *args)
{
int i, b, c, d, mc, nc, nk, m;
npy_intp dim[2];
double **ctrlmat, **icmat;
PyObject *input_ctrl, *input_k;
PyArrayObject *ctrl, *k, *ic;
if(!PyArg_ParseTuple(args, "iOO", &d, &input_ctrl, &input_k))
return NULL;
ctrl = (PyArrayObject *) PyArray_ContiguousFromObject(input_ctrl, PyArray_DOUBLE, 2, 2);
if(ctrl == NULL)
return NULL;
k = (PyArrayObject *) PyArray_ContiguousFromObject(input_k, PyArray_DOUBLE, 1, 1);
if(k == NULL)
return NULL;
mc = ctrl->dimensions[0];
nc = ctrl->dimensions[1];
nk = k->dimensions[0];
i = d + 1;
c = 0;
m = nk - d - 1;
while (i < m)
{
b = 1;
while (i < m && *(double *)(k->data + i * k->strides[0]) == *(double *)(k->data + (i + 1) * k->strides[0]))
{
b++;
i++;
}
if(b < d)
c = c + (d - b);
i++;
}
dim[0] = mc;
dim[1] = nc+c;
ic = (PyArrayObject *) PyArray_SimpleNew(2, dim, PyArray_DOUBLE);
ctrlmat = vec2mat(ctrl->data, mc, nc);
icmat = vec2mat(ic->data, mc, nc+c);
_bspbezdecom(d, ctrlmat, mc, nc, (double *)k->data, nk, icmat);
free(icmat);
free(ctrlmat);
Py_DECREF(ctrl);
Py_DECREF(k);
return Py_BuildValue("O", ic);
}
static PyObject * _Bas_bspkntins(PyObject *self, PyObject *args)
{
int d, mc, nc, nk, nu;
npy_intp dim[2];
double **ctrlmat, **icmat;
PyObject *input_ctrl, *input_k, *input_u;
PyArrayObject *ctrl, *k, *u, *ic, *ik;
if(!PyArg_ParseTuple(args, "iOOO", &d, &input_ctrl, &input_k, &input_u))
return NULL;
ctrl = (PyArrayObject *) PyArray_ContiguousFromObject(input_ctrl, PyArray_DOUBLE, 2, 2);
if(ctrl == NULL)
return NULL;
k = (PyArrayObject *) PyArray_ContiguousFromObject(input_k, PyArray_DOUBLE, 1, 1);
if(k == NULL)
return NULL;
u = (PyArrayObject *) PyArray_ContiguousFromObject(input_u, PyArray_DOUBLE, 1, 1);
if(u == NULL)
return NULL;
mc = ctrl->dimensions[0];
nc = ctrl->dimensions[1];
nk = k->dimensions[0];
nu = u->dimensions[0];
dim[0] = mc;
dim[1] = nc + nu;
ic = (PyArrayObject *) PyArray_SimpleNew(2, dim, PyArray_DOUBLE);
ctrlmat = vec2mat(ctrl->data, mc, nc);
icmat = vec2mat(ic->data, mc, nc + nu);
dim[0] = nk + nu;
ik = (PyArrayObject *) PyArray_SimpleNew(1, dim, PyArray_DOUBLE);
_bspkntins(d, ctrlmat, mc, nc, (double *)k->data, nk, (double *)u->data, nu, icmat, (double *)ik->data);
free(icmat);
free(ctrlmat);
Py_DECREF(ctrl);
Py_DECREF(k);
Py_DECREF(u);
return Py_BuildValue("(OO)", (PyObject *)ic, (PyObject *)ik);
}
RcppExport SEXP partcompTraceLinePts(SEXP Rpar, SEXP RTheta)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const NumericMatrix Theta(RTheta);
const int nfact = Theta.ncol();
const int N = Theta.nrow();
vector<double> P(N*2);
P_comp(P, par, Theta, N, nfact);
NumericMatrix ret = vec2mat(P, N, 2);
return(ret);
END_RCPP
}
RcppExport SEXP traceLinePts(SEXP Rpar, SEXP RTheta, SEXP Rot)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const NumericVector ot(Rot);
const NumericMatrix Theta(RTheta);
const int N = Theta.nrow();
const int nfact = Theta.ncol();
vector<double> P(N*2);
P_dich(P, par, Theta, ot, N, nfact);
NumericVector ret = vec2mat(P, N, 2);
return(ret);
END_RCPP
}
RcppExport SEXP nestlogitTraceLinePts(SEXP Rpar, SEXP RTheta, SEXP Rcorrect, SEXP Rncat)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const NumericMatrix Theta(RTheta);
const int correct = as<int>(Rcorrect);
const int ncat = as<int>(Rncat);
const int nfact = Theta.ncol();
const int N = Theta.nrow();
vector<double> P(N*ncat);
P_nested(P, par, Theta, N, nfact, ncat, correct);
NumericMatrix ret = vec2mat(P, N, ncat);
return(ret);
END_RCPP
}
RcppExport SEXP gpcmTraceLinePts(SEXP Rpar, SEXP RTheta, SEXP Rot, SEXP Risrating)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const NumericMatrix Theta(RTheta);
const int israting = as<int>(Risrating);
const int nfact = Theta.ncol();
const int N = Theta.nrow();
int ncat = (par.size() - nfact)/2;
NumericVector ot(Rot);
vector<double> P(N*ncat);
P_nominal(P, par, Theta, ot, N, nfact, ncat, 0, israting);
NumericMatrix ret = vec2mat(P, N, ncat);
return(ret);
END_RCPP
}
RcppExport SEXP nominalTraceLinePts(SEXP Rpar, SEXP Rncat, SEXP RTheta, SEXP RreturnNum, SEXP Rot)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const int ncat = as<int>(Rncat);
const NumericMatrix Theta(RTheta);
const int returnNum = as<int>(RreturnNum);
const int nfact = Theta.ncol();
const int N = Theta.nrow();
NumericVector ot(Rot);
vector<double> P(N*ncat);
P_nominal(P, par, Theta, ot, N, nfact, ncat, returnNum, 0);
NumericMatrix ret = vec2mat(P, N, ncat);
return(ret);
END_RCPP
}
static PyObject * _Bas_dersbasisfuns(PyObject *self, PyObject *args)
{
int d, s, k, ret, n;
double u;
double **pntmat;
int dim[2];
PyObject *input_U;
PyArrayObject *dN, *U;
ret=-1;
ret = PyArg_ParseTuple(args, "iOdiii", &s, &input_U, &u, &d, &k, &n);
if(!ret)
{
//printf("s/i=%d, u=%g, p/d=%d, k=%d, ret=%d\n", s, u, d, k, ret);
return NULL;
}
//printf("ok: s/i=%d, u=%g, p/d=%d, k=%d, ret=%d, n/nk=%d\n", s, u, d, k, ret, n);
U = (PyArrayObject *) PyArray_ContiguousFromObject(input_U, PyArray_DOUBLE, 1, 1);
if(U == NULL)
return NULL;
dim[0] = d+1;
dim[1] = k+1;
// dN array is p+1 in size
dN = (PyArrayObject *) PyArray_FromDims(2, dim, PyArray_DOUBLE);
pntmat = vec2mat(dN->data, d+1, k+1);
_dersbasisfuns(d, (double *)U->data, n, u, s, k, pntmat);
//printf("dN->data[0][0] = %g\n",pntmat[1][1]);
free(pntmat);
//static void _dersbasisfuns(int d, double *k, int nk, double u, int s,int n, double **ders)
// printf("*dN->data[0]=%g \n", (double *)dN->data[0]);
// printf("*dN->data[1]=%g \n", (double *)dN->data[1]);
//return (PyObject *)dN;
// return Py_BuildValue("O", (PyObject *)dN );
return PyArray_Return(dN);
}
// graded
RcppExport SEXP gradedTraceLinePts(SEXP Rpar, SEXP RTheta, SEXP Ritemexp, SEXP Rot, SEXP Risrating)
{
BEGIN_RCPP
const vector<double> par = as< vector<double> >(Rpar);
const NumericVector ot(Rot);
const NumericMatrix Theta(RTheta);
const int nfact = Theta.ncol();
const int N = Theta.nrow();
const int itemexp = as<int>(Ritemexp);
const int israting = as<int>(Risrating);
int nint = par.size() - nfact;
if(israting) --nint;
int totalcat = nint + 1;
if(!itemexp) ++totalcat;
vector<double> P(N * totalcat);
P_graded(P, par, Theta, ot, N, nfact, nint, itemexp, israting);
NumericMatrix ret = vec2mat(P, N, totalcat);
return(ret);
END_RCPP
}
//Estep for mirt
RcppExport SEXP Estep(SEXP Ritemtrace, SEXP Rprior, SEXP RX, SEXP Rr, SEXP Rncores)
{
BEGIN_RCPP
const vector<double> prior = as< vector<double> >(Rprior);
const vector<int> r = as< vector<int> >(Rr);
const IntegerMatrix data(RX);
const NumericMatrix itemtrace(Ritemtrace);
const int ncores = as<int>(Rncores);
const int nquad = prior.size();
const int nitems = data.ncol();
const int npat = r.size();
vector<double> expected(npat, 0.0);
vector<double> r1vec(nquad*nitems, 0.0);
List ret;
_Estep(expected, r1vec, prior, r, data, itemtrace, ncores);
NumericMatrix r1 = vec2mat(r1vec, nquad, nitems);
ret["r1"] = r1;
ret["expected"] = wrap(expected);
return(ret);
END_RCPP
}
void forward_shot_SH(struct waveAC *waveAC, struct PML_AC *PML_AC, struct matSH *matSH, float ** srcpos, int nshots, int ** recpos, int ntr, int nstage, int nfreq){
/* declaration of global variables */
extern int NSHOT1, NSHOT2, NONZERO, NX, NY, NXNY, NF;
extern int SNAP, SEISMO, MYID, INFO, N_STREAMER, READ_REC;
extern float DH;
extern char SNAP_FILE[STRING_SIZE];
extern FILE * FP;
/* declaration of local variables */
int ishot, status, nxsrc, nysrc, i;
double *null = (double *) NULL ;
int *Ap, *Ai;
double *Ax, *Az, *xr, *xi;
double time1, time2;
char filename[STRING_SIZE];
void *Symbolic, *Numeric;
/* Allocate memory for compressed sparse column form and solution vector */
Ap = malloc(sizeof(int)*(NXNY+1));
Ai = malloc(sizeof(int)*NONZERO);
Ax = malloc(sizeof(double)*NONZERO);
Az = malloc(sizeof(double)*NONZERO);
xr = malloc(sizeof(double)*NONZERO);
xi = malloc(sizeof(double)*NONZERO);
/* assemble acoustic impedance matrix */
init_A_SH_9p_pml(PML_AC,matSH,waveAC);
/* convert triplet to compressed sparse column format */
status = umfpack_zi_triplet_to_col(NXNY,NXNY,NONZERO,(*waveAC).irow,(*waveAC).icol,(*waveAC).Ar,(*waveAC).Ai,Ap,Ai,Ax,Az,NULL);
/* Here is something buggy (*waveAC).Ar != Ax and (*waveAC).Ai != Az.
Therefore, set Ax = (*waveAC).Ar and Az = (*waveAC).Ai */
for (i=0;i<NONZERO;i++){
Ax[i] = (*waveAC).Ar[i];
Az[i] = (*waveAC).Ai[i];
}
if((MYID==0)&&(INFO==1)){
printf("\n==================================== \n");
printf("\n ***** LU factorization ********** \n");
printf("\n==================================== \n\n");
time1=MPI_Wtime();
}
/* symbolic factorization */
status = umfpack_zi_symbolic(NXNY, NXNY, Ap, Ai, Ax, Az, &Symbolic, null, null);
/* sparse LU decomposition */
status = umfpack_zi_numeric(Ap, Ai, Ax, Az, Symbolic, &Numeric, null, null);
umfpack_zi_free_symbolic (&Symbolic);
if((MYID==0)&&(INFO==1)){
time2=MPI_Wtime();
printf("\n Finished after %4.2f s \n",time2-time1);
}
if((MYID==0)&&(INFO==1)){
printf("\n============================================================================================================= \n");
printf("\n ***** Solve elastic SH forward problem by FDFD for shot %d - %d (f = %3.2f Hz) on MPI process no. %d ********** \n",NSHOT1,NSHOT2-1,(*waveAC).freq, MYID);
printf("\n============================================================================================================= \n\n");
time1=MPI_Wtime();
}
/* loop over all shots */
for (ishot=NSHOT1;ishot<NSHOT2;ishot++){
/* read receiver positions from receiver files for each shot */
if(READ_REC==1){
acq.recpos=receiver(FP, &ntr, 1);
}
/* define source vector RHS */
RHS_source_AC(waveAC,srcpos,ishot);
/* solve forward problem by forward and back substitution */
status = umfpack_zi_solve(UMFPACK_A, Ap, Ai, Ax, Az, xr, xi, (*waveAC).RHSr, (*waveAC).RHSi, Numeric, null, null);
/* convert vector xr/xi to pr/pi */
vec2mat((*waveAC).pr,(*waveAC).pi,xr,xi);
/* write real part of pressure wavefield to file */
if(SNAP==1){
sprintf(filename,"%s_shot_%d.p",SNAP_FILE,ishot);
/* writemod(filename,(*waveAC).pr,3); */
writemod_true(filename,(*waveAC).pr,3);
}
/* write FD seismogram files */
if(SEISMO==1){
calc_seis_AC(waveAC,acq.recpos,ntr,ishot,nshots,nfreq);
}
if(READ_REC==1){
free_imatrix(acq.recpos,1,3,1,ntr);
ntr=0;
}
}
if((MYID==0)&&(INFO==1)){
time2=MPI_Wtime();
printf("\n Finished after %4.2f s \n",time2-time1);
}
/* free memory */
free(Ap); free(Ai); free(Ax); free(Az); free(xr); free(xi);
umfpack_zi_free_numeric (&Numeric);
}
//EAP estimates used in multipleGroup
RcppExport SEXP EAPgroup(SEXP Rlogitemtrace, SEXP Rtabdata, SEXP RTheta, SEXP Rprior, SEXP Rmu,
SEXP Rfull, SEXP Rr, SEXP Rncores)
{
BEGIN_RCPP
const int ncores = as<int>(Rncores);
const int full = as<int>(Rfull);
const vector<int> r = as< vector<int> >(Rr);
#ifdef SUPPORT_OPENMP
omp_set_num_threads(ncores);
#endif
const NumericMatrix logitemtrace(Rlogitemtrace);
const IntegerMatrix tabdata(Rtabdata);
const NumericMatrix Theta(RTheta);
const vector<double> prior = as< vector<double> >(Rprior);
const vector<double> mu = as< vector<double> >(Rmu);
const int n = prior.size(); //nquad
const int N = tabdata.nrow();
const int nitems = tabdata.ncol();
const int nfact = Theta.ncol();
vector<double> scores(N * nfact);
vector<double> scores2(N * nfact*(nfact + 1)/2);
#pragma omp parallel for
for(int pat = 0; pat < N; ++pat){
vector<double> L(n, 0.0);
for(int j = 0; j < n; ++j){
for(int i = 0; i < nitems; ++i)
if(tabdata(pat, i))
L[j] = L[j] + logitemtrace(j, i);
}
vector<double> thetas(nfact, 0.0);
double denom = 0.0;
for(int j = 0; j < n; ++j){
L[j] = exp(L[j] + log(prior[j]));
denom += L[j];
}
for(int k = 0; k < nfact; ++k){
for(int j = 0; j < n; ++j)
thetas[k] += Theta(j, k) * L[j] / denom;
scores[pat + k*N] = thetas[k];
}
int ind = 0;
vector<double> thetas2(nfact*(nfact+1)/2, 0.0);
for(int i = 0; i < nfact; ++i){
for(int k = 0; k < nfact; ++k){
if(i <= k){
for(int j = 0; j < n; ++j)
thetas2[ind] += (Theta(j, i) - thetas[i]) * (Theta(j, k) - thetas[k]) *
L[j] / denom;
thetas2[ind] += (thetas[i] - mu[i]) * (thetas[k] - mu[k]);
scores2[pat + ind*N] = thetas2[ind];
ind += 1;
}
}
}
}
if(full){
const int NN = std::accumulate(r.begin(), r.end(), 0);
NumericMatrix fullscores(NN, nfact);
NumericMatrix scoresmat = vec2mat(scores, N, nfact);
int ind = 0;
for(int pat = 0; pat < N; ++pat){
for(int j = 0; j < r[pat]; ++j){
for(int i = 0; i < nfact; ++i)
fullscores(ind, i) = scoresmat(pat, i);
++ind;
}
}
return(fullscores);
}
List ret;
ret["scores"] = vec2mat(scores, N, nfact);
ret["scores2"] = vec2mat(scores2, N, nfact*(nfact + 1)/2);
return(ret);
END_RCPP
}
double
Spacetime::IterateOptimization_discrete(void)
{
//----------------------------------------------------------------------------------------------//
// clear the state and costate sequences from the last solver iteration //
//----------------------------------------------------------------------------------------------//
setState(state_0);
GSequence.clear();
CSequence.clear();
MSequence.clear();
MInvSequence.clear();
stateSequence.clear();
costateSequence.clear();
//----------------------------------------------------------------------------------------------//
// compute the G, C, and M matrices for each timestep before the backwards sweep //
//----------------------------------------------------------------------------------------------//
setState(state_0);
stateSequence.push_back(state_0);
for (int t = 0; t < numTimeSteps; t++)
{
// compute G, C, M, and MInv state matrices
matrix<double> G = computeG_discrete();
matrix<double> M = computeM_discrete();
matrix<double> C = computeC_discrete();
matrix<double> MInv = !M;
// store G, C, M, and MInv state matrices
GSequence.push_back(G);
CSequence.push_back(C);
MSequence.push_back(M);
MInvSequence.push_back(MInv);
// apply torque and advance system
stepPhysics_discrete(uSequence[t]);
// store current state
stateSequence.push_back(getState());
}
//----------------------------------------------------------------------------------------------//
// backwards sweep //
//----------------------------------------------------------------------------------------------//
// clear values from previous pass
kSequence.clear();
KSequence.clear();
valueSequence.clear();
// declare V derivative values
matrix<double> Vx = -1.0 * ~(state_d - stateSequence[numTimeSteps-1]);
matrix<double> Vxx = I(4);
// backward pass
double final_error = 0.5 * (~(state_d - stateSequence[numTimeSteps-1])*(state_d - stateSequence[numTimeSteps-1]))(0,0);
valueSequence.push_back(final_error);
for (int t = numTimeSteps-1; t >= 0; t--)
{
// compute value V = cost-to-go function =
// sum of u^2 values for all successive timesteps + final error
matrix<double> cost_t = ~uSequence[t]*uSequence[t];
valueSequence.push_back(cost_t(0,0) + valueSequence[t]);
// compute first derivative values at current timestep
matrix<double> Lu = compute_Lu(t);
matrix<double> Lx = compute_Lx(t);
matrix<double> Fx = compute_Fx(t);
matrix<double> Fu = compute_Fu(t);
// compute second derivative values at current timestep
matrix<double> Luu = compute_Luu(t);
matrix<double> Lxx = compute_Lxx(t);
matrix<double> Lux = compute_Lux(t);
matrix<double> Lxu = compute_Lxu(t);
std::vector<matrix<double>> Fuu = compute_Fuu(t);
std::vector<matrix<double>> F*x = compute_Fux(t);
std::vector<matrix<double>> Fxu = compute_Fxu(t);
std::vector<matrix<double>> Fxx = compute_Fxx(t);
// compute expansion coefficients for current timestep
matrix<double> Qx = Lx + Vx*Fx;
matrix<double> Qu = Lu + Vx*Fu;
matrix<double> Qxx = Lxx + ~Fx*Vxx*Fx + vec2mat(vectorMatrixProduct(vectorTranspose(Fxx), ~Vx));
matrix<double> Quu = Luu + ~Fu*Vxx*Fu + vec2mat(vectorMatrixProduct(vectorTranspose(Fuu), ~Vx));
matrix<double> Qux = Lux + ~Fu*Vxx*Fx + vec2mat(vectorMatrixProduct(vectorTranspose(F*x), ~Vx));
matrix<double> Qxu = Lxu + ~Fx*Vxx*Fu + vec2mat(vectorMatrixProduct(vectorTranspose(Fxu), ~Vx));
// compute k and K terms for current timestep
kSequence.push_back(-(!Quu)*(~Qu));
KSequence.push_back(-(!Quu)*Qux);
// compute Vx and Vxx terms for current timestep
Vx = Qx - Qu*!Quu*Qux;
Vxx = Qxx - Qxu*!Quu*Qux;
}
// reverse sequences for forward pass
std::reverse(kSequence.begin(), KSequence.end());
std::reverse(KSequence.begin(), KSequence.end());
std::reverse(valueSequence.begin(), valueSequence.end());
// forward pass
std::vector<matrix<double>> xHatSequence;
std::vector<matrix<double>> uHatSequence;
xHatSequence.push_back(state_0);
for (int t = 0; t < numTimeSteps; t++) {
// compute uHat
matrix<double> uHat = uSequence[t] + kSequence[t] + KSequence[t]*(xHatSequence[t] - stateSequence[t]);
uHatSequence.push_back(uHat);
// compute next xHat
matrix<double> xHat = F(xHatSequence[t], uHat);
xHatSequence.push_back(xHat);
}
double uDiff = SSDvector(uSequence, uHatSequence);
stateSequence = xHatSequence;
uSequence = uHatSequence;
return uDiff;
}
