-
Notifications
You must be signed in to change notification settings - Fork 0
/
math.hpp
75 lines (51 loc) · 1.29 KB
/
math.hpp
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
#ifndef MATH_HPP
#define MATH_HPP
#include <Eigen/Core>
#include <Eigen/Geometry>
template<class T, int M = Eigen::Dynamic, int N = Eigen::Dynamic>
using matrix = Eigen::Matrix<T, M, N>;
template<class T, int M = Eigen::Dynamic>
using vector = matrix<T, M, 1>;
using real = double;
using vec = vector<real>;
using vec4 = vector<real, 4>;
using vec3 = vector<real, 3>;
using vec2 = vector<real, 2>;
using vec1 = vector<real, 1>;
using mat = matrix<real>;
using mat4x4 = matrix<real, 4, 4>;
using mat3x3 = matrix<real, 3, 3>;
template<class T>
using quaternion = Eigen::Quaternion<T>;
using quat = quaternion<real>;
struct rigid {
quat orient;
vec3 pos;
rigid(): orient(1, 0, 0, 0), pos(0, 0, 0) {}
static rigid translation(real x, real y, real z) {
rigid res;
res.pos = {x, y, z};
return res;
}
static rigid translation(vec3 t) {
return translation(t.x(), t.y(), t.z());
}
static rigid rotation(quat q) {
rigid res;
res.orient = q;
return res;
}
rigid operator*(const rigid& other) const {
rigid res;
res.orient = orient * other.orient;
res.pos = pos + orient * other.pos;
return res;
}
rigid inv() const {
rigid res;
res.orient = orient.conjugate();
res.pos = -(res.orient * pos);
return res;
}
};
#endif