1 use std::f64::consts::PI;
2 use std::ops::Mul;
3 
4 /// The projection matrix which is used to project the 3D space to the 2D display panel
5 #[derive(Clone, Debug, Copy)]
6 pub struct ProjectionMatrix([[f64; 4]; 4]);
7 
8 impl AsRef<[[f64; 4]; 4]> for ProjectionMatrix {
as_ref(&self) -> &[[f64; 4]; 4]9     fn as_ref(&self) -> &[[f64; 4]; 4] {
10         &self.0
11     }
12 }
13 
14 impl AsMut<[[f64; 4]; 4]> for ProjectionMatrix {
as_mut(&mut self) -> &mut [[f64; 4]; 4]15     fn as_mut(&mut self) -> &mut [[f64; 4]; 4] {
16         &mut self.0
17     }
18 }
19 
20 impl From<[[f64; 4]; 4]> for ProjectionMatrix {
from(data: [[f64; 4]; 4]) -> Self21     fn from(data: [[f64; 4]; 4]) -> Self {
22         ProjectionMatrix(data)
23     }
24 }
25 
26 impl Default for ProjectionMatrix {
default() -> Self27     fn default() -> Self {
28         ProjectionMatrix::rotate(PI, 0.0, 0.0)
29     }
30 }
31 
32 impl Mul<ProjectionMatrix> for ProjectionMatrix {
33     type Output = ProjectionMatrix;
mul(self, other: ProjectionMatrix) -> ProjectionMatrix34     fn mul(self, other: ProjectionMatrix) -> ProjectionMatrix {
35         let mut ret = ProjectionMatrix::zero();
36         for r in 0..4 {
37             for c in 0..4 {
38                 for k in 0..4 {
39                     ret.0[r][c] += other.0[r][k] * self.0[k][c];
40                 }
41             }
42         }
43         ret.normalize();
44         ret
45     }
46 }
47 
48 impl Mul<(i32, i32, i32)> for ProjectionMatrix {
49     type Output = (i32, i32);
mul(self, (x, y, z): (i32, i32, i32)) -> (i32, i32)50     fn mul(self, (x, y, z): (i32, i32, i32)) -> (i32, i32) {
51         let (x, y, z) = (x as f64, y as f64, z as f64);
52         let m = self.0;
53         (
54             (x * m[0][0] + y * m[0][1] + z * m[0][2] + m[0][3]) as i32,
55             (x * m[1][0] + y * m[1][1] + z * m[1][2] + m[1][3]) as i32,
56         )
57     }
58 }
59 
60 impl Mul<(f64, f64, f64)> for ProjectionMatrix {
61     type Output = (i32, i32);
mul(self, (x, y, z): (f64, f64, f64)) -> (i32, i32)62     fn mul(self, (x, y, z): (f64, f64, f64)) -> (i32, i32) {
63         let m = self.0;
64         (
65             (x * m[0][0] + y * m[0][1] + z * m[0][2] + m[0][3]) as i32,
66             (x * m[1][0] + y * m[1][1] + z * m[1][2] + m[1][3]) as i32,
67         )
68     }
69 }
70 
71 impl ProjectionMatrix {
72     /// Returns the identity matrix
one() -> Self73     pub fn one() -> Self {
74         ProjectionMatrix([
75             [1.0, 0.0, 0.0, 0.0],
76             [0.0, 1.0, 0.0, 0.0],
77             [0.0, 0.0, 1.0, 0.0],
78             [0.0, 0.0, 0.0, 1.0],
79         ])
80     }
81     /// Returns the zero maxtrix
zero() -> Self82     pub fn zero() -> Self {
83         ProjectionMatrix([[0.0; 4]; 4])
84     }
85     /// Returns the matrix which shift the coordinate
shift(x: f64, y: f64, z: f64) -> Self86     pub fn shift(x: f64, y: f64, z: f64) -> Self {
87         ProjectionMatrix([
88             [1.0, 0.0, 0.0, x],
89             [0.0, 1.0, 0.0, y],
90             [0.0, 0.0, 1.0, z],
91             [0.0, 0.0, 0.0, 1.0],
92         ])
93     }
94     /// Returns the matrix which rotates the coordinate
95     #[allow(clippy::many_single_char_names)]
rotate(x: f64, y: f64, z: f64) -> Self96     pub fn rotate(x: f64, y: f64, z: f64) -> Self {
97         let (c, b, a) = (x, y, z);
98         ProjectionMatrix([
99             [
100                 a.cos() * b.cos(),
101                 a.cos() * b.sin() * c.sin() - a.sin() * c.cos(),
102                 a.cos() * b.sin() * c.cos() + a.sin() * c.sin(),
103                 0.0,
104             ],
105             [
106                 a.sin() * b.cos(),
107                 a.sin() * b.sin() * c.sin() + a.cos() * c.cos(),
108                 a.sin() * b.sin() * c.cos() - a.cos() * c.sin(),
109                 0.0,
110             ],
111             [-b.sin(), b.cos() * c.sin(), b.cos() * c.cos(), 0.0],
112             [0.0, 0.0, 0.0, 1.0],
113         ])
114     }
115     /// Returns the matrix that applies a scale factor
scale(factor: f64) -> Self116     pub fn scale(factor: f64) -> Self {
117         ProjectionMatrix([
118             [1.0, 0.0, 0.0, 0.0],
119             [0.0, 1.0, 0.0, 0.0],
120             [0.0, 0.0, 1.0, 0.0],
121             [0.0, 0.0, 0.0, 1.0 / factor],
122         ])
123     }
124     /// Normalize the matrix, this will make the metric unit to 1
normalize(&mut self)125     pub fn normalize(&mut self) {
126         if self.0[3][3] > 1e-20 {
127             for r in 0..4 {
128                 for c in 0..4 {
129                     self.0[r][c] /= self.0[3][3];
130                 }
131             }
132         }
133     }
134 
135     /// Get the distance of the point in guest coordinate from the screen in pixels
projected_depth(&self, (x, y, z): (i32, i32, i32)) -> i32136     pub fn projected_depth(&self, (x, y, z): (i32, i32, i32)) -> i32 {
137         let r = &self.0[2];
138         (r[0] * x as f64 + r[1] * y as f64 + r[2] * z as f64 + r[3]) as i32
139     }
140 }
141 
142 /// The helper struct to build a projection matrix
143 #[derive(Copy, Clone)]
144 pub struct ProjectionMatrixBuilder {
145     /// Specifies the yaw of the 3D coordinate system
146     pub yaw: f64,
147     /// Specifies the pitch of the 3D coordinate system
148     pub pitch: f64,
149     /// Specifies the scale of the 3D coordinate system
150     pub scale: f64,
151     pivot_before: (i32, i32, i32),
152     pivot_after: (i32, i32),
153 }
154 
155 impl Default for ProjectionMatrixBuilder {
default() -> Self156     fn default() -> Self {
157         Self {
158             yaw: 0.5,
159             pitch: 0.15,
160             scale: 1.0,
161             pivot_after: (0, 0),
162             pivot_before: (0, 0, 0),
163         }
164     }
165 }
166 
167 impl ProjectionMatrixBuilder {
168     /// Creates a new, default projection matrix builder object.
new() -> Self169     pub fn new() -> Self {
170         Self::default()
171     }
172 
173     /// Set the pivot point, which means the 3D coordinate "before" should be mapped into
174     /// the 2D coordinatet "after"
set_pivot(&mut self, before: (i32, i32, i32), after: (i32, i32)) -> &mut Self175     pub fn set_pivot(&mut self, before: (i32, i32, i32), after: (i32, i32)) -> &mut Self {
176         self.pivot_before = before;
177         self.pivot_after = after;
178         self
179     }
180 
181     /// Build the matrix based on the configuration
into_matrix(self) -> ProjectionMatrix182     pub fn into_matrix(self) -> ProjectionMatrix {
183         let mut ret = if self.pivot_before == (0, 0, 0) {
184             ProjectionMatrix::default()
185         } else {
186             let (x, y, z) = self.pivot_before;
187             ProjectionMatrix::shift(-x as f64, -y as f64, -z as f64) * ProjectionMatrix::default()
188         };
189 
190         if self.yaw.abs() > 1e-20 {
191             ret = ret * ProjectionMatrix::rotate(0.0, self.yaw, 0.0);
192         }
193 
194         if self.pitch.abs() > 1e-20 {
195             ret = ret * ProjectionMatrix::rotate(self.pitch, 0.0, 0.0);
196         }
197 
198         if (self.scale - 1.0).abs() > 1e-20 {
199             ret = ret * ProjectionMatrix::scale(self.scale);
200         }
201 
202         if self.pivot_after != (0, 0) {
203             let (x, y) = self.pivot_after;
204             ret = ret * ProjectionMatrix::shift(x as f64, y as f64, 0.0);
205         }
206 
207         ret
208     }
209 }
210