pq_fixed.h

/*
 * pq_fixed.h
 *
 * Utility functions for fixed-point arithmetic.
 *
 * (c) 2022 Sofian Audry        :: info(@)sofianaudry(.)com
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#ifndef PQ_FIXED_H_
#define PQ_FIXED_H_
 
#include "pq_globals.h"
#include "pq_constrain.h"
 
#include <stdint.h>
 
namespace pq {
 
// Fixed-point types.
typedef uint32_t q0_32u_t;
typedef uint16_t q0_16u_t;
typedef uint8_t  q0_8u_t;
 
// Fixed-point constants.
constexpr q0_32u_t FIXED_32_MAX = static_cast<q0_32u_t>(0xFFFFFFFF);
constexpr q0_32u_t HALF_FIXED_32_MAX = static_cast<q0_32u_t>(0x80000000);
constexpr float    INV_FIXED_32_MAX = 1.0f / FIXED_32_MAX;
 
constexpr q0_16u_t FIXED_16_MAX = static_cast<q0_16u_t>(0xFFFF);
constexpr q0_16u_t HALF_FIXED_16_MAX = static_cast<q0_16u_t>(0x8000);
constexpr float    INV_FIXED_16_MAX = 1.0f / FIXED_16_MAX;
 
constexpr q0_8u_t FIXED_8_MAX = static_cast<q0_8u_t>(0xFF);
constexpr q0_8u_t HALF_FIXED_8_MAX = static_cast<q0_8u_t>(0x80);
constexpr float   INV_FIXED_8_MAX = 1.0f / FIXED_8_MAX;
 
template <typename I>
inline I floatToFixed(float value, I high) {
#if defined(PQ_IEEE_754_SUPPORTED)
  uint32_t ui;
  memcpy(&ui, &value, sizeof ui); // safe bit copy
 
  // Negative -> clamp to 0.0f
  if (ui & 0x80000000u) return static_cast<I>(0);
 
  // Closed upper bound: x > 1.0f -> 1.0f
  if (ui >= 0x3F800000u) return high;
 
  return static_cast<I>(value + value * high); // x * (toHigh + 1)
#else
  return (value <= 0.0f) ? 0 : (value >= 1.0f) ? high : static_cast<I>(value + value * high);
#endif
}
 
template <typename I>
inline float fixedToFloat(I value, I high) {
  return constrain01(value / static_cast<float>(high));
}
 
template <typename I>
inline float fixedToFloatInv(I value, float invHigh) {
  return constrain01(value * invHigh);
}
 
inline float fixed32ToFloat(q0_32u_t x) { return fixedToFloatInv(x, INV_FIXED_32_MAX); }
 
inline q0_32u_t floatToFixed32(float x) { return floatToFixed(x, FIXED_32_MAX); }
 
inline float fixed16ToFloat(q0_16u_t x) { return fixedToFloatInv(x, INV_FIXED_16_MAX); }
 
inline q0_16u_t floatToFixed16(float x) { return floatToFixed(x, FIXED_16_MAX); }
 
inline float fixed8ToFloat(q0_8u_t x) { return fixedToFloatInv(x, INV_FIXED_8_MAX); }
 
inline q0_8u_t floatToFixed8(float x) { return floatToFixed(x, FIXED_8_MAX); }
 
}
 
#endif