double SPDTensorDL::f(Variable *x) const
{
const double *xptr = x->ObtainReadData();
/*each slice of Xaj is B alpha_j in [(6), CS15]*/
SharedSpace *Xalpha = new SharedSpace(3, dim, dim, N);
double *Xalphaptr = Xalpha->ObtainWriteEntireData();
integer dd = dim * dim, nnum = num, NN = N;
/*Xalpha <-- \mathbb{B} alpha*/
dgemm_(GLOBAL::N, GLOBAL::N, &dd, &NN, &nnum, &GLOBAL::DONE, const_cast<double *> (xptr), &dd,
alpha, &nnum, &GLOBAL::DZERO, Xalphaptr, &dd);
x->AddToTempData("Xalpha", Xalpha);
/*compute cholesky decomposition for all slices in Xalpha*/
SPDTensor *Mani = dynamic_cast<SPDTensor *> (Domain);
Mani->CholeskyRepresentation(x);
const SharedSpace *SharedL = x->ObtainReadTempData("XaL");
const double *L = SharedL->ObtainReadData();
SharedSpace *SharedlogLXL = new SharedSpace(3, dim, dim, N);
double *logLXL = SharedlogLXL->ObtainWriteEntireData();
double *Ltmp = new double[dim * dim];
integer length = dim * dim, ddim = dim, info;
for (integer i = 0; i < N; i++)
{
dcopy_(&length, const_cast<double *> (L) + i * length, &GLOBAL::IONE, Ltmp, &GLOBAL::IONE);
/*Solve the linear system Ls X = Li, i.e., X = Ls^{-1} Li. The solution X is stored in Li.
Note that Li is a lower triangular matrix.
Details: http://www.netlib.org/lapack/explore-html/d6/d6f/dtrtrs_8f.html */
dtrtrs_(GLOBAL::L, GLOBAL::N, GLOBAL::N, &ddim, &ddim, Ls + dim * dim * i, &ddim, Ltmp, &ddim, &info);
if (info != 0)
{
std::cout << "Warning: Solving linear system in SPDTensorDL::f failed with info:" << info << "!" << std::endl;
}
dgemm_(GLOBAL::N, GLOBAL::T, &ddim, &ddim, &ddim, &GLOBAL::DONE, Ltmp, &ddim, Ltmp, &ddim, &GLOBAL::DZERO, logLXL + ddim * ddim * i, &ddim);
Matrix MMt(logLXL + ddim * ddim * i, ddim, ddim);
Matrix::LogSymmetricM(GLOBAL::L, MMt, MMt);
}
delete[] Ltmp;
length = dim * dim * N;
double result = dnrm2_(&length, logLXL, &GLOBAL::IONE);
x->AddToTempData("logLXL", SharedlogLXL);
result *= result;
result /= 2.0;
/*add \Omega(X) = \sum \tr(X_i)*/
for (integer i = 0; i < num; i++)
{
for (integer j = 0; j < dim; j++)
{
result += lambdaX * xptr[i * dim * dim + j * dim + j];
}
}
return result;
};
void L2Sphere::DiffRetraction(Variable *x, Vector *etax, Variable *y, Vector *xix, Vector *result, bool IsEtaXiSameDir) const
{
if (IsEtaXiSameDir)
{
VectorTransport(x, etax, y, xix, result);
if (IsEtaXiSameDir && (HasHHR || UpdBetaAlone))
{
const double *etaxTV = etax->ObtainReadData();
const double *xixTV = xix->ObtainReadData();
double EtatoXi = sqrt(Metric(x, etax, etax) / Metric(x, xix, xix));
SharedSpace *beta = new SharedSpace(1, 3);
double *betav = beta->ObtainWriteEntireData();
betav[0] = sqrt(Metric(x, etax, etax) / Metric(x, result, result)) / EtatoXi;
betav[1] = Metric(x, etax, etax);
betav[2] = Metric(x, result, result) * EtatoXi * EtatoXi;
etax->AddToTempData("beta", beta);
if (HasHHR)
{
Vector *TReta = result->ConstructEmpty();
result->CopyTo(TReta);
ScaleTimesVector(x, betav[0] * EtatoXi, TReta, TReta);
SharedSpace *SharedTReta = new SharedSpace(TReta);
etax->AddToTempData("betaTReta", SharedTReta);
}
}
return;
}
std::cout << "Warning: The differentiated retraction has not been implemented!" << std::endl;
xix->CopyTo(result);
};
void Stiefel::ObtainPerp(Variable *x) const
{
const double *xM = x->ObtainReadData();
SharedSpace *SharedSpacePerp = new SharedSpace(2, n, n - p);
double *Perp = SharedSpacePerp->ObtainWriteEntireData();
for (integer i = 0; i < n * (n - p); i++)
Perp[i] = genrand_gaussian();
double *temp = new double[p * (n - p)];
char *transn = const_cast<char *> ("n"), *transt = const_cast<char *> ("t");
double one = 1, zero = 0, neg_one = -1;
integer P = p, N = n, NmP = n - p;
// temp = X^T * Perp
dgemm_(transt, transn, &P, &NmP, &N, &one, const_cast<double *> (xM), &N, Perp, &N, &zero, temp, &P);
// Perp = Perp - X * temp
dgemm_(transn, transn, &N, &NmP, &P, &neg_one, const_cast<double *> (xM), &N, temp, &P, &one, Perp, &N);
delete[] temp;
integer *jpvt = new integer[NmP];
integer lwork = 2 * NmP + (1 + NmP) * INITIALBLOCKSIZE, info;
double *tau = new double[NmP + lwork];
double *work = tau + NmP;
for (integer i = 0; i < NmP; i++)
jpvt[i] = 0;
dgeqp3_(&N, &NmP, Perp, &N, jpvt, tau, work, &lwork, &info);
if (info < 0)
Rcpp::Rcout << "Error in qr decomposition!" << std::endl;
dorgqr_(&N, &NmP, &NmP, Perp, &N, tau, work, &lwork, &info);
if (info < 0)
Rcpp::Rcout << "Error in forming Q matrix!" << std::endl;
delete[] jpvt;
delete[] tau;
x->AddToTempData("Perp", SharedSpacePerp);
};
void ProductManifold::Retraction(Variable *x, Vector *etax, Variable *result) const
{
#ifdef TESTELASTICCURVESRO
if (x->TempDataExist("w"))
{
const SharedSpace *Sharedw = x->ObtainReadTempData("w");
const double *wptr = Sharedw->ObtainReadData();
SharedSpace *Sharedww = new SharedSpace(1, 1);
double *wwptr = Sharedww->ObtainWriteEntireData();
wwptr[0] = wptr[0] * 0.8;
result->AddToTempData("w", Sharedww);
}
#endif
ProdVariable *prodx = dynamic_cast<ProdVariable *> (x);
ProdVector *prodetax = dynamic_cast<ProdVector *> (etax);
ProdVector *prodresult = dynamic_cast<ProdVector *> (result);
prodresult->NewMemoryOnWrite();
for (integer i = 0; i < numofmani; i++)
{
for (integer j = powsinterval[i]; j < powsinterval[i + 1]; j++)
{
manifolds[i]->Retraction(prodx->GetElement(j), prodetax->GetElement(j), prodresult->GetElement(j));
}
}
#ifdef CHECKMEMORY
prodresult->CheckMemory();
#endif
};
void mexProblem::ObtainElementFromMxArray(Element *X, const mxArray *Xmx)
{
// copy the main data from mxArray to X
double *Xmxptr = mxGetPr(GetFieldbyName(Xmx, 0, "main"));
integer lengthX = X->Getlength();
integer inc = 1;
double *Xptr = X->ObtainWriteEntireData();
dcopy_(&lengthX, Xmxptr, &inc, Xptr, &inc);
// copy temp data from mxArray to X
integer nfields = mxGetNumberOfFields(Xmx);
const char **fnames;
mxArray *tmp;
fnames = static_cast<const char **> (mxCalloc(nfields, sizeof(*fnames)));
for (integer i = 0; i < nfields; i++)
{
fnames[i] = mxGetFieldNameByNumber(Xmx, i);
if (strcmp(fnames[i], "main") != 0)
{
tmp = GetFieldbyName(Xmx, 0, fnames[i]);
double *tmpptr = mxGetPr(tmp);
integer length = mxGetM(tmp);
SharedSpace *Sharedtmp = new SharedSpace(1, length);
double *Sharedtmpptr = Sharedtmp->ObtainWriteEntireData();
dcopy_(&length, tmpptr, &inc, Sharedtmpptr, &inc);
X->AddToTempData(fnames[i], Sharedtmp);
}
}
};
void ProductManifold::DiffRetraction(Variable *x, Vector *etax, Variable *y, Vector *xix, Vector *result, bool IsEtaXiSameDir) const
{
ProdVariable *prodx = dynamic_cast<ProdVariable *> (x);
ProdVector *prodetax = dynamic_cast<ProdVector *> (etax);
ProdVariable *prody = dynamic_cast<ProdVariable *> (y);
ProdVector *prodxix = dynamic_cast<ProdVector *> (xix);
ProdVector *prodresult = dynamic_cast<ProdVector *> (result);
if (xix == result)
{
ProdVector *prodresultTemp = prodresult->ConstructEmpty();
prodresultTemp->NewMemoryOnWrite();
for (integer i = 0; i < numofmani; i++)
{
for (integer j = powsinterval[i]; j < powsinterval[i + 1]; j++)
{
manifolds[i]->DiffRetraction(prodx->GetElement(j), prodetax->GetElement(j), prody->GetElement(j), prodxix->GetElement(j), prodresultTemp->GetElement(j), IsEtaXiSameDir);
}
}
prodresultTemp->CopyTo(prodresult);
delete prodresultTemp;
}
else
{
prodresult->NewMemoryOnWrite();
for (integer i = 0; i < numofmani; i++)
{
for (integer j = powsinterval[i]; j < powsinterval[i + 1]; j++)
{
manifolds[i]->DiffRetraction(prodx->GetElement(j), prodetax->GetElement(j), prody->GetElement(j), prodxix->GetElement(j), prodresult->GetElement(j), IsEtaXiSameDir);
}
}
}
#ifdef CHECKMEMORY
prodresult->CheckMemory();
#endif
if (IsEtaXiSameDir)
{
const double *etaxTV = etax->ObtainReadData();
const double *xixTV = xix->ObtainReadData();
double EtatoXi = sqrt(Metric(x, etax, etax) / Metric(x, xix, xix));
SharedSpace *beta = new SharedSpace(1, 1);
double *betav = beta->ObtainWriteEntireData();
betav[0] = sqrt(Metric(x, etax, etax) / Metric(x, result, result)) / EtatoXi;
etax->AddToTempData("beta", beta);
Vector *TReta = result->ConstructEmpty();
result->CopyTo(TReta);
ScaleTimesVector(x, betav[0] * EtatoXi, TReta, TReta);
SharedSpace *SharedTReta = new SharedSpace(TReta);
etax->AddToTempData("betaTReta", SharedTReta);
}
};
void Stiefel::EucHvToHv(Variable *x, Vector *etax, Vector *exix, Vector *xix, const Problem *prob) const
{
if (metric == EUCLIDEAN)
{
char *transn = const_cast<char *> ("n"), *transt = const_cast<char *> ("t");
double one = 1, zero = 0;
integer inc = 1, N = n, P = p, Length = N * P;
double *symxtegfptr;
SharedSpace *symxtegf;
if (x->TempDataExist("symxtegf"))
{
symxtegf = const_cast<SharedSpace *> (x->ObtainReadTempData("symxtegf"));
symxtegfptr = const_cast<double *> (symxtegf->ObtainReadData());
}
else
{
const double *xxM = x->ObtainReadData();
const SharedSpace *Sharedegf = x->ObtainReadTempData("EGrad");
Vector *egfVec = Sharedegf->GetSharedElement();
const double *egf = egfVec->ObtainReadData();
symxtegf = new SharedSpace(2, p, p);
symxtegfptr = symxtegf->ObtainWriteEntireData();
dgemm_(transt, transn, &P, &P, &N, &one, const_cast<double *> (xxM), &N, const_cast<double *> (egf), &N, &zero, symxtegfptr, &P);
for (integer i = 0; i < p; i++)
{
for (integer j = i + 1; j < p; j++)
{
symxtegfptr[i + j * p] += symxtegfptr[j + i * p];
symxtegfptr[i + j * p] /= 2.0;
symxtegfptr[j + i * p] = symxtegfptr[i + j * p];
}
}
}
exix->CopyTo(xix);
double *resultTV = xix->ObtainWritePartialData();
const double *etaxTV = etax->ObtainReadData();
double negone = -1;
dgemm_(transn, transn, &N, &P, &P, &negone, const_cast<double *> (etaxTV), &N, symxtegfptr, &P, &one, resultTV, &N);
ExtrProjection(x, xix, xix);
if (!x->TempDataExist("symxtegf"))
{
x->AddToTempData("symxtegf", symxtegf);
}
return;
}
Rcpp::Rcout << "Warning:The function converting action of Eucidean Hessian to action of Riemannian Hessian has not been done!" << std::endl;
};
double EucQuadratic::f(Variable *x) const
{
const double *v = x->ObtainReadData();
SharedSpace *Temp = new SharedSpace(1, Dim);
double *temp = Temp->ObtainWriteEntireData();
char *transn = const_cast<char *> ("n");
double one = 1, zero = 0;
integer inc = 1, N = Dim;
dgemv_(transn, &N, &N, &one, A, &N, const_cast<double *> (v), &inc, &zero, temp, &inc);
x->AddToTempData("Ax", Temp);
return ddot_(&N, const_cast<double *> (v), &inc, temp, &inc);
};
void juliaProblem::EucHessianEta(Variable *x, Vector *etax, Vector *exix) const
{
// x->Print("cpp hf x");//---
// etax->Print("cpp hf etax");//---
jl_value_t* array_type = jl_apply_array_type(jl_float64_type, 1);
double *xptr = x->ObtainWritePartialData();
jl_array_t *arrx = jl_ptr_to_array_1d(array_type, xptr, x->Getlength(), 0);
double *etaxptr = etax->ObtainWritePartialData();
jl_array_t *arretax = jl_ptr_to_array_1d(array_type, etaxptr, etax->Getlength(), 0);
jl_array_t *arrtmp = nullptr;
if(x->TempDataExist(("Tmp")))
{
const SharedSpace *Tmp = x->ObtainReadTempData("Tmp");
// Tmp->Print("cpp hf inTmp");//---
const double *tmpptr = Tmp->ObtainReadData();
arrtmp = jl_ptr_to_array_1d(array_type, const_cast<double *> (tmpptr), Tmp->Getlength(), 0);
} else
{
arrtmp = jl_ptr_to_array_1d(array_type, nullptr, 0, 0);
}
jl_value_t *retresult = jl_call3(jl_Hess, (jl_value_t *) arrx, (jl_value_t *) arrtmp, (jl_value_t *) arretax);
jl_array_t *jl_exix = (jl_array_t *) jl_get_nth_field(retresult, 0);
jl_array_t *outtmp = (jl_array_t *) jl_get_nth_field(retresult, 1);
if(jl_array_len(jl_exix) != etax->Getlength())
{
std::cout << "error: the size of the action of the Hessian is not correct!" << std::endl;
exit(EXIT_FAILURE);
}
integer exixlen = exix->Getlength();
double *exixptr = exix->ObtainWriteEntireData();
dcopy_(&exixlen, (double*)jl_array_data(jl_exix), &GLOBAL::IONE, exixptr, &GLOBAL::IONE);
// exix->Print("cpp hf exix:");//---
integer outtmplen = jl_array_len(outtmp);
if(outtmplen != 0)
{
SharedSpace *sharedouttmp = new SharedSpace(1, outtmplen);
double *outtmpptr = sharedouttmp->ObtainWriteEntireData();
dcopy_(&outtmplen, (double*)jl_array_data(outtmp), &GLOBAL::IONE, outtmpptr, &GLOBAL::IONE);
x->RemoveFromTempData("Tmp");
x->AddToTempData("Tmp", sharedouttmp);
}
};
double EucQuadratic::f(Variable *x) const
{
const double *v = x->ObtainReadData();
SharedSpace *Temp = new SharedSpace(1, Dim);
double *temp = Temp->ObtainWriteEntireData();
char *transn = const_cast<char *> ("n");
double one = 1, zero = 0;
integer inc = 1, N = Dim;
// temp <- A * v, details: http://www.netlib.org/lapack/explore-html/dc/da8/dgemv_8f.html
dgemv_(transn, &N, &N, &one, A, &N, const_cast<double *> (v), &inc, &zero, temp, &inc);
x->AddToTempData("Ax", Temp);
// output v^T temp, details: http://www.netlib.org/lapack/explore-html/d5/df6/ddot_8f.html
return ddot_(&N, const_cast<double *> (v), &inc, temp, &inc);
};
double juliaProblem::f(Variable *x) const
{
// x->Print("cpp f x");//---
jl_value_t* array_type = jl_apply_array_type(jl_float64_type, 1);
double *xptr = x->ObtainWritePartialData();
jl_array_t *arrx = jl_ptr_to_array_1d(array_type, xptr, x->Getlength(), 0);
jl_array_t *arrtmp = nullptr;
if(x->TempDataExist(("Tmp")))
{
const SharedSpace *Tmp = x->ObtainReadTempData("Tmp");
const double *tmpptr = Tmp->ObtainReadData();
arrtmp = jl_ptr_to_array_1d(array_type, const_cast<double *> (tmpptr), Tmp->Getlength(), 0);
} else
{
arrtmp = jl_ptr_to_array_1d(array_type, nullptr, 0, 0);
}
jl_value_t *retresult = jl_call2(jl_f, (jl_value_t *) arrx, (jl_value_t *) arrtmp);
jl_get_nth_field(retresult, 0);
jl_value_t *fx = jl_get_nth_field(retresult, 0);
jl_array_t *outtmp = (jl_array_t *) jl_get_nth_field(retresult, 1);
integer outtmplen = jl_array_len(outtmp);
SharedSpace *sharedouttmp = new SharedSpace(1, outtmplen);
double *outtmpptr = sharedouttmp->ObtainWriteEntireData();
dcopy_(&outtmplen, (double*)jl_array_data(outtmp), &GLOBAL::IONE, outtmpptr, &GLOBAL::IONE);
// sharedouttmp->Print("cpp f tmp:");//----
x->RemoveFromTempData("Tmp");
x->AddToTempData("Tmp", sharedouttmp);
if(jl_is_float64(fx))
{
double result = jl_unbox_float64(fx);
// std::cout << "cpp f fx:" << result << std::endl;//-----
return result;
}
std::cout << "Error: The objectve function must return a number of double precision!" << std::endl;
exit(EXIT_FAILURE);
};
void Stiefel::ConRetraction(Variable *x, Vector *etax, Variable *result) const
{ // only accept intrinsic approach
const double *V = etax->ObtainReadData();
Vector *exetax = nullptr;
double *M = new double[3 * n * n + 2 * n];
double *wr = M + n * n;
double *wi = wr + n;
double *Vs = wi + n;
double *VsT = Vs + n * n;
double r2 = sqrt(2.0);
integer idx = 0;
for (integer i = 0; i < p; i++)
{
M[i + i * n] = 0;
for (integer j = i + 1; j < p; j++)
{
M[j + i * n] = V[idx] / r2;
M[i + j * n] = -M[j + i * n];
idx++;
}
}
for (integer i = 0; i < p; i++)
{
for (integer j = p; j < n; j++)
{
M[j + i * n] = V[idx];
M[i + j * n] = -V[idx];
idx++;
}
}
for (integer i = p; i < n; i++)
{
for (integer j = p; j < n; j++)
{
M[j + i * n] = 0;
}
}
char *jobv = const_cast<char *> ("V"), *sortn = const_cast<char *> ("N");
integer N = n, P = p, NmP = n - p, sdim, info;
integer lwork = -1;
double lworkopt;
dgees_(jobv, sortn, nullptr, &N, M, &N, &sdim, wr, wi, Vs, &N, &lworkopt, &lwork, nullptr, &info);
lwork = static_cast<integer> (lworkopt);
double *work = new double[lwork];
dgees_(jobv, sortn, nullptr, &N, M, &N, &sdim, wr, wi, Vs, &N, work, &lwork, nullptr, &info);
char *transn = const_cast<char *> ("n"), *transt = const_cast<char *> ("t");
double cosv, sinv;
integer two = 2, inc = 1;
double one = 1, zero = 0;
double block[4];
for (integer i = 0; i < n; i++)
{
if (i + 1 < n && fabs(M[i + (i + 1) * n]) > std::numeric_limits<double>::epsilon())
{
cosv = cos(M[i + (i + 1) * n]);
sinv = sin(M[i + (i + 1) * n]);
block[0] = cosv; block[1] = -sinv; block[2] = sinv; block[3] = cosv;
dgemm_(transn, transn, &N, &two, &two, &one, Vs + i * n, &N, block, &two, &zero, VsT + i * n, &N);
i++;
}
else
{
dcopy_(&N, Vs + i * n, &inc, VsT + i * n, &inc);
}
}
dgemm_(transn, transt, &N, &N, &N, &one, VsT, &N, Vs, &N, &zero, M, &N);
if (!x->TempDataExist("Perp"))
{
ObtainPerp(x);
}
const SharedSpace *SharedSpacePerp = x->ObtainReadTempData("Perp");
const double *Perp = SharedSpacePerp->ObtainReadData();
const double *xM = x->ObtainReadData();
double *resultM = result->ObtainWriteEntireData();
SharedSpace *ResultSharedPerp = new SharedSpace(2, n, n - p);
double *ResultPerp = ResultSharedPerp->ObtainWriteEntireData();
dgemm_(transn, transn, &P, &P, &P, &one, const_cast<double *> (xM), &N, M, &N, &zero, resultM, &N);
dgemm_(transn, transn, &P, &P, &NmP, &one, const_cast<double *> (Perp), &N, M + p, &N, &one, resultM, &N);
dgemm_(transn, transn, &NmP, &P, &P, &one, const_cast<double *> (xM + p), &N, M, &N, &zero, resultM + p, &N);
dgemm_(transn, transn, &NmP, &P, &NmP, &one, const_cast<double *> (Perp + p), &N, M + p, &N, &one, resultM + p, &N);
dgemm_(transn, transn, &P, &NmP, &P, &one, const_cast<double *> (xM), &N, M + n * p, &N, &zero, ResultPerp, &N);
dgemm_(transn, transn, &P, &NmP, &NmP, &one, const_cast<double *> (Perp), &N, M + n * p + p, &N, &one, ResultPerp, &N);
dgemm_(transn, transn, &NmP, &NmP, &P, &one, const_cast<double *> (xM + p), &N, M + n * p, &N, &zero, ResultPerp + p, &N);
dgemm_(transn, transn, &NmP, &NmP, &NmP, &one, const_cast<double *> (Perp + p), &N, M + n * p + p, &N, &one, ResultPerp + p, &N);
result->AddToTempData("Perp", ResultSharedPerp);
delete[] work;
delete[] M;
};
void Stiefel::ObtainExtrHHR(Variable *x, Vector *intretax, Vector *result) const
{
if (!x->TempDataExist("HHR"))
{
const double *xM = x->ObtainReadData();
SharedSpace *HouseHolderResult = new SharedSpace(2, x->Getsize()[0], x->Getsize()[1]);
double *ptrHHR = HouseHolderResult->ObtainWriteEntireData();
SharedSpace *HHRTau = new SharedSpace(1, x->Getsize()[1]);
double *tau = HHRTau->ObtainWriteEntireData();
integer N = x->Getsize()[0], P = x->Getsize()[1], Length = N * P, inc = 1;
double one = 1, zero = 0;
dcopy_(&Length, const_cast<double *> (xM), &inc, ptrHHR, &inc);
integer *jpvt = new integer[P];
integer info;
integer lwork = -1;
double lworkopt;
for (integer i = 0; i < P; i++)
jpvt[i] = i + 1;
dgeqp3_(&N, &P, ptrHHR, &N, jpvt, tau, &lworkopt, &lwork, &info);
lwork = static_cast<integer> (lworkopt);
double *work = new double[lwork];
dgeqp3_(&N, &P, ptrHHR, &N, jpvt, tau, work, &lwork, &info);
x->AddToTempData("HHR", HouseHolderResult);
x->AddToTempData("HHRTau", HHRTau);
if (info < 0)
Rcpp::Rcout << "Error in qr decomposition!" << std::endl;
for (integer i = 0; i < P; i++)
{
if (jpvt[i] != (i + 1))
Rcpp::Rcout << "Error in qf retraction!" << std::endl;
}
delete[] jpvt;
delete[] work;
}
const double *xM = x->ObtainReadData();
const SharedSpace *HHR = x->ObtainReadTempData("HHR");
const SharedSpace *HHRTau = x->ObtainReadTempData("HHRTau");
const double *ptrHHR = HHR->ObtainReadData();
const double *ptrHHRTau = HHRTau->ObtainReadData();
const double *intretaxTV = intretax->ObtainReadData();
double *resultTV = result->ObtainWriteEntireData();
char *transn = const_cast<char *> ("n"), *sidel = const_cast<char *> ("l");
integer N = x->Getsize()[0], P = x->Getsize()[1], inc = 1, Length = N * P;
integer info;
double r2 = sqrt(2.0);
integer idx = 0;
for (integer i = 0; i < p; i++)
{
resultTV[i + i * n] = 0;
for (integer j = i + 1; j < p; j++)
{
resultTV[j + i * n] = intretaxTV[idx] / r2;
resultTV[i + j * n] = -resultTV[j + i * n];
idx++;
}
}
for (integer i = 0; i < p; i++)
{
for (integer j = p; j < n; j++)
{
resultTV[j + i * n] = intretaxTV[idx];
idx++;
}
}
double sign;
for (integer i = 0; i < p; i++)
{
sign = (ptrHHR[i + n * i] >= 0) ? 1 : -1;
dscal_(&P, &sign, resultTV + i, &N);
}
integer lwork = -1;
double lworkopt;
dormqr_(sidel, transn, &N, &P, &P, const_cast<double *> (ptrHHR), &N, const_cast<double *> (ptrHHRTau), resultTV, &N, &lworkopt, &lwork, &info);
lwork = static_cast<integer> (lworkopt);
double *work = new double[lwork];
dormqr_(sidel, transn, &N, &P, &P, const_cast<double *> (ptrHHR), &N, const_cast<double *> (ptrHHRTau), resultTV, &N, work, &lwork, &info);
delete[] work;
};
void Stiefel::DiffqfRetraction(Variable *x, Vector *etax, Variable *y, Vector *xix, Vector *result, bool IsEtaXiSameDir) const
{
Vector *extempx = EMPTYEXTR->ConstructEmpty();
const double *extempxTV;
if (IsIntrApproach)
{
ObtainExtr(x, xix, extempx);
extempxTV = extempx->ObtainReadData();
}
else
{
xix->CopyTo(extempx);
extempxTV = extempx->ObtainWritePartialData();
}
const double *yM = y->ObtainReadData();
double *resultTV = result->ObtainWriteEntireData();
const SharedSpace *HHR = y->ObtainReadTempData("HHR");
const double *ptrHHR = HHR->ObtainReadData();
double *YtVRinv = new double[p * p];
integer inc = 1, N = n, P = p;
char *left = const_cast<char *> ("r"), *up = const_cast<char *> ("u"),
*transn = const_cast<char *> ("n"), *transt = const_cast<char *> ("t"), *nonunit = const_cast<char *> ("n");
double one = 1, zero = 0;
dtrsm_(left, up, transn, nonunit, &N, &P, &one, const_cast<double *> (ptrHHR), &N, const_cast<double *> (extempxTV), &N);
double sign;
for (integer i = 0; i < P; i++)
{
sign = (ptrHHR[i + i * N] >= 0) ? 1 : -1;
dscal_(&N, &sign, const_cast<double *> (extempxTV + i * N), &inc);
}
dgemm_(transt, transn, &P, &P, &N, &one, const_cast<double *> (yM), &N, const_cast<double *> (extempxTV), &N, &zero, YtVRinv, &P);
for (integer i = 0; i < p; i++)
{
YtVRinv[i + p * i] = -YtVRinv[i + p * i];
for (integer j = i + 1; j < p; j++)
{
YtVRinv[i + p * j] = -YtVRinv[j + p * i] - YtVRinv[i + p * j];
YtVRinv[j + p * i] = 0;
}
}
dgemm_(transn, transn, &N, &P, &P, &one, const_cast<double *> (yM), &N, YtVRinv, &P, &one, const_cast<double *> (extempxTV), &N);
if (IsIntrApproach)
{
ObtainIntr(y, extempx, result);
}
else
{
extempx->CopyTo(result);
}
delete[] YtVRinv;
delete extempx;
if (IsEtaXiSameDir && (HasHHR || UpdBetaAlone))
{
const double *etaxTV = etax->ObtainReadData();
const double *xixTV = xix->ObtainReadData();
double EtatoXi = sqrt(Metric(x, etax, etax) / Metric(x, xix, xix));
SharedSpace *beta = new SharedSpace(1, 3);
double *betav = beta->ObtainWriteEntireData();
betav[0] = sqrt(Metric(x, etax, etax) / Metric(x, result, result)) / EtatoXi;
betav[1] = Metric(x, etax, etax);
betav[2] = Metric(x, result, result) * EtatoXi * EtatoXi;
etax->AddToTempData("beta", beta);
if (HasHHR)
{
Vector *TReta = result->ConstructEmpty();
result->CopyTo(TReta);
ScaleTimesVector(x, betav[0] * EtatoXi, TReta, TReta);
SharedSpace *SharedTReta = new SharedSpace(TReta);
etax->AddToTempData("betaTReta", SharedTReta);
}
}
};
void Stiefel::qfRetraction(Variable *x, Vector *etax, Variable *result) const
{
//x->Print("x in qf:");//---
const double *U = x->ObtainReadData();
const double *V;
Vector *exetax = nullptr;
if (IsIntrApproach)
{
exetax = EMPTYEXTR->ConstructEmpty();
ObtainExtr(x, etax, exetax);
V = exetax->ObtainReadData();
//exetax->Print("exetax:");//---
}
else
{
V = etax->ObtainReadData();
}
double *resultM = result->ObtainWriteEntireData();
SharedSpace *HouseHolderResult = new SharedSpace(2, x->Getsize()[0], x->Getsize()[1]);
double *ptrHHR = HouseHolderResult->ObtainWriteEntireData();
SharedSpace *HHRTau = new SharedSpace(1, x->Getsize()[1]);
double *tau = HHRTau->ObtainWriteEntireData();
integer N = x->Getsize()[0], P = x->Getsize()[1], Length = N * P, inc = 1;
double one = 1, zero = 0;
dcopy_(&Length, const_cast<double *> (V), &inc, ptrHHR, &inc);
daxpy_(&Length, &one, const_cast<double *> (U), &inc, ptrHHR, &inc);
integer *jpvt = new integer[P];
integer info;
integer lwork = -1;
double lworkopt;
for (integer i = 0; i < P; i++)
jpvt[i] = i + 1;
dgeqp3_(&N, &P, ptrHHR, &N, jpvt, tau, &lworkopt, &lwork, &info);
lwork = static_cast<integer> (lworkopt);
double *work = new double[lwork];
dgeqp3_(&N, &P, ptrHHR, &N, jpvt, tau, work, &lwork, &info);
if (info < 0)
Rcpp::Rcout << "Error in qr decomposition!" << std::endl;
for (integer i = 0; i < P; i++)
{
if (jpvt[i] != (i + 1))
Rcpp::Rcout << "Error in qf retraction!" << std::endl;
}
double *signs = new double[P];
for (integer i = 0; i < P; i++)
signs[i] = (ptrHHR[i + i * N] >= 0) ? 1 : -1;
dcopy_(&Length, ptrHHR, &inc, resultM, &inc);
dorgqr_(&N, &P, &P, resultM, &N, tau, work, &lwork, &info);
if (info < 0)
Rcpp::Rcout << "Error in forming Q matrix!" << std::endl;
for (integer i = 0; i < P; i++)
dscal_(&N, signs + i, resultM + i * N, &inc);
result->AddToTempData("HHR", HouseHolderResult);
result->AddToTempData("HHRTau", HHRTau);
delete[] jpvt;
delete[] work;
delete[] signs;
if (exetax != nullptr)
delete exetax;
//result->Print("result in qf:");//---
};
double ShapePathStraighten::f(Variable *x) const
{
const double *l = x->ObtainReadData();
const double *O = l + numP;
const double *m = O + dim * dim;
//double *q2_new = new double[numP*dim];
SharedSpace *Shared_q2_new = new SharedSpace(2, numP, dim); //===================
double *q2_new = Shared_q2_new->ObtainWriteEntireData(); //==================
Apply_Oml(O, m, l, numP, dim, q2_coefs, q2_new);
//ForDebug::Print("q2q2=============", q2_new, numP, dim);
//******************************compute path_x*********************************
//initiate
PSCVariable PSCV(numP, dim, numC);
PSCV.Generate(q1, q2_new);
//PSCV.Print();
PreShapeCurves PSCurves(numP, dim, numC);
PreShapePathStraighten PreSPSprob(numP, dim, numC);
PreSPSprob.SetDomain(&PSCurves);
//get updated preshape geodesic
RSD *RSDsolver = new RSD(&PreSPSprob, &PSCV);
//RSDsolver->LineSearch_LS = static_cast<LSAlgo> (i);
RSDsolver->Debug= NOOUTPUT;
RSDsolver->LineSearch_LS = INPUTFUN;
RSDsolver->LinesearchInput = &ShapePathStraiLinesearchInput;
RSDsolver->Max_Iteration = 16;
RSDsolver->IsPureLSInput = true;
//RSDsolver->Stop_Criterion = GRAD_F;
// RSDsolver->CheckParams();
RSDsolver->Run();
//std::cout << "energy:" << RSDsolver->Getfinalfun() << std::endl;//--
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% forcout %%%%%%%%%%%%%%%%%%%%%%
//store preshape geodesic to compute distance and eta
//current preshape geodesic between q1 and q2_new stored in PreGeodesic
PSCVariable *PreGeodesic = PSCV.ConstructEmpty();
RSDsolver->GetXopt()->CopyTo(PreGeodesic);
//PreGeodesic->Print();
//#############################Compute Distance##############################
//######Compute Dalpha######
const double *GeoPath = PreGeodesic->ObtainReadData();
//PreGeodesic->Print("kdsjfl;akjsdlfjals;dfjal;dsjfal;dsjfl;asdjfl;asdjfl;asjd");
double *Dalpha = new double[numP*dim*numC];
integer stp = numC - 1;
for (integer t = 0; t < numC; t++)
{
if (t != 0) {
for (integer j = 0; j < dim; j++)
{
for (integer i = 0; i < numP; i++)
{
Dalpha[t*numP*dim+j*numP+i] = stp*(GeoPath[t*numP*dim+j*numP+i] - GeoPath[(t-1)*numP*dim+j*numP+i]);
}
}
}
//Project c(tau/numC) into T_alpha(M)
if (t == 0)
{
for (integer j = 0; j < dim; j++)
{
for (integer i = 0; i < numP; i++)
{
Dalpha[j*numP+i] = 0.0;
}
}
}
else
{
PreShapePathStraighten::Item_2(GeoPath + t*numP*dim, numP, dim, Dalpha + t*numP*dim);
}
}
//ForDebug::Print("alphaalphalphalphalphalph", Dalpha, numP, dim, numC);
//#######Compute Distance#########
double intv;
double *temp = new double[numC];
double distance;
for (integer i = 0; i < numC; i++)
{
temp[i] = PreShapePathStraighten::InnerProd_Q(Dalpha+i*numP*dim, Dalpha+i*numP*dim, numP, dim);
temp[i] = sqrt(temp[i]);
}
intv = 1.0/(numC-1);
distance = ElasticCurvesRO::Trapz(temp, numC, intv);
//x->Print("================================");
//std::cout << "++++++++++++++++++++++++++++++++"<< "distance" << distance<<std::endl;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% forcout %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//#############################Compute and store eta##############################
integer NXD = numP*dim;
double coeff;
SharedSpace *SharedEta = new SharedSpace(2, numP, dim);
double *eta = SharedEta->ObtainWriteEntireData();
dcopy_(&NXD, const_cast<double *>(Dalpha+(numC-1)*numP*dim), &GLOBAL::IONE, eta, &GLOBAL::IONE);
coeff = 1.0/std::sqrt(PreShapePathStraighten::InnerProd_Q(Dalpha+(numC-1)*numP*dim, Dalpha+(numC-1)*numP*dim, numP, dim));
dscal_(&NXD, &coeff, eta, &GLOBAL::IONE);
//std::cout <<"#########eta#####"<< PreShapePathStraighten::InnerProd_Q(eta, eta, numP, dim)<<std::endl;
x->AddToTempData("eta", SharedEta);
x->AddToTempData("q2_new", Shared_q2_new);
//############################# Return ##############################
delete RSDsolver;
//delete [] q2_new;
delete [] Dalpha;
delete [] temp;
return distance;
}