summaryrefslogtreecommitdiff
path: root/ucoo/math/quaternion.tcc
blob: 1db845b66c53769f9975424da569fee568b65ef8 (plain)
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
#ifndef ucoo_math_quaternion_tcc
#define ucoo_math_quaternion_tcc
// ucoolib - Microcontroller object oriented library. {{{
//
// Copyright (C) 2015 Nicolas Schodet
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// }}}
#include <cmath>

namespace ucoo {

template<typename T>
constexpr
Quaternion<T>::Quaternion ()
{
}

template<typename T>
constexpr
Quaternion<T>::Quaternion (T w_, T x_, T y_, T z_)
    : w (w_), x (x_), y (y_), z (z_)
{
}

template<typename T>
Quaternion<T>::Quaternion (const YawPitchRoll<T> &ypr)
{
    T cyaw = std::cos (ypr.yaw / 2);
    T syaw = std::sin (ypr.yaw / 2);
    T cpitch = std::cos (ypr.pitch / 2);
    T spitch = std::sin (ypr.pitch / 2);
    T croll = std::cos (ypr.roll / 2);
    T sroll = std::sin (ypr.roll / 2);
    w = cpitch * croll * cyaw + spitch * sroll * syaw;
    x = cpitch * cyaw * sroll - croll * spitch * syaw;
    y = cpitch * sroll * syaw + croll * cyaw * spitch;
    z = cpitch * croll * syaw - cyaw * spitch * sroll;
}

template<typename T>
vect3d<T>
Quaternion<T>::rotate (const vect3d<T> &v) const
{
    T nx = w*w*v.x + 2*y*w*v.z - 2*z*w*v.y + x*x*v.x
        + 2*y*x*v.y + 2*z*x*v.z - z*z*v.x - y*y*v.x;
    T ny = 2*x*y*v.x + y*y*v.y + 2*z*y*v.z + 2*w*z*v.x
        - z*z*v.y + w*w*v.y - 2*x*w*v.z - x*x*v.y;
    T nz = 2*x*z*v.x + 2*y*z*v.y + z*z*v.z - 2*w*y*v.x
        - y*y*v.z + 2*w*x*v.y - x*x*v.z + w*w*v.z;
    return vect3d<T> (nx, ny, nz);
}

template<typename T>
T
Quaternion<T>::yaw () const
{
    return std::atan2 (2 * (x * y + w * z), w * w + x * x - y * y - z * z);
}

template<typename T>
T
Quaternion<T>::pitch () const
{
    return std::asin (-2 * (x * z - w * y));
}

template<typename T>
T
Quaternion<T>::roll () const
{
    return std::atan2 (2 * (y * z + w * x), w * w - x * x - y * y + z * z);
}

template<typename T>
Quaternion<T>
Quaternion<T>::operator* (const Quaternion<T> &r) const
{
    return Quaternion<T> (
        w * r.w - x * r.x - y * r.y - z * r.z,
        w * r.x + x * r.w + y * r.z - z * r.y,
        w * r.y - x * r.z + y * r.w + z * r.x,
        w * r.z + x * r.y - y * r.x + z * r.w
        );
}

} // namespace ucoo

#endif // ucoo_math_quaternion_tcc