C program for secant method to find roots of polynomial - c

Here is the question:
Write a program to find the real roots of the following equation using the Secant method:
f(x) = 23x^4 -13x^3 + 3x^2 - 5x + 38.
Let h = r(i) - r(i-1), where r(i) is the root computed in iteration i of your program. Your program should continue to refine its answer until h < 10 - 3. This value is known as the convergence criterion. Your program should print the final value of the root and the number of iterations required to compute it.
Here is my program:
#include <stdio.h>
double function ( double i );
int main ()
{
double x_1, x_2, h_x, temp, a, b;
int count = 0;
printf("Enter first approximation: ");
scanf("%lf", &x_1);
printf("Enter second approximation: ");
scanf("%lf", &x_2);
a = function ( x_1 );
b = function ( x_2 );
h_x = x_2 - x_1;
if ( h_x < 0 )
{
h_x = ( h_x < 0 )? - h_x: h_x;
}
while ( h_x >= ( 1.E-3 ) && count <= 999999999 )
{
a = function ( x_1 );
b = function ( x_2 );
temp = ( ( x_1 * b ) - ( x_2 * a ) / ( b - a ) );
x_1 = x_2;
x_2 = temp;
printf("%lf\n", x_1);
printf("%lf\n", x_2);
count += 1;
h_x = x_2 - x_1;
if ( h_x < 0 )
{
h_x = ( h_x < 0 )? - h_x: h_x;
}
}
printf("Final value of root is %lf\n", x_2);
printf("%d iterations were required to compute it\n", count);
return 0;
}
double function ( double i )
{
double result;
result = ( 23 * i * i * i * i ) - ( 13 * i * i * i ) + ( 3 * i * i ) - ( 5 * i ) + 38;
return result;
}
The problem with my code is that it doesn't work for any other input that is different from 0 and 1.
I don't see what's the problem with my code though, I have added a code for absolute error, and the formula looks right to me. It isn't logical to me that with different initial guesses, the root returned is different, and just loops forever.
I would just like to know if it is a coding mistake, or a math error. Or is it that the roots oscillate? I just don't know what's wrong with my program. Can anyone help me out? Thanks.

Your midpoint formula is off. It seems to be a mix of two variants of the formula. The formula close to the derivation as root of the secant line
y = f(x_2)+(x-x2) * (f(x2)-f(x1))/(x2-x1)
gives
x3 = x2 - f(x2)*(x2-x1)/(f(x2)-f(x1)).
Bringing everything into one fraction gives the equivalent
x3 = (x1*f(x2) -x2*f(x1)) / (f(x2)-f(x1)).
You get a shorter formula for the polynomial using the Newton-Horner trick,
f(x) = ((( 23 * x - 13 ) * x + 3 ) * x - 5 ) * x + 38.
You should avoid the repeated computation of the same quantity, even if it does not have any noticeable effect here. Declare variables for the values f(x1) and f(x2).
Use the absolute value function to compute absolute values. hx = fabs(x2-x1). Or use hx=(hx<0)?-hx:hx; to avoid including math.h.

Related

printing the result of multiplication of return of function ( int ) and a float number constant

i have a problem where the function somme_diagonale return the right result when called in main, but as soon as i multiply it to ( 2 / 9 ) and print the result it appears to be 0.
function :
{
int i, s = 0 ;
for ( i = 0 ; i < n ; i++ )
{
s = s + mat [ i ] [ i ] ;
}
return s ;
}```
call :
``` printf("resultat est : %f", ( float ) ( 2 / 9 ) * somme_diagonale ( F ) );```
i have tested that somme_diagonale ( F ) returns 165 ( int ).
can someone help me ?
Hi, in order to have a float value, either the 2 or the 9 must be cast. This, though, must happen before the division is performed. (2/9) produces a result of type int, which only then is cast to float.
What you should do is replace that piece of code with of these options (sorry in advance, I'm not a fan of spaces):
(float)2 / 9 * somme_diagonale(F)
2 / (float)9 * somme_diagonale(F)
(float)2 / (float)9 * somme_diagonale(F)

Performance of n-section root finding algorithm

I wrote a n-section algorithm for finding roots of a function. The working principle is exactly the same as is bisection method, only the range is divided into N equal parts instead. This is my C code:
/*
Compile with: clang -O3 -o nsect nsect.c -Wall -DCOUNT=5000000 -DNSECT=? -funroll-loops
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#define N 6
#ifndef COUNT
#error COUNT not defined!
#endif
#ifndef NSECT
#define NSECT 2
#endif
float polynomial[N];
float horner( const float poly[N], float x )
{
float val = poly[N-1];
for ( int i = N - 2; i >= 0; i-- )
val = poly[i] + x * val;
return val;
}
float f( float x )
{
return horner( polynomial, x );
}
float nsect( float a, float b, const float eps_x )
{
float fa = f( a );
float fb = f( b );
if ( fa == 0 ) return a;
else if ( fb == 0 ) return b;
else if ( fa * fb > 0 ) return 0;
float x[NSECT];
float fx[NSECT];
while ( b - a > eps_x )
{
x[0] = a;
if ( ( fx[0] = f( x[0] ) ) == 0 ) return x[0];
int found = 0;
for ( int i = 0; i < NSECT - 1; i++ )
{
x[i + 1] = a + ( b - a ) * (float)( i + 1 ) / NSECT;
if ( ( fx[i + 1] = f( x[i + 1] ) ) == 0 )
return x[i + 1];
else if ( fx[i] * fx[i + 1] < 0 )
{
a = x[i];
b = x[i + 1];
found = 1;
break;
}
}
if ( !found )
a = x[NSECT - 1];
}
return ( a + b ) * 0.5f;
}
int main( int argc, char **argv )
{
struct timeval t0, t1;
float *polys = malloc( COUNT * sizeof( float ) * N );
float *p = polys;
for ( int i = 0; i < COUNT * N; i++ )
scanf( "%f", p++ );
float xsum = 0; // So the code isn't optimized when we don't print the roots
p = polys;
gettimeofday( &t0, NULL );
for ( int i = 0; i < COUNT; i++, p += N )
{
memcpy( polynomial, p, N * sizeof( float ) );
float x = nsect( -100, 100, 1e-3 );
xsum += x;
#ifdef PRINT_ROOTS
fprintf( stderr, "%f\n", x );
#endif
}
gettimeofday( &t1, NULL );
fprintf( stderr, "xsum: %f\n", xsum );
printf( "%f ms\n", ( ( t1.tv_sec - t0.tv_sec ) * 1e6 + ( t1.tv_usec - t0.tv_usec ) ) * 1e-3 );
free( polys );
}
EDIT: This is the command I used to compile the code: clang -O3 -o nsect nsect.c -Wall -DCOUNT=5000000 -DNSECT=? -funroll-loops.
I ran everything on i7-8700k.
I decided to test the algorithm performance for different N values. The test consists of measuring time needed to find any root in range (-100;100) for each of 5,000,000 polynomials of degree 5. The polynomials are randomly generated and have real coefficients ranging from -5 to 5. The polynomial values are calculated using Horner's method.
These are results I got from running the code 10 times for each N (x=N, y=time [ms]):
My understanding of the worst case performance here is that the amount of work to be done in the main while loop is proportional to N. The main loop needs logN(C) (where C > 1 is a constant - ratio of initial search range to requested accuracy) iterations to complete. This yields following equation:
The plot looks very similar to the violet curve I used above to roughly match the data:
Now, I have some (hopefully correct) conclusions and questions:
Firstly, is my approach valid at all?
Does the function I came up with really describe the relation between number of operations and N?
I think this is the most interesting one - I'm wondering what causes such significant difference between N=2 and all the others? I consistently get this pattern for all my test data.
Moreover:
That function has minimum in e&approx;2.718..., which is closer to 3 than to 2. Also, f(3) < f(2) holds. Given that the equation I came up with is correct, I think that would imply that trisection method may actually be more efficient than bisection. It seems a bit counter intuitive, but the measurements seem to acknowledge that. Is this right?
If so, why does trisection method seem to be so unpopular compared to bisection?
Thank you
I am commenting on the general question, not on your particular code, which I don't fully understand.
Assuming that there is a single root known to be in an interval of length L and the desired accuracy is ε, you will need log(L/ε)/log(N) subdivision stages. Every subdivision stage requires N-1 evaluations of the function (not N), to tell which subinterval among N contains the root.
Hence, neglecting overhead, the total cost is proportional to (N-1)/log(N). The values of this ratio are, starting from N=2:
1.44, 1.82, 2.16, 2.49, 2.79...
and higher.
Hence the theoretical optimum is with N=2. This is why trisection is not used.

Formula nested in loop won't execute properly

I'm trying to solve a code to run this series pi = 4 - 4/3 + 4/5 - 4/7 + 4/9 ... and so on. The thing is, as i run with higher values, the function tends to 4, not 3,1415.... The program seem to be running only the greatest value that n assumes. Also i cant make %%.lflf work to set decimals according to a variable, (p). The algorithm seems correct but after days i'm desperate for any help, greatly appreciated.
if ( 2 == route ) {
printf("piseries calculator\n");
double pi,n,p;
printf("define precision");
scanf("%lf",&p);
n=0;
while (n++ <= p ) {
pi = (4) - (n * ( 4 / ( 1 + 2 *(n)))) +( n * ( 4 / ( 3 + 2 * (n))));
}
printf("%%.lflf",p,pi);
return 0;
}
First of all, make n and p integers.
Secondly n starts at 3 doesn't it?
Third the Pi series is 4.0 (- fraction + fraction)...
Finally you can printf using %.*lf to increase/limit the precision of the output.
if ( 2 == route )
{
printf("piseries calculator\n");
double pi=4.0;
int n,p;
printf("define precision");
scanf("%d",&p);
for(n=3;n<p;n+=2)
{
pi -= 4.0/n;
n+=2;
pi += 4.0/n;
}
printf("%.*lf",p,pi);
return 0;
}
I would scale it as well for those calculations
#include <stdio.h>
#define Niterations 1000
#define SCALE 1000
int main(void) {
double pi = 4.0 * SCALE;
for(unsigned i = 0; i < Niterations; i ++)
{
double f = 4.0 * SCALE / (3 + i * 2);
pi += (i & 1) ? f : -f;
}
printf("N iterations = %u, pi = %lf", Niterations, pi / SCALE);
// your code goes here
return 0;
}
Test it https://ideone.com/mSZNyg
Result: N iterations = 10000, pi = 3.141692643590519029572760700830258429050445556640625000000000 ...

Issues trying to scale up sine wave in c

Hopefully somebody can point out why this isnt working or where i may be going wrong. Im producing a sine wave by way of for loops in c. The ultimate aim is to produce a .ppm file displaying this. Im working on a 1 to 1 pixel ratio. My box is 128H*256W. The sine wave is displaying but due to the answer being produced in rads the result is a very small two pixel high "wave". I assume this is due to the rad values being between 1 and -1. This is my code. I tried just simply timesing by a greater number to increase the size of the y values in the hopes it would plot correctly but this does little or worse causes the applicattion to stop running. Any ideas very welcome.
for (x = 0; x < H; x++)
{
y =(int) H/2+ sin(x*(2*PI));
y = y * 50;
image[y][x][1] = 0;
image[y][x][2] = 255;
image[y][x][3] = 0;
}
EDIT: This is what is being produced in the .ppm file when opened via infraview. Also im #defining PI 3.141592653589793. Again is this possibly an area of issue.
first sine wave .ppm
I conject that y is an int.
Your sin value will be truncated to an integer; 0 for most cases, but very occasionally -1 or +1.
The fix is simple: use a floating point for y, and cast once you want to use it as an array index.
As y is commented to be an int and H appears to be an int constant, perform calculations as double first, then convert to int.
Use round to avoid truncations effect of simply casting a double to int.
y = (int) round(50*(sin(x*(2*PI)) + H/2.0));
Original code also scaled H/2 by 50. I think code may only want to scale the sin() and not the offset.
#define XOffset 0
#define YOffset (H/2.0)
#define XScale (2*PI)
#define YScale 50
y = (int) round(YScale*sin(x*XScale + XOffset) + YOffset);
Defensive programming tip: since y is calculated, insure it is in the valid index range before using it as an index.
// Assuming image` is a fixed sized array
#define Y_MAX (sizeof image/sizeof image[0] - 1)
if (y >= 0 && y <= Y_MAX) {
image[y][x][1] = 0;
image[y][x][2] = 255;
image[y][x][3] = 0;
}
y = y * 50, where y = H/2 (+ or - 1) gives you y around 25*H, which is out of bounds.
A closer approximation is this:
y = (int) ( H/2 + H/2 * sin(x*2*PI) )
which gives the extremes H/2 - H/2 = 0 and H/2 + H/2 = H, which is one too high. So, we scale not by H/2 but by (H-1)/2:
y = (int) ( H/2 + (H-1)/2 * sin(x*2*PI) )
which gives us an y-range 0 to H-1.
To have a bit more control over the period of the sine wave, let's write it like this:
sin( x/W * 2*PI )
Here, we divide x by W so that x/W itself will range from 0 to 1.
It is then scaled by 2*PI to produce a range from 0 to 2π. This will plot one period of the sine wave across the entire width. If we introduce a frequency factor f:
sin( f * x/W * 2*PI )
we can now say how many periods to draw, even fractions. For f=1 it will draw one period, f=2 two periods, and f=1 half a period.
Here's a small JS demo showing three values for f: 0.5 is red, 1 is green and 2 is white:
var c = document.getElementById('c'),
W = c.width,
H = c.height,
ctx = c.getContext('2d'),
image = ctx.getImageData(0,0,W,H);
for ( var i = 0; i < image.data.length; i +=4) {
image.data[i+0]=0;
image.data[i+1]=0;
image.data[i+2]=0;
image.data[i+3]=255;
}
function render(image,colidx,f) {
for ( var x = 0; x < W; x++ )
{
var y = H/2 - Math.round( H/2 * Math.sin(f*x/W*Math.PI*2) );
if ( y >=0 && y < H ) {
if ( colidx & 1 ) image.data[ 4*( W*y + x ) + 0] = 255;
if ( colidx & 2 ) image.data[ 4*( W*y + x ) + 1] = 255;
if ( colidx & 4 ) image.data[ 4*( W*y + x ) + 2] = 255;
}
}
}
render(image,1,0.5);
render(image,2,1);
render(image,7,2);
ctx.putImageData(image, 0,0);
canvas{ border: 1px solid red;}
<canvas id='c' width='256' height='128'></canvas>
The code then becomes:
float f = 1;
for (x = 0; x < W; x++)
{
y = (int) ( (H-1)/2 + (H-1)/2 * sin(f * x/W * 2*PI) );
image[y][x][0] = 0;
image[y][x][1] = 255;
image[y][x][2] = 0;
}

Optimize C code preventing while loops [duplicate]

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

Resources