forked from VDrift/vdrift
-
Notifications
You must be signed in to change notification settings - Fork 1
/
mat.um
102 lines (79 loc) · 3.01 KB
/
mat.um
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
95
96
97
98
99
100
// Umka 3D vector/matrix library
type (
Vec* = [3]real
Mat* = [3]Vec
)
// Vector operations
fn (u: ^Vec) add*(v: Vec): Vec {return Vec{u[0] + v[0], u[1] + v[1], u[2] + v[2]}}
fn (u: ^Vec) sub*(v: Vec): Vec {return Vec{u[0] - v[0], u[1] - v[1], u[2] - v[2]}}
fn (u: ^Vec) mul*(a: real): Vec {return Vec{u[0] * a, u[1] * a, u[2] * a}}
fn (u: ^Vec) div*(a: real): Vec {return Vec{u[0] / a, u[1] / a, u[2] / a}}
fn (u: ^Vec) dot*(v: Vec): real {return u[0] * v[0] + u[1] * v[1] + u[2] * v[2]}
fn (u: ^Vec) cross*(v: Vec): Vec {return Vec{u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]}}
fn (u: ^Vec) elementwise*(v: Vec): Vec {return Vec{u[0] * v[0], u[1] * v[1], u[2] * v[2]}}
fn (v: ^Vec) norm*(): real {return sqrt(v.dot(v^))}
fn (v: ^Vec) normalize*(): Vec {return v.div(v.norm())}
// Matrix operations
fn (m: ^Mat) add*(n: Mat): Mat {return Mat{m[0].add(n[0]), m[1].add(n[1]), m[2].add(n[2])}}
fn (m: ^Mat) sub*(n: Mat): Mat {return Mat{m[0].sub(n[0]), m[1].sub(n[1]), m[2].sub(n[2])}}
fn (m: ^Mat) mul*(a: real): Mat {return Mat{m[0].mul(a), m[1].mul(a), m[2].mul(a)}}
fn (m: ^Mat) div*(a: real): Mat {return Mat{m[0].div(a), m[1].div(a), m[2].div(a)}}
fn (m: ^Mat) mulv*(v: Vec): Vec {return Vec{m[0].dot(v), m[1].dot(v), m[2].dot(v)}}
fn (m: ^Mat) mulm*(n: Mat): Mat {
var res: Mat
for i := 0; i < 3; i++ {
for j := 0; j < 3; j++ {
for k := 0; k < 3; k++ {
res[i][j] += m[i][k] * n[k][j]
}
}
}
return res
}
fn identity*(): Mat {
var res: Mat
for i := 0; i < 3; i++ {
res[i][i] = 1
}
return res
}
fn (m: ^Mat) transpose*(): Mat {
var res: Mat
for i := 0; i < 3; i++ {
for j := 0; j < 3; j++ {
res[i][j] = m[j][i]
}
}
return res
}
fn (m: ^Mat) normalize*(): Mat {
return m.add(m.mulm(identity().sub(m.transpose().mulm(m^))).mul(0.5))
}
// Vector/matrix conversions
fn (v: ^Vec) toRateMat*(): Mat {
return Mat{Vec{ 0, -v[2], v[1]},
Vec{ v[2], 0, -v[0]},
Vec{-v[1], v[0], 0 }}
}
fn (v: ^Vec) toAttMat*(): Mat { // ENU version
// v = {pitch, roll, yaw}
sr := sin(v[1]); cr := cos(v[1])
sp := sin(v[0]); cp := cos(v[0])
sy := sin(v[2]); cy := cos(v[2])
rollMat := Mat{Vec{ cr, 0, sr },
Vec{ 0, 1, 0 },
Vec{-sr, 0, cr }}
pitchMat := Mat{Vec{ 1, 0, 0 },
Vec{ 0, cp, -sp },
Vec{ 0, sp, cp }}
yawMat := Mat{Vec{ cy, sy, 0 },
Vec{-sy, cy, 0 },
Vec{ 0, 0, 1 }}
return yawMat.mulm(pitchMat).mulm(rollMat)
}
fn (m: ^Mat) toAttAngles*(): Vec { // ENU version
pitch := atan2( m[2][1], sqrt(m[2][0] * m[2][0] + m[2][2] * m[2][2]))
roll := atan2(-m[2][0], m[2][2])
yaw := atan2( m[0][1], m[1][1])
return Vec{pitch, roll, yaw}
}