I'm trying to optimize a certain objective function using Ensmallen that is a library for mathematical optimization that i found on Github.
Unfortunally the examples that i found in the documentation didn't turn out to be so usefull.
I have a lenght function that depends on two parameters: k_i and k_f, and i want to minimize it.
This function is subjected to 4 constraint: constraint1, ... , constraint4.
Theese constraints depends on: k_i, k_f and s that is a parameter that goeas from 0 to 1 and represent the parametrization of my lenght.
Basically this is a sort of simplification of the semi infinite programming problem. the variable "s" increse from 0 to 1 with 50 steps and for every step i would like to have my objective function that satisfies my 4 constraints.
x_i, y_i, theta_i, x_f, y_f, theta_f, speed, diameter, uplim, downlim are constants.
From the optimization i want to find the value of k_i and k_f.
So this is a part of my code:
double Evaluate(const arma::mat& x) {
k_i = x(0,0);
k_f = x(1,0);
return sqrt(k_f*k_f)*(1.0/3.0E1)+sqrt(k_i*k_i)*(1.0/3.0E1)+sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/3.0E1)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2);
}
void Gradient(const arma::mat& x, arma::mat& g) {
k_i = x(0,0);
k_f = x(1,0);
g.resize(2,1);
g(0,0) = (k_i*9.8E1-x_f*cos(theta_i)*3.36E2+x_i*cos(theta_i)*3.36E2-y_f*sin(theta_i)*3.36E2+y_i*sin(theta_i)*3.36E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.28E2+x_f*cos(theta_i)*3.84E2-x_i*cos(theta_i)*3.84E2+y_f*sin(theta_i)*3.84E2-y_i*sin(theta_i)*3.84E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.8E1-x_f*cos(theta_i)*2.16E2+x_i*cos(theta_i)*2.16E2-y_f*sin(theta_i)*2.16E2+y_i*sin(theta_i)*2.16E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*1.28E2-x_f*cos(theta_i)*5.76E2+x_i*cos(theta_i)*5.76E2-y_f*sin(theta_i)*5.76E2+y_i*sin(theta_i)*5.76E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*5.78E2-x_f*cos(theta_i)*1.836E3+x_i*cos(theta_i)*1.836E3-y_f*sin(theta_i)*1.836E3+y_i*sin(theta_i)*1.836E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*7.938E3+x_f*cos(theta_i)*6.804E3-x_i*cos(theta_i)*6.804E3+y_f*sin(theta_i)*6.804E3-y_i*sin(theta_i)*6.804E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*9.8E1+x_f*cos(theta_i)*1.764E3-x_i*cos(theta_i)*1.764E3+y_f*sin(theta_i)*1.764E3-y_i*sin(theta_i)*1.764E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.178E3-x_f*cos(theta_i)*8.316E3+x_i*cos(theta_i)*8.316E3-y_f*sin(theta_i)*8.316E3+y_i*sin(theta_i)*8.316E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.0-x_f*cos(theta_i)*1.2E1+x_i*cos(theta_i)*1.2E1-y_f*sin(theta_i)*1.2E1+y_i*sin(theta_i)*1.2E1+k_f*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_i*1.0/sqrt(k_i*k_i)*(1.0/3.0E1);
g(1,0) = (k_f*9.8E1-x_f*cos(theta_f)*3.36E2+x_i*cos(theta_f)*3.36E2-y_f*sin(theta_f)*3.36E2+y_i*sin(theta_f)*3.36E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.28E2+x_f*cos(theta_f)*3.84E2-x_i*cos(theta_f)*3.84E2+y_f*sin(theta_f)*3.84E2-y_i*sin(theta_f)*3.84E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.8E1-x_f*cos(theta_f)*2.16E2+x_i*cos(theta_f)*2.16E2-y_f*sin(theta_f)*2.16E2+y_i*sin(theta_f)*2.16E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*1.28E2-x_f*cos(theta_f)*5.76E2+x_i*cos(theta_f)*5.76E2-y_f*sin(theta_f)*5.76E2+y_i*sin(theta_f)*5.76E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*5.78E2-x_f*cos(theta_f)*1.836E3+x_i*cos(theta_f)*1.836E3-y_f*sin(theta_f)*1.836E3+y_i*sin(theta_f)*1.836E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*7.938E3+x_f*cos(theta_f)*6.804E3-x_i*cos(theta_f)*6.804E3+y_f*sin(theta_f)*6.804E3-y_i*sin(theta_f)*6.804E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*9.8E1+x_f*cos(theta_f)*1.764E3-x_i*cos(theta_f)*1.764E3+y_f*sin(theta_f)*1.764E3-y_i*sin(theta_f)*1.764E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.178E3-x_f*cos(theta_f)*8.316E3+x_i*cos(theta_f)*8.316E3-y_f*sin(theta_f)*8.316E3+y_i*sin(theta_f)*8.316E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.0-x_f*cos(theta_f)*1.2E1+x_i*cos(theta_f)*1.2E1-y_f*sin(theta_f)*1.2E1+y_i*sin(theta_f)*1.2E1+k_i*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_f*1.0/sqrt(k_f*k_f)*(1.0/3.0E1);
}
size_t NumConstraints() {
return 50;
}
double EvaluateConstraint(const size_t i, const arma::mat& x){
k_i = x(0,0);
k_f = x(1,0);
double index_ensmallen = i;
double s = index_ensmallen/49;
double v = sqrt((s*s*s*s)*pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0+(s*s)*pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+(k_i*k_i)*pow(cos(theta_i),2.0)+(k_i*k_i)*pow(sin(theta_i),2.0)+(s*s*s*s)*pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+(s*s)*pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0-(s*s*s)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1-(s*s*s)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1-k_i*s*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0-k_i*s*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*(s*s)*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0+k_i*(s*s)*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0);
double w = -(s*(k_i*sin(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0-k_i*cos(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-(s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*6.0-(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-k_i*sin(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*2.0+k_i*cos(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*2.0)/(-s*(k_i*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0)+(k_i*k_i)*pow(cos(theta_i),2.0)+(k_i*k_i)*pow(sin(theta_i),2.0)+(s*s*s*s)*(pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0)-(s*s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1+(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1)+(s*s)*(pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0+pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+k_i*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0+k_i*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0));
double constraint1 = w - tan(uplim)*v;
double constraint2 = tan(downlim)*v - w;
double constraint3 = v*(speed-1) + diameter*speed*w;
double constraint4 = v*(speed-1) - diameter*speed*w;
if ( (constraint1 >0) || (constraint2 >0) || (constraint3 >0) || (constraint4 >0) ){
return DBL_MAX; //constraint unsatisfied
}
return 0;
}//close EvaluateConstraint
void GradientConstraint(const size_t i, const arma::mat& x, arma::mat& g){
g.resize(2,1);
k_i = x(0,0);
k_f = x(1,0);
double s=i/49;
//g(0,0) = 0
//g(1,0) = 0
}//close gradientconstraint
When i try to use this optimizator i find that k_i and k_f are equal to Nan. But i tested it with the optimizer "fmincon" of Matlab and i obtain some valid results!
So i don't know what to do, i thought that maybe i have written something wrong here in the EvaluateConstraint method because when i try to print their value i find "Nan".
Edit: Raising the value of the initial point (for example x0:(500, 500)) "unlocks" the simulation, the optimizator don't use the same value every iteration, it changes as it should be, but still no solution.
Related
I am trying to replicate Matlab's Fsolve as my project is in C++ solving an implicit RK4 scheme. I am using the NLopt library using the NLOPT_LD_MMA algorithm. I have run the required section in matlab and it is considerably faster. I was wondering whether anyone had any ideas of a better Fsolve equivalent in C++? Another reason is that I would like f1 and f2 to both tend to zero and it seems suboptimal to calculate the L2 norm to include both of them as NLopt seems to only allow a scalar return value from the objective function. Does anyone have any ideas of an alternative library or perhaps using a different algorithm/constraints to more closely replicate the default fsolve.
Would it be better (faster) perhaps to call the python scipy.minimise.fsolve from C++?
double implicitRK4(double time, double V, double dt, double I, double O, double C, double R){
const int number_of_parameters = 2;
double lb[number_of_parameters];
double ub[number_of_parameters];
lb[0] = -999; // k1 lb
lb[1] = -999;// k2 lb
ub[0] = 999; // k1 ub
ub[1] = 999; // k2 ub
double k [number_of_parameters];
k[0] = 0.01;
k[1] = 0.01;
kOptData addData(time,V,dt,I,O,C,R);
nlopt_opt opt; //NLOPT_LN_MMA NLOPT_LN_COBYLA
opt = nlopt_create(NLOPT_LD_MMA, number_of_parameters);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_result nlopt_remove_inequality_constraints(nlopt_opt opt);
// nlopt_result nlopt_remove_equality_constraints(nlopt_opt opt);
nlopt_set_min_objective(opt,solveKs,&addData);
double minf;
if (nlopt_optimize(opt, k, &minf) < 0) {
printf("nlopt failed!\n");
}
else {
printf("found minimum at f(%g,%g,%g) = %0.10g\n", k[0],k[1],minf);
}
nlopt_destroy(opt);
return V + (1/2)*dt*k[0] + (1/2)*dt*k[1];```
double solveKs(unsigned n, const double *x, double *grad, void *my_func_data){
kOptData *unpackdata = (kOptData*) my_func_data;
double t1,y1,t2,y2;
double f1,f2;
t1 = unpackdata->time + ((1/2)-(1/6)*sqrt(3));
y1 = unpackdata->V + (1/4)*unpackdata->dt*x[0] + ((1/4)-(1/6)*sqrt(3))*unpackdata->dt*x[1];
t2 = unpackdata->time + ((1/2)+(1/6)*sqrt(3));
y2 = unpackdata->V + ((1/4)+(1/6)*sqrt(3))*unpackdata->dt*x[0] + (1/4)*unpackdata->dt*x[1];
f1 = x[0] - stateDeriv_implicit(t1,y1,unpackdata->dt,unpackdata->I,unpackdata->O,unpackdata->C,unpackdata->R);
f2 = x[1] - stateDeriv_implicit(t2,y2,unpackdata->dt,unpackdata->I,unpackdata->O,unpackdata->C,unpackdata->R);
return sqrt(pow(f1,2) + pow(f2,2));
My matlab version below seems to be a lot simpler but I would prefer the whole code in c++!
k1 = 0.01;
k2 = 0.01;
x0 = [k1,k2];
fun = #(x)solveKs(x,t,z,h,I,OCV1,Cap,Rct,static);
options = optimoptions('fsolve','Display','none');
k = fsolve(fun,x0,options);
% Calculate the next state vector from the previous one using RungeKutta
% update equation
znext = z + (1/2)*h*k(1) + (1/2)*h*k(2);``
function [F] = solveKs(x,t,z,h,I,O,C,R,static)
t1 = t + ((1/2)-(1/6)*sqrt(3));
y1 = z + (1/4)*h*x(1) + ((1/4)-(1/6)*sqrt(3))*h *x(2);
t2 = t + ((1/2)+(1/6)*sqrt(3));
y2 = z + ((1/4)+(1/6)*sqrt(3))*h*x(1) + (1/4)*h*x(2);
F(1) = x(1) - stateDeriv_implicit(t1,y1,h,I,O,C,R,static);
F(2) = x(2) - stateDeriv_implicit(t2,y2,h,I,O,C,R,static);
end
What is wrong with this iteration?
This particular piece of code is causing my program to crash. When I disable the code it works but of course giving wrong results. It's supposed to compare sigma with sigma_last until they remain equal at e-14.
This is what I tried first:
long double sigma_last = NULL;
do{
if(sigma_last != NULL){
sigma = sigma_last;
}
sigma1 = atan( tan(beta1) / cos(A1) );
sigmaM = (2*sigma1 + sigma) / 2;
d_sigma = B*sin(sigma)*(cos(2*sigmaM)+(1/4)*B*(cos(sigma)
*(-1+2*pow(cos(2*sigmaM),2)))-(1/6)*B*cos(2*sigmaM)
*(-3+4*pow(sin(sigma),2))*(-3+4*pow(cos(2*sigmaM),2)));
sigma_last = sigma + d_sigma;
}
while(set_precision_14(sigma)<= set_precision_14(sigma_last) || set_precision_14(sigma)>= set_precision_14(sigma_last));
Then I tried using a pointer (desperately):
long double *sigma_last;
*sigma_last = NULL;
do{
if(*sigma_last != NULL){
sigma = *sigma_last;
}
sigma1 = atan( tan(beta1) / cos(A1) );
sigmaM = (2*sigma1 + sigma) / 2;
d_sigma = B*sin(sigma)*(cos(2*sigmaM)+(1/4)*B*(cos(sigma)
*(-1+2*pow(cos(2*sigmaM),2)))-(1/6)*B*cos(2*sigmaM)
*(-3+4*pow(sin(sigma),2))*(-3+4*pow(cos(2*sigmaM),2)));
*sigma_last = sigma + d_sigma;
}
while(set_precision_14(sigma)<= set_precision_14(*sigma_last) || set_precision_14(sigma)>= set_precision_14(*sigma_last));
Finding the source of error in entire code and trying to solve it took me hours, cannot really come up with another "maybe this?" . Feel free to smite me.
Here's a github link to my full code if anyone out there's interested.
Your first (and only) iteration, sigma_last will be null, resulting in crash:
*sigma_last = NULL; // <-- dereferencing uninitialized ptr here
if(*sigma_last != NULL) { // <-- dereferencing uninitialized ptr here too
and if that would have been fixed, here:
*sigma_last == sigma + d_sigma;
This is because you have not set sigma_last to point to some valid floating-point space in memory. There doesn't seem to be any point to using a pointer in this particular case, so if I were you, I'd drop it and use a normal long double instead, as in your first attempt.
In your first example you assign NULL, which is really the value zero, to sigma_last. If zero is not what you're intending, you could either go with a value that most certainly will be out of range (say 1e20 and then compare to say < 1e19) or keep a separate boolan for the job. I personally prefer the first option:
long double sigma_last = 1e20;
...
if(sigma_last < 1e19){
sigma = sigma_last;
}
A better way still would be to use an infinite, or finite, loop and then break out at a certain condition. This will make the code easier to read.
Logic
Finally, you seem to have a problem with your logic in the while, since the comparison sigma <= sigma_last || sigma >= sigma_last is always true. It's always smaller, bigger, or equal.
sigma_last does not need to be a pointer. You just need to somehow flag its value to know whether it was already set or not. From your code I am not sure if we can use zero for this purpose, but we can use some constant (long double minimum value), like this one:
#include <float.h>
const long double invalid_constant = LDBL_MIN;
Try this:
long double DESTINATION_CALCULATION_plusplus ( double phi, double lambda, double S, double azimuth,
double a, double b, double *phi2, double* lambda2, double* azimuth2){
phi = phi*M_PI/180;
lambda = lambda*M_PI/180;
double A1;
double eu2 = (pow(a, 2) - pow(b, 2)) / pow(b, 2); //second eccentricity
double c = pow(a,2) / b;
double v = sqrt(1 + (eu2 * pow(cos(phi) , 2)));
double beta1 = tan(phi) / v;
double Aeq = asin( cos(beta1) * sin(azimuth) );
double f = (a - b) / a; //flattening
double beta = atan((1-f)*tan(phi));
double u2 = pow(cos(Aeq),2)*eu2;
//////////////////////////////----------------------------------------------
long double sigma1 = atan( tan(beta1)/ cos(azimuth) );
long double A = 1 + u2*(4096 + u2*(-768+u2*(320-175*u2))) / 16384;
long double B = u2*(256 + u2*(-128+u2*(74-47*u2)))/1024;
long double sigma = S / (b*A);
long double sigmaM = (2*sigma1 + sigma) /2;
long double d_w;
long double d_sigma;
////////////////////////////------------------------------------------------
double C;
double d_lambda;
long double sigma_last=invalid_constant;
do{
if(sigma_last != invalid_constant){
sigma = sigma_last;
}
sigma1 = atan( tan(beta1) / cos(A1) );
sigmaM = (2*sigma1 + sigma) / 2;
d_sigma = B*sin(sigma)*(cos(2*sigmaM)+(1/4)*B*(cos(sigma)
*(-1+2*pow(cos(2*sigmaM),2)))-(1/6)*B*cos(2*sigmaM)
*(-3+4*pow(sin(sigma),2))*(-3+4*pow(cos(2*sigmaM),2)));
sigma_last = sigma + d_sigma;
}
while(set_precision_14(sigma)<= set_precision_14(sigma_last) || set_precision_14(sigma)>= set_precision_14(sigma_last));
sigma = sigma_last;
*phi2 = atan((sin(beta1)*cos(sigma)+cos(beta1)*sin(sigma)*cos(azimuth))/((1-f)
*sqrt(pow(sin(Aeq),2)+pow((sin(beta1)*sin(sigma)-cos(beta1)*cos(sigma)*cos(azimuth)),2))));
d_w = (sin(sigma)*sin(azimuth))/(cos(beta1)*cos(sigma) - sin(beta1)* sin(sigma)*cos(azimuth));
C = (f/16)*pow(cos(Aeq),2)*(4+f*(4-3*pow(cos(Aeq),2)));
d_lambda = d_w - (1-C)*f*sin(azimuth)*(sigma + C*sin(sigma)*
(cos(2*sigmaM)+C*cos(sigma)*(-1+2*pow(cos(2*sigmaM),2))));
*lambda2 = lambda + d_lambda;
*azimuth2 = sin(Aeq) / (-sin(beta1)*sin(sigma)+cos(beta1)*cos(sigma)*cos(azimuth));
*azimuth2 = *azimuth2 * 180/M_PI;
*lambda2 = *lambda2 * 180/M_PI;
*phi2 = *phi2 * 180/M_PI;
}
I am using Rcpp to integrate a piece of C++ code in R. I am implementing a function in C++ in two ways:
// [[Rcpp::export]]
double rhobiweight(double x,double k = 2.0){
double rho = 1.0;
if(abs(x)<k){
rho = 1.0-pow((1.0-pow(x/k,2)),3);
}
return rho/Erho(k) ;
}
// [[Rcpp::export]]
double rhobiweight2(double x,double k = 2.0){
double rho = 1.0-pow((1.0-pow(x/k,2)),3);
if(abs(x)>k){
rho = 1.0;
}
return rho/Erho(k) ;
}
If the x-value is between 2 and 3, I get different results of these functions. I can't figure out why.
> set.seed(1)
> x = 3*rnorm(10^5)
> c = x
> c2 = x
> for(i in 1:length(x)){
+ c[i] = rhobiweight(x[i])
+ c2[i] = rhobiweight2(x[i])
+ }
> print(sum(c-c2))
[1] -18564.31
The problem comes from your if statement inside the function. The negative of < is >=. so you should either replace < with <= in one function or > with >= in the other, depending on the expected behaviour.
Your problem happens for value between 2.0 and 2.9 inclusive because the abs(int) will always return 2 for this range.
abs(x) takes an int an return an int. Your double x is implicitly conversion to an int in your case.
I have two double variable. double a = 0.10000, double b = 0.1. How can I make sure the comparison (a == b) is always true ?
If you are being paranoid about using == on doubles or floats (which you should be) you can always check that they are close within a small tolerance.
bool same = fabs(a-b) < 0.000001;
The other answers here require you to scale the tolerance factor manually, which I wouldn't advise. For instance if you are comparing two numbers less than one millionth, one answer will always say the two numbers are "close enough." The other answer instead leaves it to the caller to specify which is equally error-prone.
I would instead suggest something like the following function. It will return 0 if the two doubles are within the stated range of each other, otherwise -1 (if d1 is smaller), or +1. Using fabs() may require you to link with the math library, such as with -lm.
#include <algorithm> // for max()
#include <cmath> // for fabs()
int double_compare( double d1, double d2 ) {
double dEpsilon = .00000001;
double dLarger = std::max( std::fabs(d1), std::fabs(d2) );
double dRange = dLarger * dEpsilon;
if ( std::fabs( d1 - d2 ) < dRange )
return 0;
return d1 < d2 ? -1: 1;
}
New answer to old question, but using epsilons is the way to go, check this example:
bool equals(const double a, const double b, const double maxRelativDiff = numeric_limits<double>::epsilon()) {
double difference = fabs(a - b);
const auto absoluteA = fabs(a);
const auto absoluteB = fabs(b);
double biggerBoi = (absoluteB > absoluteA) ? absoluteB : absoluteA; // Get the bigger number
return difference <= (biggerBoi * maxRelativDiff);
}
In this case you're checking if they are equal up to maxRelativDiff, so 0.0001 == 0.0001.
Check: https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon
/**
* Returns a number between kLowerBound and kUpperBound
* e.g.: Wrap(-1, 0, 4); // Returns 4
* e.g.: Wrap(5, 0, 4); // Returns 0
*/
int Wrap(int const kX, int const kLowerBound, int const kUpperBound)
{
// Suggest an implementation?
}
The sign of a % b is only defined if a and b are both non-negative.
int Wrap(int kX, int const kLowerBound, int const kUpperBound)
{
int range_size = kUpperBound - kLowerBound + 1;
if (kX < kLowerBound)
kX += range_size * ((kLowerBound - kX) / range_size + 1);
return kLowerBound + (kX - kLowerBound) % range_size;
}
The following should work independently of the implementation of the mod operator:
int range = kUpperBound - kLowerBound + 1;
kx = ((kx-kLowerBound) % range);
if (kx<0)
return kUpperBound + 1 + kx;
else
return kLowerBound + kx;
An advantage over other solutions is, that it uses only a single % (i.e. division), which makes it pretty efficient.
Note (Off Topic):
It's a good example, why sometimes it is wise to define intervals with the upper bound being being the first element not in the range (such as for STL iterators...). In this case, both "+1" would vanish.
Fastest solution, least flexible: Take advantage of native datatypes that will do wrapping in the hardware.
The absolute fastest method for wrapping integers would be to make sure your data is scaled to int8/int16/int32 or whatever native datatype. Then when you need your data to wrap the native data type will be done in hardware! Very painless and orders of magnitude faster than any software wrapping implementation seen here.
As an example case study:
I have found this to be very useful when I need a fast implementation of sin/cos implemented using a look-up-table for a sin/cos implementation. Basically you make scale your data such that INT16_MAX is pi and INT16_MIN is -pi. Then have you are set to go.
As a side note, scaling your data will add some up front finite computation cost that usually looks something like:
int fixedPoint = (int)( floatingPoint * SCALING_FACTOR + 0.5 )
Feel free to exchange int for something else you want like int8_t / int16_t / int32_t.
Next fastest solution, more flexible: The mod operation is slow instead if possible try to use bit masks!
Most of the solutions I skimmed are functionally correct... but they are dependent on the mod operation.
The mod operation is very slow because it is essentially doing a hardware division. The laymans explanation of why mod and division are slow is to equate the division operation to some pseudo-code for(quotient = 0;inputNum> 0;inputNum -= divisor) { quotient++; } ( def of quotient and divisor ). As you can see, the hardware division can be fast if it is a low number relative to the divisor... but division can also be horribly slow if it is much greater than the divisor.
If you can scale your data to a power of two then you can use a bit mask which will execute in one cycle ( on 99% of all platforms ) and your speed improvement will be approximately one order of magnitude ( at the very least 2 or 3 times faster ).
C code to implement wrapping:
#define BIT_MASK (0xFFFF)
int wrappedAddition(int a, int b) {
return ( a + b ) & BIT_MASK;
}
int wrappedSubtraction(int a, int b) {
return ( a - b ) & BIT_MASK;
}
Feel free to make the #define something that is run time. And feel free to adjust the bit mask to be whatever power of two that you need. Like 0xFFFFFFFF or power of two you decide on implementing.
p.s. I strongly suggest reading about fixed point processing when messing with wrapping/overflow conditions. I suggest reading:
Fixed-Point Arithmetic: An Introduction by Randy Yates August 23, 2007
Please do not overlook this post. :)
Is this any good?
int Wrap(N,L,H){
H=H-L+1; return (N-L+(N<L)*H)%H+L;
}
This works for negative inputs, and all arguments can be negative so long as L is less than H.
Background... (Note that H here is the reused variable, set to original H-L+1).
I had been using (N-L)%H+L when incrementing, but unlike in Lua, which I used before starting to learn C a few months back, this would NOT work if I used inputs below the lower bound, never mind negative inputs. (Lua is built in C, but I don't know what it's doing, and it likely wouldn't be fast...)
I decided to add +(N<L)*H to make (N-L+(N<L)*H)%H+L, as C seems to be defined such that true=1 and false=0. It works well enough for me, and seems to answer the original question neatly. If anyone knows how to do it without the MOD operator % to make it dazzlingly fast, please do it. I don't need speed right now, but some time I will, no doubt.
EDIT:
That function fails if N is lower than L by more than H-L+1 but this doesn't:
int Wrap(N,L,H){
H-=L; return (N-L+(N<L)*((L-N)/H+1)*++H)%H+L;
}
I think it would break at the negative extreme of the integer range in any system, but should work for most practical situations. It adds an extra multiplication and a division, but is still fairly compact.
(This edit is just for completion, because I came up with a much better way, in a newer post in this thread.)
Crow.
Personally I've found solutions to these types of functions to be cleaner if range is exclusive and divisor is restricted to positive values.
int ifloordiv(int x, int y)
{
if (x > 0)
return x / y;
if (x < 0)
return (x + 1) / y - 1;
return 0
}
int iwrap(int x, int y)
{ return x - y * ifloordiv(x, y);
}
Integrated.
int iwrap(int x, int y)
{
if (x > 0)
return x % y;
if (x < 0)
return (x + 1) % y + y - 1;
return 0;
}
Same family. Why not?
int ireflect(int x, int y)
{
int z = iwrap(x, y*2);
if (z < y)
return z;
return y*2-1 - z;
}
int ibandy(int x, int y)
{
if (y != 1)
return ireflect(abs(x + x / (y - 1)), y);
return 0;
}
Ranged functionality can be implemented for all functions with,
// output is in the range [min, max).
int func2(int x, int min, int max)
{
// increment max for inclusive behavior.
assert(min < max);
return func(x - min, max - min) + min;
}
Actually, since -1 % 4 returns -1 on every system I've even been on, the simple mod solution doesn't work. I would try:
int range = kUpperBound - kLowerBound +1;
kx = ((kx - kLowerBound) % range) + range;
return (kx % range) + kLowerBound;
if kx is positive, you mod, add range, and mod back, undoing the add. If kx is negative, you mod, add range which makes it positive, then mod again, which doesn't do anything.
My other post got nasty, all that 'corrective' multiplication and division got out of hand. After looking at Martin Stettner's post, and at my own starting conditions of (N-L)%H+L, I came up with this:
int Wrap(N,L,H){
H=H-L+1; N=(N-L)%H+L; if(N<L)N+=H; return N;
}
At the extreme negative end of the integer range it breaks as my other one would, but it will be faster, and is a lot easier to read, and avoids the other nastiness that crept in to it.
Crow.
I would suggest this solution:
int Wrap(int const kX, int const kLowerBound, int const kUpperBound)
{
int d = kUpperBound - kLowerBound + 1;
return kLowerBound + (kX >= 0 ? kX % d : -kX % d ? d - (-kX % d) : 0);
}
The if-then-else logic of the ?: operator makes sure that both operands of % are nonnegative.
I would give an entry point to the most common case lowerBound=0, upperBound=N-1. And call this function in the general case. No mod computation is done where I is already in range. It assumes upper>=lower, or n>0.
int wrapN(int i,int n)
{
if (i<0) return (n-1)-(-1-i)%n; // -1-i is >=0
if (i>=n) return i%n;
return i; // In range, no mod
}
int wrapLU(int i,int lower,int upper)
{
return lower+wrapN(i-lower,1+upper-lower);
}
An answer that has some symmetry and also makes it obvious that when kX is in range, it is returned unmodified.
int Wrap(int const kX, int const kLowerBound, int const kUpperBound)
{
int range_size = kUpperBound - kLowerBound + 1;
if (kX < kLowerBound)
return kX + range_size * ((kLowerBound - kX) / range_size + 1);
if (kX > kUpperBound)
return kX - range_size * ((kX - kUpperBound) / range_size + 1);
return kX;
}
I've faced this problem as well. This is my solution.
template <> int mod(const int &x, const int &y) {
return x % y;
}
template <class T> T mod(const T &x, const T &y) {
return ::fmod((T)x, (T)y);
}
template <class T> T wrap(const T &x, const T &max, const T &min = 0) {
if(max < min)
return x;
if(x > max)
return min + mod(x - min, max - min + 1);
if(x < min)
return max - mod(min - x, max - min + 1);
return x;
}
I don't know if it's good, but I'd thought I'd share since I got directed here when doing a Google search on this problem and found the above solutions lacking to my needs. =)
In the special case where the lower bound is zero, this code avoids division, modulus and multiplication. The upper bound does not have to be a power of two. This code is overly verbose and looks bloated, but compiles into 3 instructions: subtract, shift (by constant), and 'and'.
#include <climits> // CHAR_BIT
// -------------------------------------------------------------- allBits
// sign extend a signed integer into an unsigned mask:
// return all zero bits (+0) if arg is positive,
// or all one bits (-0) for negative arg
template <typename SNum>
static inline auto allBits (SNum arg) {
static constexpr auto argBits = CHAR_BIT * sizeof( arg);
static_assert( argBits < 256, "allBits() sign extension may fail");
static_assert( std::is_signed< SNum>::value, "SNum must be signed");
typedef typename std::make_unsigned< SNum>::type UNum;
// signed shift required, but need unsigned result
const UNum mask = UNum( arg >> (argBits - 1));
return mask;
}
// -------------------------------------------------------------- boolWrap
// wrap reset a counter without conditionals:
// return arg >= limit? 0 : arg
template <typename UNum>
static inline auto boolWrap (const UNum arg, const UNum limit) {
static_assert( ! std::is_signed< UNum>::value, "UNum assumed unsigned");
typedef typename std::make_signed< UNum>::type SNum;
const SNum negX = SNum( arg) - SNum( limit);
const auto signX = allBits( negX); // +0 or -0
return arg & signX;
}
// example usage:
for (int j= 0; j < 15; ++j) {
cout << j << boolWrap( j, 11);
}
For negative kX, you can add:
int temp = kUpperBound - kLowerBound + 1;
while (kX < 0) kX += temp;
return kX%temp + kLowerBound;
Why not using Extension methods.
public static class IntExtensions
{
public static int Wrap(this int kX, int kLowerBound, int kUpperBound)
{
int range_size = kUpperBound - kLowerBound + 1;
if (kX < kLowerBound)
kX += range_size * ((kLowerBound - kX) / range_size + 1);
return kLowerBound + (kX - kLowerBound) % range_size;
}
}
Usage: currentInt = (++currentInt).Wrap(0, 2);