box2d_rust/math_functions/
rotation.rs

1// Rotation (b2Rot) operations, angle helpers, and vector rotation.
2// Part of the math_functions module.
3
4use super::*;
5
6/// Make a rotation using an angle in radians
7pub fn make_rot(radians: f32) -> Rot {
8    let cs = compute_cos_sin(radians);
9    Rot {
10        c: cs.cosine,
11        s: cs.sine,
12    }
13}
14
15/// Make a rotation using a unit vector
16pub fn make_rot_from_unit_vector(unit_vector: Vec2) -> Rot {
17    debug_assert!(is_normalized(unit_vector));
18    Rot {
19        c: unit_vector.x,
20        s: unit_vector.y,
21    }
22}
23
24/// Is this rotation normalized?
25pub fn is_normalized_rot(q: Rot) -> bool {
26    // larger tolerance due to failure on mingw 32-bit
27    let qq = q.s * q.s + q.c * q.c;
28    1.0 - 0.0006 < qq && qq < 1.0 + 0.0006
29}
30
31/// Get the inverse of a rotation
32pub fn invert_rot(a: Rot) -> Rot {
33    Rot { c: a.c, s: -a.s }
34}
35
36/// Normalized linear interpolation
37/// <https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/>
38/// <https://web.archive.org/web/20170825184056/http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/>
39pub fn nlerp(q1: Rot, q2: Rot, t: f32) -> Rot {
40    let omt = 1.0 - t;
41    let q = Rot {
42        c: omt * q1.c + t * q2.c,
43        s: omt * q1.s + t * q2.s,
44    };
45
46    let mag = (q.s * q.s + q.c * q.c).sqrt();
47    let inv_mag = if mag > 0.0 { 1.0 / mag } else { 0.0 };
48    Rot {
49        c: q.c * inv_mag,
50        s: q.s * inv_mag,
51    }
52}
53
54/// Compute the angular velocity necessary to rotate between two rotations over a given time
55/// * `q1` - initial rotation
56/// * `q2` - final rotation
57/// * `inv_h` - inverse time step
58pub fn compute_angular_velocity(q1: Rot, q2: Rot, inv_h: f32) -> f32 {
59    // ds/dt = omega * cos(t)
60    // dc/dt = -omega * sin(t)
61    // s2 = s1 + omega * h * c1
62    // c2 = c1 - omega * h * s1
63
64    // omega * h * s1 = c1 - c2
65    // omega * h * c1 = s2 - s1
66    // omega * h = (c1 - c2) * s1 + (s2 - s1) * c1;
67    // omega * h = s1 * c1 - c2 * s1 + s2 * c1 - s1 * c1
68    // omega * h = s2 * c1 - c2 * s1 = sin(a2 - a1) ~= a2 - a1 for small delta
69    inv_h * (q2.s * q1.c - q2.c * q1.s)
70}
71
72/// Get the angle in radians in the range [-pi, pi]
73pub fn rot_get_angle(q: Rot) -> f32 {
74    atan2(q.s, q.c)
75}
76
77/// Get the x-axis
78pub fn rot_get_x_axis(q: Rot) -> Vec2 {
79    Vec2 { x: q.c, y: q.s }
80}
81
82/// Get the y-axis
83pub fn rot_get_y_axis(q: Rot) -> Vec2 {
84    Vec2 { x: -q.s, y: q.c }
85}
86
87/// Multiply two rotations: q * r
88pub fn mul_rot(q: Rot, r: Rot) -> Rot {
89    // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
90    // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
91    // s(q + r) = qs * rc + qc * rs
92    // c(q + r) = qc * rc - qs * rs
93    Rot {
94        s: q.s * r.c + q.c * r.s,
95        c: q.c * r.c - q.s * r.s,
96    }
97}
98
99/// Transpose multiply two rotations: inv(a) * b
100/// This rotates a vector local in frame b into a vector local in frame a
101pub fn inv_mul_rot(a: Rot, b: Rot) -> Rot {
102    // [ ac as] * [bc -bs] = [ac*bc+qs*bs -ac*bs+as*bc]
103    // [-as ac]   [bs  bc]   [-as*bc+ac*bs as*bs+ac*bc]
104    // s(a - b) = ac * bs - as * bc
105    // c(a - b) = ac * bc + as * bs
106    Rot {
107        s: a.c * b.s - a.s * b.c,
108        c: a.c * b.c + a.s * b.s,
109    }
110}
111
112/// Relative angle between a and b
113pub fn relative_angle(a: Rot, b: Rot) -> f32 {
114    // sin(b - a) = bs * ac - bc * as
115    // cos(b - a) = bc * ac + bs * as
116    let s = a.c * b.s - a.s * b.c;
117    let c = a.c * b.c + a.s * b.s;
118    atan2(s, c)
119}
120
121/// Convert any angle into the range [-pi, pi]
122pub fn unwind_angle(radians: f32) -> f32 {
123    // Assuming this is deterministic
124    remainder_f32(radians, 2.0 * PI)
125}
126
127/// C remainderf: IEEE 754 remainder, result in [-|y|/2, |y|/2].
128/// Rust has no stable std equivalent (`f32::rem_euclid` differs), so implement it directly.
129fn remainder_f32(x: f32, y: f32) -> f32 {
130    if y == 0.0 || x.is_infinite() || x.is_nan() || y.is_nan() {
131        return f32::NAN;
132    }
133
134    // Round-half-to-even quotient, computed in f64 to avoid double-rounding error
135    // for the magnitudes used here (angle unwinding).
136    let q = (x as f64 / y as f64).round_ties_even();
137    (x as f64 - q * y as f64) as f32
138}
139
140/// Rotate a vector
141pub fn rotate_vector(q: Rot, v: Vec2) -> Vec2 {
142    Vec2 {
143        x: q.c * v.x - q.s * v.y,
144        y: q.s * v.x + q.c * v.y,
145    }
146}
147
148/// Inverse rotate a vector
149pub fn inv_rotate_vector(q: Rot, v: Vec2) -> Vec2 {
150    Vec2 {
151        x: q.c * v.x + q.s * v.y,
152        y: -q.s * v.x + q.c * v.y,
153    }
154}
box2d_rust/math_functions/rotation.rs

box2d_rust/math_functions/
rotation.rs
