]>
Dogcows Code - chaz/yoink/blob - src/quaternion.hh
2 /*******************************************************************************
4 Copyright (c) 2009, Charles McGarvey
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
10 * Redistributions of source code must retain the above copyright notice,
11 this list of conditions and the following disclaimer.
12 * Redistributions in binary form must reproduce the above copyright notice,
13 this list of conditions and the following disclaimer in the documentation
14 and/or other materials provided with the distribution.
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
20 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 *******************************************************************************/
29 #ifndef _QUATERNION_HH_
30 #define _QUATERNION_HH_
40 // Quaternion -> 3x3 Matrix
42 // | w^2 + x^2 - y^2 - z^2 2xy - 2wz 2xz + 2yw |
43 // | 2xy + 2wz w^2 - x^2 + y^2 - z^2 2yz - 2wx |
44 // | 2xz - 2wy 2yz - 2wx w^2 - x^2 - y^2 + z^2 |
55 quaternion(scalar X
, scalar Y
, scalar Z
, scalar W
) {
56 vec
.x
= X
; vec
.y
= Y
; vec
.z
= Z
; w
= W
;
58 quaternion(vector3 v
, scalar W
= 1.0) {
61 quaternion(scalar q
[4]) {
62 vec
.x
= q
[0]; vec
.y
= q
[1]; vec
.z
= q
[2]; w
= q
[3];
65 // Accessible by index.
66 const scalar
& operator [] (int i
) const {
67 assert(i
>= 0 && i
<= 3 && "Index into quaternion out of bounds.");
68 return *((&vec
.x
) + i
);
70 scalar
& operator [] (int i
) {
71 assert(i
>= 0 && i
<= 3 && "Index into quaternion out of bounds.");
73 return *((&vec
.x
) + i
);
77 quaternion
operator + (const quaternion
& q
) const {
78 return quaternion(vec
+ q
.vec
, w
+ q
.w
);
80 quaternion
operator - (const quaternion
& q
) const {
81 return quaternion(vec
- q
.vec
, w
- q
.w
);
83 quaternion
operator * (const quaternion
& q
) const {
84 return quaternion(q
.w
* vec
+ w
* q
.vec
+ q
.vec
.cross(vec
),
85 w
* q
.w
- q
.vec
.dot(vec
));
87 quaternion
operator * (scalar s
) const {
88 return quaternion(vec
* s
, w
* s
);
90 quaternion
operator / (scalar s
) const {
91 return quaternion(vec
/ s
, w
/ s
);
94 quaternion
& operator += (const quaternion
& q
) {
95 vec
+= q
.vec
; w
+= q
.w
;
98 quaternion
& operator -= (const quaternion
& q
) {
99 vec
-= q
.vec
; w
-= q
.w
;
102 quaternion
& operator *= (const quaternion
& q
) {
104 w
= W
* q
.w
- q
.vec
.dot(vec
);
105 vec
= q
.w
* vec
+ W
* q
.vec
+ q
.vec
.cross(vec
);
108 quaternion
& operator *= (scalar s
) {
112 quaternion
& operator /= (scalar s
) {
118 quaternion
operator - () const {
119 return quaternion(-vec
, -w
);
122 // Quaternion operations.
123 quaternion
conjugate() const {
124 return quaternion(-vec
, w
);
126 void normalize(const vector3
& v
)
128 scalar len
= length();
129 if (len
!= 0.0) { *this /= len
; }
132 scalar
length2() const {
133 return vec
.x
* vec
.x
+ vec
.y
* vec
.y
+ vec
.z
* vec
.z
+ w
* w
;
135 scalar
length() const {
136 return std::sqrt(length2());
139 // Converting to other types.
140 matrix3
matrix() const {
141 scalar x2
= vec
.x
* vec
.x
;
142 scalar y2
= vec
.y
* vec
.y
;
143 scalar z2
= vec
.z
* vec
.z
;
145 scalar xy
= vec
.x
* vec
.y
;
146 scalar xz
= vec
.x
* vec
.z
;
147 scalar yzwx
= 2 * (vec
.y
* vec
.z
- w
* vec
.x
);
148 scalar wy
= w
* vec
.y
;
149 scalar wz
= w
* vec
.z
;
150 return matrix3(x2
- y2
- z2
+ w2
, 2 * (xy
- wz
), 2 * (xz
+ wy
),
151 2 * (xy
+ wz
), w2
- x2
+ y2
- z2
, yzwx
,
152 2 * (xz
- wy
), yzwx
, w2
- x2
- y2
+ z2
);
154 vector3
axis(scalar
& angle
) { // Axis of rotation, w/ angle of rotation.
155 vector3 axisVec
= axis();
156 angle
= 2 * std::acos(w
);
160 scalar sa
= std::sqrt(1 - w
* w
);
164 // Checking equality.
165 bool operator == (const quaternion
& q
) const {
166 return vec
== q
.vec
&& equals(w
,q
.w
);
168 bool operator != (const quaternion
& q
) const {
169 return !(*this == q
);
177 inline quaternion
operator * (scalar s
, const quaternion
& q
) {
180 inline quaternion
operator / (scalar s
, const quaternion
& q
) {
181 return quaternion(s
/ q
.vec
, s
/ q
.w
);
187 #endif // _QUATERNION_HH_
This page took 0.044155 seconds and 4 git commands to generate.