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