1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
use crate::{matrix, vector, FArray, FArray2};
use crate::{Float, QArray, Quaternion, SqMatrix, SqMatrix3, SqMatrix4, Transform, Vector};
#[derive(Clone, Copy, Debug)]
pub struct FQArrayTrans<F: Float> {
quat: QArray<F, FArray<F, 3>, FArray<F, 4>>,
trans_scale: FArray<F, 4>,
}
impl<F: Float> std::default::Default for FQArrayTrans<F> {
fn default() -> Self {
Self {
quat: QArray::default(),
trans_scale: FArray::from_array([F::zero(), F::zero(), F::zero(), F::one()]),
}
}
}
impl<F> std::fmt::Display for FQArrayTrans<F>
where
F: Float,
{
fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(
f,
"trans[+({},{},{}) rot{} *{}]",
self.trans_scale[0],
self.trans_scale[1],
self.trans_scale[2],
self.quat,
self.trans_scale[3],
)
}
}
impl<F: Float>
Transform<
F,
FArray<F, 3>,
FArray<F, 4>,
FArray2<F, 4, 16>,
QArray<F, FArray<F, 3>, FArray<F, 4>>,
> for FQArrayTrans<F>
{
fn of_trs(t: FArray<F, 3>, r: QArray<F, FArray<F, 3>, FArray<F, 4>>, s: F) -> Self {
Self {
quat: r,
trans_scale: FArray::from_array([t[0], t[1], t[2], s]),
}
}
fn get_scale(&self) -> F {
self.trans_scale[3]
}
fn get_translation(&self) -> FArray<F, 3> {
FArray::from_array([
self.trans_scale[0],
self.trans_scale[1],
self.trans_scale[2],
])
}
fn get_rotation(&self) -> QArray<F, FArray<F, 3>, FArray<F, 4>> {
self.quat
}
fn inverse(&self) -> Self {
let scale = self.trans_scale[3];
if scale.abs() < F::epsilon() {
Self::default()
} else {
let scale = F::one() / scale;
let trans = FArray::from_array([
self.trans_scale[0],
self.trans_scale[1],
self.trans_scale[2],
]);
let iquat = self.quat.conjugate();
let trans = -(iquat.apply3(&trans)) * scale;
Self::of_trs(trans, iquat, scale)
}
}
fn invert(&mut self) {
*self = self.inverse();
}
fn as_mat(&self) -> FArray2<F, 4, 16> {
let mut m = FArray2::<F, 4, 16>::zero();
self.quat.set_rotation4(&mut m);
m *= self.trans_scale[3];
m[3] = self.trans_scale[0];
m[7] = self.trans_scale[1];
m[11] = self.trans_scale[2];
m
}
}