My Project: dynamics.mod/NewtonDynamics/packages/dMath/dQuaternion.h Source File

My Project

dQuaternion.h
Go to the documentation of this file.
1 /* Copyright (c) <2009> <Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely
10 */
11
12
13
14 #ifndef __dQuaternion__
15 #define __dQuaternion__
16
17 #include "dVector.h"
18 class dMatrix;
19
20 D_MSC_VECTOR_ALIGMENT
21 class dQuaternion
22 {
23 public:
24 dQuaternion ();
25 dQuaternion (const dMatrix& matrix);
26 dQuaternion (dFloat q0, dFloat q1, dFloat q2, dFloat q3);
27 dQuaternion (const dVector& unit_Axis, dFloat Angle = 0.0f);
28
29 void Scale (dFloat scale);
30 void Normalize ();
31 inline dFloat DotProduct (const dQuaternion& QB) const;
32 dQuaternion Inverse () const;
33
34 dVector RotateVector (const dVector& point) const;
35 dVector UnrotateVector (const dVector& point) const;
36
37 //dVector GetEulerAngles (dEulerAngleOrder order = m_pitchYawRoll) const;
38 void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
39 dVector CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const;
40 dQuaternion Slerp (const dQuaternion &q1, dFloat t) const;
41 dQuaternion IntegrateOmega (const dVector& omega, dFloat timestep) const;
42
43 dQuaternion operator* (const dQuaternion &B) const;
44 dQuaternion operator+ (const dQuaternion &B) const;
45 dQuaternion operator- (const dQuaternion &B) const;
46
47 dFloat m_q0;
48 dFloat m_q1;
49 dFloat m_q2;
50 dFloat m_q3;
51 };
52
53
54
55
56 inline dQuaternion::dQuaternion ()
57 {
58 m_q0 = 1.0f;
59 m_q1 = 0.0f;
60 m_q2 = 0.0f;
61 m_q3 = 0.0f;
62 }
63
64 inline dQuaternion::dQuaternion (dFloat Q0, dFloat Q1, dFloat Q2, dFloat Q3)
65 {
66 m_q0 = Q0;
67 m_q1 = Q1;
68 m_q2 = Q2;
69 m_q3 = Q3;
70 // dAssert (dAbs (DotProduct (*this) - 1.0f) < 1.0e-4f);
71 }
72
73
74
75 inline void dQuaternion::Scale (dFloat scale)
76 {
77 m_q0 *= scale;
78 m_q1 *= scale;
79 m_q2 *= scale;
80 m_q3 *= scale;
81 }
82
83 inline void dQuaternion::Normalize ()
84 {
85 Scale (1.0f / dSqrt (DotProduct (*this)));
86 }
87
88 inline dFloat dQuaternion::DotProduct (const dQuaternion &QB) const
89 {
90 return m_q0 * QB.m_q0 + m_q1 * QB.m_q1 + m_q2 * QB.m_q2 + m_q3 * QB.m_q3;
91 }
92
93 inline dQuaternion dQuaternion::Inverse () const
94 {
95 return dQuaternion (m_q0, -m_q1, -m_q2, -m_q3);
96 }
97
98 /*
99 inline dQuaternion operator+ (const dQuaternion &A, const dQuaternion &B)
100 {
101 return dQuaternion (A.m_q0 + B.m_q0, A.m_q1 + B.m_q1, A.m_q2 + B.m_q2, A.m_q3 + B.m_q3);
102 }
103
104 inline dQuaternion operator- (const dQuaternion &A, const dQuaternion &B)
105 {
106 return dQuaternion (A.m_q0 - B.m_q0, A.m_q1 - B.m_q1, A.m_q2 - B.m_q2, A.m_q3 - B.m_q3);
107 }
108 */
109
110 inline dQuaternion dQuaternion::operator+ (const dQuaternion &B) const
111 {
112 return dQuaternion (m_q0 + B.m_q0, m_q1 + B.m_q1, m_q2 + B.m_q2, m_q3 + B.m_q3);
113 }
114
115 inline dQuaternion dQuaternion::operator- (const dQuaternion &B) const
116 {
117 return dQuaternion (m_q0 - B.m_q0, m_q1 - B.m_q1, m_q2 - B.m_q2, m_q3 - B.m_q3);
118 }
119
120
121 inline dQuaternion dQuaternion::operator* (const dQuaternion &B) const
122 {
123 return dQuaternion (B.m_q0 * m_q0 - B.m_q1 * m_q1 - B.m_q2 * m_q2 - B.m_q3 * m_q3,
124 B.m_q1 * m_q0 + B.m_q0 * m_q1 - B.m_q3 * m_q2 + B.m_q2 * m_q3,
125 B.m_q2 * m_q0 + B.m_q3 * m_q1 + B.m_q0 * m_q2 - B.m_q1 * m_q3,
126 B.m_q3 * m_q0 - B.m_q2 * m_q1 + B.m_q1 * m_q2 + B.m_q0 * m_q3);
127 }
128
129 /*
130 inline dVector dQuaternion::GetXYZ_EulerAngles() const
131 {
132 dFloat pitch;
133 dFloat yaw;
134 dFloat roll;
135 dFloat val = 2.0f * (m_q2 * m_q0 - m_q3 * m_q1);
136 if (val >= 0.99995f) {
137 pitch = 2.0f * atan2(m_q1, m_q0);
138 yaw = (0.5f * 3.141592f);
139 roll = 0.0f;
140 } else if (val <= -0.99995f) {
141 pitch = 2.0f * atan2(m_q1, m_q0);
142 yaw = -(0.5f * 3.141592f);
143 roll = 0.0f;
144 } else {
145 yaw = dAsin (val);
146 pitch = dAtan2 (2.0f * (m_q1 * m_q0 + m_q3 * m_q2), 1.0f - 2.0f * (m_q1 * m_q1 + m_q2 * m_q2));
147 roll = dAtan2 (2.0f * (m_q3 * m_q0 + m_q1 * m_q2), 1.0f - 2.0f * (m_q2 * m_q2 + m_q3 * m_q3));
148 }
149 return dVector (pitch, yaw, roll);
150 }
151 */
152
153
154
155 #endif
156
dQuaternion::Inverse
dQuaternion Inverse() const
Definition: dQuaternion.h:93
dQuaternion::DotProduct
dFloat DotProduct(const dQuaternion &QB) const
Definition: dQuaternion.h:88
dQuaternion::m_q1
dFloat m_q1
Definition: dQuaternion.h:48
dQuaternion::m_q0
dFloat m_q0
Definition: dQuaternion.h:47
dMatrix
Definition: dMatrix.h:28
dQuaternion::UnrotateVector
dVector UnrotateVector(const dVector &point) const
Definition: dQuaternion.cpp:181
dQuaternion::m_q3
dFloat m_q3
Definition: dQuaternion.h:50
dQuaternion::GetEulerAngles
void GetEulerAngles(dVector &euler1, dVector &euler2, dEulerAngleOrder order=m_pitchYawRoll) const
Definition: dQuaternion.cpp:188
dQuaternion::Normalize
void Normalize()
Definition: dQuaternion.h:83
dFloat
float dFloat
Definition: dMathDefines.h:27
dEulerAngleOrder
dEulerAngleOrder
Definition: dMathDefines.h:54
dQuaternion::m_q2
dFloat m_q2
Definition: dQuaternion.h:49
dSqrt
#define dSqrt(x)
Definition: dMathDefines.h:38
dQuaternion::Scale
void Scale(dFloat scale)
Definition: dQuaternion.h:75
dQuaternion::operator-
dQuaternion operator-(const dQuaternion &B) const
Definition: dQuaternion.h:115
dQuaternion::Slerp
dQuaternion Slerp(const dQuaternion &q1, dFloat t) const
Definition: dQuaternion.cpp:118
dQuaternion::dQuaternion
dQuaternion()
Definition: dQuaternion.h:56
dQuaternion::IntegrateOmega
dQuaternion IntegrateOmega(const dVector &omega, dFloat timestep) const
Definition: dQuaternion.cpp:196
m_pitchYawRoll
Definition: dMathDefines.h:56
dQuaternion::CalcAverageOmega
dVector CalcAverageOmega(const dQuaternion &q1, dFloat invdt) const
Definition: dQuaternion.cpp:96
D_MSC_VECTOR_ALIGMENT
#define D_MSC_VECTOR_ALIGMENT
Definition: dMathDefines.h:52
dQuaternion::RotateVector
dVector RotateVector(const dVector &point) const
Definition: dQuaternion.cpp:175
dQuaternion
Definition: dQuaternion.h:21
timestep
dgFloat32 timestep
Definition: dgBallConstraint.h:34
dQuaternion::operator+
dQuaternion operator+(const dQuaternion &B) const
Definition: dQuaternion.h:110
dVector.h
dVector
Definition: dVector.h:54
dQuaternion::operator*
dQuaternion operator*(const dQuaternion &B) const
Definition: dQuaternion.h:121