#![allow(clippy::cast_possible_truncation)]
#![allow(clippy::cast_precision_loss)]
use std::f32::consts;
use crate::base::floating_point::f32_saturate2int;
pub type Scalar = f32;
pub const SCALAR_1: Scalar = 1.0;
pub const SCALAR_HALF: Scalar = 0.5;
pub const SCALAR_SQRT_2: Scalar = consts::SQRT_2;
pub const SCALAR_PI: Scalar = consts::PI;
#[allow(clippy::excessive_precision)]
pub const SCALAR_TAN_PI_OVER_8: Scalar = 0.414_213_562;
pub const SCALAR_MAX: Scalar = f32::MAX;
pub const SCALAR_MIN: Scalar = f32::MIN;
pub const SCALAR_INFINITY: Scalar = f32::INFINITY;
pub const SCALAR_NEGATIVE_INFINITY: Scalar = f32::NEG_INFINITY;
pub const SCALAR_NAN: Scalar = f32::NAN;
pub const SCALAR_NEARLY_ZERO: Scalar = 1.0 / (1 << 12) as Scalar;
pub const SCALAR_SIN_COS_NEARLY_ZERO: Scalar = 1.0 / (1 << 16) as Scalar;
pub const SCALAR_ROOT_2_OVER_2: f32 = consts::FRAC_1_SQRT_2;
pub trait ScalarExt {
#[must_use]
fn invert(self) -> Self;
#[must_use]
fn average(self, b: Self) -> Self;
#[must_use]
fn half(self) -> Self;
#[must_use]
fn square(self) -> Self;
#[must_use]
fn ceil_to_int(self) -> i32;
#[must_use]
fn floor_to_int(self) -> i32;
#[must_use]
fn round_to_int(self) -> i32;
#[must_use]
fn trunc_to_int(self) -> i32;
#[must_use]
fn is_int(&self) -> bool;
#[must_use]
fn sign_as_int(self) -> i32;
#[must_use]
fn sign_as_scalar(self) -> Self;
#[must_use]
fn nearly_zero(self) -> bool;
#[must_use]
fn nearly_zero_tolerance(self, tolerance: Scalar) -> bool;
#[must_use]
fn fuzzy_equal(self, other: Self) -> bool;
#[must_use]
fn nearly_equal(self, other: Self) -> bool;
#[must_use]
fn nearly_equal_tolerance(self, other: Self, tolerance: Scalar) -> bool;
#[must_use]
fn sin_snap_to_zero(self) -> Self;
#[must_use]
fn cos_snap_to_zero(self) -> Self;
#[must_use]
fn interp(self, other: Self, t: Self) -> Self;
#[must_use]
fn mid_point(self, other: Self) -> Self;
#[must_use]
fn tpin(self, low: Self, hi: Self) -> Self;
}
impl ScalarExt for Scalar {
fn invert(self) -> Self {
debug_assert!(self != 0.0);
1.0 / self
}
fn average(self, b: Self) -> Self {
(self + b) / 2.0
}
fn half(self) -> Self {
self / 2.0
}
fn square(self) -> Self {
self * self
}
fn ceil_to_int(self) -> i32 {
self.ceil() as i32
}
fn floor_to_int(self) -> i32 {
self.floor() as i32
}
fn round_to_int(self) -> i32 {
self.round() as i32
}
fn trunc_to_int(self) -> i32 {
f32_saturate2int(self)
}
fn is_int(&self) -> bool {
self.fuzzy_equal(self.floor())
}
fn sign_as_int(self) -> i32 {
if self == 0.0 {
0
} else if self < 0.0 {
-1
} else {
1
}
}
fn sign_as_scalar(self) -> Self {
if self == 0.0 {
0.0
} else if self < 0.0 {
-1.0
} else {
1.0
}
}
fn nearly_zero(self) -> bool {
self.abs() <= SCALAR_NEARLY_ZERO
}
fn nearly_zero_tolerance(self, tolerance: Scalar) -> bool {
debug_assert!(tolerance >= 0.0);
self.abs() <= tolerance
}
fn fuzzy_equal(self, other: Self) -> bool {
(self - other).abs() <= Self::EPSILON
}
fn nearly_equal(self, other: Self) -> bool {
(self - other).abs() <= SCALAR_NEARLY_ZERO
}
fn nearly_equal_tolerance(self, other: Self, tolerance: Scalar) -> bool {
debug_assert!(tolerance >= 0.0);
(self - other).abs() <= tolerance
}
fn sin_snap_to_zero(self) -> Self {
let v = self.sin();
if v.nearly_zero_tolerance(SCALAR_SIN_COS_NEARLY_ZERO) {
0.0
} else {
v
}
}
fn cos_snap_to_zero(self) -> Self {
let v = self.cos();
if v.nearly_zero_tolerance(SCALAR_SIN_COS_NEARLY_ZERO) {
0.0
} else {
v
}
}
fn interp(self, other: Self, t: Self) -> Self {
debug_assert!((0.0..=1.0).contains(&t));
(other - self).mul_add(t, self)
}
fn mid_point(self, other: Self) -> Self {
const LO: f32 = f32::MIN_POSITIVE * 2.0;
const HI: f32 = f32::MAX / 2.0;
let (a, b) = (self, other);
let abs_a = a.abs();
let abs_b = b.abs();
if abs_a <= HI && abs_b <= HI {
(a + b) / 2.0
} else if abs_a < LO {
a + (b / 2.0)
} else if abs_b < LO {
(a / 2.0) + b
} else {
(a / 2.0) + (b / 2.0)
}
}
#[must_use]
fn tpin(self, low: Self, hi: Self) -> Self {
low.max(self.min(hi))
}
}
#[must_use]
pub fn scalars_equal(a: &[Scalar], b: &[Scalar], n: usize) -> bool {
debug_assert!(a.len() >= n);
debug_assert!(b.len() >= n);
for i in 0..n {
if !a[i].fuzzy_equal(b[i]) {
return false;
}
}
true
}
#[must_use]
pub fn scalars_are_finite(array: &[Scalar]) -> bool {
array.iter().all(|scalar| scalar.is_finite())
}
#[must_use]
pub fn scalar_interp_func(
search_key: Scalar,
keys: &[Scalar],
values: &[Scalar],
length: usize,
) -> Scalar {
debug_assert!(length > 0);
debug_assert!(keys.len() >= length);
debug_assert!(values.len() >= length);
#[cfg(debug_assertions)]
for i in 1..length {
debug_assert!(keys[i - 1] <= keys[i]);
}
let mut right = 0;
while right < length && keys[right] < search_key {
right += 1;
}
if right == length {
return values[length - 1];
}
if right == 0 {
return values[0];
}
let left_key = keys[right - 1];
let right_key = keys[right];
debug_assert!(right_key > left_key);
let fract = (search_key - left_key) / (right_key - left_key);
values[right - 1].interp(values[right], fract)
}