box2d_rust/math_functions/
scalar.rs

1// Scalar helpers: int/float min/max/abs/clamp, deterministic atan2 and cos/sin.
2// Part of the math_functions module.
3
4use super::*;
5
6/// @return the minimum of two integers
7pub fn min_int(a: i32, b: i32) -> i32 {
8    if a < b {
9        a
10    } else {
11        b
12    }
13}
14
15/// @return the maximum of two integers
16pub fn max_int(a: i32, b: i32) -> i32 {
17    if a > b {
18        a
19    } else {
20        b
21    }
22}
23
24/// @return the absolute value of an integer
25pub fn abs_int(a: i32) -> i32 {
26    if a < 0 {
27        -a
28    } else {
29        a
30    }
31}
32
33/// @return an integer clamped between a lower and upper bound
34pub fn clamp_int(a: i32, lower: i32, upper: i32) -> i32 {
35    if a < lower {
36        lower
37    } else if a > upper {
38        upper
39    } else {
40        a
41    }
42}
43
44/// <https://en.wikipedia.org/wiki/Floor_and_ceiling_functions>
45pub fn ceiling_int(numerator: i32, denominator: i32) -> i32 {
46    debug_assert!(denominator > 0 && numerator >= 0);
47    (numerator + denominator - 1) / denominator
48}
49
50/// @return the minimum of two floats
51/// Matches the C ternary exactly, including NaN propagation (`a < b` is false for NaN).
52pub fn min_float(a: f32, b: f32) -> f32 {
53    if a < b {
54        a
55    } else {
56        b
57    }
58}
59
60/// @return the maximum of two floats
61pub fn max_float(a: f32, b: f32) -> f32 {
62    if a > b {
63        a
64    } else {
65        b
66    }
67}
68
69/// @return the absolute value of a float
70pub fn abs_float(a: f32) -> f32 {
71    if a < 0.0 {
72        -a
73    } else {
74        a
75    }
76}
77
78/// @return a float clamped between a lower and upper bound
79pub fn clamp_float(a: f32, lower: f32, upper: f32) -> f32 {
80    if a < lower {
81        lower
82    } else if a > upper {
83        upper
84    } else {
85        a
86    }
87}
88
89/// Compute an approximate arctangent in the range [-pi, pi]
90/// This is hand coded for cross-platform determinism. The atan2f
91/// function in the standard library is not cross-platform deterministic.
92/// Accurate to around 0.0023 degrees
93// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
94pub fn atan2(y: f32, x: f32) -> f32 {
95    // Added check for (0,0) to match atan2f and avoid NaN
96    if x == 0.0 && y == 0.0 {
97        return 0.0;
98    }
99
100    let ax = abs_float(x);
101    let ay = abs_float(y);
102    let mx = max_float(ay, ax);
103    let mn = min_float(ay, ax);
104    let a = mn / mx;
105
106    // Minimax polynomial approximation to atan(a) on [0,1]
107    let s = a * a;
108    let c = s * a;
109    let q = s * s;
110    let mut r = 0.024840285 * q + 0.18681418;
111    let t = -0.094097948 * q - 0.33213072;
112    r = r * s + t;
113    r = r * c + a;
114
115    // Map to full circle
116    if ay > ax {
117        r = 1.57079637 - r;
118    }
119
120    if x < 0.0 {
121        r = 3.14159274 - r;
122    }
123
124    if y < 0.0 {
125        r = -r;
126    }
127
128    r
129}
130
131/// Compute the cosine and sine of an angle in radians. Implemented
132/// for cross-platform determinism.
133// Approximate cosine and sine for determinism. In my testing cosf and sinf produced
134// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust
135// this result.
136// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula
137pub fn compute_cos_sin(radians: f32) -> CosSin {
138    let x = unwind_angle(radians);
139    let pi2 = PI * PI;
140
141    // cosine needs angle in [-pi/2, pi/2]
142    let c: f32 = if x < -0.5 * PI {
143        let y = x + PI;
144        let y2 = y * y;
145        -(pi2 - 4.0 * y2) / (pi2 + y2)
146    } else if x > 0.5 * PI {
147        let y = x - PI;
148        let y2 = y * y;
149        -(pi2 - 4.0 * y2) / (pi2 + y2)
150    } else {
151        let y2 = x * x;
152        (pi2 - 4.0 * y2) / (pi2 + y2)
153    };
154
155    // sine needs angle in [0, pi]
156    let s: f32 = if x < 0.0 {
157        let y = x + PI;
158        -16.0 * y * (PI - y) / (5.0 * pi2 - 4.0 * y * (PI - y))
159    } else {
160        16.0 * x * (PI - x) / (5.0 * pi2 - 4.0 * x * (PI - x))
161    };
162
163    let mag = (s * s + c * c).sqrt();
164    let inv_mag = if mag > 0.0 { 1.0 / mag } else { 0.0 };
165    CosSin {
166        cosine: c * inv_mag,
167        sine: s * inv_mag,
168    }
169}
box2d_rust/math_functions/scalar.rs

box2d_rust/math_functions/
scalar.rs
