File content as of revision 12:1c341b119b23:

#include "Utility.hpp"

#include <cmath>

namespace utility
{
    bool is_almost_equal(double a, double b, double tolerance)
    {
        double difference = std::abs(a-b);
        return (difference <= tolerance);
    }
    
    //Return angle between 0 and 360 degree
    double wrap_angle(double angle)
    {
        return angle - 360 * std::floor( angle / 360 );
    } 
    
    double degree_from_radian(const double angle_radian)
    {
        const double angle_degree = angle_radian * 180.0 / PI;
        return wrap_angle(angle_degree);
    }
    
    int update_bit(const int previous_4_bytes, const int position, const bool new_bit_value)
    {
        const int all_zero_but_one_at_position = 0x1 << position;
        const int all_one_but_zero_at_position = ~all_zero_but_one_at_position;
        const int all_zero_but_new_bit_value_at_position = new_bit_value << position;
        const int all_unchanged_but_zero_at_position = previous_4_bytes & all_one_but_zero_at_position;
        const int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zero_at_position | all_zero_but_new_bit_value_at_position;
        return all_unchanged_but_new_bit_value_at_position;
    }
}