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
107
108
109
110
111
112
113
114
|
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __dgQuaternion__
#define __dgQuaternion__
#include "dgStdafx.h"
class dgVector;
class dgMatrix;
DG_MSC_VECTOR_ALIGMENT
class dgQuaternion {
public:
dgQuaternion();
dgQuaternion(const dgMatrix &matrix);
dgQuaternion(dgFloat32 q0, dgFloat32 q1, dgFloat32 q2, dgFloat32 q3);
dgQuaternion(const dgVector &unit_Axis, dgFloat32 angle = dgFloat32(0.0f));
void Scale(dgFloat32 scale);
void Normalize();
inline dgFloat32 DotProduct(const dgQuaternion &QB) const;
dgQuaternion Inverse() const;
dgQuaternion Slerp(const dgQuaternion &q1, dgFloat32 t) const;
dgVector CalcAverageOmega(const dgQuaternion &q1, dgFloat32 dt) const;
dgQuaternion operator* (const dgQuaternion &B) const;
dgQuaternion operator+ (const dgQuaternion &B) const;
dgQuaternion operator- (const dgQuaternion &B) const;
dgFloat32 m_q0;
dgFloat32 m_q1;
dgFloat32 m_q2;
dgFloat32 m_q3;
} DG_GCC_VECTOR_ALIGMENT;
inline dgQuaternion::dgQuaternion() {
m_q0 = dgFloat32(1.0f);
m_q1 = dgFloat32(0.0f);
m_q2 = dgFloat32(0.0f);
m_q3 = dgFloat32(0.0f);
}
inline dgQuaternion::dgQuaternion(dgFloat32 Q0, dgFloat32 Q1, dgFloat32 Q2, dgFloat32 Q3) {
m_q0 = Q0;
m_q1 = Q1;
m_q2 = Q2;
m_q3 = Q3;
// NEWTON_ASSERT (dgAbsf (DotProduct (*this) -dgFloat32 (1.0f)) < dgFloat32(1.0e-4f));
}
inline void dgQuaternion::Scale(dgFloat32 scale) {
m_q0 *= scale;
m_q1 *= scale;
m_q2 *= scale;
m_q3 *= scale;
}
inline void dgQuaternion::Normalize() {
Scale(dgRsqrt(DotProduct(*this)));
}
inline dgFloat32 dgQuaternion::DotProduct(const dgQuaternion &QB) const {
return m_q0 * QB.m_q0 + m_q1 * QB.m_q1 + m_q2 * QB.m_q2 + m_q3 * QB.m_q3;
}
inline dgQuaternion dgQuaternion::Inverse() const {
return dgQuaternion(m_q0, -m_q1, -m_q2, -m_q3);
}
inline dgQuaternion dgQuaternion::operator+ (const dgQuaternion &B) const {
return dgQuaternion(m_q0 + B.m_q0, m_q1 + B.m_q1, m_q2 + B.m_q2, m_q3 + B.m_q3);
}
inline dgQuaternion dgQuaternion::operator- (const dgQuaternion &B) const {
return dgQuaternion(m_q0 - B.m_q0, m_q1 - B.m_q1, m_q2 - B.m_q2, m_q3 - B.m_q3);
}
inline dgQuaternion dgQuaternion::operator* (const dgQuaternion &B) const {
return dgQuaternion(B.m_q0 * m_q0 - B.m_q1 * m_q1 - B.m_q2 * m_q2 - B.m_q3 * m_q3,
B.m_q1 * m_q0 + B.m_q0 * m_q1 - B.m_q3 * m_q2 + B.m_q2 * m_q3,
B.m_q2 * m_q0 + B.m_q3 * m_q1 + B.m_q0 * m_q2 - B.m_q1 * m_q3,
B.m_q3 * m_q0 - B.m_q2 * m_q1 + B.m_q1 * m_q2 + B.m_q0 * m_q3);
}
#endif
|
