(C++) Rotate one angle to another by the shortest turn

[Warchief]

This code should help anyone to follow one angle to another once you know both angles. The important code is the "DOIT" method; the others are posted as an example, but should be changed to fit your application behaviour.

/-----------------------------------/
/ Start core code
/-----------------------------------/
Snippet should be like this:

Code: Select all

// Try angles
void DOIT( float from, float to )
{	
	// Normalize
	NormalizeAngle( from );
	NormalizeAngle( to );

	// Equal, do nothing
	if( from == to ) return;

	// max and min
	float max = MAX(from, to);
	float min = MIN(from, to);
   
	// Differences
	float difN = max - min;
	float difI = (360 - max) + min;
   
	// If normal is lower than inverse
	if( difN <= difI )
		if( from > to )			
			RotateLeft( from, to );
		else
			RotateRight( from, to );
	else
		if( from > to )
			RotateRight( from, to );
		else
			RotateLeft ( from, to );
}

/-----------------------------------/
/ End core code
/-----------------------------------/

Where normalize could be like:

Code: Select all

// Normalize to interval 0 - 360
void NormalizeAngle( float& ang )
{
	while( ang >= 360 ) ang -= 360;
	while( ang < 0 ) ang += 360;
}

And rotations may be:

Code: Select all

// Rotates angle to the right (adding)
void RotateRight( float from, float to )
{
	// Vars
	float oFrom = from;
	int stepsDone = 0;

	// Step vars
	float step = 1.0f;
	bool stop  = fabsf(from - to) < step;

	// GO!
	while( !stop ) {
		// Add step
		from += step;
		// Normalize or maybe we wont get there
		NormalizeAngle( from );
		// Step done
		++stepsDone;
		// Continue?
		stop  = fabsf(from - to) < step;		
	}
}

// Rotates angle to the left (substracting)
void RotateLeft(float from, float to)
{
	// Vars
	float oFrom = from;
	int stepsDone = 0;

	// Step vars
	float step = 1.0f;
	bool stop  = fabsf(from - to) < step;

	// GO!
	while( !stop ) {
		// Substract step
		from -= step;
		// Normalize or maybe we wont get there
		NormalizeAngle( from );
		// Step done
		++stepsDone;
		// Continue?
		stop  = fabsf(from - to) < step;		
	}
}

[Warchief]

/-----------------------------------/
/ SEE IT IN ACTION!!
/-----------------------------------/
This code tries 10 random pairs of angles to rotate from one to the other. If you press 'return', 10 more pairs are tested. Exit with Ctrl-C. In this example, steps are done by 1 degree, so you may expect not to detect a turn of more than 180 steps (=degrees).
It will show turns from 0 degrees to [175, 185] interval.

Copy the code to a File.cpp and run.

Code: Select all

/**********************************************************
 *
 * This code is totally free. Use at your own risk.
 * @originalAuthor Warchief
 *
 **********************************************************/

// Common includes
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

// MAX macro
#ifndef MAX
#define MAX(a,b) a > b ? a : b
#endif

// MIN macro
#ifndef MIN
#define MIN(a,b) a < b ? a : b
#endif

// Forward declaration
void NormalizeAngle( float& ang );
void Try(float f, float t);
void RotateRight(float f, float t);
void RotateLeft(float f, float t);

// Main
int _tmain(int argc, _TCHAR* argv[]);

// ----
// MAIN
// ----
int _tmain(int argc, _TCHAR* argv[])
{
	// Random seed
	srand( (unsigned int) time(NULL) );

	for(int d=-5; d<=5; ++d) {
		Try( (float)0, (float)(180 + d) );		
	}
	
	// Always
	while( true ) {
		getchar();

		// 10 random turns
		for(int i=0; i<10; ++i) {
			// Random
			int rand1 = -500 + (int)(1000*rand()/(RAND_MAX+1.0f));
			int rand2 = -500 + (int)(1000*rand()/(RAND_MAX+1.0f));
			Try( (float)rand1, (float)rand2 );
		}
	}
	return 0;
}

// Normalize to interval 0 - 360
void NormalizeAngle( float& ang )
{
	while( ang >= 360 ) ang -= 360;
	while( ang < 0 ) ang += 360;
}

// Try angles
void Try( float from, float to )
{	
	printf(" [%.0f to %.0f] ", from, to);
	
	// Normalize
	NormalizeAngle( from );
	NormalizeAngle( to );

	// Equal, do nothing
	if( from == to ) return;

	// max and min
	float max = MAX(from, to);
	float min = MIN(from, to);
   
	// Differences
	float difN = max - min;
	float difI = (360 - max) + min;
   
	// If normal is lower than inverse
	if( difN <= difI )
		if( from > to )			
			RotateLeft( from, to );
		else
			RotateRight( from, to );
	else
		if( from > to )
			RotateRight( from, to );
		else
			RotateLeft ( from, to );
}

// Rotates angle to the right (adding)
void RotateRight( float from, float to )
{
	// Vars
	float oFrom = from;
	int stepsDone = 0;

	// Step vars
	float step = 1.0f;
	bool stop  = fabsf(from - to) < step;

	// GO!
	while( !stop ) {
		// Add step
		from += step;
		// Normalize or maybe we wont get there
		NormalizeAngle( from );
		// Step done
		++stepsDone;
		// Continue?
		stop  = fabsf(from - to) < step;		
	}

	printf("%.0f to %.0f Right in %d steps\n", oFrom, to, stepsDone);	
}

// Rotates angle to the left (substracting)
void RotateLeft(float from, float to)
{
	// Vars
	float oFrom = from;
	int stepsDone = 0;

	// Step vars
	float step = 1.0f;
	bool stop  = fabsf(from - to) < step;

	// GO!
	while( !stop ) {
		// Substract step
		from -= step;
		// Normalize or maybe we wont get there
		NormalizeAngle( from );
		// Step done
		++stepsDone;
		// Continue?
		stop  = fabsf(from - to) < step;		
	}

	printf("%.0f to %.0f Left in %d steps\n", oFrom, to, stepsDone);	
}