If you are working in robotics, there will be moment where you need to scale angles into range of [-π, π] since many control laws require this range as input constraint. Here is a trick to normalize given angles in radians to [-π, π] range. The trick is to usage of built-in atan2 function.


#include <math.h> // cos, sin
#include <cmath.h> // atan2
def normalize(angle)
	// Input angle is in radians
	return atan2(sin(angle),cos(angle))


import numpy as np # if using numpy version
import math # if using math version
def normalize(angle):
	# Numpy version is faster
	return np.arctan2(np.sin(angle),np.cos(angle))
	# If you do not want to deal with numpy, here is math version
	return math.arctan2(np.sin(angle),np.cos(angle))