File content as of revision 13:bb9669053eb3:

#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);
    }
    
    unsigned int update_bit(const unsigned int previous_4_bytes, const int position, const bool new_bit_value)
    {
        return update_bits(previous_4_bytes, position, position, new_bit_value);
    }
    
    unsigned int update_bits(const unsigned int previous_4_bytes, const int start_bit, const int stop_bit, const unsigned int new_bits)
    {
        const int all_ones_but_n_zeros_right_shifted = (0xFFFFFFFF << (stop_bit - start_bit + 1));
        const int all_zeros_but_n_ones_right_shifted = ~all_ones_but_n_zeros_right_shifted;
        
        const int all_zeros_but_ones_at_position = all_zeros_but_n_ones_right_shifted << start_bit;
        const int all_ones_but_zeros_at_position = ~all_zeros_but_ones_at_position;
        
        const int all_zeros_but_new_bits_at_position = new_bits << start_bit;
        const int all_unchanged_but_zeros_at_position = previous_4_bytes & all_ones_but_zeros_at_position;
        const int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zeros_at_position | all_zeros_but_new_bits_at_position;
        return all_unchanged_but_new_bit_value_at_position;
    }
}