static inline A0_n asin(const A0_n a0_n)
{
const A0 a0 = a0_n;
A0 sign, x;
x = nt2::abs(a0);
sign = nt2::bitofsign(a0);
const bA0 x_larger_05 = gt(x, nt2::Half<A0>());
A0 z = if_else(x_larger_05, nt2::Half<A0>()*nt2::oneminus(x), nt2::sqr(x));
x = if_else(x_larger_05, sqrt(z), x);
// remez polynomial of degree 4 for (asin(rx)-rx)/(rx*rx*rx) in [0, 0.25]
// 2120752146 values (99.53%) within 0.0 ULPs
// 9954286 values (0.47%) within 0.5 ULPs
// 4.0 cycles/element SSE4.2 g++-4.8
A0 z1 = horner<NT2_HORNER_COEFF_T(sA0, 5,
( 0x3d2cb352,
0x3cc617e3,
0x3d3a3ec7,
0x3d9980f6,
0x3e2aaae4
)
)> (z);
z1 = nt2::fma(z1, z*x, x);
z = if_else(x_larger_05, nt2::Pio_2<A0>()-(z1+z1), z1);
return nt2::b_xor(z, sign);
}
static inline A0 finalize(const A0& a0, const A0& y)
{
#ifdef BOOST_SIMD_NO_NANS
BOOST_AUTO_TPL(test, nt2::is_ltz(a0));
#else
BOOST_AUTO_TPL(test, nt2::logical_or(nt2::is_ltz(a0), nt2::is_nan(a0)));
#endif
A0 y1 = nt2::if_nan_else(test, y);
#ifndef BOOST_SIMD_NO_INFINITIES
y1 = if_else(nt2::is_equal(a0, nt2::Inf<A0>()), a0, y1);
#endif
return if_else(is_eqz(a0), nt2::Minf<A0>(), y1);
}
static BOOST_FORCEINLINE i_t select(A0& x)
{
// find significand in antilog table A[]
i_t i = One<i_t>();
i = if_else((x <= twomio16(Nine<i_t>())) , Nine<i_t>(), i);
i = seladd ((x <= twomio16(i+Four<i_t>())), i, Four<i_t>());
i = seladd ((x <= twomio16(i+Two<i_t>())) , i, Two<i_t>() );
i = if_else((x >= twomio16(One<i_t>())) , Mone<i_t>(), i);
i = inc(i);
A0 tmp = twomio16(i);
x -= tmp;
x -= continuation(shr(i, 1));
x /= tmp;
return i;
}
void casadi_bfgs(const casadi_int* sp_h, T1* h, const T1* dx,
const T1* glag, const T1* glag_old, T1* w) {
// Local variables
casadi_int nx;
T1 *yk, *qk, dxBkdx, omega, theta, phi;
// Dimension
nx = sp_h[0];
// Work vectors
yk = w; w += nx;
qk = w; w += nx;
// yk = glag - glag_old
casadi_copy(glag, nx, yk);
casadi_axpy(nx, -1., glag_old, yk);
// qk = H*dx
casadi_fill(qk, nx, 0.);
casadi_mv(h, sp_h, dx, qk, 0);
// Calculating theta
dxBkdx = casadi_dot(nx, dx, qk);
// C-REPLACE "if_else" "casadi_if_else"
omega = if_else(casadi_dot(nx, yk, dx) < 0.2 * casadi_dot(nx, dx, qk),
0.8 * dxBkdx / (dxBkdx - casadi_dot(nx, dx, yk)), 1);
// yk = omega * yk + (1 - omega) * qk;
casadi_scal(nx, omega, yk);
casadi_axpy(nx, 1 - omega, qk, yk);
theta = 1. / casadi_dot(nx, dx, yk);
phi = 1. / casadi_dot(nx, qk, dx);
// Update H
casadi_rank1(h, sp_h, theta, yk, yk);
casadi_rank1(h, sp_h, -phi, qk, qk);
}
int main() {
int i, ans_idx = 0;
for(i = 0; i < NR_DATA; i ++) {
nemu_assert(if_else(test_data[i]) == ans[ans_idx ++]);
}
return 0;
}
static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
{
typedef typename meta::as_integer<A0>::type i_type;
A0 n = tofloat(rem_pio2_medium(x, xr, xc));
xr = xr+n*Pio_2<A0>();
xr = if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
}
static inline result_type rem(A0 const& x)
{
A0 xr;
typedef typename nt2::meta::as_integer<A0>::type iA0;
iA0 n = rem_pio2_medium(x, xr);
xr += nt2::tofloat(n)*Pio_2<A0>();
xr = if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
return xr;
}
void go(){
typename std::conditional<do_it,B,A>::type data;
std::cout << std::is_same<decltype(data), A>::value << ' ';
std::cout << if_else(std::integral_constant<bool, do_it>(),
[this](B& data){ return fn1(data); },
[this](A& data){ return fn2(data); },
data
) << '\n';
}
>>>>>>> Fix for Issue #163 and #178
BOOST_FORCEINLINE result_type
operator()(A0 const& a0, State const& p, Data const& t) const
{
i_t nl = nt2::splat<i_t>(numel(a0));
i_t p1 = nt2::enumerate<i_t>(p) % nl;
i_t spl = selsub( nt2::lt(pl,nl), pl, nl );
return if_else( nt2::lt(pl,nl)
, nt2::run(boost::proto::child_c<0>(a0),p1 ,t)
, nt2::run(boost::proto::child_c<0>(a0),sp1,t)
);
}
static inline A0_n acos(const A0_n a0_n)
{
// 2130706432 values computed.
// 1968272987 values (92.38%) within 0.0 ULPs
// 162433445 values (7.62%) within 0.5 ULPs
// 8.5 cycles/element SSE4.2 g++-4.8
const A0 a0 = a0_n;
A0 x = nt2::abs(a0);
bA0 x_larger_05 = gt(x, nt2::Half<A0>());
x = if_else(x_larger_05, nt2::sqrt(fma(nt2::Mhalf<A0>(), x, nt2::Half<A0>())), a0);
x = asin(x);
x = seladd(x_larger_05, x, x);
x = nt2::if_else(lt(a0, nt2::Mhalf<A0>()), nt2::Pi<A0>()-x, x);
return nt2::if_else(x_larger_05, x, nt2::Pio_2<A0>()-x);
}
int main(void)
{
ADC_Init();
LED_Init();
while(1)
{
ADCSRA |= (1<<ADSC); //uruchomienie poj. konwersji
while(ADCSRA & (1<<ADSC));
if_else(&ADC);
}
}
void SQPInternal::init(){
// Call the init method of the base class
NLPSolverInternal::init();
// Read options
maxiter_ = getOption("maxiter");
maxiter_ls_ = getOption("maxiter_ls");
c1_ = getOption("c1");
beta_ = getOption("beta");
merit_memsize_ = getOption("merit_memory");
lbfgs_memory_ = getOption("lbfgs_memory");
tol_pr_ = getOption("tol_pr");
tol_du_ = getOption("tol_du");
regularize_ = getOption("regularize");
if(getOption("hessian_approximation")=="exact")
hess_mode_ = HESS_EXACT;
else if(getOption("hessian_approximation")=="limited-memory")
hess_mode_ = HESS_BFGS;
if (hess_mode_== HESS_EXACT && H_.isNull()) {
if (!getOption("generate_hessian")){
casadi_error("SQPInternal::evaluate: you set option 'hessian_approximation' to 'exact', but no hessian was supplied. Try with option \"generate_hessian\".");
}
}
// If the Hessian is generated, we use exact approximation by default
if (bool(getOption("generate_hessian"))){
setOption("hessian_approximation", "exact");
}
// Allocate a QP solver
CRSSparsity H_sparsity = hess_mode_==HESS_EXACT ? H_.output().sparsity() : sp_dense(n_,n_);
H_sparsity = H_sparsity + DMatrix::eye(n_).sparsity();
CRSSparsity A_sparsity = J_.isNull() ? CRSSparsity(0,n_,false) : J_.output().sparsity();
QPSolverCreator qp_solver_creator = getOption("qp_solver");
qp_solver_ = qp_solver_creator(H_sparsity,A_sparsity);
// Set options if provided
if(hasSetOption("qp_solver_options")){
Dictionary qp_solver_options = getOption("qp_solver_options");
qp_solver_.setOption(qp_solver_options);
}
qp_solver_.init();
// Lagrange multipliers of the NLP
mu_.resize(m_);
mu_x_.resize(n_);
// Lagrange gradient in the next iterate
gLag_.resize(n_);
gLag_old_.resize(n_);
// Current linearization point
x_.resize(n_);
x_cand_.resize(n_);
x_old_.resize(n_);
// Constraint function value
gk_.resize(m_);
gk_cand_.resize(m_);
// Hessian approximation
Bk_ = DMatrix(H_sparsity);
// Jacobian
Jk_ = DMatrix(A_sparsity);
// Bounds of the QP
qp_LBA_.resize(m_);
qp_UBA_.resize(m_);
qp_LBX_.resize(n_);
qp_UBX_.resize(n_);
// QP solution
dx_.resize(n_);
qp_DUAL_X_.resize(n_);
qp_DUAL_A_.resize(m_);
// Gradient of the objective
gf_.resize(n_);
// Create Hessian update function
if(hess_mode_ == HESS_BFGS){
// Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
SXMatrix Bk = ssym("Bk",H_sparsity);
SXMatrix x = ssym("x",input(NLP_X_INIT).sparsity());
SXMatrix x_old = ssym("x",x.sparsity());
SXMatrix gLag = ssym("gLag",x.sparsity());
SXMatrix gLag_old = ssym("gLag_old",x.sparsity());
SXMatrix sk = x - x_old;
SXMatrix yk = gLag - gLag_old;
SXMatrix qk = mul(Bk, sk);
// Calculating theta
SXMatrix skBksk = inner_prod(sk, qk);
SXMatrix omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk),
0.8 * skBksk / (skBksk - inner_prod(sk, yk)),
1);
yk = omega * yk + (1 - omega) * qk;
SXMatrix theta = 1. / inner_prod(sk, yk);
SXMatrix phi = 1. / inner_prod(qk, sk);
SXMatrix Bk_new = Bk + theta * mul(yk, trans(yk)) - phi * mul(qk, trans(qk));
// Inputs of the BFGS update function
vector<SXMatrix> bfgs_in(BFGS_NUM_IN);
bfgs_in[BFGS_BK] = Bk;
bfgs_in[BFGS_X] = x;
bfgs_in[BFGS_X_OLD] = x_old;
bfgs_in[BFGS_GLAG] = gLag;
bfgs_in[BFGS_GLAG_OLD] = gLag_old;
bfgs_ = SXFunction(bfgs_in,Bk_new);
bfgs_.setOption("number_of_fwd_dir",0);
bfgs_.setOption("number_of_adj_dir",0);
bfgs_.init();
// Initial Hessian approximation
B_init_ = DMatrix::eye(n_);
}
// Header
if(bool(getOption("print_header"))){
cout << "-------------------------------------------" << endl;
cout << "This is CasADi::SQPMethod." << endl;
switch (hess_mode_) {
case HESS_EXACT:
cout << "Using exact Hessian" << endl;
break;
case HESS_BFGS:
cout << "Using limited memory BFGS Hessian approximation" << endl;
break;
}
cout << endl;
cout << "Number of variables: " << setw(9) << n_ << endl;
cout << "Number of constraints: " << setw(9) << m_ << endl;
cout << "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.size() << endl;
cout << "Number of nonzeros in Lagrangian Hessian: " << setw(9) << H_sparsity.size() << endl;
cout << endl;
}
}
void Sqpmethod::init() {
// Call the init method of the base class
NlpSolverInternal::init();
// Read options
max_iter_ = getOption("max_iter");
max_iter_ls_ = getOption("max_iter_ls");
c1_ = getOption("c1");
beta_ = getOption("beta");
merit_memsize_ = getOption("merit_memory");
lbfgs_memory_ = getOption("lbfgs_memory");
tol_pr_ = getOption("tol_pr");
tol_du_ = getOption("tol_du");
regularize_ = getOption("regularize");
exact_hessian_ = getOption("hessian_approximation")=="exact";
min_step_size_ = getOption("min_step_size");
// Get/generate required functions
gradF();
jacG();
if (exact_hessian_) {
hessLag();
}
// Allocate a QP solver
Sparsity H_sparsity = exact_hessian_ ? hessLag().output().sparsity()
: Sparsity::dense(nx_, nx_);
H_sparsity = H_sparsity + Sparsity::diag(nx_);
Sparsity A_sparsity = jacG().isNull() ? Sparsity(0, nx_)
: jacG().output().sparsity();
// QP solver options
Dict qp_solver_options;
if (hasSetOption("qp_solver_options")) {
qp_solver_options = getOption("qp_solver_options");
}
// Allocate a QP solver
qp_solver_ = QpSolver("qp_solver", getOption("qp_solver"),
make_map("h", H_sparsity, "a", A_sparsity),
qp_solver_options);
// Lagrange multipliers of the NLP
mu_.resize(ng_);
mu_x_.resize(nx_);
// Lagrange gradient in the next iterate
gLag_.resize(nx_);
gLag_old_.resize(nx_);
// Current linearization point
x_.resize(nx_);
x_cand_.resize(nx_);
x_old_.resize(nx_);
// Constraint function value
gk_.resize(ng_);
gk_cand_.resize(ng_);
// Hessian approximation
Bk_ = DMatrix::zeros(H_sparsity);
// Jacobian
Jk_ = DMatrix::zeros(A_sparsity);
// Bounds of the QP
qp_LBA_.resize(ng_);
qp_UBA_.resize(ng_);
qp_LBX_.resize(nx_);
qp_UBX_.resize(nx_);
// QP solution
dx_.resize(nx_);
qp_DUAL_X_.resize(nx_);
qp_DUAL_A_.resize(ng_);
// Gradient of the objective
gf_.resize(nx_);
// Create Hessian update function
if (!exact_hessian_) {
// Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
SX Bk = SX::sym("Bk", H_sparsity);
SX x = SX::sym("x", input(NLP_SOLVER_X0).sparsity());
SX x_old = SX::sym("x", x.sparsity());
SX gLag = SX::sym("gLag", x.sparsity());
SX gLag_old = SX::sym("gLag_old", x.sparsity());
SX sk = x - x_old;
SX yk = gLag - gLag_old;
SX qk = mul(Bk, sk);
// Calculating theta
SX skBksk = inner_prod(sk, qk);
SX omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk),
0.8 * skBksk / (skBksk - inner_prod(sk, yk)),
1);
yk = omega * yk + (1 - omega) * qk;
SX theta = 1. / inner_prod(sk, yk);
SX phi = 1. / inner_prod(qk, sk);
SX Bk_new = Bk + theta * mul(yk, yk.T()) - phi * mul(qk, qk.T());
// Inputs of the BFGS update function
vector<SX> bfgs_in(BFGS_NUM_IN);
bfgs_in[BFGS_BK] = Bk;
bfgs_in[BFGS_X] = x;
bfgs_in[BFGS_X_OLD] = x_old;
bfgs_in[BFGS_GLAG] = gLag;
bfgs_in[BFGS_GLAG_OLD] = gLag_old;
bfgs_ = SXFunction("bfgs", bfgs_in, make_vector(Bk_new));
// Initial Hessian approximation
B_init_ = DMatrix::eye(nx_);
}
// Header
if (static_cast<bool>(getOption("print_header"))) {
userOut()
<< "-------------------------------------------" << endl
<< "This is casadi::SQPMethod." << endl;
if (exact_hessian_) {
userOut() << "Using exact Hessian" << endl;
} else {
userOut() << "Using limited memory BFGS Hessian approximation" << endl;
}
userOut()
<< endl
<< "Number of variables: " << setw(9) << nx_ << endl
<< "Number of constraints: " << setw(9) << ng_ << endl
<< "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.nnz() << endl
<< "Number of nonzeros in Lagrangian Hessian: " << setw(9) << H_sparsity.nnz() << endl
<< endl;
}
}
typename disable_if< meta::is_bitwise_logical<A0_>, result_type>::type
BOOST_FORCEINLINE operator()(A0_ const& a0, A1 const& a1) const
{
return if_else(a0, a1, Zero<A1>());
}
int main() {
F1 f(7);
test1( 5*x_) ;
test2(x_ + 2*y_);
test2(x_ + 2*y_ + x_);
test2(x_/2.0 + 2*y_);
test2( f(x_) );
test2( f(x_) + 2*y_);
test2( 1/f(x_) + 2*y_);
#ifdef LONG
test2( 1/f(x_) + 2*y_ + x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_);
#endif
#ifdef LONG2
test2( 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_+ x_ + 2*x_ + 1/f(x_) + 2*y_ + x_ );
#endif
{
auto expr1 = x_ *2 ;
auto myf1 = make_function(expr1, x_);
std::cout<< myf1(2) << " = "<< 4 << std::endl;
auto expr = x_ + 2*y_;
auto myf = make_function(expr, x_,y_);
auto myf_r = make_function(expr, y_,x_);
std::cout<< myf(2,5) << " = "<< 12 << std::endl;
std::cout<< myf(5,2) << " = " << 9<<std::endl;
std::cout<< myf_r(2,5) << " = "<< 9 <<std::endl;
std::cout<< myf_r(5,2) << " = "<< 12 << std::endl;
std::cout<<"-------------"<<std::endl;
}
{
// testing the LHS wrting on an object caught by ref
F1 f(7);
std::cerr << " operator(double) still ok "<< f(2) << std::endl;
std::cout<< " f.v before assign "<<f.v<<" "<< std::endl;
f(x_ ) << 8*x_ ;
//f(x_ + y_) << 8*x_ ;// leads to a compile error as expected
// test.cpp:129:14: error: no viable overloaded '='
// f(x_ + y_) << 8*x_ ;
// ~~~~~~~~~~ ^ ~~~~
std::cout<< " f.v after assign "<<f.v<<" "<< std::endl;
std::cout<<"-------------"<<std::endl;
std::cerr <<F1{9}(2,x_, F1{2})<<std::endl;
auto expr = F1{9}(x_);
expr << 7*x_;
std::cerr << expr << std::endl ;
F1{9}(x_ ) << 8*x_ ;
std::cerr<<"-------------"<<std::endl;
}
{
// testing fnt of 2 variables
F2 ff;
std::cout<<"expr = "<< (ff(x_,y_) + 2*y_)<< std::endl;
std::cout<<"eval(expr,x_ =1, y_ =2) = "<< eval(ff(x_,y_) + 2*y_ , x_=x, y_=y) << " and it should be "<< ff(x,y) + 2*y <<std::endl;
auto tmp = ff(2.0, y_) ;
std::cout<<" tmp =" << tmp<<std::endl;
std::cout<<"another = "<< eval( tmp , x_=x) << std::endl;
std::cout<<"another = "<< eval( ff(x_,2) , x_=x) <<std::endl;
std::cout<<"-------------"<<std::endl;
}
{
// testing expression if
TEST(eval( if_else( true , 2*x_ , y_) , x_=1, y_=3));
TEST(eval( if_else( false , 2*x_ , y_) ,x_=1, y_=3));
TEST(eval( if_else( x_>y_ , 2*x_ , y_) ,x_=1, y_=3));
}
std::cout << (x_ < y_) <<std::endl;
}
typename disable_if< meta::is_bitwise_logical<A0_>, result_type>::type
operator()(A0_ const& a0, A1 const& a1) const
{
return if_else(a0, Allbits<A1>(), a1);
}
static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
{
xr = if_else(gt(x, Pi<A0>()), x-Twopi<A0>(),
if_else(lt(x, -Pi<A0>()), x+Twopi<A0>(), x));
xc = Zero<A0>();
}
int Switch::eval_sx(const SXElem** arg, SXElem** res,
casadi_int* iw, SXElem* w, void* mem) const {
// Input and output buffers
const SXElem** arg1 = arg + n_in_;
SXElem** res1 = res + n_out_;
// Extra memory needed for chaining if_else calls
std::vector<SXElem> w_extra(nnz_out());
std::vector<SXElem*> res_tempv(n_out_);
SXElem** res_temp = get_ptr(res_tempv);
for (casadi_int k=0; k<f_.size()+1; ++k) {
// Local work vector
SXElem* wl = w;
// Local work vector
SXElem* wll = get_ptr(w_extra);
if (k==0) {
// For the default case, redirect the temporary results to res
copy_n(res, n_out_, res_temp);
} else {
// For the other cases, store the temporary results
for (casadi_int i=0; i<n_out_; ++i) {
res_temp[i] = wll;
wll += nnz_out(i);
}
}
copy_n(arg+1, n_in_-1, arg1);
copy_n(res_temp, n_out_, res1);
const Function& fk = k==0 ? f_def_ : f_[k-1];
// Project arguments with different sparsity
for (casadi_int i=0; i<n_in_-1; ++i) {
if (arg1[i]) {
const Sparsity& f_sp = fk.sparsity_in(i);
const Sparsity& sp = sparsity_in_[i+1];
if (f_sp!=sp) {
SXElem *t = wl; wl += f_sp.nnz(); // t is non-const
casadi_project(arg1[i], sp, t, f_sp, wl);
arg1[i] = t;
}
}
}
// Temporary memory for results with different sparsity
for (casadi_int i=0; i<n_out_; ++i) {
if (res1[i]) {
const Sparsity& f_sp = fk.sparsity_out(i);
const Sparsity& sp = sparsity_out_[i];
if (f_sp!=sp) { res1[i] = wl; wl += f_sp.nnz();}
}
}
// Evaluate the corresponding function
if (fk(arg1, res1, iw, wl, 0)) return 1;
// Project results with different sparsity
for (casadi_int i=0; i<n_out_; ++i) {
if (res1[i]) {
const Sparsity& f_sp = fk.sparsity_out(i);
const Sparsity& sp = sparsity_out_[i];
if (f_sp!=sp) casadi_project(res1[i], f_sp, res_temp[i], sp, wl);
}
}
if (k>0) { // output the temporary results via an if_else
SXElem cond = k-1==arg[0][0];
for (casadi_int i=0; i<n_out_; ++i) {
if (res[i]) {
for (casadi_int j=0; j<nnz_out(i); ++j) {
res[i][j] = if_else(cond, res_temp[i][j], res[i][j]);
}
}
}
}
}
return 0;
}
