2.50: svn merge https://svn.blender.org/svnroot/bf-blender/trunk/blender -r19323...
[blender.git] / intern / moto / include / MT_Quaternion.inl
1 #include "MT_Optimize.h"
2
3 GEN_INLINE MT_Quaternion& MT_Quaternion::operator*=(const MT_Quaternion& q) {
4     setValue(m_co[3] * q[0] + m_co[0] * q[3] + m_co[1] * q[2] - m_co[2] * q[1],
5              m_co[3] * q[1] + m_co[1] * q[3] + m_co[2] * q[0] - m_co[0] * q[2],
6              m_co[3] * q[2] + m_co[2] * q[3] + m_co[0] * q[1] - m_co[1] * q[0],
7              m_co[3] * q[3] - m_co[0] * q[0] - m_co[1] * q[1] - m_co[2] * q[2]);
8     return *this;
9 }
10
11 GEN_INLINE void MT_Quaternion::conjugate() {
12     m_co[0] = -m_co[0]; m_co[1] = -m_co[1]; m_co[2] = -m_co[2];
13 }
14
15 GEN_INLINE MT_Quaternion MT_Quaternion::conjugate() const {
16     return MT_Quaternion(-m_co[0], -m_co[1], -m_co[2], m_co[3]);
17 }
18   
19 GEN_INLINE void MT_Quaternion::invert() {
20     conjugate();
21     *this /= length2();
22 }
23
24 GEN_INLINE MT_Quaternion MT_Quaternion::inverse() const {
25     return conjugate() / length2();
26 }
27
28 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 
29 //       pg. 124-132
30 GEN_INLINE MT_Quaternion MT_Quaternion::random() {
31     MT_Scalar x0 = MT_random();
32     MT_Scalar r1 = sqrt(MT_Scalar(1.0) - x0), r2 = sqrt(x0);
33     MT_Scalar t1 = MT_2_PI * MT_random(), t2 = MT_2_PI * MT_random();
34     MT_Scalar c1 = cos(t1), s1 = sin(t1);
35     MT_Scalar c2 = cos(t2), s2 = sin(t2);
36     return MT_Quaternion(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
37 }
38
39 GEN_INLINE MT_Quaternion operator*(const MT_Quaternion& q1, 
40                                    const MT_Quaternion& q2) {
41     return MT_Quaternion(q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
42                          q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
43                          q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
44                          q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]); 
45 }
46
47 GEN_INLINE MT_Quaternion operator*(const MT_Quaternion& q, const MT_Vector3& w)
48 {
49     return MT_Quaternion( q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
50                           q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
51                           q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
52                          -q[0] * w[0] - q[1] * w[1] - q[2] * w[2]); 
53 }
54
55 GEN_INLINE MT_Quaternion operator*(const MT_Vector3& w, const MT_Quaternion& q)
56 {
57     return MT_Quaternion( w[0] * q[3] + w[1] * q[2] - w[2] * q[1],
58                           w[1] * q[3] + w[2] * q[0] - w[0] * q[2],
59                           w[2] * q[3] + w[0] * q[1] - w[1] * q[0],
60                          -w[0] * q[0] - w[1] * q[1] - w[2] * q[2]); 
61 }
62
63 GEN_INLINE MT_Scalar MT_Quaternion::angle(const MT_Quaternion& q) const 
64 {
65         MT_Scalar s = sqrt(length2() * q.length2());
66         assert(s != MT_Scalar(0.0));
67         
68         s = dot(q) / s;
69         
70         s = MT_clamp(s, -1.0, 1.0);
71         
72         return acos(s);
73 }
74
75 GEN_INLINE MT_Quaternion MT_Quaternion::slerp(const MT_Quaternion& q, const MT_Scalar& t) const
76 {
77         MT_Scalar d, s0, s1;
78         MT_Scalar s = dot(q);
79         bool neg = (s < 0.0);
80
81         if (neg)
82                 s = -s;
83         if ((1.0 - s) > 0.0001) 
84         {
85                 MT_Scalar theta = acos(s);
86                 d = MT_Scalar(1.0) / sin(theta);
87                 s0 = sin((MT_Scalar(1.0) - t) * theta);
88                 s1 = sin(t * theta);
89         }
90         else
91         {
92                 d = MT_Scalar(1.0);
93                 s0 = MT_Scalar(1.0) - t;
94                 s1 = t;
95         }
96         if (neg)
97                 s1 = -s1;
98         return d*(*this * s0 + q * s1);
99 }
100