yog_graphics/
math.rs

1//! Vectors and 4×4 matrices (column-major, as JOML and the hosts'
2//! `PoseStack` keep them).
3
4use std::ops::{Add, Mul, Neg, Sub};
5
6/// A point or a direction in a plane.
7#[derive(Debug, Clone, Copy, PartialEq, Default)]
8pub struct Vec2 {
9    pub x: f32,
10    pub y: f32,
11}
12
13impl Vec2 {
14    pub const ZERO: Self = Self::new(0.0, 0.0);
15
16    pub const fn new(x: f32, y: f32) -> Self {
17        Self { x, y }
18    }
19
20    pub fn dot(self, o: Self) -> f32 {
21        self.x * o.x + self.y * o.y
22    }
23
24    pub fn length(self) -> f32 {
25        self.dot(self).sqrt()
26    }
27
28    pub fn distance(self, o: Self) -> f32 {
29        (self - o).length()
30    }
31
32    /// This one scaled to length 1 (zero stays zero).
33    pub fn normalize(self) -> Self {
34        let l = self.length();
35        if l < 1e-6 { self } else { self * (1.0 / l) }
36    }
37
38    pub fn lerp(self, to: Self, t: f32) -> Self {
39        self + (to - self) * t
40    }
41
42    /// Turned a quarter left (`y` up) — a line's normal.
43    pub fn perp(self) -> Self {
44        Self::new(-self.y, self.x)
45    }
46}
47
48impl Add for Vec2 {
49    type Output = Self;
50    fn add(self, o: Self) -> Self {
51        Self::new(self.x + o.x, self.y + o.y)
52    }
53}
54
55impl Sub for Vec2 {
56    type Output = Self;
57    fn sub(self, o: Self) -> Self {
58        Self::new(self.x - o.x, self.y - o.y)
59    }
60}
61
62impl Mul<f32> for Vec2 {
63    type Output = Self;
64    fn mul(self, k: f32) -> Self {
65        Self::new(self.x * k, self.y * k)
66    }
67}
68
69impl Neg for Vec2 {
70    type Output = Self;
71    fn neg(self) -> Self {
72        Self::new(-self.x, -self.y)
73    }
74}
75
76#[derive(yog_data::Schema, Debug, Clone, Copy, PartialEq, Default)]
77pub struct Vec3 {
78    pub x: f32,
79    pub y: f32,
80    pub z: f32,
81}
82
83impl Vec3 {
84    pub const ZERO: Self = Self::new(0.0, 0.0, 0.0);
85    pub const ONE: Self = Self::new(1.0, 1.0, 1.0);
86
87    pub const fn new(x: f32, y: f32, z: f32) -> Self {
88        Self { x, y, z }
89    }
90
91    pub fn from_array(a: [f32; 3]) -> Self {
92        Self::new(a[0], a[1], a[2])
93    }
94
95    pub fn to_array(self) -> [f32; 3] {
96        [self.x, self.y, self.z]
97    }
98
99    pub fn dot(self, o: Self) -> f32 {
100        self.x * o.x + self.y * o.y + self.z * o.z
101    }
102
103    pub fn cross(self, o: Self) -> Self {
104        Self::new(
105            self.y * o.z - self.z * o.y,
106            self.z * o.x - self.x * o.z,
107            self.x * o.y - self.y * o.x,
108        )
109    }
110
111    pub fn length(self) -> f32 {
112        self.dot(self).sqrt()
113    }
114
115    /// This one scaled to length 1 (zero stays zero).
116    pub fn normalize(self) -> Self {
117        let l = self.length();
118        if l < 1e-6 { self } else { self * (1.0 / l) }
119    }
120
121    pub fn lerp(self, to: Self, t: f32) -> Self {
122        self + (to - self) * t
123    }
124
125    /// Component-wise product.
126    pub fn scale(self, o: Self) -> Self {
127        Self::new(self.x * o.x, self.y * o.y, self.z * o.z)
128    }
129}
130
131impl Add for Vec3 {
132    type Output = Self;
133    fn add(self, o: Self) -> Self {
134        Self::new(self.x + o.x, self.y + o.y, self.z + o.z)
135    }
136}
137
138impl Sub for Vec3 {
139    type Output = Self;
140    fn sub(self, o: Self) -> Self {
141        Self::new(self.x - o.x, self.y - o.y, self.z - o.z)
142    }
143}
144
145impl Mul<f32> for Vec3 {
146    type Output = Self;
147    fn mul(self, k: f32) -> Self {
148        Self::new(self.x * k, self.y * k, self.z * k)
149    }
150}
151
152impl Neg for Vec3 {
153    type Output = Self;
154    fn neg(self) -> Self {
155        Self::new(-self.x, -self.y, -self.z)
156    }
157}
158
159/// A 4×4 matrix, column-major: `m[col * 4 + row]`.
160#[derive(Debug, Clone, Copy, PartialEq)]
161pub struct Mat4(pub [f32; 16]);
162
163impl Default for Mat4 {
164    fn default() -> Self {
165        Self::IDENTITY
166    }
167}
168
169impl Mat4 {
170    pub const IDENTITY: Self = Self([
171        1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
172    ]);
173
174    fn at(&self, row: usize, col: usize) -> f32 {
175        self.0[col * 4 + row]
176    }
177
178    pub fn translation(v: Vec3) -> Self {
179        let mut m = Self::IDENTITY;
180        m.0[12] = v.x;
181        m.0[13] = v.y;
182        m.0[14] = v.z;
183        m
184    }
185
186    pub fn scaling(v: Vec3) -> Self {
187        let mut m = Self::IDENTITY;
188        m.0[0] = v.x;
189        m.0[5] = v.y;
190        m.0[10] = v.z;
191        m
192    }
193
194    /// Turned `a` radians about x (right-handed, as `Axis.XP`).
195    pub fn rotation_x(a: f32) -> Self {
196        let (s, c) = a.sin_cos();
197        Self([
198            1.0, 0.0, 0.0, 0.0, 0.0, c, s, 0.0, 0.0, -s, c, 0.0, 0.0, 0.0, 0.0, 1.0,
199        ])
200    }
201
202    pub fn rotation_y(a: f32) -> Self {
203        let (s, c) = a.sin_cos();
204        Self([
205            c, 0.0, -s, 0.0, 0.0, 1.0, 0.0, 0.0, s, 0.0, c, 0.0, 0.0, 0.0, 0.0, 1.0,
206        ])
207    }
208
209    pub fn rotation_z(a: f32) -> Self {
210        let (s, c) = a.sin_cos();
211        Self([
212            c, s, 0.0, 0.0, -s, c, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
213        ])
214    }
215
216    /// JOML's `rotationZYX(z, y, x)` (a model part's turn) — z, then y,
217    /// then x applied to the vector last.
218    pub fn rotation_zyx(r: Vec3) -> Self {
219        Self::rotation_z(r.z) * Self::rotation_y(r.y) * Self::rotation_x(r.x)
220    }
221
222    pub fn transform_point(&self, p: Vec3) -> Vec3 {
223        Vec3::new(
224            self.at(0, 0) * p.x + self.at(0, 1) * p.y + self.at(0, 2) * p.z + self.at(0, 3),
225            self.at(1, 0) * p.x + self.at(1, 1) * p.y + self.at(1, 2) * p.z + self.at(1, 3),
226            self.at(2, 0) * p.x + self.at(2, 1) * p.y + self.at(2, 2) * p.z + self.at(2, 3),
227        )
228    }
229
230    /// A direction: turned and scaled, not moved.
231    pub fn transform_dir(&self, d: Vec3) -> Vec3 {
232        Vec3::new(
233            self.at(0, 0) * d.x + self.at(0, 1) * d.y + self.at(0, 2) * d.z,
234            self.at(1, 0) * d.x + self.at(1, 1) * d.y + self.at(1, 2) * d.z,
235            self.at(2, 0) * d.x + self.at(2, 1) * d.y + self.at(2, 2) * d.z,
236        )
237    }
238
239    /// A normal, kept a unit vector (good for rotations and uniform scales,
240    /// which is what bodies use).
241    pub fn transform_normal(&self, n: Vec3) -> Vec3 {
242        self.transform_dir(n).normalize()
243    }
244}
245
246impl Mul for Mat4 {
247    type Output = Self;
248    fn mul(self, o: Self) -> Self {
249        let mut r = [0.0; 16];
250        for col in 0..4 {
251            for row in 0..4 {
252                r[col * 4 + row] = (0..4).map(|k| self.at(row, k) * o.at(k, col)).sum();
253            }
254        }
255        Self(r)
256    }
257}
258
259/// Degrees to radians, for the formats (which are in degrees).
260pub fn rad(deg: Vec3) -> Vec3 {
261    Vec3::new(deg.x.to_radians(), deg.y.to_radians(), deg.z.to_radians())
262}
263
264/// Smoothstep's polynomial, 3t² − 2t³: its t² coefficient.
265const SMOOTHSTEP_SQUARE: f32 = 3.0;
266
267/// `t` (clamped to 0–1) eased in and out: 3t² − 2t³.
268pub fn smoothstep(t: f32) -> f32 {
269    let t = t.clamp(0.0, 1.0);
270    t * t * (SMOOTHSTEP_SQUARE - 2.0 * t)
271}
272
273#[cfg(test)]
274mod tests {
275    use super::*;
276
277    fn close(a: Vec3, b: Vec3) -> bool {
278        (a - b).length() < 1e-5
279    }
280
281    #[test]
282    fn rotations_are_right_handed() {
283        let half = std::f32::consts::FRAC_PI_2;
284        assert!(close(
285            Mat4::rotation_x(half).transform_point(Vec3::new(0.0, 1.0, 0.0)),
286            Vec3::new(0.0, 0.0, 1.0)
287        ));
288        assert!(close(
289            Mat4::rotation_y(half).transform_point(Vec3::new(0.0, 0.0, 1.0)),
290            Vec3::new(1.0, 0.0, 0.0)
291        ));
292        assert!(close(
293            Mat4::rotation_z(half).transform_point(Vec3::new(1.0, 0.0, 0.0)),
294            Vec3::new(0.0, 1.0, 0.0)
295        ));
296    }
297
298    #[test]
299    fn moving_comes_after_turning() {
300        let m = Mat4::translation(Vec3::new(5.0, 0.0, 0.0))
301            * Mat4::rotation_zyx(Vec3::new(0.0, 0.0, std::f32::consts::FRAC_PI_2))
302            * Mat4::scaling(Vec3::ONE);
303        assert!(close(
304            m.transform_point(Vec3::new(1.0, 0.0, 0.0)),
305            Vec3::new(5.0, 1.0, 0.0)
306        ));
307    }
308}
yog_graphics/math.rs

yog_graphics/
math.rs
