Extract fixedmat4 into its own separate module.

src/math/fixed_transform.rs

@@ -0,0 +1,136 @@
+use std::io;
+
+use bevy::prelude::*;
+
+use crate::math::{
+    int::{FGWide, FGi32, FGi32Ext},
+    vec3::FGVec3,
+};
+
+/// Parent transforms are composed after quantization, which makes every runtime sample platform-independent.
+#[derive(Clone, Copy, Debug, PartialEq, Eq, serde::Serialize, serde::Deserialize, Hash)]
+pub struct FixedMat4 {
+    pub cols: [[FGi32; 4]; 4],
+}
+
+impl FixedMat4 {
+    pub const IDENTITY: Self = Self {
+        cols: [
+            [FGi32::ONE, FGi32::ZERO, FGi32::ZERO, FGi32::ZERO],
+            [FGi32::ZERO, FGi32::ONE, FGi32::ZERO, FGi32::ZERO],
+            [FGi32::ZERO, FGi32::ZERO, FGi32::ONE, FGi32::ZERO],
+            [FGi32::ZERO, FGi32::ZERO, FGi32::ZERO, FGi32::ONE],
+        ],
+    };
+
+    pub fn from_mat4(matrix: Mat4) -> io::Result<Self> {
+        let values = matrix.to_cols_array();
+        let mut cols = [[FGi32::ZERO; 4]; 4];
+        for (index, value) in values.into_iter().enumerate() {
+            cols[index / 4][index % 4] = FGi32::checked_from_num(value).ok_or_else(|| {
+                io::Error::new(
+                    io::ErrorKind::InvalidData,
+                    "animation transform overflows Q16.16",
+                )
+            })?;
+        }
+        Ok(Self { cols })
+    }
+
+    pub fn to_mat4(self) -> Mat4 {
+        Mat4::from_cols_array_2d(&self.cols.map(|column| column.map(|value| value.to_num())))
+    }
+
+    pub fn mul(self, rhs: Self) -> Self {
+        let mut cols = [[FGi32::ZERO; 4]; 4];
+        for column in 0..4 {
+            for row in 0..4 {
+                let mut value = FGWide::ZERO;
+                for k in 0..4 {
+                    value +=
+                        FGWide::from_num(self.cols[k][row]) * FGWide::from_num(rhs.cols[column][k]);
+                }
+                cols[column][row] = FGi32::from_num(value);
+            }
+        }
+        Self { cols }
+    }
+
+    pub fn transform_point(self, point: FGVec3) -> FGVec3 {
+        let input = [point.x, point.y, point.z, FGi32::ONE];
+        let mut output = [FGi32::ZERO; 3];
+        for row in 0..3 {
+            let mut value = FGWide::ZERO;
+            for (column, input) in input.into_iter().enumerate() {
+                value += FGWide::from_num(self.cols[column][row]) * FGWide::from_num(input);
+            }
+            output[row] = FGi32::from_num(value);
+        }
+        FGVec3 {
+            x: output[0],
+            y: output[1],
+            z: output[2],
+        }
+    }
+
+    pub fn translate(&mut self, translation: FGVec3) {
+        self.cols[3][0] = translation.x;
+        self.cols[3][1] = translation.y;
+        self.cols[3][2] = translation.z;
+    }
+    pub fn with_translation(mut self, translation: FGVec3) -> Self {
+        self.translate(translation);
+        self
+    }
+
+    pub fn with_rotation(mut self, rotation: FGVec3) -> Self {
+        let (sin_x, cos_x) = rotation.x.sin_cos_rads();
+        let (sin_y, cos_y) = rotation.y.sin_cos_rads();
+        let (sin_z, cos_z) = rotation.z.sin_cos_rads();
+        self.cols[0][0] = cos_y * cos_z;
+        self.cols[0][1] = cos_x * sin_z + sin_x * sin_y * cos_z;
+        self.cols[0][2] = sin_x * sin_z - cos_x * sin_y * cos_z;
+        self.cols[1][0] = -cos_y * sin_z;
+        self.cols[1][1] = cos_x * cos_z - sin_x * sin_y * sin_z;
+        self.cols[1][2] = sin_x * cos_z + cos_x * sin_y * sin_z;
+        self.cols[2][0] = sin_y;
+        self.cols[2][1] = -sin_x * cos_y;
+        self.cols[2][2] = cos_x * cos_y;
+        self
+    }
+
+    pub fn get_translation(&self) -> FGVec3 {
+        FGVec3::new(self.cols[3][0], self.cols[3][1], self.cols[3][2])
+    }
+
+    pub fn rotate_y(self, quarter_turns: u8) -> Self {
+        let rotation = match quarter_turns % 4 {
+            0 => Self::IDENTITY,
+            1 => Self {
+                cols: [
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::NEG_ONE, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ONE, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::ONE, FGi32::ZERO, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::ZERO, FGi32::ONE],
+                ],
+            },
+            2 => Self {
+                cols: [
+                    [FGi32::NEG_ONE, FGi32::ZERO, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ONE, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::NEG_ONE, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::ZERO, FGi32::ONE],
+                ],
+            },
+            _ => Self {
+                cols: [
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::ONE, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ONE, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::NEG_ONE, FGi32::ZERO, FGi32::ZERO, FGi32::ZERO],
+                    [FGi32::ZERO, FGi32::ZERO, FGi32::ZERO, FGi32::ONE],
+                ],
+            },
+        };
+        self.mul(rotation)
+    }
+}