/
Quaternion.h
231 lines (190 loc) · 5.96 KB
/
Quaternion.h
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
#pragma once
/// \file
/// \brief Quaternion data types and related operations.
#include "Vector3.h"
#include "Vector4.h"
// Forward declaration, include Matrix4.h for definition
class Matrix4;
/// \brief A quaternion stored in single-precision floating-point.
class Quaternion :
public Vector4
{
public:
Quaternion() :
Vector4()
{}
Quaternion(const Vector4& vector) :
Vector4(vector)
{}
// Construct a Quaternion out of the 4 arguments
Quaternion(ElementType x_, ElementType y_, ElementType z_, ElementType w_) :
Vector4(x_, y_, z_, w_)
{}
// Construct a Quaternion out of a Vector3 plus a fourth argument
Quaternion(const Vector3& other, ElementType w_) :
Vector4(other, w_)
{}
/**
* Returns the identity quaternion (named constructor)
*/
static const Quaternion& Identity();
/**
* Constructs a quaternion which rotates between two points on the unit-sphere (from and to).
*/
static Quaternion createForUnitVectors(const Vector3& from, const Vector3& to);
/**
* Constructs a rotation quaternion for the given euler angles.
* Each component of the given vector refers to an angle (in degrees)
* of one of the x-/y-/z-axis.
*/
static Quaternion createForEulerXYZDegrees(const Vector3& eulerXYZ);
/**
* Constructs a quat for the given axis and angle
*/
static Quaternion createForAxisAngle(const Vector3& axis, double angle);
/**
* Constructs a rotation quaternion about a given axis.
*/
static Quaternion createForX(double angle);
static Quaternion createForY(double angle);
static Quaternion createForZ(double angle);
/**
* Returns this quaternion multiplied by the other one.
*/
Quaternion getMultipliedBy(const Quaternion& other) const;
/**
* Multiplies this quaternion by the other one in place.
* Equivalent to: *this = getMultipliedBy(other);
*/
void multiplyBy(const Quaternion& other);
/**
* Multiplies this quaternion by the other one in place.
* Equivalent to: *this = other.getMultipliedBy(*this);
*/
void preMultiplyBy(const Quaternion& other);
/**
* Returns the inverse of this quaternion.
*/
Quaternion getInverse() const;
/**
* Conjugates/inverts this quaternion in place.
*/
void conjugate();
/**
* Returns a normalised copy of this quaternion.
*/
Quaternion getNormalised() const;
/**
* Normalise this quaternion in-place.
*/
void normalise();
/**
* Returns the given point as transformed by this quaternion
*/
Vector3 transformPoint(const Vector3& point) const;
};
inline const Quaternion& Quaternion::Identity()
{
static Quaternion _identity(0, 0, 0, 1);
return _identity;
}
inline Quaternion Quaternion::createForUnitVectors(const Vector3& from, const Vector3& to)
{
return Quaternion(from.crossProduct(to), from.dot(to));
}
inline Quaternion Quaternion::createForEulerXYZDegrees(const Vector3& eulerXYZ)
{
double cx = cos(degrees_to_radians(eulerXYZ[0] * 0.5f));
double sx = sin(degrees_to_radians(eulerXYZ[0] * 0.5f));
double cy = cos(degrees_to_radians(eulerXYZ[1] * 0.5f));
double sy = sin(degrees_to_radians(eulerXYZ[1] * 0.5f));
double cz = cos(degrees_to_radians(eulerXYZ[2] * 0.5f));
double sz = sin(degrees_to_radians(eulerXYZ[2] * 0.5f));
return Quaternion(
cz * cy * sx - sz * sy * cx,
cz * sy * cx + sz * cy * sx,
sz * cy * cx - cz * sy * sx,
cz * cy * cx + sz * sy * sx
);
}
inline Quaternion Quaternion::createForAxisAngle(const Vector3& axis, double angle)
{
angle *= 0.5f;
double sa = sin(angle);
return Quaternion(axis[0] * sa, axis[1] * sa, axis[2] * sa, cos(angle));
}
inline Quaternion Quaternion::createForX(double angle)
{
angle *= 0.5f;
return Quaternion(sin(angle), 0, 0, cos(angle));
}
inline Quaternion Quaternion::createForY(double angle)
{
angle *= 0.5f;
return Quaternion(0, sin(angle), 0, cos(angle));
}
inline Quaternion Quaternion::createForZ(double angle)
{
angle *= 0.5f;
return Quaternion(0, 0, sin(angle), cos(angle));
}
inline Quaternion Quaternion::getMultipliedBy(const Quaternion& other) const
{
return Quaternion(
w() * other.x() + x() * other.w() + y() * other.z() - z() * other.y(),
w() * other.y() + y() * other.w() + z() * other.x() - x() * other.z(),
w() * other.z() + z() * other.w() + x() * other.y() - y() * other.x(),
w() * other.w() - x() * other.x() - y() * other.y() - z() * other.z()
);
}
inline void Quaternion::multiplyBy(const Quaternion& other)
{
*this = getMultipliedBy(other);
}
inline void Quaternion::preMultiplyBy(const Quaternion& other)
{
*this = other.getMultipliedBy(*this);
}
inline Quaternion Quaternion::getInverse() const
{
return Quaternion(-getVector3(), w());
}
inline void Quaternion::conjugate()
{
*this = getInverse();
}
inline Quaternion Quaternion::getNormalised() const
{
const double n = 1.0f / sqrt(x() * x() + y() * y() + z() * z() + w() * w());
return Quaternion(x() * n, y() * n, z() * n, w() * n);
}
inline void Quaternion::normalise()
{
*this = getNormalised();
}
inline Vector3 Quaternion::transformPoint(const Vector3& point) const
{
double xx = x() * x();
double yy = y() * y();
double zz = z() * z();
double ww = w() * w();
double xy2 = x() * y() * 2;
double xz2 = x() * z() * 2;
double xw2 = x() * w() * 2;
double yz2 = y() * z() * 2;
double yw2 = y() * w() * 2;
double zw2 = z() * w() * 2;
return Vector3(
ww * point.x() + yw2 * point.z() - zw2 * point.y() + xx * point.x() + xy2 * point.y() + xz2 * point.z() - zz * point.x() - yy * point.x(),
xy2 * point.x() + yy * point.y() + yz2 * point.z() + zw2 * point.x() - zz * point.y() + ww * point.y() - xw2 * point.z() - xx * point.y(),
xz2 * point.x() + yz2 * point.y() + zz * point.z() - yw2 * point.x() - yy * point.z() + xw2 * point.y() - xx * point.z() + ww * point.z()
);
}
const double c_half_sqrt2 = 0.70710678118654752440084436210485;
const float c_half_sqrt2f = static_cast<float>(c_half_sqrt2);
/// Stream insertion for Quaternion
inline std::ostream& operator<< (std::ostream& s, const Quaternion& q)
{
return s << "Quaternion(x=" << q.x() << ", y=" << q.y() << ", z=" << q.z()
<< ", w=" << q.w() << ")";
}