embedded_gfx/
mesh.rs

1use embedded_graphics_core::pixelcolor::{Rgb565, WebColors};
2use log::error;
3use nalgebra::{Point3, Similarity3, UnitQuaternion, Vector3};
4
5#[derive(Debug, PartialEq)]
6pub enum RenderMode {
7    Points,
8    Lines,
9    Solid,
10    SolidLightDir(Vector3<f32>),
11}
12#[derive(Debug, Default)]
13pub struct Geometry<'a> {
14    pub vertices: &'a [(f32, f32, f32)],
15    pub faces: &'a [(usize, usize, usize)],
16    pub colors: &'a [Rgb565],
17    pub lines: &'a [(usize, usize)],
18    pub normals: &'a [(f32, f32, f32)],
19}
20
21impl<'a> Geometry<'a> {
22    fn check_validity(&self) -> bool {
23        if self.vertices.is_empty() {
24            error!("Vertices are empty");
25            return false;
26        }
27
28        for face in self.faces {
29            if face.0 >= self.vertices.len()
30                || face.1 >= self.vertices.len()
31                || face.2 >= self.vertices.len()
32            {
33                error!("Face vertices are out of bounds");
34                return false;
35            }
36        }
37
38        for line in self.lines {
39            if line.0 >= self.vertices.len() || line.1 >= self.vertices.len() {
40                error!("Line vertices are out of bounds");
41                return false;
42            }
43        }
44
45        if !self.colors.is_empty() && self.colors.len() != self.vertices.len() {
46            error!("Colors are not the same length as vertices");
47            return false;
48        }
49
50        true
51    }
52
53    pub fn lines_from_faces(faces: &[(usize, usize, usize)]) -> Vec<(usize, usize)> {
54        let mut lines = Vec::new();
55        for face in faces {
56            for line in &[(face.0, face.1), (face.1, face.2), (face.2, face.0)] {
57                let (a, b) = if line.0 < line.1 {
58                    (line.0, line.1)
59                } else {
60                    (line.1, line.0)
61                };
62                if !lines.contains(&(a, b)) {
63                    lines.push((a, b));
64                }
65            }
66        }
67
68        lines
69    }
70}
71
72pub struct K3dMesh<'a> {
73    pub similarity: Similarity3<f32>,
74    pub model_matrix: nalgebra::Matrix4<f32>,
75
76    pub color: Rgb565,
77    pub render_mode: RenderMode,
78    pub geometry: Geometry<'a>,
79}
80
81impl<'a> K3dMesh<'a> {
82    pub fn new(geometry: Geometry) -> K3dMesh {
83        assert!(geometry.check_validity());
84        let sim = Similarity3::new(Vector3::new(0.0, 0.0, 0.0), nalgebra::zero(), 1.0);
85        K3dMesh {
86            model_matrix: sim.to_homogeneous(),
87            similarity: sim,
88            color: Rgb565::CSS_WHITE,
89            render_mode: RenderMode::Points,
90            geometry,
91        }
92    }
93
94    pub fn set_color(&mut self, color: Rgb565) {
95        self.color = color;
96    }
97
98    pub fn set_render_mode(&mut self, mode: RenderMode) {
99        self.render_mode = mode;
100    }
101
102    pub fn set_position(&mut self, x: f32, y: f32, z: f32) {
103        self.similarity.isometry.translation.x = x;
104        self.similarity.isometry.translation.y = y;
105        self.similarity.isometry.translation.z = z;
106        self.update_model_matrix();
107    }
108
109    pub fn get_position(&self) -> Point3<f32> {
110        self.similarity.isometry.translation.vector.into()
111    }
112
113    pub fn set_attitude(&mut self, roll: f32, pitch: f32, yaw: f32) {
114        self.similarity.isometry.rotation = UnitQuaternion::from_euler_angles(roll, pitch, yaw);
115        self.update_model_matrix();
116    }
117
118    pub fn set_target(&mut self, target: Point3<f32>) {
119        let view = Similarity3::look_at_rh(
120            &self.similarity.isometry.translation.vector.into(),
121            &target,
122            &Vector3::y(),
123            1.0,
124        );
125
126        self.similarity = view;
127        self.update_model_matrix();
128    }
129
130    pub fn set_scale(&mut self, s: f32) {
131        if s == 0.0 {
132            return;
133        }
134        self.similarity.set_scaling(s);
135        self.update_model_matrix();
136    }
137
138    fn update_model_matrix(&mut self) {
139        self.model_matrix = self.similarity.to_homogeneous();
140    }
141}
embedded_gfx/mesh.rs

embedded_gfx/
mesh.rs
