Revision 0ecc7129

View differences:

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