Revision 0ecc7129
utility/quaternion.h | ||
---|---|---|
32 | 32 |
namespace imu |
33 | 33 |
{ |
34 | 34 |
|
35 |
|
|
36 |
|
|
37 | 35 |
class Quaternion |
38 | 36 |
{ |
39 | 37 |
public: |
... | ... | |
105 | 103 |
*this = this->scale(1/mag); |
106 | 104 |
} |
107 | 105 |
|
108 |
|
|
109 |
Quaternion conjugate() const |
|
106 |
const Quaternion conjugate() const |
|
110 | 107 |
{ |
111 |
Quaternion q; |
|
112 |
q.w() = _w; |
|
113 |
q.x() = -_x; |
|
114 |
q.y() = -_y; |
|
115 |
q.z() = -_z; |
|
116 |
return q; |
|
108 |
return Quaternion(_w, -_x, -_y, -_z); |
|
117 | 109 |
} |
118 | 110 |
|
119 | 111 |
void fromAxisAngle(Vector<3> axis, double theta) |
... | ... | |
251 | 243 |
} |
252 | 244 |
|
253 | 245 |
|
254 |
Quaternion operator * (Quaternion q) const
|
|
246 |
const Quaternion operator*(const Quaternion& q) const
|
|
255 | 247 |
{ |
256 |
Quaternion ret;
|
|
257 |
ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
|
|
258 |
ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
|
|
259 |
ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
|
|
260 |
ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
|
|
261 |
return ret;
|
|
248 |
return Quaternion(
|
|
249 |
_w*q._w - _x*q._x - _y*q._y - _z*q._z,
|
|
250 |
_w*q._x + _x*q._w + _y*q._z - _z*q._y,
|
|
251 |
_w*q._y - _x*q._z + _y*q._w + _z*q._x,
|
|
252 |
_w*q._z + _x*q._y - _y*q._x + _z*q._w
|
|
253 |
);
|
|
262 | 254 |
} |
263 | 255 |
|
264 |
Quaternion operator + (Quaternion q) const
|
|
256 |
const Quaternion operator+(const Quaternion& q) const
|
|
265 | 257 |
{ |
266 |
Quaternion ret; |
|
267 |
ret._w = _w + q._w; |
|
268 |
ret._x = _x + q._x; |
|
269 |
ret._y = _y + q._y; |
|
270 |
ret._z = _z + q._z; |
|
271 |
return ret; |
|
258 |
return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); |
|
272 | 259 |
} |
273 | 260 |
|
274 |
Quaternion operator - (Quaternion q) const
|
|
261 |
const Quaternion operator-(const Quaternion& q) const
|
|
275 | 262 |
{ |
276 |
Quaternion ret; |
|
277 |
ret._w = _w - q._w; |
|
278 |
ret._x = _x - q._x; |
|
279 |
ret._y = _y - q._y; |
|
280 |
ret._z = _z - q._z; |
|
281 |
return ret; |
|
263 |
return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); |
|
282 | 264 |
} |
283 | 265 |
|
284 |
Quaternion operator / (float scalar) const
|
|
266 |
const Quaternion operator/(double scalar) const
|
|
285 | 267 |
{ |
286 |
Quaternion ret; |
|
287 |
ret._w = this->_w/scalar; |
|
288 |
ret._x = this->_x/scalar; |
|
289 |
ret._y = this->_y/scalar; |
|
290 |
ret._z = this->_z/scalar; |
|
291 |
return ret; |
|
268 |
return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); |
|
292 | 269 |
} |
293 | 270 |
|
294 |
Quaternion operator * (float scalar) const
|
|
271 |
const Quaternion operator*(double scalar) const
|
|
295 | 272 |
{ |
296 |
Quaternion ret; |
|
297 |
ret._w = this->_w*scalar; |
|
298 |
ret._x = this->_x*scalar; |
|
299 |
ret._y = this->_y*scalar; |
|
300 |
ret._z = this->_z*scalar; |
|
301 |
return ret; |
|
273 |
return scale(scalar); |
|
302 | 274 |
} |
303 | 275 |
|
304 |
Quaternion scale(double scalar) const |
|
276 |
const Quaternion scale(double scalar) const
|
|
305 | 277 |
{ |
306 |
Quaternion ret; |
|
307 |
ret._w = this->_w*scalar; |
|
308 |
ret._x = this->_x*scalar; |
|
309 |
ret._y = this->_y*scalar; |
|
310 |
ret._z = this->_z*scalar; |
|
311 |
return ret; |
|
278 |
return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); |
|
312 | 279 |
} |
313 | 280 |
|
314 | 281 |
private: |
315 | 282 |
double _w, _x, _y, _z; |
316 | 283 |
}; |
317 | 284 |
|
318 |
|
|
319 |
}; |
|
285 |
} // namespace |
|
320 | 286 |
|
321 | 287 |
#endif |
Also available in: Unified diff