1use std::ops::Mul;
4
5use crate::vmf::entities::Entity;
6use crate::vmf::world::{Side, Solid};
7
8#[derive(Debug, Clone, Copy, PartialEq)]
10pub struct Transform {
11 pub origin: [f64; 3],
12 pub rot: [[f64; 3]; 3],
14}
15
16impl Transform {
17 pub const IDENTITY: Self = Self {
18 origin: [0.0; 3],
19 rot: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
20 };
21
22 pub fn from_entity(ent: &Entity) -> Self {
24 Self {
25 origin: vec3(ent, "origin"),
26 rot: angle_matrix(vec3(ent, "angles")),
27 }
28 }
29
30 pub fn point(&self, p: [f64; 3]) -> [f64; 3] {
32 let r = self.rotate(p);
33 std::array::from_fn(|i| round(r[i] + self.origin[i]))
34 }
35
36 pub fn vector(&self, v: [f64; 3]) -> [f64; 3] {
38 self.rotate(v).map(round)
39 }
40
41 pub fn angles(&self, local: [f64; 3]) -> [f64; 3] {
43 matrix_angles(mat_mul(&self.rot, &angle_matrix(local))).map(round)
44 }
45
46 pub fn inverse(&self) -> Self {
48 let rot = std::array::from_fn(|i| std::array::from_fn(|j| self.rot[j][i]));
49 let back = Self { origin: [0.0; 3], rot };
50 Self {
51 origin: back.vector(self.origin.map(|v| -v)),
52 rot,
53 }
54 }
55
56 pub fn apply_solid(&self, solid: &mut Solid) {
58 if *self == Self::IDENTITY {
60 return;
61 }
62 for side in &mut solid.sides {
63 self.apply_side(side);
64 }
65 }
66
67 pub fn apply_entity(&self, ent: &mut Entity) {
70 if *self == Self::IDENTITY {
71 return;
72 }
73
74 if ent.get("origin").is_some() {
75 let origin = self.point(vec3(ent, "origin"));
76 ent.set("origin".to_string(), fmt_vec3(origin));
77 }
78
79 if ent.get("angles").is_some() && self.rot != Self::IDENTITY.rot {
81 let angles = self.angles(vec3(ent, "angles"));
82 ent.set("angles".to_string(), fmt_vec3(angles));
83 }
84
85 for solid in ent.solids.iter_mut().flatten() {
86 self.apply_solid(solid);
87 }
88 }
89
90 fn apply_side(&self, side: &mut Side) {
91 if let Some(p) = parse::<9>(&side.plane) {
92 let [a, b, c] = [0, 3, 6].map(|i| fmt_vec3(self.point([p[i], p[i + 1], p[i + 2]])));
93 side.plane = format!("({a}) ({b}) ({c})");
94 }
95 self.apply_axis(&mut side.u_axis);
96 self.apply_axis(&mut side.v_axis);
97
98 let Some(disp) = &mut side.dispinfo else {
99 return;
100 };
101
102 if let Some(start) = parse::<3>(&disp.start_position) {
103 disp.start_position = format!("[{}]", fmt_vec3(self.point(start)));
104 }
105
106 for rows in [&mut disp.normals, &mut disp.offsets, &mut disp.offset_normals] {
109 for row in &mut rows.rows {
110 self.apply_row(row);
111 }
112 }
113 }
114
115 fn apply_axis(&self, axis: &mut String) {
117 let Some([x, y, z, shift, scale]) = parse::<5>(axis).filter(|&[.., scale]| scale != 0.0) else {
118 return;
119 };
120 let dir = self.vector([x, y, z]);
121
122 let shift = round(shift - dot(self.origin, dir) / scale);
124 *axis = format!("[{} {shift}] {scale}", fmt_vec3(dir));
125 }
126
127 fn apply_row(&self, row: &mut String) {
128 let values: Option<Vec<f64>> = row.split_whitespace().map(|v| v.parse().ok()).collect();
129 let Some(values) = values.filter(|v| v.len() % 3 == 0) else {
130 return;
131 };
132
133 *row = values
134 .chunks_exact(3)
135 .map(|v| fmt_vec3(self.vector([v[0], v[1], v[2]])))
136 .collect::<Vec<_>>()
137 .join(" ");
138 }
139
140 fn rotate(&self, v: [f64; 3]) -> [f64; 3] {
141 self.rot.map(|row| dot(row, v))
142 }
143}
144
145impl Mul for Transform {
147 type Output = Self;
148
149 fn mul(self, child: Self) -> Self {
150 Self {
151 origin: self.point(child.origin),
152 rot: mat_mul(&self.rot, &child.rot),
153 }
154 }
155}
156
157fn round(v: f64) -> f64 {
158 (v * 1e6).round() / 1e6 + 0.0
159}
160
161fn vec3(ent: &Entity, key: &str) -> [f64; 3] {
162 let mut out = [0.0; 3];
163 if let Some(value) = ent.get(key) {
164 for (slot, part) in out.iter_mut().zip(value.split_whitespace()) {
165 *slot = part.parse().unwrap_or(0.0);
166 }
167 }
168
169 out
170}
171
172fn parse<const N: usize>(value: &str) -> Option<[f64; N]> {
174 let mut parts = value
175 .split(|c: char| c.is_whitespace() || matches!(c, '(' | ')' | '[' | ']'))
176 .filter(|part| !part.is_empty());
177 let mut out = [0.0; N];
178
179 for slot in &mut out {
180 *slot = parts.next()?.parse().ok()?;
181 }
182
183 parts.next().is_none().then_some(out)
184}
185
186fn fmt_vec3(v: [f64; 3]) -> String {
187 format!("{} {} {}", v[0], v[1], v[2])
188}
189
190fn dot(a: [f64; 3], b: [f64; 3]) -> f64 {
191 a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
192}
193
194fn mat_mul(a: &[[f64; 3]; 3], b: &[[f64; 3]; 3]) -> [[f64; 3]; 3] {
195 std::array::from_fn(|i| std::array::from_fn(|j| (0..3).map(|k| a[i][k] * b[k][j]).sum()))
196}
197
198fn angle_matrix([pitch, yaw, roll]: [f64; 3]) -> [[f64; 3]; 3] {
200 let (sp, cp) = pitch.to_radians().sin_cos();
201 let (sy, cy) = yaw.to_radians().sin_cos();
202 let (sr, cr) = roll.to_radians().sin_cos();
203
204 [
205 [cp * cy, sp * sr * cy - cr * sy, sp * cr * cy + sr * sy],
206 [cp * sy, sp * sr * sy + cr * cy, sp * cr * sy - sr * cy],
207 [-sp, sr * cp, cr * cp],
208 ]
209}
210
211fn matrix_angles(m: [[f64; 3]; 3]) -> [f64; 3] {
213 let xy = m[0][0].hypot(m[1][0]);
214 let pitch = (-m[2][0]).atan2(xy);
215
216 let (yaw, roll) = if xy > 0.001 {
217 (m[1][0].atan2(m[0][0]), m[2][1].atan2(m[2][2]))
218 } else {
219 ((-m[0][1]).atan2(m[1][1]), 0.0)
220 };
221
222 [pitch, yaw, roll].map(f64::to_degrees)
223}