double IIRNotch::processSample(double input){
bool v = false;
Eigen::Vector3d x_temp ( input , x(0), x(1) );
Eigen::Vector3d y_temp ( 0, y(0), y(1) );
if(v) std::cout << input << "\n";
if(v) std::cout << x_temp.transpose() << " x_temp\n";
if(v) std::cout << y_temp.transpose() << " y_temp\n";
if(v) std::cout << b.transpose() << " b\n";
if(v) std::cout << a.transpose() << " a\n";
if(v){
Eigen::Vector3d bit = x_temp.cross(b);
std::cout << bit.transpose() << " bit\n";
}
double output = (x_temp.dot(b)) - (y_temp.dot(a));
double temp_x = x(0);
x << input , temp_x ;
double temp_y = y(0);
y << output, temp_y ;
if(v) std::cout << x.transpose() << " x\n";
if(v) std::cout << y.transpose() << " y\n\n";
return output;
}
void Conversions::ecef2tgp(double ecef_x, double ecef_y, double ecef_z, double* tgp_x, double* tgp_y, double* tgp_z)
{
matrix x_l(3,1), _x_ecef(3,1), x_temp(3,1);
if (originset)
{
// ECEF -> LOCAL
_x_ecef(0,0,ecef_x);
_x_ecef(1,0,ecef_y);
_x_ecef(2,0,ecef_z);
// std::cout << "_x_ecef = " << _x_ecef << std::endl;
// std::cout << "x0_ecef = " << this->x0_ecef << std::endl;
x_temp = (_x_ecef - x0_ecef); //T_el *
x_l = T_el * x_temp;
// std::cout << "x_temp = " << x_temp << std::endl;
// std::cout << "T_el = " << T_el << std::endl;
// std::cout << "x_l = " << x_l << std::endl;
*tgp_x=x_l.GetElement(0,0);
*tgp_y=x_l.GetElement(1,0);
*tgp_z=x_l.GetElement(2,0);
}
else
{
*tgp_x=*tgp_y=*tgp_z=0.0;
}
}
void RK4_step( // replaces x(t) by x(t + dt)
Matrix<double,1>& x, // solution vector
double dt, // fixed time step
Matrix<double,1> flow(Matrix<double,1>&)) // derivative vector
{
int n = x.size();
Matrix<double,1> f(n), k1(n), k2(n), k3(n), k4(n), x_temp(n);
f = flow(x);
for (int i = 0; i < n; i++) {
k1[i] = dt * f[i];
x_temp[i] = x[i] + k1[i] / 2;
}
f = flow(x_temp);
for (int i = 0; i < n; i++) {
k2[i] = dt * f[i];
x_temp[i] = x[i] + k2[i] / 2;
}
f = flow(x_temp);
for (int i = 0; i < n; i++) {
k3[i] = dt * f[i];
x_temp[i] = x[i] + k3[i];
}
f = flow(x_temp);
for (int i = 0; i < n; i++)
k4[i] = dt * f[i];
for (int i = 0; i < n; i++)
x[i] += (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) / 6;
}
void Correlation::addResultCurve()
{
ApplicationWindow *app = dynamic_cast<ApplicationWindow *>(parent());
if (!app)
return;
QLocale locale = app->locale();
if (d_n > d_table->numRows())
d_table->setNumRows(d_n);
int cols = d_table->numCols();
int cols2 = cols+1;
d_table->addCol();
d_table->addCol();
int n = d_n/2;
QVarLengthArray<double> x_temp(d_n), y_temp(d_n);//double x_temp[d_n], y_temp[d_n];
for (int i = 0; i<d_n; i++){
double x = i - n;
x_temp[i] = x;
double y;
if(i < n)
y = d_x[n + i];
else
y = d_x[i - n];
y_temp[i] = y;
d_table->setText(i, cols, QString::number(x));
d_table->setText(i, cols2, locale.toString(y, 'g', app->d_decimal_digits));
}
QStringList l = d_table->colNames().grep(tr("Lag"));
QString id = QString::number((int)l.size()+1);
QString label = objectName() + id;
d_table->setColName(cols, tr("Lag") + id);
d_table->setColName(cols2, label);
d_table->setColPlotDesignation(cols, Table::X);
d_table->setHeaderColType();
if (d_graphics_display){
if (!d_output_graph)
d_output_graph = createOutputGraph()->activeGraph();
DataCurve *c = new DataCurve(d_table, d_table->colName(cols), d_table->colName(cols2));
c->setData(x_temp.data(), y_temp.data(), d_n);//c->setData(x_temp, y_temp, d_n);
c->setPen(QPen(ColorBox::color(d_curveColorIndex), 1));
d_output_graph->insertPlotItem(c, Graph::Line);
d_output_graph->updatePlot();
}
}
void Convolution::addResultCurve()
{
ApplicationWindow *app = (ApplicationWindow *)parent();
if (!app)
return;
int cols = d_table->numCols();
int cols2 = cols+1;
d_table->addCol();
d_table->addCol();
#ifdef Q_CC_MSVC
QVarLengthArray<double> x_temp(d_n);
#else
double x_temp[d_n];
#endif
QLocale locale = app->locale();
for (int i = 0; i<d_n; i++){
double x = i+1;
x_temp[i] = x;
d_table->setText(i, cols, QString::number(x));
d_table->setText(i, cols2, locale.toString(d_x[i], 'g', app->d_decimal_digits));
}
QStringList l = d_table->colNames().grep(tr("Index"));
QString id = QString::number((int)l.size()+1);
QString label = objectName() + id;
d_table->setColName(cols, tr("Index") + id);
d_table->setColName(cols2, label);
d_table->setColPlotDesignation(cols, Table::X);
d_table->setHeaderColType();
if (d_graphics_display){
if (!d_output_graph)
createOutputGraph();
DataCurve *c = new DataCurve(d_table, d_table->colName(cols), d_table->colName(cols2));
#ifdef Q_CC_MSVC
c->setData(x_temp.data(), d_x, d_n);
#else
c->setData(x_temp, d_x, d_n);
#endif
c->setPen(QPen(ColorBox::color(d_curveColorIndex), 1));
d_output_graph->insertPlotItem(c, Graph::Line);
d_output_graph->updatePlot();
}
}
void least_squares_eqcon(Eigen::MatrixBase<data1__> &x, const Eigen::MatrixBase<data2__> &A, const Eigen::MatrixBase<data3__> &b, const Eigen::MatrixBase<data4__> &B, const Eigen::MatrixBase<data5__> &d)
{
typedef Eigen::Matrix<typename Eigen::MatrixBase<data4__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> B_matrixD;
B_matrixD Q, R, temp_B, temp_R;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data2__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> A_matrixD;
A_matrixD A1, A2, temp_A;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data5__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> d_matrixD;
d_matrixD y;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data3__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> b_matrixD;
b_matrixD z, rhs;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data1__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> x_matrixD;
x_matrixD x_temp(A.cols(), b.cols());
typename A_matrixD::Index p(B.rows()), n(A.cols());
#ifdef DEBUG
typename A_matrixD::Index m(b.rows());
#endif
// build Q and R
Eigen::HouseholderQR<B_matrixD> qr(B.transpose());
Q=qr.householderQ();
temp_B=qr.matrixQR();
temp_R=Eigen::TriangularView<B_matrixD, Eigen::Upper>(temp_B);
R=temp_R.topRows(p);
assert((R.rows()==p) && (R.cols()==p));
assert((Q.rows()==n) && (Q.cols()==n));
// build A1 and A2
temp_A=A*Q;
A1=temp_A.leftCols(p);
A2=temp_A.rightCols(n-p);
#ifdef DEBUG
assert((A1.rows()==m) && (A1.cols()==p));
assert((A2.rows()==m) && (A2.cols()==n-p));
#endif
assert(A1.cols()==p);
assert(A2.cols()==n-p);
// solve for y
y=R.transpose().lu().solve(d);
// setup the unconstrained optimization
rhs=b-A1*y;
least_squares_uncon(z, A2, rhs);
// build the solution
x_temp.topRows(p)=y;
x_temp.bottomRows(n-p)=z;
x=Q*x_temp;
}
std::vector<
typename viennacl::result_of::cpu_value_type<typename VectorT::value_type>::type
>
bisect(VectorT const & alphas, VectorT const & betas)
{
typedef typename viennacl::result_of::value_type<VectorT>::type ScalarType;
typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type CPU_ScalarType;
std::size_t size = betas.size();
std::vector<CPU_ScalarType> x_temp(size);
std::vector<CPU_ScalarType> beta_bisect;
std::vector<CPU_ScalarType> wu;
double rel_error = std::numeric_limits<CPU_ScalarType>::epsilon();
beta_bisect.push_back(0);
for(std::size_t i = 1; i < size; i++){
beta_bisect.push_back(betas[i] * betas[i]);
}
double xmin = alphas[size - 1] - std::fabs(betas[size - 1]);
double xmax = alphas[size - 1] + std::fabs(betas[size - 1]);
for(std::size_t i = 0; i < size - 1; i++)
{
double h = std::fabs(betas[i]) + std::fabs(betas[i + 1]);
if (alphas[i] + h > xmax)
xmax = alphas[i] + h;
if (alphas[i] - h < xmin)
xmin = alphas[i] - h;
}
double eps1 = 1e-6;
/*double eps2 = (xmin + xmax > 0) ? (rel_error * xmax) : (-rel_error * xmin);
if(eps1 <= 0)
eps1 = eps2;
else
eps2 = 0.5 * eps1 + 7.0 * eps2; */
double x0 = xmax;
for(std::size_t i = 0; i < size; i++)
{
x_temp[i] = xmax;
wu.push_back(xmin);
}
for(long k = size - 1; k >= 0; --k)
{
double xu = xmin;
for(long i = k; i >= 0; --i)
{
if(xu < wu[k-i])
{
xu = wu[i];
break;
}
}
if(x0 > x_temp[k])
x0 = x_temp[k];
double x1 = (xu + x0) / 2.0;
while (x0 - xu > 2.0 * rel_error * (std::fabs(xu) + std::fabs(x0)) + eps1)
{
std::size_t a = 0;
double q = 1;
for(std::size_t i = 0; i < size; i++)
{
if(q != 0)
q = alphas[i] - x1 - beta_bisect[i] / q;
else
q = alphas[i] - x1 - std::fabs(betas[i] / rel_error);
if(q < 0)
a++;
}
if (a <= static_cast<std::size_t>(k))
{
xu = x1;
if(a < 1)
wu[0] = x1;
else
{
wu[a] = x1;
if(x_temp[a - 1] > x1)
x_temp[a - 1] = x1;
}
}
else
x0 = x1;
x1 = (xu + x0) / 2.0;
}
x_temp[k] = x1;
}
return x_temp;
}
RcppExport SEXP readBeadFindMask(SEXP beadFindFile_in, SEXP x_in, SEXP y_in) {
SEXP rl = R_NilValue; // Use this when there is nothing to be returned.
char *exceptionMesg = NULL;
try {
// Recasting of input arguments
Rcpp::StringVector beadFindFile_temp(beadFindFile_in);
std::string *beadFindFile = new std::string(beadFindFile_temp(0));
// x
Rcpp::IntegerVector x_temp(x_in);
uint64_t nX = x_temp.size();
Rcpp::IntegerVector x(nX);
int xMin = INT_MAX;
int xMax = -1;
int newVal = 0;
for(uint64_t i=0; i<nX; i++) {
newVal = x_temp(i);
x(i) = newVal;
if(newVal < xMin)
xMin = newVal;
if(newVal > xMax)
xMax = newVal;
}
// y
Rcpp::IntegerVector y_temp(y_in);
uint64_t nY = y_temp.size();
Rcpp::IntegerVector y(nX);
int yMin = INT_MAX;
int yMax = -1;
for(uint64_t i=0; i<nY; i++) {
newVal = y_temp(i);
y(i) = newVal;
if(newVal < yMin)
yMin = newVal;
if(newVal > yMax)
yMax = newVal;
}
// Ensure lower bounds are positive and less than the upper bounds, then proceed to open the file
if(nX != nY) {
exceptionMesg = strdup("x and y should be of the same length");
} else if(nX < 0) {
exceptionMesg = strdup("x and y should be of positive length");
} else if(xMin < 0) {
exceptionMesg = strdup("xMin must be positive");
} else if(yMin < 0) {
exceptionMesg = strdup("yMin must be positive");
} else if(xMin > xMax) {
exceptionMesg = strdup("xMin must be less than xMax");
} else if(yMin > yMax) {
exceptionMesg = strdup("yMin must be less than yMax");
} else {
FILE *fp = NULL;
fp = fopen(beadFindFile->c_str(),"rb");
if(!fp) {
std::string exception = "unable to open beadFindFile " + *beadFindFile;
exceptionMesg = strdup(exception.c_str());
} else {
int32_t nRow = 0;
int32_t nCol = 0;
if ((fread (&nRow, sizeof(uint32_t), 1, fp )) != 1) {
// Read number of rows (aka "y" or "height")
std::string exception = "Problem reading nRow from beadFindFile" + *beadFindFile;
exceptionMesg = strdup(exception.c_str());
} else if ((fread (&nCol, sizeof(uint32_t), 1, fp )) != 1) {
// Read number of cols (aka "x" or "width")
std::string exception = "Problem reading nCol from beadFindFile" + *beadFindFile;
exceptionMesg = strdup(exception.c_str());
} else {
// Ensure upper bounds are within range before continuing
if(yMax >= nRow) {
exceptionMesg = strdup("yMax must be less than the number of rows");
} else if(xMax >= nCol) {
exceptionMesg = strdup("xMax must be less than the number of cols");
} else {
// Iterate over mask data and store out the encoded Boolean values
uint16_t mask = 0;
Rcpp::IntegerVector maskEmpty(nX);
Rcpp::IntegerVector maskBead(nX);
Rcpp::IntegerVector maskLive(nX);
Rcpp::IntegerVector maskDud(nX);
Rcpp::IntegerVector maskReference(nX);
Rcpp::IntegerVector maskTF(nX);
Rcpp::IntegerVector maskLib(nX);
Rcpp::IntegerVector maskPinned(nX);
Rcpp::IntegerVector maskIgnore(nX);
Rcpp::IntegerVector maskWashout(nX);
Rcpp::IntegerVector maskExclude(nX);
Rcpp::IntegerVector maskKeypass(nX);
Rcpp::IntegerVector maskFilteredBadKey(nX);
Rcpp::IntegerVector maskFilteredShort(nX);
Rcpp::IntegerVector maskFilteredBadPPF(nX);
Rcpp::IntegerVector maskFilteredBadResidual(nX);
int64_t headerSkipBytes = 2 * sizeof(uint32_t);
int64_t dataSkipBytes = sizeof(uint16_t);
int64_t offset;
for (uint64_t i=0; i < nX; i++) {
int col = x(i);
int row = y(i);
offset = headerSkipBytes + (row * nCol + col) * dataSkipBytes;
fseek(fp, offset, SEEK_SET);
if ((fread (&mask, sizeof(uint16_t), 1, fp)) != 1) {
std::string exception = "Problem reading mask values from " + *beadFindFile;
exceptionMesg = strdup(exception.c_str());
break;
} else {
maskEmpty(i) = (mask & MaskEmpty) > 0;
maskBead(i) = (mask & MaskBead) > 0;
maskLive(i) = (mask & MaskLive) > 0;
maskDud(i) = (mask & MaskDud) > 0;
maskReference(i) = (mask & MaskReference) > 0;
maskTF(i) = (mask & MaskTF) > 0;
maskLib(i) = (mask & MaskLib) > 0;
maskPinned(i) = (mask & MaskPinned) > 0;
maskIgnore(i) = (mask & MaskIgnore) > 0;
maskWashout(i) = (mask & MaskWashout) > 0;
maskExclude(i) = (mask & MaskExclude) > 0;
maskKeypass(i) = (mask & MaskKeypass) > 0;
maskFilteredBadKey(i) = (mask & MaskFilteredBadKey) > 0;
maskFilteredShort(i) = (mask & MaskFilteredShort) > 0;
maskFilteredBadPPF(i) = (mask & MaskFilteredBadPPF) > 0;
maskFilteredBadResidual(i) = (mask & MaskFilteredBadResidual) > 0;
}
}
// Check if there are any non-zero values for Keypass and Exclude and filtered data.
// If not then it is likely that this is a legacy bfmask.bin file and we won't return
// these fields.
//bool haveFiltered=0;
//for (uint64_t i=0; i < nX; i++) {
// if(maskKeypass(i) || maskExclude(i) || maskFilteredBadKey(i) || maskFilteredShort(i) ||
// maskFilteredBadPPF(i) || maskFilteredBadResidual(i)) {
// haveFiltered=1;
// break;
// }
//}
// Build result set to be returned as a list to R.
std::map<std::string,SEXP> map;
map["beadFindMaskFile"] = Rcpp::wrap( *beadFindFile );
map["nCol"] = Rcpp::wrap( nCol );
map["nRow"] = Rcpp::wrap( nRow );
map["col"] = Rcpp::wrap( x );
map["row"] = Rcpp::wrap( y );
map["maskEmpty"] = Rcpp::wrap( maskEmpty );
map["maskBead"] = Rcpp::wrap( maskBead );
map["maskLive"] = Rcpp::wrap( maskLive );
map["maskDud"] = Rcpp::wrap( maskDud );
map["maskReference"] = Rcpp::wrap( maskReference );
map["maskTF"] = Rcpp::wrap( maskTF );
map["maskLib"] = Rcpp::wrap( maskLib );
map["maskPinned"] = Rcpp::wrap( maskPinned );
map["maskIgnore"] = Rcpp::wrap( maskIgnore );
map["maskWashout"] = Rcpp::wrap( maskWashout );
//if(haveFiltered) {
map["maskExclude"] = Rcpp::wrap( maskExclude );
map["maskKeypass"] = Rcpp::wrap( maskKeypass );
map["maskFilteredBadKey"] = Rcpp::wrap( maskFilteredBadKey );
map["maskFilteredShort"] = Rcpp::wrap( maskFilteredShort );
map["maskFilteredBadPPF"] = Rcpp::wrap( maskFilteredBadPPF );
map["maskFilteredBadResidual"] = Rcpp::wrap( maskFilteredBadResidual );
//}
// Get the list to be returned to R.
rl = Rcpp::wrap( map ) ;
}
}
fclose(fp);
}
}
delete beadFindFile;
} catch(std::exception& ex) {
forward_exception_to_r(ex);
} catch(...) {
::Rf_error("c++ exception (unknown reason)");
}
if(exceptionMesg != NULL)
Rf_error(exceptionMesg);
return rl;
}
