1 | /* |
---|
2 | |
---|
3 | Brian Curless |
---|
4 | |
---|
5 | Computer Graphics Laboratory |
---|
6 | Stanford University |
---|
7 | |
---|
8 | --------------------------------------------------------------------- |
---|
9 | |
---|
10 | Copyright (1997) The Board of Trustees of the Leland Stanford Junior |
---|
11 | University. Except for commercial resale, lease, license or other |
---|
12 | commercial transactions, permission is hereby given to use, copy, |
---|
13 | modify this software for academic purposes only. No part of this |
---|
14 | software or any derivatives thereof may be used in the production of |
---|
15 | computer models for resale or for use in a commercial |
---|
16 | product. STANFORD MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY KIND |
---|
17 | CONCERNING THIS SOFTWARE. No support is implied or provided. |
---|
18 | |
---|
19 | */ |
---|
20 | |
---|
21 | |
---|
22 | #include "Quaternion.h" |
---|
23 | #include "Matrix4f.h" |
---|
24 | |
---|
25 | #define X 0 |
---|
26 | #define Y 1 |
---|
27 | #define Z 2 |
---|
28 | #define W 3 |
---|
29 | |
---|
30 | |
---|
31 | Quaternion::Quaternion() |
---|
32 | { |
---|
33 | q[0] = 0; |
---|
34 | q[1] = 0; |
---|
35 | q[2] = 0; |
---|
36 | q[3] = 1; |
---|
37 | } |
---|
38 | |
---|
39 | Quaternion::~Quaternion() |
---|
40 | { |
---|
41 | } |
---|
42 | |
---|
43 | |
---|
44 | void |
---|
45 | Quaternion::setValue(float q0, float q1, float q2, float q3) |
---|
46 | { |
---|
47 | q[0] = q0; |
---|
48 | q[1] = q1; |
---|
49 | q[2] = q2; |
---|
50 | q[3] = q3; |
---|
51 | } |
---|
52 | |
---|
53 | void |
---|
54 | Quaternion::setValue(float *quat) |
---|
55 | { |
---|
56 | q[0] = quat[0]; |
---|
57 | q[1] = quat[1]; |
---|
58 | q[2] = quat[2]; |
---|
59 | q[3] = quat[3]; |
---|
60 | } |
---|
61 | |
---|
62 | |
---|
63 | void |
---|
64 | Quaternion::setValue(Quaternion &quat) |
---|
65 | { |
---|
66 | q[0] = quat.q[0]; |
---|
67 | q[1] = quat.q[1]; |
---|
68 | q[2] = quat.q[2]; |
---|
69 | q[3] = quat.q[3]; |
---|
70 | } |
---|
71 | |
---|
72 | |
---|
73 | /****************************************************************************** |
---|
74 | Convert a quaternion into a rotation matrix. Does *not* assume a unit |
---|
75 | quaternion. From Ken Shoemake. |
---|
76 | |
---|
77 | ******************************************************************************/ |
---|
78 | void |
---|
79 | Quaternion::toMatrix(Matrix4f &mat) |
---|
80 | { |
---|
81 | float s; |
---|
82 | float xs,ys,zs; |
---|
83 | float wx,wy,wz; |
---|
84 | float xx,xy,xz; |
---|
85 | float yy,yz,zz; |
---|
86 | |
---|
87 | /* for unit q, just set s = 2 or set xs = this->q[X] + this->q[X], etc. */ |
---|
88 | |
---|
89 | s = 2 / (this->q[X]*this->q[X] + this->q[Y]*this->q[Y] |
---|
90 | + this->q[Z]*this->q[Z] + this->q[W]*this->q[W]); |
---|
91 | |
---|
92 | xs = this->q[X] * s; |
---|
93 | ys = this->q[Y] * s; |
---|
94 | zs = this->q[Z] * s; |
---|
95 | |
---|
96 | wx = this->q[W] * xs; |
---|
97 | wy = this->q[W] * ys; |
---|
98 | wz = this->q[W] * zs; |
---|
99 | |
---|
100 | xx = this->q[X] * xs; |
---|
101 | xy = this->q[X] * ys; |
---|
102 | xz = this->q[X] * zs; |
---|
103 | |
---|
104 | yy = this->q[Y] * ys; |
---|
105 | yz = this->q[Y] * zs; |
---|
106 | zz = this->q[Z] * zs; |
---|
107 | |
---|
108 | mat.m[X][X] = 1 - (yy + zz); |
---|
109 | mat.m[X][Y] = xy - wz; |
---|
110 | mat.m[X][Z] = xz + wy; |
---|
111 | mat.m[X][W] = 0; |
---|
112 | |
---|
113 | mat.m[Y][X] = xy + wz; |
---|
114 | mat.m[Y][Y] = 1 - (xx + zz); |
---|
115 | mat.m[Y][Z] = yz - wx; |
---|
116 | mat.m[Y][W] = 0; |
---|
117 | |
---|
118 | mat.m[Z][X] = xz - wy; |
---|
119 | mat.m[Z][Y] = yz + wx; |
---|
120 | mat.m[Z][Z] = 1 - (xx + yy); |
---|
121 | mat.m[Z][W] = 0; |
---|
122 | |
---|
123 | mat.m[W][X] = 0; |
---|
124 | mat.m[W][Y] = 0; |
---|
125 | mat.m[W][Z] = 0; |
---|
126 | mat.m[W][W] = 1; |
---|
127 | } |
---|
128 | |
---|
129 | |
---|
130 | /****************************************************************************** |
---|
131 | Convert a rotation mat.mrix into a unit quaternion. From Ken Shoemake. |
---|
132 | |
---|
133 | ******************************************************************************/ |
---|
134 | void |
---|
135 | Quaternion::fromMatrix(Matrix4f &mat) |
---|
136 | { |
---|
137 | int i,j,k; |
---|
138 | float tr,s; |
---|
139 | static int nxt[3] = {Y, Z, X}; |
---|
140 | |
---|
141 | tr = mat.m[X][X] + mat.m[Y][Y] + mat.m[Z][Z]; |
---|
142 | |
---|
143 | if (tr > 0) { |
---|
144 | s = sqrt (tr + 1); |
---|
145 | this->q[W] = s * 0.5; |
---|
146 | s = 0.5 / s; |
---|
147 | this->q[X] = (mat.m[Z][Y] - mat.m[Y][Z]) * s; |
---|
148 | this->q[Y] = (mat.m[X][Z] - mat.m[Z][X]) * s; |
---|
149 | this->q[Z] = (mat.m[Y][X] - mat.m[X][Y]) * s; |
---|
150 | } |
---|
151 | else { |
---|
152 | i = X; |
---|
153 | if (mat.m[Y][Y] > mat.m[X][X]) |
---|
154 | i = Y; |
---|
155 | if (mat.m[Z][Z] > mat.m[i][i]) |
---|
156 | i = Z; |
---|
157 | j = nxt[i]; |
---|
158 | k = nxt[j]; |
---|
159 | s = sqrt (1 + (mat.m[i][i] - (mat.m[j][j] + mat.m[k][k]))); |
---|
160 | this->q[i] = s * 0.5; |
---|
161 | s = 0.5 / s; |
---|
162 | this->q[W] = (mat.m[k][j] - mat.m[j][k]) * s; |
---|
163 | this->q[j] = (mat.m[j][i] + mat.m[i][j]) * s; |
---|
164 | this->q[k] = (mat.m[k][i] + mat.m[i][k]) * s; |
---|
165 | } |
---|
166 | } |
---|
167 | |
---|
168 | |
---|
169 | void |
---|
170 | Quaternion::normalize() |
---|
171 | { |
---|
172 | float norm; |
---|
173 | norm = sqrt(this->q[X]*this->q[X] + this->q[Y]*this->q[Y] |
---|
174 | +this->q[Z]*this->q[Z] + this->q[W]*this->q[W]); |
---|
175 | |
---|
176 | if (norm != 0) { |
---|
177 | norm = 1/norm; |
---|
178 | this->q[X] = norm*this->q[X]; |
---|
179 | this->q[Y] = norm*this->q[Y]; |
---|
180 | this->q[Z] = norm*this->q[Z]; |
---|
181 | this->q[W] = norm*this->q[W]; |
---|
182 | } else { |
---|
183 | /* Uh oh. Do nothing? */ |
---|
184 | } |
---|
185 | } |
---|
186 | |
---|
187 | void |
---|
188 | Quaternion::toAxisAngle(float &angle, Vec3f &axis) |
---|
189 | { |
---|
190 | float norm; |
---|
191 | |
---|
192 | angle = acos(this->q[W])*360/M_PI; |
---|
193 | norm = sqrt(this->q[X]*this->q[X] |
---|
194 | + this->q[Y]*this->q[Y] + this->q[Z]*this->q[Z]); |
---|
195 | if (norm != 0) { |
---|
196 | norm = 1/norm; |
---|
197 | axis.setValue(this->q[X]*norm, |
---|
198 | this->q[Y]*norm, this->q[Z]*norm); |
---|
199 | } else { |
---|
200 | axis.setValue(0,0,1); |
---|
201 | } |
---|
202 | } |
---|
203 | |
---|