I'm looking for some nice C code that will accomplish effectively:
while (deltaPhase >= M_PI) deltaPhase -= M_TWOPI;
while (deltaPhase < -M_PI) deltaPhase += M_TWOPI;
What are my options?
Edit Apr 19, 2013:
Modulo function updated to handle boundary cases as noted by aka.nice and arr_sea:
static const double _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348;
static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696;
// Floating-point modulo
// The result (the remainder) has same sign as the divisor.
// Similar to matlab's mod(); Not similar to fmod() - Mod(-3,4)= 1 fmod(-3,4)= -3
template<typename T>
T Mod(T x, T y)
{
static_assert(!std::numeric_limits<T>::is_exact , "Mod: floating-point type expected");
if (0. == y)
return x;
double m= x - y * floor(x/y);
// handle boundary cases resulted from floating-point cut off:
if (y > 0) // modulo range: [0..y)
{
if (m>=y) // Mod(-1e-16 , 360. ): m= 360.
return 0;
if (m<0 )
{
if (y+m == y)
return 0 ; // just in case...
else
return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14
}
}
else // modulo range: (y..0]
{
if (m<=y) // Mod(1e-16 , -360. ): m= -360.
return 0;
if (m>0 )
{
if (y+m == y)
return 0 ; // just in case...
else
return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14
}
}
return m;
}
// wrap [rad] angle to [-PI..PI)
inline double WrapPosNegPI(double fAng)
{
return Mod(fAng + _PI, _TWO_PI) - _PI;
}
// wrap [rad] angle to [0..TWO_PI)
inline double WrapTwoPI(double fAng)
{
return Mod(fAng, _TWO_PI);
}
// wrap [deg] angle to [-180..180)
inline double WrapPosNeg180(double fAng)
{
return Mod(fAng + 180., 360.) - 180.;
}
// wrap [deg] angle to [0..360)
inline double Wrap360(double fAng)
{
return Mod(fAng ,360.);
}
One-liner constant-time solution:
Okay, it's a two-liner if you count the second function for [min,max) form, but close enough — you could merge them together anyways.
/* change to `float/fmodf` or `long double/fmodl` or `int/%` as appropriate */
/* wrap x -> [0,max) */
double wrapMax(double x, double max)
{
/* integer math: `(max + x % max) % max` */
return fmod(max + fmod(x, max), max);
}
/* wrap x -> [min,max) */
double wrapMinMax(double x, double min, double max)
{
return min + wrapMax(x - min, max - min);
}
Then you can simply use deltaPhase = wrapMinMax(deltaPhase, -M_PI, +M_PI).
The solutions is constant-time, meaning that the time it takes does not depend on how far your value is from [-PI,+PI) — for better or for worse.
Verification:
Now, I don't expect you to take my word for it, so here are some examples, including boundary conditions. I'm using integers for clarity, but it works much the same with fmod() and floats:
Positive x:
wrapMax(3, 5) == 3: (5 + 3 % 5) % 5 == (5 + 3) % 5 == 8 % 5 == 3
wrapMax(6, 5) == 1: (5 + 6 % 5) % 5 == (5 + 1) % 5 == 6 % 5 == 1
Negative x:
Note: These assume that integer modulo copies left-hand sign; if not, you get the above ("Positive") case.
wrapMax(-3, 5) == 2: (5 + (-3) % 5) % 5 == (5 - 3) % 5 == 2 % 5 == 2
wrapMax(-6, 5) == 4: (5 + (-6) % 5) % 5 == (5 - 1) % 5 == 4 % 5 == 4
Boundaries:
wrapMax(0, 5) == 0: (5 + 0 % 5) % 5 == (5 + 0) % 5 == 5 % 5 == 0
wrapMax(5, 5) == 0: (5 + 5 % 5) % 5 == (5 + 0) % 5== 5 % 5 == 0
wrapMax(-5, 5) == 0: (5 + (-5) % 5) % 5 == (5 + 0) % 5 == 5 % 5 == 0
Note: Possibly -0 instead of +0 for floating-point.
The wrapMinMax function works much the same: wrapping x to [min,max) is the same as wrapping x - min to [0,max-min), and then (re-)adding min to the result.
I don't know what would happen with a negative max, but feel free to check that yourself!
If ever your input angle can reach arbitrarily high values, and if continuity matters, you can also try
atan2(sin(x),cos(x))
This will preserve continuity of sin(x) and cos(x) better than modulo for high values of x, especially in single precision (float).
Indeed, exact_value_of_pi - double_precision_approximation ~= 1.22e-16
On the other hand, most library/hardware use a high precision approximation of PI for applying the modulo when evaluating trigonometric functions (though x86 family is known to use a rather poor one).
Result might be in [-pi,pi], you'll have to check the exact bounds.
Personaly, I would prevent any angle to reach several revolutions by wrapping systematically and stick to a fmod solution like the one of boost.
There is also fmod function in math.h but the sign causes trouble so that a subsequent operation is needed to make the result fir in the proper range (like you already do with the while's). For big values of deltaPhase this is probably faster than substracting/adding `M_TWOPI' hundreds of times.
deltaPhase = fmod(deltaPhase, M_TWOPI);
EDIT:
I didn't try it intensively but I think you can use fmod this way by handling positive and negative values differently:
if (deltaPhase>0)
deltaPhase = fmod(deltaPhase+M_PI, 2.0*M_PI)-M_PI;
else
deltaPhase = fmod(deltaPhase-M_PI, 2.0*M_PI)+M_PI;
The computational time is constant (unlike the while solution which gets slower as the absolute value of deltaPhase increases)
I would do this:
double wrap(double x) {
return x-2*M_PI*floor(x/(2*M_PI)+0.5);
}
There will be significant numerical errors. The best solution to the numerical errors is to store your phase scaled by 1/PI or by 1/(2*PI) and depending on what you are doing store them as fixed point.
Instead of working in radians, use angles scaled by 1/(2π) and use modf, floor etc. Convert back to radians to use library functions.
This also has the effect that rotating ten thousand and a half revolutions is the same as rotating half then ten thousand revolutions, which is not guaranteed if your angles are in radians, as you have an exact representation in the floating point value rather than summing approximate representations:
#include <iostream>
#include <cmath>
float wrap_rads ( float r )
{
while ( r > M_PI ) {
r -= 2 * M_PI;
}
while ( r <= -M_PI ) {
r += 2 * M_PI;
}
return r;
}
float wrap_grads ( float r )
{
float i;
r = modff ( r, &i );
if ( r > 0.5 ) r -= 1;
if ( r <= -0.5 ) r += 1;
return r;
}
int main ()
{
for (int rotations = 1; rotations < 100000; rotations *= 10 ) {
{
float pi = ( float ) M_PI;
float two_pi = 2 * pi;
float a = pi;
a += rotations * two_pi;
std::cout << rotations << " and a half rotations in radians " << a << " => " << wrap_rads ( a ) / two_pi << '\n' ;
}
{
float pi = ( float ) 0.5;
float two_pi = 2 * pi;
float a = pi;
a += rotations * two_pi;
std::cout << rotations << " and a half rotations in grads " << a << " => " << wrap_grads ( a ) / two_pi << '\n' ;
}
std::cout << '\n';
}}
Here is a version for other people finding this question that can use C++ with Boost:
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>
template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
// copy the sign of the value in radians to the value of pi
T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
// set the value of rad to the appropriate signed value between pi and -pi
rad = fmod(rad+signedPI,(2*boost::math::constants::pi<T>())) - signedPI;
return rad;
}
C++11 version, no Boost dependency:
#include <cmath>
// Bring the 'difference' between two angles into [-pi; pi].
template <typename T>
T normalizeRadiansPiToMinusPi(T rad) {
// Copy the sign of the value in radians to the value of pi.
T signed_pi = std::copysign(M_PI,rad);
// Set the value of difference to the appropriate signed value between pi and -pi.
rad = std::fmod(rad + signed_pi,(2 * M_PI)) - signed_pi;
return rad;
}
I encountered this question when searching for how to wrap a floating point value (or a double) between two arbitrary numbers. It didn't answer specifically for my case, so I worked out my own solution which can be seen here. This will take a given value and wrap it between lowerBound and upperBound where upperBound perfectly meets lowerBound such that they are equivalent (ie: 360 degrees == 0 degrees so 360 would wrap to 0)
Hopefully this answer is helpful to others stumbling across this question looking for a more generic bounding solution.
double boundBetween(double val, double lowerBound, double upperBound){
if(lowerBound > upperBound){std::swap(lowerBound, upperBound);}
val-=lowerBound; //adjust to 0
double rangeSize = upperBound - lowerBound;
if(rangeSize == 0){return upperBound;} //avoid dividing by 0
return val - (rangeSize * std::floor(val/rangeSize)) + lowerBound;
}
A related question for integers is available here:
Clean, efficient algorithm for wrapping integers in C++
A two-liner, non-iterative, tested solution for normalizing arbitrary angles to [-π, π):
double normalizeAngle(double angle)
{
double a = fmod(angle + M_PI, 2 * M_PI);
return a >= 0 ? (a - M_PI) : (a + M_PI);
}
Similarly, for [0, 2π):
double normalizeAngle(double angle)
{
double a = fmod(angle, 2 * M_PI);
return a >= 0 ? a : (a + 2 * M_PI);
}
In the case where fmod() is implemented through truncated division and has the same sign as the dividend, it can be taken advantage of to solve the general problem thusly:
For the case of (-PI, PI]:
if (x > 0) x = x - 2PI * ceil(x/2PI) #Shift to the negative regime
return fmod(x - PI, 2PI) + PI
And for the case of [-PI, PI):
if (x < 0) x = x - 2PI * floor(x/2PI) #Shift to the positive regime
return fmod(x + PI, 2PI) - PI
[Note that this is pseudocode; my original was written in Tcl, and I didn't want to torture everyone with that. I needed the first case, so had to figure this out.]
deltaPhase -= floor(deltaPhase/M_TWOPI)*M_TWOPI;
The way suggested you suggested is best. It is fastest for small deflections. If angles in your program are constantly being deflected into the proper range, then you should only run into big out of range values rarely. Therefore paying the cost of a complicated modular arithmetic code every round seems wasteful. Comparisons are cheap compared to modular arithmetic (http://embeddedgurus.com/stack-overflow/2011/02/efficient-c-tip-13-use-the-modulus-operator-with-caution/).
In C99:
float unwindRadians( float radians )
{
const bool radiansNeedUnwinding = radians < -M_PI || M_PI <= radians;
if ( radiansNeedUnwinding )
{
if ( signbit( radians ) )
{
radians = -fmodf( -radians + M_PI, 2.f * M_PI ) + M_PI;
}
else
{
radians = fmodf( radians + M_PI, 2.f * M_PI ) - M_PI;
}
}
return radians;
}
If linking against glibc's libm (including newlib's implementation) you can access
__ieee754_rem_pio2f() and __ieee754_rem_pio2() private functions:
extern __int32_t __ieee754_rem_pio2f (float,float*);
float wrapToPI(float xf){
const float p[4]={0,M_PI_2,M_PI,-M_PI_2};
float yf[2];
int q;
int qmod4;
q=__ieee754_rem_pio2f(xf,yf);
/* xf = q * M_PI_2 + yf[0] + yf[1] /
* yf[1] << y[0], not sure if it could be ignored */
qmod4= q % 4;
if (qmod4==2)
/* (yf[0] > 0) defines interval (-pi,pi]*/
return ( (yf[0] > 0) ? -p[2] : p[2] ) + yf[0] + yf[1];
else
return p[qmod4] + yf[0] + yf[1];
}
Edit: Just realised that you need to link to libm.a, I couldn't find the symbols declared in libm.so
I have used (in python):
def WrapAngle(Wrapped, UnWrapped ):
TWOPI = math.pi * 2
TWOPIINV = 1.0 / TWOPI
return UnWrapped + round((Wrapped - UnWrapped) * TWOPIINV) * TWOPI
c-code equivalent:
#define TWOPI 6.28318531
double WrapAngle(const double dWrapped, const double dUnWrapped )
{
const double TWOPIINV = 1.0/ TWOPI;
return dUnWrapped + round((dWrapped - dUnWrapped) * TWOPIINV) * TWOPI;
}
notice that this brings it in the wrapped domain +/- 2pi so for +/- pi domain you need to handle that afterward like:
if( angle > pi):
angle -= 2*math.pi
Exponential limit of most 32 bit machines is
exp( +/-700 )
But I would like to do an exponential calculation
res = exp( x ) / exp( d )
when x or d are bigger than 700 I use the fact that
exp( x + y ) = exp( x ) . exp( y )
So my calculation would be something along the line of
res = (exp( x - z ).exp(z)) / (exp( d - z ).exp(z))
or
res = exp( x - z ) / exp( d - z )
where (x-z) < 700
But this approach is flawed in some cases, for example where x = 6000 and d = 10000
If we use z=5300 then
res = exp( 6000 - 5300 ) / exp( 10000 - 5300 )
res = exp( 700 ) / exp( 47000 )
But exp( 47000 ) = 0 on a 32 bit machine.
If I replace z = 9300 then I get the opposite effect.
res = exp( -3300 ) / exp( 700 )
So how could I solve the above equations, (that should return a 32bit valid number I think), given the limitations of the computer?
Edit
The reason for doing this is I am using the formula
P( a ) = P(y1) * P(y2) * P(y3) ... P(yN)
In order to prevent overflow I then do
a = log( P(y1) ) + log( P(y2) ) + log (P(y3)) ... log( P(yN) )
b = log( P(z1) ) + log( P(z2) ) + log (P(z3)) ... log( P(zN) )
...
z = log( P(zz1) ) + log( P(zz2) ) + log (P(zz3)) ... log( P(zzN) )
to get a total I do
total = a + b ... z
and to calculate the percentage I do
(exp(a) / exp( total ) ) * 100
but it is possible that "a" and/or "total" are greater than 700
I guess the question could be how could I calculate the percentage without using the exponential
It doesn't matter that the answer should be a 32 bit number if some of the intermediate steps in the calculations aren't.
For math that goes outside the bounds of an int or long type, you probably need to start using something like GMP.
https://gmplib.org/
I assume that you want to compute this:
p = exp(a) / exp(b)
And since a^b/a^c == a^(b-c) this reduces to
p = exp(a - b)
which can be easily computed if that difference is below that critical exponent.
If it isn't, then your result cannot be represented by primitive datatypes like double (because it's either extremely large or extremely small), you then need some kind of arbitrary precision numbers, probably provided by some library.
But if you only need to print the result, or store it somehow, then you can easily compute even extremely large numbers:
For that, you change to base 10 (for displaying), compute the equivalent exponent therefore (tExponent = log10(eExponent)), and get that value into the allowed range between std::numeric_limits::max_exponent10 and std::numeric_limits::min_exponent10, saving the difference as scaling factor.
For now, I just have a quick and dirty live example, showing
exp(90000) / exp(100) = 1.18556 scaled by 10^39043
(Check at wolfram alpha)
Note: When I wrote this, it was pretty late in the evening. I'm leaving this here for an "alternative" approach.
Now, generally, there's
a^b = [a^(b/c)]^c
And since
(a/b)^c = (a^c)/(b^c)
holds, too, I guess the easiest approach here is to just divide both exponents as long as one of them is above your critical value, then do the exponentiation, divide the results, and finally use the divisor of the former exponents as exponent for the quotient:
double large_exp_quot(
double eNum,
double eDenom,
unsigned int const critical = 200) {
if (abs(eNum - eDenom) > critical) {
throw out_of_range{"That won't work, resulting exponent is too large"};
}
unsigned int eDivisor = 1;
while (abs(eNum) > critical or abs(eDenom) > critical) {
eNum /= 2;
eDenom /= 2;
eDivisor *= 2;
}
return pow(exp(eNum) / exp(eDenom), eDivisor);
}
But this will only work, if the result of your computation can actually be stored using the C++ primitive datatypes, in this case double. The example you gave ... with exponents 6000 and 10000 ... is obviously not representable with a double (it's e^(-4000) and thus incredibly small)
Numerically unstable computation: exp(a) / exp(b)
Equivalent stable computation: exp(a - b)
Numerically unstable computation: Πi=1..n pi
Equivalent stable computation: exp(Σi=1..n log(pi))
Lets have two points, (x1, y1) and (x2,y2)
dx = |x1 - x2|
dy = |y1 - y2|
D_manhattan = dx + dy where dx,dy >= 0
I am a bit stuck with how to get x1 - x2 positive for |x1 - x2|, presumably I introduce a binary variable representing the polarity, but I am not allowed multiplying a polarity switch to x1 - x2 as they are all unknown variables and that would result in a quadratic.
If you are minimizing an increasing function of |x| (or maximizing a decreasing function, of course),
you can always have the aboslute value of any quantity x in a lp as a variable absx such as:
absx >= x
absx >= -x
It works because the value absx will 'tend' to its lower bound, so it will either reach x or -x.
On the other hand, if you are minimizing a decreasing function of |x|, your problem is not convex and cannot be modelled as a lp.
For all those kind of questions, it would be much better to add a simplified version of your problem with the objective, as this it often usefull for all those modelling techniques.
Edit
What I meant is that there is no general solution to this kind of problem: you cannot in general represent an absolute value in a linear problem, although in practical cases it is often possible.
For example, consider the problem:
max y
y <= | x |
-1 <= x <= 2
0 <= y
it is bounded and has an obvious solution (2, 2), but it cannot be modelled as a lp because the domain is not convex (it looks like the shape under a 'M' if you draw it).
Without your model, it is not possible to answer the question I'm afraid.
Edit 2
I am sorry, I did not read the question correctly. If you can use binary variables and if all your distances are bounded by some constant M, you can do something.
We use:
a continuous variable ax to represent the absolute value of the quantity x
a binary variable sx to represent the sign of x (sx = 1 if x >= 0)
Those constraints are always verified if x < 0, and enforce ax = x otherwise:
ax <= x + M * (1 - sx)
ax >= x - M * (1 - sx)
Those constraints are always verified if x >= 0, and enforce ax = -x otherwise:
ax <= -x + M * sx
ax >= -x - M * sx
This is a variation of the "big M" method that is often used to linearize quadratic terms. Of course you need to have an upper bound of all the possible values of x (which, in your case, will be the value of your distance: this will typically be the case if your points are in some bounded area)
Is there the better way to calculate the average of two doubles? How could I improve / correct my code below?
double original_one, original_two; // can be any double >= 0
double used_one = original_one;
double used_two = original_two;
if ( original_one == 0 ) used_one = 1;
if ( original_two == 0 ) used_two = 1;
double average = used_one * used_two / 2; // average!
The arithmetic mean of two numbers is computed by adding them, and dividing by two...
double average = (original_one + original_two) / 2;
This is one way to compute the average, there are several more but this is the most common.
Suppose you have have a rectangle, bottom-left point 0,0 and upper-right point is 100,100.
Now two line intersects the rectangle. I have to find out the coordinate of the intersection point. I have done that. Now the problem is I can't tell whether it is inside the rectangle or not. I used double comparison. But I think it is giving me wrong answer. Suppose the intersection point is ( x , y ). I used this checking for comparison : if( x >= 0.0 && x <= 100.0 && y >= 0.0 && y <= 100.0 ). What should I do?
//this function generates line
line genline( int x1 , int y1 , int x2 , int y2 ){
line l ;
l.A = y2 - y1 ;
l.B = x1 - x2 ;
l.C = l.A * x1 + l.B * y1 ;
return l ;
}
//this function checks intersection
bool intersect( line m ,line n ) {
int det = m.A * n.B - m.B * n.A ;
if( det == 0 ){
return false ;
}
else {
double x = ( n.B * m.C - m.B * n.C ) / ( det * 1.0 ) ;
double y = ( m.A * n.C - n.A * m.C ) / ( det * 1.0 ) ;
if( x >= 0.0 && x <= L && y >= 0.0 && y <= W ) { return true ; }
else{ return false ; }
}
}
EDIT:
Both the line are stretched to infinity.
Your math looks like it's right. By the way, If a line intersects something, it is always inside that something.
Checking to see if a point is inside a rectangle is relatively easy. However, the challenge is to find the intersection between two line segments. There are a large number of corner cases to that problem and limited accuracy of floating point numbers play a huge roll here.
Your algorithm seems to be overly simplistic. For a deeper discussion about this topic you can look at this and this. This two parts article investigates the problem of finding the intersection of two lines using floating point numbers. Notice that they are about MATLAB not C++ though that does not change the problem and the algorithms are easily translatable to any language.
Depending on application, even with clever tricks floating point representation might not simply cut it for some geometry problems. CGAL is a C++ library dedicated to computational geometry that deals with these kind problems. When necessary it uses arbitrary precision arithmetic to handle degenerate cases.
When you're dealing with floating point (or double), testing for equality is naïve and will fail in edge cases. Every comparison you make should be in reference to "epsilon", an extremely small quantity that doesn't matter. If two numbers are within epsilon for each other, then they are considered equal.
For example, instead of "if(a == b)", you need:
bool isEqual(double a, double b, double epsilon = 1.E-10)
{ return fabs(a - b) <= epsilon;
}
Pick a suitable value for epsilon depending on your problem domain.