Solve quadratic equation when coefficients may be 0 - c

I was given a problem to write a C program which would solve the equation ax2+bx+c=0, where a, b and c are coefficients with double type. Any of the coefficients may be zero. In this problem it is unclear to me how to handle the double variables.
Here is my code. As for now, I know that my program can't distinguish between two roots and infinitely many roots. It also doesn't detect the "linear equation situation". How can I make it detect an infinite number of solutions? I was also advised in the comments to calculate the root with the minus before the discriminant if b > 0 and then use the Viet's theorem. I understand that it is because it is always more accurate to sum two numbers. I also guess I should do the exact opposite with b < 0. But what if b == 0 ? In this case, the program will not do anything. Or should I just include b == 0 in b < 0 and have b <= 0 ?
#include <stdio.h>
#include <math.h>
#include <float.h>
int main() {
double a, b, c, x1, x2;
scanf("%lf", &a);
scanf("%lf", &b);
scanf("%lf", &c); // just reading variables
//ax^2+bx+c=0
if ((b * b - 4 * a * c) < 0) {
printf("no");
} else {
x1 = (-b + sqrt(b * b - 4 * a * c)) / (2 * a); //calculating roots
x2 = (-b - sqrt(b * b - 4 * a * c)) / (2 * a);
if ((fabs((a * x1 * x1 + b * x1 + c)) < DBL_EPSILON) & (fabs((a * x2 * x2 + b * x2 + c)) < DBL_EPSILON)) { //plugging the roots in
if (fabs((x1 - x2)) < DBL_EPSILON) { //checking if the roots are equal
printf("%lf", &x1); // if they are equal, we print only one of them
} else {
printf("%lf", &x1); // if they are not equal, we print both.
printf("\n %lf", &x2);
}
} else { // if there are no two valid roots
if ((fabs((a * x1 * x1 + b * x1 + c)) < DBL_EPSILON)) // we try to find one root.
printf("%lf", &x1);
if (fabs((a * x2 * x2 + b * x2 + c)) < DBL_EPSILON)
printf("%lf", &x2);
if ((fabs((a * x1 * x1 + b * x1 + c)) > DBL_EPSILON) & (fabs((a * x2 * x2 + b * x2 + c)) > DBL_EPSILON)) // if both of the plugged roots don't satisfy the equation
printf("no");
}
}
return 0;
}

Solve quadratic equation when coefficients may be 0
How can I make it detect an infinite number of solutions?
When a==0 && b == 0 && c == 0.
No DBL_EPSILON needed really anywhere in this code. See also #Eric Postpischil.
But what if b == 0 ?
if (b == 0) { // y = a*x*x + c
if (a) {
double dd = -c/a;
if (dd >= 0) {
double d = sqrt(d);
printf_roots("+/- roots", d,-d);
} else {
printf_roots("Complex roots", NAN, NAN); // Note NAN may not exist
}
} else if (c) { // y = 0*x*x + c, c != 0
printf_roots("No roots", NAN, NAN);
} else { // y = 0*x + 0
printf_roots("Infinite roots", -HUGE_VAL, HUGE_VAL);
}
Or should I just include b == 0 in b < 0 and have b <= 0 ?
Unless the coding goal requires a special output when b==0, I would only vector code on b==0 as a subtest when a==0 occurred.
if (a==0) {
if (b == 0) {
The quadric equation, like much FP code, can readily overflow and hit 0, both cases losing all precision.
Consider the code below: the unnecessary subtraction may cause overflow or truncation to 0 versus the second which may not. It is dependent on many things.
if ((b * b - 4 * a * c) < 0)
//
if (b * b < 4 * a * c)
Further, C allows various calculations to occur using wider math. Research FLT_EVAL_METHOD. Because of this, to prevent sqrt(value_less_than_0), code should calculate the discriminate and then test the object x that is going to be applied to sqrt(x).
//if ((b * b - 4 * a * c) < 0) {
// printf("no");
//} else {
// x1 = (-b + sqrt(b * b - 4 * a * c))
double discriminate = b * b - 4 * a * c;
if (discriminate < 0) {
printf("no");
} else {
double d = sqrt(discriminate);
x1 = (-b + d)
As to the idea of "calculate the root with the minus before the discriminant if b > 0 and then use the Viet's theorem", I'd suggest for improved retained precision the below which does not subtract like signed values.
double d = sqrt(discriminate);
// Note x1*x2 = c/a
if (b < 0) {
x2 = (-b + d)/(2*a);
x1 = c/a/x2;
} else {
x1 = (-b - d)/(2*a);
x2 = c/a/x1;
}
printf_roots("2 roots", x1, x2);
Notes on printf("%lf", &x1);. You are not compiling with all warnings enabled. Save time - enable them. Should be printf("%lf", x1); No &.
Further double is floating point. For FP code development use "%e", "%a" or"%g" to full see significant information.
printf("%g\n", some_double);
// or better
printf("%.*e\n", DBL_DECIMAL_DIG -1, some_double);

Since division by zero is not allowed, you have to split the problem into 4 cases :
a != 0:
this is case you treated in your code.
a == 0 && b != 0 :
This is a linear equation where the solution is x = -c/b
a == 0 && b == 0 && c != 0 : There's no possible value for x.
In this last case, a, b and c are equals to 0 : there's infinitly many solutions for x.
EDIT: comparisons with epsilon removed since they seem to be useless

There are some problems in your code:
you should check the return values of scanf() to avoid undefined behavior on invalid input.
you should use local variables for intermediary results to improve code readability
your printf statements are incorrect: you should pass the values of the double variables instead of their addresses: printf("%lf", &x1); should read:
printf("%f", x1);
Regarding the degenerate cases, you should just test those before trying to resolve the second degree equation.
Here is a corrected version:
#include <stdio.h>
#include <math.h>
int main() {
double a, b, c, delta, x1, x2;
if (scanf("%lf%lf%lf", &a, &b, &c) != 3) {
printf("invalid input\n");
return 1;
}
if (a == 0) {
// not a quadratic equation
if (b != 0) {
printf("one solution: %g\n", -c / b);
} else {
if (c != 0) {
printf("no solution\n");
} else {
printf("all real values are solutions\n");
}
}
} else {
delta = b * b - 4 * a * c;
if (delta < 0) {
printf("no real solution\n");
} else
if (delta == 0) {
printf("one double solution: %g\n", -b / (2 * a));
} else {
x1 = (-b + sqrt(delta)) / (2 * a);
x2 = (-b - sqrt(delta)) / (2 * a);
printf("two solutions: %g, %g\n", x1, x2);
}
}
return 0;
}

Related

Read the coefficients a,b,c of the quadratic equation ax^2+bx+c and print it roots nicely for imaginary roots print in x+iy form

#include <math.h>
#include <stdio.h>
main() {
int a, b, c, x, x1, x2;
printf("enter the values of a,b,c:");
scanf("%d%d%d", &a, &b, &c);
printf("The quadratic equation is %d*pow(x,2)+%d*x+%d=0", a, b, c);
if (pow(b, 2) - 4 * a * c >= 0) {
x1 = (-b + sqrt(pow(b, 2) - 4 * a * c)) / 2 * a;
x2 = (-b - sqrt(pow(b, 2) - 4 * a * c)) / 2 * a;
printf("the roots of the equation are x1=%d,x2=%d", x1, x2);
}
else
printf("roots of the equation in the form of x+iy and x-iy");
return 0;
}
Is this code alright for the given question, i had a bit confusion at that printing imaginary roots. could you please help
Use proper main prototype.
Use floating point numbers instead of integers
pow(b,2) == b*b
/ 2 * a -> / (2 * a)
int main(void) {
double a, b, c, x, x1, x2;
printf("enter the values of a,b,c:");
if(scanf("%lf %lf %lf", &a, &b, &c) != 3) { /* handle error */}
printf("\nThe quadratic equation is %f*x^2+%f*x+%f=0\n", a, b, c);
if (b*b - 4 * a * c >= 0) {
x1 = (-b + sqrt(b*b - 4 * a * c)) / (2 * a);
x2 = (-b - sqrt(b*b - 4 * a * c)) / (2 * a);
printf("the roots of the equation are x1=%f,x2=%f\n", x1, x2);
}
else
{
double r = -b / (2*a);
double z = sqrt(fabs(b*b - 4 * a * c));
printf("the roots of the equation are x1 = %f + i%f, x2 = %f - i%f", r,z,r,z);
}
}
https://gcc.godbolt.org/z/Ys1s8bWY7

(Beginner) Can't find the problem with this code in C, it returns weird values for the floats (-nanloat)

It's just a super basic program that finds the roots of a quadratic function. After the calculations the output for the solutions shows "-nanloat" instead of the actual answer. Here's the problematic code:
/*If the polynomial has real solutions, calculate and print them */
if (nRSolutions > 0){
sol = ((-b + sqrt(pow(2.0,b) + 4 * a * c)) / (2.0 * a));
printf("La solución a la ecuación es %float", sol);
/*That means "the solution to the equation is (float)"*/
}
if (nRSolutions = 2){
sol2 = ((-b - sqrt(pow(2.0,b) + 4 * a * c)) / (2.0 * a));
printf("La segunda solución a la ecuación es %float", sol2);
/*That means "the second solution to the equation is (float)"*/
}
Here's the variable declaration in case that might be part of the problem, but I really don't think that's it.
/* d = discriminant value; sol = solution; sol2 = second solution*/
float a, b, c, d, sol, sol2;
int nRSolutions;
nRSolutions = 0;
They are all defined afterwards.
Here's your problem:
if (nRSolutions = 2){
You're not comparing but assigning, and since the value you're assigning is nonzero the condition will always be true. You instead want:
if (nRSolutions == 2){
Wrong equation (in addition to issue identified by#dbush). See Quadratic formula:
// sqrt(pow(2.0,b) + 4 * a * c))
sqrt(pow(b, 2.0) - 4 * a * c))
// ^^^^^^^^^^^ ^
Even better as
double discriminate = b*b - 4*a*c;
if (discriminate < 0.0) TBD_code();
else sol = (-b + sqrt(discriminate)) / (2.0 * a);
Tip for improve precision: minimize subtraction cancellation
double discriminate = b*b - 4*a*c;
if (discriminate < 0.0) TBD_code();
// Add or subtract such that the result is larger
if (b < 0) sol = (-b + sqrt(discriminate)) / (2.0 * a);
else sol = (-b - sqrt(discriminate)) / (2.0 * a);
// We know a*sol*sol2 == c
sol2 = sol ? 0.0 : c/a/sol;

Whats wrong with my code for finding the roots of the quadratic equation?

If the quadratic equation is written as
ax^2 + bx + c=0
And my code is written as this....
I used temp to get the value of the equation under the square roots.
root1 = [-b + [(b^2 - (4*a*c)]^(1/2) ] / 2*a
root2 = [-b - [(b^2 - (4*a*c)]^(1/2) ] / 2*a
//Program for finding the roots of Quadratic equations
#include<stdio.h>
#include<math.h>
main()
{
float a,b,c,temp,root1,root2 ; //aloting the values
printf(“Finding the roots of variable \n Enter the values of a,b,c ”) ;
scanf(“%f,%f,%f”,&a,&b,&c ) ; //Getting the values of a,b and c
temp=b*b –(4*a*c) ; //used in the equation
if (a!=0 && temp>=0) //using the condition
{
root1 = (-b + sqrt(temp))/a*2 ;
root2 = (-b - sqrt(temp))/a*2 ;
printf(“Roots of the equation are %f and %f”, &root1,&root2) ;
}
else
printf(“There is no real value of roots of the given equation ”) ;
}
Add (..), should be
root1 = (-b + sqrt(temp)) / (a * 2);
root2 = (-b - sqrt(temp)) / (a * 2);
Technically, if a == 0 real (not complex) roots are quite possible, e.g. 0 * x**2 + 3 * x = 6. So
if (a != 0) {
if (temp >= 0) {
...
}
else /* quadratic equation with complex roots only */
printf(“There is no real value of roots of the given equation”);
}
else if (b != 0) { /* 0 * x**2 + b * x + c = 0 */
/* the only root */
float root = -c / b;
printf(“Root of the equation is %f”, &root);
}
else if (c != 0) /* 0 * x**2 + 0 * x + c = 0 */
printf(“No roots”);
else /* 0 * x**2 + 0 * x + 0 = 0 */
printf(“Arbitrary number is a root”);
Or, if you don't want to solve for all possible cases
if (a != 0) {
if (temp >= 0) {
...
}
else /* quadratic equation with complex roots only */
printf(“There is no real value of roots of the given equation”);
}
else
printf(“The equation is not quadratic one”);
Two obvious problems.
First
root1 = (-b + sqrt(temp))/a*2;
is equivalent, due to rules of operator precedence, to (note where I've added parentheses).
root1 = ((-b + sqrt(temp))/a)*2;
which is wrong. It should be something like
root1 = (-b + sqrt(temp))/(a*2) ;
A similar comment applies for the calculation of root2.
Second, the statement
printf(“Roots of the equation are %f and %f”, &root1,&root2);
attempts to print the addresses of root1 and root2 (not the values of root1 and root2). That actually causes undefined behaviour. You need to remove the &s (which are required to use scanf() correctly, but not printf()).
There are some other flaws in your code (bad technique) but I won't deal with those.

False position method reached the MAX iterations limit

I am trying to write a program to find roots of a continuous function using False Position Method. But, I am getting the same value of c again and again and same value is assigned to a, resulting in reaching the limit of MAX_ITER. How should I avoid this ? Am I not using the algorithm correctly ?
#include<stdio.h>
#include<math.h>
#define F(x) ((2*x)+1)
#define ERROR 0.00001
#define MAX_ITER 1000
float FalsePosition(float a, float b)
{
float c;
int iter = 0;
do
{
c = (b - F(b)) * (b - a) / (F(b) - F(a));
printf("F(a): %f, F(b) : %f, F(c) : %f, a: %f, b : %f, c : %f\n", F(a), F(b), F(c), a, b, c);
if((F(c) > 0 && F(a) > 0) || (F(c) < 0 && F(a) < 0))
{
a = c;
}
else
{
b=c;
}
iter++;
}
while(fabsf(a-b) > ERROR && iter < MAX_ITER);
return a;
}
int main()
{
float a = -2.5;
float b = 2.5;
printf("Finding root in the interval [%f, %f]\n", a, b);
if((F(a)>0 && F(b)>0) || (F(a)<0 && F(b)<0))
{
printf("No root lie in the interval [%f, %f]", a, b);
}
else
{
printf("The root is : %f\n", FalsePosition(a, b));
}
return 0;
}
Your formula for c is wrong, it should be
c = b - (f(b) * (b - a)) / (f(b) - f(a));
see here
To prevent reaching MAX_ITER iterations, you may want to watch the change in c something like
previousValue = c;
c = b - (f(b) * (b - a)) / (f(b) - f(a));
and then, the while condition would be
while ((fabs(previousValue - c) > ERROR) && (iter < MAX_ITER));
you can initialize previousValue to a befor beginig the loop.
And finally you should return c not a.

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