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
|
#include <fml/quat.h>
#include <cmath>
using namespace fml;
namespace fml {
quat::quat(scalar w_, scalar x_, scalar y_, scalar z_) : w(w_), x(x_), y(y_), z(z_) { }
quat::quat(scalar x_, scalar y_, scalar z_) : w(0), x(x_), y(y_), z(z_) { }
quat::quat(scalar w_, vec3 vec) : w(w_), x(vec[0]), y(vec[1]), z(vec[2]) { }
quat::quat(vec3 vec) : w(0), x(vec[0]), y(vec[1]), z(vec[2]) { }
quat::quat() : w(0), x(0), y(0), z(0) { }
quat::operator vec3() const
{
return vec3(this->x, this->y, this->z);
}
quat operator*(const quat &lhs, const quat &rhs)
{
return quat(lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z,
lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y,
lhs.w * rhs.y - lhs.x * rhs.z + lhs.y * rhs.w + lhs.z * rhs.x,
lhs.w * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.w);
}
quat quat::conjugate() const
{
return quat(this->w, -this->x, -this->y, -this->z);
}
quat quat::rotateby(const quat &rotquat) const
{
return rotquat * (*this) * rotquat.conjugate();
}
quat quat::from_angleaxis(scalar angle, vec3 axis)
{
scalar si = std::sin(angle / 2);
scalar co = std::cos(angle / 2);
return quat(co, si * axis[0], si * axis[1], si * axis[2]);
}
std::ostream &operator<<(std::ostream &os, const quat &q)
{
return os << "(" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << ")";
}
}
|
