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;
}
I've built a good function for rotating bitmaps in degrees.
The problem that occurred after some optimizations is that the output image is getting slightly re-positioned during the rotation.
I've recorded a 360o rotating video for you to see what do I mean.
And the code as follows:
#define OFFSET_OF_ID (0x0)
#define OFFSET_OF_SIZE (0x2)
#define OFFSET_OF_PIXELS (0xA)
#define OFFSET_OF_NDIB (0xE)
#define OFFSET_OF_WIDTH (0x12)
#define OFFSET_OF_HEIGHT (0x16)
#define OFFSET_OF_BPP (0x1C)
#define OFFSET_OF_NRAW (0x22)
typedef unsigned char byte, pixel[3];
typedef unsigned short word;
typedef unsigned long dword;
typedef unsigned long long ddword;
byte*
bmp_rotate
(byte *buffer, float angle)
{
const dword src_width = *( (dword*)&buffer[OFFSET_OF_WIDTH]);
const dword src_height = *( (dword*)&buffer[OFFSET_OF_HEIGHT]);
const dword src_nraw = *( (dword*)&buffer[OFFSET_OF_NRAW]);
const dword src_pixels = *( (dword*)&buffer[OFFSET_OF_PIXELS]);
const dword src_bpp = *( (dword*)&buffer[OFFSET_OF_BPP]);
const dword single = src_bpp / 8;
const dword row = src_width * single;
dword rowsize = (row % 4) ? (row + 4 - row % 4) : (row);
byte *dest = calloc( src_pixels + src_nraw, sizeof(byte) );
double midX, midY;
int i, j;
double sin_angle = sin(angle);
double cos_angle = cos(angle);
midX = src_width / 2.0f;
midY = src_height / 2.0f;
memcpy(dest, buffer, src_pixels);
for(j = 0; j < src_height; j++)
{
dword dest_offset = src_pixels + j * rowsize;
double deltaY = j - midY;
double deltaX = 0 - midX;
double x_computation, y_computation;
x_computation = midX + deltaX * cos_angle + deltaY * sin_angle + 0.5f;
y_computation = midY - deltaX * sin_angle + deltaY * cos_angle + 0.5f;
for(i = 0; i < src_width; i++)
{
ddword rotX = x_computation;
ddword rotY = y_computation;
if(rotX >= 0 && rotX < src_width && rotY >= 0 && rotY < src_height)
{
ddword src_offset = src_pixels + rotY * rowsize + rotX * single;
memcpy(&dest[dest_offset], &buffer[src_offset], sizeof(pixel));
}
x_computation += cos_angle;
y_computation -= sin_angle;
dest_offset += single;
}
}
return dest;
}
What causes this indecent behavior?
I can't really see your problem in the video, but I assume that the full 360° rotation is achieved in various partial rotations.
If we look at a 180° rotation, the offset becomes clear: The sine is 0 and the cosine is −1. The rotated coordinates of the top left corner (0,0) is then:
x' = xm + (x - xm) * cosa + (y - ym) * sina = xm + xm = w
y' = ym - (x - xm) * sina + (y - ym) * cosa = ym + ym = h
The coordinates (w, h) are the exclusive right and bottom borders.
Your working coordinates are real numbers. The integer values describe the left and top coordinates. Converting the (positive) real coords to integers will truncate the fractional part and yield the zero-based pixel indices.
You add 0.5 to your working coordinates in the target space once to enforce proper index calculation. Instead, you should treat the real coordinates of all pixels as the middle of that pixel:
x(i) = i + 0.5
y(j) = j + 0.5
All your calculations remain correct, except:
double deltaY = j + 0.5 - midY;
double deltaX = 0 + 0.5 - midX;
x_computation = midX + deltaX * cos_angle + deltaY * sin_angle;
y_computation = midY - deltaX * sin_angle + deltaY * cos_angle;
The idea is to treat pixel coordinates as pixel centres in both source and target space. The correct representation in target space is achieved automatically by rounding towards zero when convering to integer.
It may be a rounding-related problem. I have also faced this kind of problem in my projects in the past. Please check the lines where "integer to other types" or "other types to integer" conversions occur.
I am converting a YUV422 frame to a PPM file - (RGB888).
The image is converting mostly. Colors are fine. But each seems row is askew, as if extra
Here is the frame conversion function:
BYTE* CYUV::toRGB()
{
BYTE* rgb = new BYTE[m_width*m_height*3];
BYTE* rgb_ret = rgb;
BYTE* p_yuv = m_yuv_frame;
for (int i=m_width*m_height/2; i--;)
{
BYTE y, u, v, y2;
BYTE r,g, b;
y2 = *p_yuv++;
v = *p_yuv++;
y = *p_yuv++;
u = *p_yuv++;
r = (y + 1.370705 * (v - 128));
g = (y - (0.698001 * (v - 128)) - (0.337633 * (u - 128)));
b = (y + (1.732446 * (u - 128)));
*rgb++ = b < 0 ? 0 : b > 255 ? 255 : b;
*rgb++ = g < 0 ? 0 : g > 255 ? 255 : g;
*rgb++ = r < 0 ? 0 : r > 255 ? 255 : r;
r = (y2 + 1.370705 * (v - 128));
g = (y2 - (0.698001 * (v - 128)) - (0.337633 * (u - 128)));
b = (y2 + (1.732446 * (u - 128)));
*rgb++ = b < 0 ? 0 : b > 255 ? 255 : b;
*rgb++ = g < 0 ? 0 : g > 255 ? 255 : g;
*rgb++ = r < 0 ? 0 : r > 255 ? 255 : r;
}
return rgb_ret;
}
I believe the actual yuv-to-rgb pixel conversion is correct, because I had tried many other formulae, with color distorted results.
As for the PPM file, that is good also, since all image readers handle it.
As for the original YUV4222 frame, it to is fine - I display it using SDL without this distortion.
Any suggestions?
TIA
ken
OK: Solved this.
It was simple enough: write out the y converted pixel BEFORE the y2.
There were SO many solutions out there, but few seem completely correct.
FYI: This DOES work, but only for a yuv 4:2:2 format. This seems to be the most common one with the fourcc label of YUYV. yuv 4:2:0 is much more complex to convert. yuv 4:4:4 should be the same as this solution fetching only 1 y component, and writing out only one rgb triplet. (can't verify that, but I that was the comment along with this solution.
Hope this helps someone.
ken
I need a function to animate a sine wave infinitely over time. The sine wave moves to the left.
My sine wave is built using the following equation:
A * sin(B * x + C) + D
Now to animate the sine wave as if it is moving to the left, I simply increase C by 1 everytime I refresh the screen. Now this is all fine and dandy for a few minutes but I need to have that animation run for hours. I can't just have an integer build up 60 times a second forever. How does someone deal with this? Do I just try to find a point where the sine wave crosses 0 and then restart the animation from 0?
I just need to have the logic of something like this explained.
EDIT #1
I forgot to mention that there's a randomized component to my sine. The sine is not continuously the same. A and D are sinusoidal functions tied to that integer at the moment. The sine needs to look random with varying periods and amplitudes.
EDIT #2
Edited see Edit 3
EDIT #3
#Potatoswatter I tried implementing your technique but I don't think I'm getting it. Here's what I got:
static double i = 0;
i = i + (MPI / 2);
if ( i >= 800 * (MPI / 2) ) i -= 800 * (MPI / 2);
for (k = 0; k < 800; ++k)
{
double A1 = 145 * sin((rand1 * (k - 400) + i) / 300) + rand3; // Amplitude
double A2 = 100 * sin((rand2 * (k - 400) + i) / 300) + rand2; // Amplitude
double A3 = 168 * sin((rand3 * (k - 400) + i) / 300) + rand1; // Amplitude
double B1 = 3 + rand1 + (sin((rand3 * k) * i) / (500 * rand1)); // Period
double B2 = 3 + rand2 + (sin((rand2 * k) * i) / 500); // Period
double B3 = 3 + rand3 + (sin((rand1 * k) * i) / (500 * rand3)); // Period
double x = k; // Current x
double C1 = 10 * i; // X axis move
double C2 = 11 * i; // X axis move
double C3 = 12 * i; // X axis move
double D1 = rand1 + sin(rand1 * x / 600) * 4; // Y axis move
double D2 = rand2 + sin(rand2 * x / 500) * 4; // Y axis move
double D3 = rand3 + cos(rand3 * x / 400) * 4; // Y axis move
sine1[k] = (double)A1 * sin((B1 * x + C1) / 400) + D1;
sine2[k] = (double)A2 * sin((B2 * x + C2) / 300) + D2 + 100;
sine3[k] = (double)A3 * cos((B3 * x + C3) / 500) + D3 + 50;
}
How do I modify this to make it work?
Halp!
Sine has a period of 2 pi, meaning that sin(x) = sin(x + 2 * M_PI), for any x.
So, you could just increase C by, say, pi/n where n is any integer, as you refresh the screen, and after 2n refreshes, reset C (to 0, or whatever).
Edit for clarity: the integer n is not meant to change over time.
Instead, pick some n, for example, let's say n = 10. Now, every frame, increase x by pi / 10. After 20 frames, you have increased x by a total of 20 * pi / 10 = 2 * pi. Since sin(x + 2 * pi) = sin(x), you may as well just reset your sin(...) input to just x, and start the process over.
sin is periodic, with a period of 2π. Therefore, if the argument is greater than 2π, you can subtract 2 * M_PI from it and get the same answer.
Instead of using a single variable k to compute all waves of various speeds, use three variables double k1, k2, k3, and keep them bound in the range from 0 to 2π.
if ( k2 >= 2 * M_PI ) k2 -= 2 * M_PI;
They may be individually updated by adding some value each frame. If the increment may be more than 2π then subtracting a single 2π won't bring them back into range, but you can use fmod() instead.
I decided to change my course of action. I just drive i with the system's monotonic clock like so:
struct timespec spec;
int ms;
time_t s;
static unsigned long long etime = 0;
clock_gettime(CLOCK_MONOTONIC, &spec);
s = spec.tv_sec;
ms = spec.tv_nsec / 10000000;
etime = concatenate((long)s, ms);
Then I simply changed i to etime in my sine equations. Here's the concatenating function I used for this purpose:
unsigned concatenate(unsigned x, unsigned y) {
x = x * 100;
return x + y;
}
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