ColumnVector GPSModel::ExpectedValueGet() const {
this->y_(1) = x_(POSITION_X);
this->y_(2) = x_(POSITION_Y);
this->y_(3) = x_(VELOCITY_X);
this->y_(4) = x_(VELOCITY_Y);
return y_;
}
ColumnVector HeadingModel::ExpectedValueGet() const {
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = atan2(2*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz);
return y_;
}
double TimeSyncFilter::getLocalTimestamp(double device_time) {
if (!is_initialized_) {
std::cout << "[WARN]: Timesync filter not initialized yet! Hack "
"initializing now.";
double local_time = ros::Time::now().toSec();
initialize(device_time, local_time);
}
double dt = device_time - last_update_device_time_;
return device_time + x_(0) + dt * x_(1);
}
ColumnVector MagneticModel::ExpectedValueGet() const {
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = (qw*qw+qx*qx-qy*qy-qz*qz) * magnetic_field_(1) + (2.0*qx*qy+2.0*qw*qz) * magnetic_field_(2) + (2.0*qx*qz-2.0*qw*qy) * magnetic_field_(3);
y_(2) = (2.0*qx*qy-2.0*qw*qz) * magnetic_field_(1) + (qw*qw-qx*qx+qy*qy-qz*qz) * magnetic_field_(2) + (2.0*qy*qz+2.0*qw*qx) * magnetic_field_(3);
y_(3) = (2.0*qx*qz+2.0*qw*qy) * magnetic_field_(1) + (2.0*qy*qz-2.0*qw*qx) * magnetic_field_(2) + (qw*qw-qx*qx-qy*qy+qz*qz) * magnetic_field_(3);
return y_;
}
void RBFVectorFieldGenerator2D::createSamples()
{
samples_.clear();
if (useSameSeed_.get()) {
mt_.seed(seed_.get());
}
samples_.resize(seeds_.get());
std::generate(samples_.begin(), samples_.end(),
[&]() { return std::make_pair(dvec2(x_(mt_), x_(mt_)), randomVector()); });
}
Point divide_interval(Interval tmp, Fraction K)
{
Point tmp_point;
Fraction S_lg_X = tmp.getS().getX();
Fraction S_lg_Y = tmp.getS().getY();
Fraction E_lg_X = tmp.getE().getX();
Fraction E_lg_Y = tmp.getE().getY();
Fraction rvec_X = E_lg_X - S_lg_X;
Fraction rvec_Y = E_lg_Y - S_lg_Y;
if(K.lg_u_() + rvec_X.lg_u_() + S_lg_X.lg_l_() > 17 ||
K.lg_l_() + rvec_X.lg_l_() + S_lg_X.lg_u_() > 17 ||
K.lg_u_() + rvec_Y.lg_u_() + S_lg_Y.lg_l_() > 17 ||
K.lg_l_() + rvec_Y.lg_l_() + S_lg_Y.lg_u_() > 17 ||
K.lg_l_() + rvec_X.lg_l_() + S_lg_X.lg_l_() > 18 ||
K.lg_l_() + rvec_Y.lg_l_() + S_lg_Y.lg_l_() > 18)
{
double tmp_x = S_lg_X.numerical() + K.numerical() * (E_lg_X.numerical() - S_lg_X.numerical());
double tmp_y = S_lg_Y.numerical() + K.numerical() * (E_lg_Y.numerical() - S_lg_Y.numerical());
Fraction x_(tmp_x, 17);
Fraction y_(tmp_y, 17);
return tmp_point.upload(x_, y_);
}
else
{
Fraction x = S_lg_X + K * rvec_X;
Fraction y = S_lg_Y + K * rvec_Y;
return tmp_point.upload(x, y);
}
}
void ParticleContainerLCTest::setUp() {
sim = new Simulation;
sim->cutoff = 3.0;
sim->delta_t = 0.00005;
sim->meshWidth = 1.1225;
sim->boundaries->setBoundary(BoundaryConds::LEFT, BoundaryConds::REFLECTING);
sim->boundaries->setBoundary(BoundaryConds::RIGHT, BoundaryConds::REFLECTING);
sim->domainSize[0] = 6.0;
sim->domainSize[1] = 6.0;
sim->domainSize[2] = 3.0;
ParticleContainer pc1;
pc = new ParticleContainerLC(&pc1, sim);
utils::Vector<double, 3> x(1.1);
utils::Vector<double, 3> x_(5.1);
x_[2]=0;
utils::Vector<double, 3> v(0.0);
v[0] = 10;
p = new Particle(x, v, 1.0, 0);
p_ = new Particle(x_, v, 1.0, 0);
LOG4CXX_INFO(testlog, "p:" << p->toString());
LOG4CXX_INFO(testlog, "p_:" << p_->toString());
pc->add(*p);
pc->add(*p_);
}
static void docx_rect(double x0, double y0, double x1, double y1,
const pGEcontext gc, pDevDesc dd) {
DOCX_dev *docx_obj = (DOCX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(2);
Rcpp::NumericVector y_(2);
x_[0] = x0;
y_[0] = y0;
x_[1] = x1;
y_[1] = y1;
docx_obj->clp->set_data(x_, y_);
docx_obj->clp->clip_polygon();
Rcpp::NumericVector x__ = docx_obj->clp->get_x();
Rcpp::NumericVector y__ = docx_obj->clp->get_y();
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<wps:wsp>", docx_obj->file);
write_nv_pr_docx(dd, "rc");
fputs("<wps:spPr>", docx_obj->file);
fprintf(docx_obj->file, "%s", xfrm_.xml().c_str());
fprintf(docx_obj->file,"%s", a_prstgeom::a_tag("rect").c_str());
if( fill_.is_visible() > 0 )
fprintf(docx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(docx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</wps:spPr>", docx_obj->file);
fprintf(docx_obj->file, "%s",empty_body_text::wps_tag().c_str());
fputs("</wps:wsp>", docx_obj->file);
}
void
arrangement::keep_arc(Arrangement_2::Edge_iterator &e, Arrangement_2 ©, Walk_pl &walk_pl)
{
e->set_data("none");
Conic_point_2 p;
// if it is a segment
if (e->curve().orientation() == CGAL::COLLINEAR)
{
Conic_point_2 source = e->curve().source();
Conic_point_2 target = e->curve().target();
double x = CGAL::to_double((target.x() + source.x()) /2);
double y = CGAL::to_double((target.y() + source.y()) /2);
Rational x_(x);
Rational y_(y);
p = Conic_point_2(x_,y_);
}
else // if it is an arc
{
int n = 2;
approximated_point_2* points = new approximated_point_2[n + 1];
e->curve().polyline_approximation(n, points); // there is 3 points
p = Conic_point_2(Rational(points[1].first),Rational(points[1].second));
}
Arrangement_2::Vertex_handle v = insert_point(copy, p, walk_pl);
try
{
if (v->face()->data() != 1)
nonCriticalRegions.remove_edge(e, false, false);
copy.remove_isolated_vertex(v);
}
catch (const std::exception exn) {}
}
svd_solve_result(Input& a,
const B& b,
const rtype_t & rcond = rtype_t(-1))
: a_(a)
{
const nt2_la_int ml = size(a_, 1);
const nt2_la_int nl = size(a_, 2);
const nt2_la_int nrhs = size(b, 2);
const nt2_la_int lda = a_.leading_size();
// const nt2_la_int ldb = b.leading_size();
rtab_t s(of_size(nt2::min(ml, nl), 1));
// typically is a non-square, so we need to create tmp x because is
// x is n x nrhs, while b is m x nrhs. we need to make copies of
// these so that the routine won't corrupt data around x and b
if (ml != nl)
{
nt2_la_int mm = std::max(std::max(ml,nl),1);
x_ = nt2::expand(b, nt2::of_size(mm, nrhs));
nt2_la_int ldx_ = x_.leading_size();
nt2::details::gelsd(&ml, &nl, &nrhs, a_.raw(), &lda, x_.raw(), &ldx_,
s.raw(), &rcond, &rank_, &info_);
x_ = x_(_(1, nl), _(1, nrhs));
// BOOST_ASSERT_MSG(info!= 0, "lapack error : gelsd in solve_svd_ip(1)");
}
else
{
x_ = b;
nt2_la_int ldx_ = x_.leading_size();
nt2::details::gelsd(&ml, &nl, &nrhs, a_.raw(), &lda, x_.raw(), &ldx_,
s.raw(), &rcond, &rank_, &info_);
// BOOST_ASSERT_MSG(info == 0, "lapack error : gelsd in solve_svd_ip(2)");
}
}
void ArmadilloSolver::solve_irls()
{
// Weight vector
fvec wsq = ones<fmat>(y.n_elem);
//fvec wsq_ = ones<fmat>(y.n_elem);
//fvec err_u = ones<fmat>(y.n_elem/2);
//fvec err_v = ones<fmat>(y.n_elem/2);
fmat A_(A.n_rows, A.n_cols);
//fvec y_(y.n_elem);
fvec x_(A.n_cols);
fmat T1(A.n_cols, A.n_cols);
fvec T2(A.n_cols);
if (this->debug)
{
std::cout << "[DEBUG] Solver dimensions: " << std::endl;
std::cout << "[DEBUG] \t T1: " << T1.n_rows << " x " << T1.n_cols << std::endl;
std::cout << "[DEBUG] \t T2: " << T2.n_rows << " x " << T2.n_cols << std::endl;
std::cout << "[DEBUG] \t A: " << A.n_rows << " x " << A.n_cols << std::endl;
std::cout << "[DEBUG] \t y: " << y.n_elem << std::endl;
std::cout << "[DEBUG] \t x: " << x_.n_elem << std::endl;
std::cout << "[DEBUG] \t Q: " << Q.n_rows << " x " << Q.n_cols << std::endl;
}
int iters = this->n_iters;
for (int iter=0; iter < iters; iter++) {
A_ = A;
// Re-weight rows of matrix and result vector
A_.each_col() %= wsq;
//y_ = y % wsq;
T1 = A_.t() * A_ + Q;
T2 = A_.t() * (y % wsq);
x_ = arma::solve(T1,T2);
// Computing wsq is a little different, since we want to
// weight both x and y direction of the pixel by the L2 error.
wsq = square(A * x_ - y) * 1.0/this->sigma_sq;
wsq = repmat(wsq.subvec(0,n_kp-1) + wsq.subvec(n_kp,2*n_kp-1),2,1);
// Cauchy error
// Note that the error would be 1.0/(1+f**2), but since we do not take the square
// root above, we can omit the squaring here.
for (int j=0; j < wsq.n_elem; j++)
{
wsq[j] = 1.0/(1.0+wsq[j]);
}
//wsq.transform( [](float f) {return 1.0/(1.0+f); } );
wsq = sqrt(wsq);
}
this->x = x_;
this->wsq = square(wsq);
}
dvec2 RBFVectorFieldGenerator2D::randomVector() {
dvec2 v(1, 0);
auto d = theta_(mt_);
auto c = std::cos(d);
auto s = std::sin(d);
return (mat2(c, s, -s, c) * v) * static_cast<float>((x_(mt_) * 2 - 1));
}
void Radial_grid::set_radial_points(int num_points__, double const* x__)
{
assert(num_points__ > 0);
/* set points */
x_ = mdarray<double, 1>(num_points__);
memcpy(&x_(0), x__, num_points__ * sizeof(double));
for (int i = 0; i < num_points__; i++) x_(i) = Utils::round(x_(i), 13);
/* set x^{-1} */
x_inv_ = mdarray<double, 1>(num_points__);
for (int i = 0; i < num_points__; i++) x_inv_(i) = (x_(i) == 0) ? 0 : 1.0 / x_(i);
/* set dx */
dx_ = mdarray<double, 1>(num_points__ - 1);
for (int i = 0; i < num_points__ - 1; i++) dx_(i) = x_(i + 1) - x_(i);
//== if (dx_(0) < 1e-7)
//== {
//== std::stringstream s;
//== s << "dx step near origin is small : " << Utils::double_to_string(dx_(0));
//== warning_global(__FILE__, __LINE__, s);
//== }
}
Matrix HeadingModel::dfGet(unsigned int i) const {
if (i != 0) return Matrix();
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
const double t1 = qw*qw + qx*qx - qy*qy - qz*qz;
const double t2 = 2*(qx*qy + qw*qz);
const double t3 = 1.0 / (t1*t1 + t2*t2);
C_(1,QUATERNION_W) = 2.0 * t3 * (qz * t1 - qw * t2);
C_(1,QUATERNION_X) = 2.0 * t3 * (qy * t1 - qx * t2);
C_(1,QUATERNION_Y) = 2.0 * t3 * (qx * t1 + qy * t2);
C_(1,QUATERNION_Z) = 2.0 * t3 * (qw * t1 + qz * t2);
return C_;
}
inline typename result_of::push_front<Sequence const, T>::type
push_front(Sequence const& seq, T const& x)
{
typedef typename result_of::push_front<Sequence const, T> push_front;
typedef typename push_front::single_view single_view;
typedef typename push_front::type result;
single_view x_(x);
return result(x_, seq);
}
ColumnVector RateModel::ExpectedValueGet() const {
#ifdef USE_RATE_SYSTEM_MODEL
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = (qw*qw+qx*qx-qy*qy-qz*qz) * x_(RATE_X) + (2.0*qx*qy+2.0*qw*qz) * x_(RATE_Y) + (2.0*qx*qz-2.0*qw*qy) * x_(RATE_Z) + x_(BIAS_GYRO_X);
y_(2) = (2.0*qx*qy-2.0*qw*qz) * x_(RATE_X) + (qw*qw-qx*qx+qy*qy-qz*qz) * x_(RATE_Y) + (2.0*qy*qz+2.0*qw*qx) * x_(RATE_Z) + x_(BIAS_GYRO_Y);
y_(3) = (2.0*qx*qz+2.0*qw*qy) * x_(RATE_X) + (2.0*qy*qz-2.0*qw*qx) * x_(RATE_Y) + (qw*qw-qx*qx-qy*qy+qz*qz) * x_(RATE_Z) + x_(BIAS_GYRO_Z);
#else // USE_RATE_SYSTEM_MODEL
y_(1) = 0.0;
y_(2) = 0.0;
y_(3) = 0.0;
#endif // USE_RATE_SYSTEM_MODEL
return y_;
}
inline typename result_of::push_back<Sequence const, T>::type
push_back(Sequence const& seq, T const& x)
{
typedef typename result_of::push_back<Sequence const, T> push_back;
typedef typename push_back::single_view single_view;
typedef typename push_back::type result;
single_view x_(x);
return result(seq, x_);
}
Matrix RateModel::dfGet(unsigned int i) const {
if (i != 0) return Matrix();
#ifdef USE_RATE_SYSTEM_MODEL
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
// C_(1,QUATERNION_W) = 2.0*qw * x_(RATE_X) + 2.0*qz * x_(RATE_Y) - 2.0*qy * x_(RATE_Z);
// C_(1,QUATERNION_X) = 2.0*qx * x_(RATE_X) + 2.0*qy * x_(RATE_Y) + 2.0*qz * x_(RATE_Z);
// C_(1,QUATERNION_Y) = -2.0*qy * x_(RATE_X) + 2.0*qx * x_(RATE_Y) - 2.0*qw * x_(RATE_Z);
// C_(1,QUATERNION_Z) = -2.0*qz * x_(RATE_X) + 2.0*qw * x_(RATE_Y) + 2.0*qx * x_(RATE_Z);
// C_(2,QUATERNION_W) = -2.0*qz * x_(RATE_X) + 2.0*qw * x_(RATE_Y) + 2.0*qx * x_(RATE_Z);
// C_(2,QUATERNION_X) = 2.0*qy * x_(RATE_X) - 2.0*qx * x_(RATE_Y) + 2.0*qw * x_(RATE_Z);
// C_(2,QUATERNION_Y) = 2.0*qx * x_(RATE_X) + 2.0*qy * x_(RATE_Y) + 2.0*qz * x_(RATE_Z);
// C_(2,QUATERNION_Z) = -2.0*qw * x_(RATE_X) - 2.0*qz * x_(RATE_Y) + 2.0*qy * x_(RATE_Z);
// C_(3,QUATERNION_W) = 2.0*qy * x_(RATE_X) - 2.0*qx * x_(RATE_Y) + 2.0*qw * x_(RATE_Z);
// C_(3,QUATERNION_X) = 2.0*qz * x_(RATE_X) - 2.0*qw * x_(RATE_Y) - 2.0*qx * x_(RATE_Z);
// C_(3,QUATERNION_Y) = 2.0*qw * x_(RATE_X) + 2.0*qz * x_(RATE_Y) - 2.0*qy * x_(RATE_Z);
// C_(3,QUATERNION_Z) = 2.0*qx * x_(RATE_X) + 2.0*qy * x_(RATE_Y) + 2.0*qz * x_(RATE_Z);
C_(1,RATE_X) = (qw*qw+qx*qx-qy*qy-qz*qz);
C_(1,RATE_Y) = (2.0*qx*qy+2.0*qw*qz);
C_(1,RATE_Z) = (2.0*qx*qz-2.0*qw*qy);
C_(2,RATE_X) = (2.0*qx*qy-2.0*qw*qz);
C_(2,RATE_Y) = (qw*qw-qx*qx+qy*qy-qz*qz);
C_(2,RATE_Z) = (2.0*qy*qz+2.0*qw*qx);
C_(3,RATE_X) = (2.0*qx*qz+2.0*qw*qy);
C_(3,RATE_Y) = (2.0*qy*qz-2.0*qw*qx);
C_(3,RATE_Z) = (qw*qw-qx*qx-qy*qy+qz*qz);
C_(1,BIAS_GYRO_X) = 1.0;
C_(2,BIAS_GYRO_Y) = 1.0;
C_(3,BIAS_GYRO_Z) = 1.0;
#endif // USE_RATE_SYSTEM_MODEL
return C_;
}
// Generalization of the addition operation,
// x_plus_delta = Plus(x, delta)
// with the condition that Plus(x, 0) = x.
bool HomogeneousPointLocalParameterization::plus(const double* x,
const double* delta,
double* x_plus_delta) {
Eigen::Map<const Eigen::Vector3d> delta_(delta);
Eigen::Map<const Eigen::Vector4d> x_(x);
Eigen::Map<Eigen::Vector4d> x_plus_delta_(x_plus_delta);
// Euclidean style
x_plus_delta_ = x_ + Eigen::Vector4d(delta_[0], delta_[1], delta_[2], 0);
return true;
}
Matrix MagneticModel::dfGet(unsigned int i) const {
if (i != 0) return Matrix();
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
C_full_(1,QUATERNION_W) = 2.0*qw * magnetic_field_(1) + 2.0*qz * magnetic_field_(2) - 2.0*qy * magnetic_field_(3);
C_full_(1,QUATERNION_X) = 2.0*qx * magnetic_field_(1) + 2.0*qy * magnetic_field_(2) + 2.0*qz * magnetic_field_(3);
C_full_(1,QUATERNION_Y) = -2.0*qy * magnetic_field_(1) + 2.0*qx * magnetic_field_(2) - 2.0*qw * magnetic_field_(3);
C_full_(1,QUATERNION_Z) = -2.0*qz * magnetic_field_(1) + 2.0*qw * magnetic_field_(2) + 2.0*qx * magnetic_field_(3);
C_full_(2,QUATERNION_W) = -2.0*qz * magnetic_field_(1) + 2.0*qw * magnetic_field_(2) + 2.0*qx * magnetic_field_(3);
C_full_(2,QUATERNION_X) = 2.0*qy * magnetic_field_(1) - 2.0*qx * magnetic_field_(2) + 2.0*qw * magnetic_field_(3);
C_full_(2,QUATERNION_Y) = 2.0*qx * magnetic_field_(1) + 2.0*qy * magnetic_field_(2) + 2.0*qz * magnetic_field_(3);
C_full_(2,QUATERNION_Z) = -2.0*qw * magnetic_field_(1) - 2.0*qz * magnetic_field_(2) + 2.0*qy * magnetic_field_(3);
C_full_(3,QUATERNION_W) = 2.0*qy * magnetic_field_(1) - 2.0*qx * magnetic_field_(2) + 2.0*qw * magnetic_field_(3);
C_full_(3,QUATERNION_X) = 2.0*qz * magnetic_field_(1) - 2.0*qw * magnetic_field_(2) - 2.0*qx * magnetic_field_(3);
C_full_(3,QUATERNION_Y) = 2.0*qw * magnetic_field_(1) + 2.0*qz * magnetic_field_(2) - 2.0*qy * magnetic_field_(3);
C_full_(3,QUATERNION_Z) = 2.0*qx * magnetic_field_(1) + 2.0*qy * magnetic_field_(2) + 2.0*qz * magnetic_field_(3);
// return C_full_;
// dq/dyaw * dyaw*dq = 1/2 * [-qz -qy qx qw] * 2 * [-qz; -qy; qx; qw] =
// [ qz*qz qz*qy -qz*qx -qz*qw ;
// qy*qz qy*qy -qy*qx -qy*qw ;
// -qx*qz -qx*qy qx*qx qx*qw ;
// -qw*qz -qw*qy qw*qx qw*qw ]
for(int i = 1; i <= 3; ++i) {
C_(i,QUATERNION_W) = C_full_(i,QUATERNION_W) * qz*qz + C_full_(i,QUATERNION_X) * qy*qz - C_full_(i,QUATERNION_Y) * qx*qz - C_full_(i,QUATERNION_Z) * qw*qz;
C_(i,QUATERNION_X) = C_full_(i,QUATERNION_W) * qz*qy + C_full_(i,QUATERNION_X) * qy*qy - C_full_(i,QUATERNION_Y) * qx*qy - C_full_(i,QUATERNION_Z) * qw*qy;
C_(i,QUATERNION_Y) = -C_full_(i,QUATERNION_W) * qz*qx - C_full_(i,QUATERNION_X) * qy*qx + C_full_(i,QUATERNION_Y) * qx*qx + C_full_(i,QUATERNION_Z) * qw*qx;
C_(i,QUATERNION_Z) = -C_full_(i,QUATERNION_W) * qz*qw - C_full_(i,QUATERNION_X) * qy*qw + C_full_(i,QUATERNION_Y) * qx*qw + C_full_(i,QUATERNION_Z) * qw*qw;
}
return C_;
}
inline
typename
lazy_enable_if<
traits::is_sequence<Sequence>
, result_of::push_back<Sequence const, T>
>::type
push_back(Sequence const& seq, T const& x)
{
typedef typename result_of::push_back<Sequence const, T> push_back;
typedef typename push_back::single_view single_view;
typedef typename push_back::type result;
single_view x_(x);
return result(seq, x_);
}
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
bool HomogeneousPointLocalParameterization::minus(const double* x,
const double* x_plus_delta,
double* delta) {
Eigen::Map<Eigen::Vector3d> delta_(delta);
Eigen::Map<const Eigen::Vector4d> x_(x);
Eigen::Map<const Eigen::Vector4d> x_plus_delta_(x_plus_delta);
// Euclidean style
OKVIS_ASSERT_TRUE_DBG(Exception,fabs((x_plus_delta_-x_)[3])<1e-12, "comparing homogeneous points with different scale "<<x_plus_delta_[3]<<" vs. "<<x_[3]);
delta_ = (x_plus_delta_ - x_).head<3>();
return true;
}
dvariable betacf(const dvariable& a, const dvariable& b, const dvariable& x, int MAXIT)
{
typedef tiny_ad::variable<1, 3> Float;
Float a_ (value(a), 0);
Float b_ (value(b), 1);
Float x_ (value(x), 2);
Float ans = betacf<Float>(a_, b_, x_, MAXIT);
tiny_vec<double, 3> der = ans.getDeriv();
dvariable hh;
value(hh) = ans.value;
gradient_structure::GRAD_STACK1->set_gradient_stack(default_evaluation3ind, &(value(hh)),
&(value(a)), der[0] ,&(value(b)), der[1], &(value(x)), der[2]);
return hh;
}
static void xlsx_polygon(int n, double *x, double *y, const pGEcontext gc,
pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
int i;
Rcpp::NumericVector x_(n);
Rcpp::NumericVector y_(n);
for(i = 0 ; i < n ; i++ ){
x_[i] = x[i];
y_[i] = y[i];
}
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polygon();
//
Rcpp::NumericVector x__ = xlsx_obj->clp->get_x();
Rcpp::NumericVector y__ = xlsx_obj->clp->get_y();
for(i = 0 ; i < y__.size() ; i++ ){
x__[i] += xlsx_obj->offx;
y__[i] += xlsx_obj->offy;
}
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<xdr:sp>", xlsx_obj->file);
write_nv_pr_xlsx(dd, "pg");
fputs("<xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s", xfrm_.xml().c_str());
fputs("<a:custGeom><a:avLst/>", xlsx_obj->file );
fputs( "<a:pathLst>", xlsx_obj->file );
fprintf(xlsx_obj->file, "%s", a_path::a_tag(x__, y__, 1 ).c_str());
fputs( "</a:pathLst>", xlsx_obj->file );
fputs("</a:custGeom>", xlsx_obj->file );
if( fill_.is_visible() > 0 )
fprintf(xlsx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(xlsx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s",empty_body_text::x_tag().c_str());
fputs("</xdr:sp>", xlsx_obj->file);
}
static void xlsx_polyline(int n, double *x, double *y, const pGEcontext gc,
pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(n);
Rcpp::NumericVector y_(n);
for(int i = 0 ; i < n ; i++ ){
x_[i] = x[i];
y_[i] = y[i];
}
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polyline();
std::vector<NumericVector> x_array = xlsx_obj->clp->get_x_lines();
std::vector<NumericVector> y_array = xlsx_obj->clp->get_y_lines();
for( size_t l = 0 ; l < x_array.size() ; l++ ){
xlsx_do_polyline(x_array.at(l), y_array.at(l), gc, dd);
}
}
static void xlsx_line(double x1, double y1, double x2, double y2,
const pGEcontext gc, pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(2);
Rcpp::NumericVector y_(2);
x_[0] = x1;
y_[0] = y1;
x_[1] = x2;
y_[1] = y2;
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polyline();
std::vector<NumericVector> x_array = xlsx_obj->clp->get_x_lines();
std::vector<NumericVector> y_array = xlsx_obj->clp->get_y_lines();
for( size_t l = 0 ; l < x_array.size() ; l++ ){
xlsx_do_polyline(x_array.at(l), y_array.at(l), gc, dd);
}
}
static void xlsx_rect(double x0, double y0, double x1, double y1,
const pGEcontext gc, pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(4);
Rcpp::NumericVector y_(4);
x_[0] = x0;
y_[0] = y0;
x_[1] = x1;
y_[1] = y0;
x_[2] = x1;
y_[2] = y1;
x_[3] = x0;
y_[3] = y1;
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polygon();
Rcpp::NumericVector x__ = xlsx_obj->clp->get_x();
Rcpp::NumericVector y__ = xlsx_obj->clp->get_y();
for(int i = 0 ; i < x__.size() ; i++ ){
x__[i] += xlsx_obj->offx;
y__[i] += xlsx_obj->offy;
}
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<xdr:sp>", xlsx_obj->file);
write_nv_pr_xlsx(dd, "rc");
fputs("<xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s", xfrm_.xml().c_str());
fprintf(xlsx_obj->file,"%s", a_prstgeom::a_tag("rect").c_str());
if( fill_.is_visible() > 0 )
fprintf(xlsx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(xlsx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s",empty_body_text::x_tag().c_str());
fputs("</xdr:sp>", xlsx_obj->file);
}
double Solve(const int &row_reduction_times) {
ColumnReduction();
std::vector<int> init_freeRows;
ReductionTransfer(init_freeRows);
std::vector<int> freeRows;
AugmentingRowReduction(init_freeRows, freeRows);
for(int time=1 ; time<row_reduction_times ; time++) {
std::vector<int> tmp_freeRows(freeRows);
freeRows.clear();
AugmentingRowReduction(tmp_freeRows, freeRows);
}
Augmentation(freeRows);
CheckAssignments();
///
double cost = 0.0;
for(int row=1 ; row<=n_ ; row++) {
int col = x_(row-1);
u_(row-1) = cost_mat_(row-1, col-1) - v_(col-1);
cost += u_(row-1) + v_(col-1);
}
return cost;
}
void dtkMatrixOperationsTestCase::testSolve(void)
{
dtkUnderscore _;
//Matrix-vector
dtkDenseMatrix<double> a(4,4);
a(1,_)=8.,6.,9.,9.;
a(2,_)=9.,0.,9.,4.;
a(3,_)=1.,2.,1.,8.;
a(4,_)=9.,5.,9.,1.;
dtkDenseVector<double> b(4); b = 1,2,3,4;
dtkDenseVector<double> x=dtk::lapack::solve(a,b);
dtkDenseVector<double> res(4); res = 5.34263959,0.53553299,-5.22081218,0.22588832;
for(int i=1;i<5;i++)
QVERIFY(abs(res(i)-x(i))<1e-7);
dtkDenseMatrix<double> a_(4,3);
dtkDenseVector<double> b_(4);
a_(_,1)=1.,0.,0.,0.;
a_(_,2)=0.,1.,0.,0.;
a_(_,3)=0.,0.,1.,1.;
b_ = 1, 2, 2, 1;
dtkDenseVector<double> res_(3); res_ = 1, 2, 1.5;
dtkDenseVector<double> x_= dtk::lapack::solve(a_,b_);
for(int i = 1; i < x_.length(); ++i) {
QVERIFY((x_(i) - res_(i)) < 1e-7);
}
}
/**
* Get an element of the vector
* \param[in] n element index
* \return element n
*/
double get( const int n ) const { return x_(n);}