1use crate::common::C3Vector;
4
5#[derive(Debug, Clone, Copy, PartialEq)]
7pub struct Quat {
8 pub x: f32,
9 pub y: f32,
10 pub z: f32,
11 pub w: f32,
12}
13
14impl Quat {
15 pub const IDENTITY: Self = Self {
17 x: 0.0,
18 y: 0.0,
19 z: 0.0,
20 w: 1.0,
21 };
22
23 pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
25 Self { x, y, z, w }
26 }
27
28 pub fn normalize(&self) -> Self {
30 let len = (self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w).sqrt();
31 if len > 0.0 {
32 Self {
33 x: self.x / len,
34 y: self.y / len,
35 z: self.z / len,
36 w: self.w / len,
37 }
38 } else {
39 Self::IDENTITY
40 }
41 }
42
43 pub fn slerp(&self, other: &Self, t: f32) -> Self {
45 let dot = self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w;
47
48 let (other, dot) = if dot < 0.0 {
50 (
51 Self {
52 x: -other.x,
53 y: -other.y,
54 z: -other.z,
55 w: -other.w,
56 },
57 -dot,
58 )
59 } else {
60 (*other, dot)
61 };
62
63 if dot > 0.9995 {
65 return Self {
66 x: self.x + t * (other.x - self.x),
67 y: self.y + t * (other.y - self.y),
68 z: self.z + t * (other.z - self.z),
69 w: self.w + t * (other.w - self.w),
70 }
71 .normalize();
72 }
73
74 let theta_0 = dot.acos();
76 let theta = theta_0 * t;
77 let sin_theta = theta.sin();
78 let sin_theta_0 = theta_0.sin();
79
80 let s0 = (theta_0 - theta).cos() - dot * sin_theta / sin_theta_0;
81 let s1 = sin_theta / sin_theta_0;
82
83 Self {
84 x: s0 * self.x + s1 * other.x,
85 y: s0 * self.y + s1 * other.y,
86 z: s0 * self.z + s1 * other.z,
87 w: s0 * self.w + s1 * other.w,
88 }
89 }
90}
91
92impl Default for Quat {
93 fn default() -> Self {
94 Self::IDENTITY
95 }
96}
97
98#[derive(Debug, Clone, Copy, PartialEq, Default)]
100pub struct Vec3 {
101 pub x: f32,
102 pub y: f32,
103 pub z: f32,
104}
105
106impl Vec3 {
107 pub const ZERO: Self = Self {
109 x: 0.0,
110 y: 0.0,
111 z: 0.0,
112 };
113
114 pub const ONE: Self = Self {
116 x: 1.0,
117 y: 1.0,
118 z: 1.0,
119 };
120
121 pub const fn new(x: f32, y: f32, z: f32) -> Self {
123 Self { x, y, z }
124 }
125}
126
127impl From<C3Vector> for Vec3 {
128 fn from(v: C3Vector) -> Self {
129 Self {
130 x: v.x,
131 y: v.y,
132 z: v.z,
133 }
134 }
135}
136
137impl From<Vec3> for C3Vector {
138 fn from(v: Vec3) -> Self {
139 Self {
140 x: v.x,
141 y: v.y,
142 z: v.z,
143 }
144 }
145}
146
147pub trait Lerp: Clone {
149 fn lerp(&self, other: &Self, t: f32) -> Self;
151}
152
153impl Lerp for f32 {
154 fn lerp(&self, other: &Self, t: f32) -> Self {
155 self + (other - self) * t
156 }
157}
158
159impl Lerp for f64 {
160 fn lerp(&self, other: &Self, t: f32) -> Self {
161 self + (other - self) * t as f64
162 }
163}
164
165impl Lerp for Vec3 {
166 fn lerp(&self, other: &Self, t: f32) -> Self {
167 Self {
168 x: self.x.lerp(&other.x, t),
169 y: self.y.lerp(&other.y, t),
170 z: self.z.lerp(&other.z, t),
171 }
172 }
173}
174
175impl Lerp for C3Vector {
176 fn lerp(&self, other: &Self, t: f32) -> Self {
177 Self {
178 x: self.x.lerp(&other.x, t),
179 y: self.y.lerp(&other.y, t),
180 z: self.z.lerp(&other.z, t),
181 }
182 }
183}
184
185impl Lerp for Quat {
186 fn lerp(&self, other: &Self, t: f32) -> Self {
187 self.slerp(other, t)
189 }
190}
191
192#[derive(Debug, Clone, Copy, PartialEq, Default)]
195pub struct Fixedi16(pub i16);
196
197impl Fixedi16 {
198 pub fn to_f32(self) -> f32 {
200 self.0 as f32 / 32767.0
201 }
202}
203
204impl From<Fixedi16> for f32 {
205 fn from(v: Fixedi16) -> Self {
206 v.to_f32()
207 }
208}
209
210impl From<f32> for Fixedi16 {
211 fn from(v: f32) -> Self {
212 Self((v.clamp(0.0, 1.0) * 32767.0) as i16)
213 }
214}
215
216impl Lerp for Fixedi16 {
217 fn lerp(&self, other: &Self, t: f32) -> Self {
218 let a = self.to_f32();
219 let b = other.to_f32();
220 Fixedi16::from(a.lerp(&b, t))
221 }
222}
223
224#[derive(Debug, Clone)]
226pub struct ResolvedTrack<T> {
227 pub interpolation_type: u16,
229 pub global_sequence: i16,
231 pub timestamps: Vec<Vec<u32>>,
233 pub values: Vec<Vec<T>>,
235}
236
237impl<T> ResolvedTrack<T> {
238 pub fn empty() -> Self {
240 Self {
241 interpolation_type: 0,
242 global_sequence: -1,
243 timestamps: Vec::new(),
244 values: Vec::new(),
245 }
246 }
247
248 pub fn has_data(&self) -> bool {
250 !self.timestamps.is_empty() && self.timestamps.iter().any(|ts| !ts.is_empty())
251 }
252
253 pub fn uses_global_sequence(&self) -> bool {
255 self.global_sequence >= 0
256 }
257}
258
259impl<T: Default> Default for ResolvedTrack<T> {
260 fn default() -> Self {
261 Self::empty()
262 }
263}
264
265#[cfg(test)]
266mod tests {
267 use super::*;
268
269 #[test]
270 fn test_vec3_lerp() {
271 let a = Vec3::new(0.0, 0.0, 0.0);
272 let b = Vec3::new(10.0, 20.0, 30.0);
273
274 let mid = a.lerp(&b, 0.5);
275 assert!((mid.x - 5.0).abs() < 0.001);
276 assert!((mid.y - 10.0).abs() < 0.001);
277 assert!((mid.z - 15.0).abs() < 0.001);
278 }
279
280 #[test]
281 fn test_quat_identity() {
282 let q = Quat::IDENTITY;
283 assert_eq!(q.x, 0.0);
284 assert_eq!(q.y, 0.0);
285 assert_eq!(q.z, 0.0);
286 assert_eq!(q.w, 1.0);
287 }
288
289 #[test]
290 fn test_quat_normalize() {
291 let q = Quat::new(1.0, 1.0, 1.0, 1.0);
292 let normalized = q.normalize();
293 let len = (normalized.x.powi(2)
294 + normalized.y.powi(2)
295 + normalized.z.powi(2)
296 + normalized.w.powi(2))
297 .sqrt();
298 assert!((len - 1.0).abs() < 0.001);
299 }
300
301 #[test]
302 fn test_fixedi16_conversion() {
303 let f = Fixedi16::from(0.5);
304 let back = f.to_f32();
305 assert!((back - 0.5).abs() < 0.001);
306 }
307}