//*************************************************************************************************************************
//************************************************* OLD STUFF *************************************************************
//*************************************************************************************************************************
void wholeBodyReach::pinvTrunc(const MatrixRXd &A, double tol, MatrixRXd &Apinv, MatrixRXd &Spinv, VectorXd &sv)
{
// allocate memory
int m = A.rows(), n = A.cols(), k = m<n?m:n;
Spinv.setZero(k,k);
// compute decomposition
JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD
sv = svd.singularValues();
// compute pseudoinverse of singular value matrix
for (int c=0;c<k; c++)
if ( sv(c)> tol)
Spinv(c,c) = 1/sv(c);
// compute pseudoinverse
Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose();
}
//*************************************************************************************************************************
void wholeBodyReach::pinvDampTrunc(const MatrixRXd &A, double tol, double damp, MatrixRXd &Apinv, MatrixRXd &ApinvDamp)
{
// allocate memory
int m = A.rows(), n = A.cols(), k = m<n?m:n;
MatrixRXd Spinv = MatrixRXd::Zero(k,k);
MatrixRXd SpinvD = MatrixRXd::Zero(k,k);
// compute decomposition
JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD
VectorXd sv = svd.singularValues();
// compute pseudoinverses of singular value matrix
double damp2 = damp*damp;
for (int c=0;c<k; c++)
{
SpinvD(c,c) = sv(c) / (sv(c)*sv(c) + damp2);
if ( sv(c)> tol)
Spinv(c,c) = 1/sv(c);
}
// compute pseudoinverses
Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose(); // truncated pseudoinverse
ApinvDamp = svd.matrixV() * SpinvD * svd.matrixU().transpose(); // damped pseudoinverse
}
RcppExport SEXP crossprodrowmajor(SEXP X, SEXP yes)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::Lower;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd;
typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Matrix> MapMat;
typedef Eigen::Map<const Vector> MapVec;
const bool rmyes(as<bool>(yes));
const Eigen::Map<MatrixXd> A(as<Map<MatrixXd> >(X));
//MapMat A(as<MapMat>(X));
MatrixRXd AA = A;
const int n(A.cols());
MatrixXd AtA;
if (rmyes)
{
AtA = MatrixXd(n, n).setZero().selfadjointView<Lower>().rankUpdate(AA.adjoint());
} else
{
AtA = MatrixXd(n, n).setZero().selfadjointView<Lower>().rankUpdate(A.adjoint());
}
return wrap(AtA);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
//*************************************************************************************************************************
void wholeBodyReach::assertEqual(const MatrixRXd &A, const MatrixRXd &B, string testName, double tol)
{
if(A.cols() != B.cols() || A.rows()!=B.rows())
{
cout<< testName<< ": dim(A) != dim(B): " << A.rows()<< "x"<<A.cols()<<" != "<< B.rows()<< "x"<<B.cols()<<endl;
return testFailed(testName);
}
for(int r=0; r<A.rows(); r++)
for(int c=0; c<A.cols(); c++)
if(abs(A(r,c)-B(r,c))>tol)
{
printf("%s: element %d,%d is different, absolute difference is %f\n", testName.c_str(), r, c, abs(A(r,c)-B(r,c)));
return testFailed(testName);
}
}
//port faster cross product
RcppExport SEXP crossprodxval(SEXP X, SEXP idxvec_)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXi;
using Eigen::Lower;
using Rcpp::List;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<MatrixRXd> MapRMat;
typedef Eigen::Map<const Vector> MapVec;
typedef Map<VectorXd> MapVecd;
typedef Map<VectorXi> MapVeci;
typedef Eigen::SparseVector<double> SparseVector;
typedef Eigen::SparseVector<int> SparseVectori;
const MapMat AA(as<MapMat>(X));
const MapVeci idxvec(as<MapVeci>(idxvec_));
const int n(AA.cols());
MatrixXd AtA(MatrixXd(n, n));
MatrixRXd A = AA;
int nfolds = idxvec.maxCoeff();
List xtxlist(nfolds);
for (int k = 1; k < nfolds + 1; ++k)
{
VectorXi idxbool = (idxvec.array() == k).cast<int>();
int nrow = idxbool.size();
int numelem = idxbool.sum();
VectorXi idx(numelem);
int c = 0;
for (int i = 0; i < nrow; ++i)
{
if (idxbool(i) == 1)
{
idx(c) = i;
++c;
}
}
int nvars = A.cols() - 1;
MatrixXd sub(numelem, n);
for (int r = 0; r < numelem; ++r)
{
sub.row(r) = A.row(idx(r));
}
MatrixXd AtAtmp(MatrixXd(n, n).setZero().
selfadjointView<Lower>().rankUpdate(sub.adjoint() ));
xtxlist[k-1] = AtAtmp;
}
return wrap(xtxlist);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
