1 /* -*- c++ -*-
2 FILE: SQuat.cpp
3 RCS REVISION: $Revision: 1.8 $
4
5 COPYRIGHT: (c) 1999 -- 2003 Melinda Green, Don Hatch, and Jay Berkenbilt - Superliminal Software
6
7 LICENSE: Free to use and modify for non-commercial purposes as long as the
8 following conditions are adhered to:
9 1) Obvious credit for the source of this code and the designs it embodies
10 are clearly made, and
11 2) Ports and derived versions of 4D Magic Cube programs are not distributed
12 without the express written permission of the authors.
13
14 DESCRIPTION:
15 Implementation ofsimple quaternion library
16 */
17
18 #include "squat.h"
19 #include "vec.h"
20
21 #define X 0
22 #define Y 1
23 #define Z 2
24
25
Vector3()26 Vector3::Vector3 ()
27 {
28 // ZEROVEC3(elements);
29 }
30
Vector3(double x,double y,double z)31 Vector3::Vector3 (double x, double y, double z)
32 {
33 elements[X] = x;
34 elements[Y] = y;
35 elements[Z] = z;
36 }
37
Length() const38 double Vector3::Length() const
39 {
40 return sqrt(NORMSQRD3(elements));
41 }
42
Normal() const43 Vector3 Vector3::Normal() const
44 {
45 Vector3 norm(*this);
46 double len = Length();
47 VDS3(norm, norm, len);
48 return norm;
49 }
50
InitFromVector(const Vector3 & a,double r)51 void SQuat::InitFromVector(const Vector3 &a, double r)
52 {
53 Vector3 normvec(a.Normal());
54 double half_angle = r * 0.5;
55 double sin_half_angle = sin(half_angle);
56 rot = cos(half_angle);
57 VXS3(axis, normvec, sin_half_angle);
58 }
59
SQuat(const Vector3 & a,double r,int homo_vals)60 SQuat::SQuat (const Vector3 &a, double r, int homo_vals)
61 {
62 if (homo_vals)
63 {
64 axis = a;
65 rot = r;
66 }
67 else
68 InitFromVector(a, r);
69 }
70
SQuat(double x,double y,double z,double r,int homo_vals)71 SQuat::SQuat (double x, double y, double z, double r, int homo_vals)
72 {
73 if (homo_vals)
74 {
75 rot = r;
76 axis = Vector3 (x, y, z);
77 }
78 else
79 InitFromVector(Vector3 (x, y, z), r);
80 }
81
82
SQuat(const SQuat & q)83 SQuat::SQuat (const SQuat &q) :
84 axis(q.axis),
85 rot(q.rot)
86 {
87 }
88
SQuat(const Matrix3 & m)89 SQuat::SQuat (const Matrix3 &m)
90 {
91 double trace = TRACE3(m);
92 #ifdef PREMULT
93 if (trace > 0.0)
94 {
95 double s = sqrt(trace + 1.0);
96 rot = s * 0.5;
97 s = 0.5 / s;
98 const Vector3 a(m[2][1] - m[1][2],
99 m[0][2] - m[2][0], m[1][0] - m[0][1]);
100 VXS3(axis, a, s);
101 }
102 else
103 {
104 int i, j, k;
105 i = 0;
106 if (m[1][1] > m[0][0])
107 i = 1;
108 if (m[2][2] > m[i][i])
109 i = 2;
110 j = (i + 1) % 3;
111 k = (i + 2) % 3;
112 double s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
113 axis[i] = s * 0.5;
114 s = 0.5 / s;
115 rot = s * (m[k][j] - m[j][k]);
116 axis[j] = s * m[j][i] + m[i][j];
117 axis[k] = s * m[k][i] + m[i][k];
118 }
119 #else
120 if (trace > 0.0)
121 {
122 double s = sqrt(trace + 1.0);
123 rot = s * 0.5;
124 s = 0.5 / s;
125 const Vector3 a(m[1][2] - m[2][1],
126 m[2][0] - m[0][2], m[0][1] - m[1][0]);
127 VXS3(axis, a, s);
128 }
129 else
130 {
131 int i, j, k;
132 i = 0;
133 if (m[1][1] > m[0][0])
134 i = 1;
135 if (m[2][2] > m[i][i])
136 i = 2;
137 j = (i + 1) % 3;
138 k = (i + 2) % 3;
139 double s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
140 axis[i] = s * 0.5;
141 s = 0.5 / s;
142 rot = s * (m[j][k] - m[k][j]);
143 axis[j] = s * m[i][j] + m[j][i];
144 axis[k] = s * m[i][k] + m[k][i];
145 }
146 #endif
147 }
148
149
operator *(const SQuat & q) const150 SQuat SQuat::operator * (const SQuat &q) const
151 {
152 Vector3 v1, v2, scaled_sum, cross, new_axis;
153 VXS3(v1, q.axis, rot);
154 VXS3(v2, axis, q.rot);
155 VPV3(scaled_sum, v1, v2); // scaled_sum = rot*q.axis +
156 // q.rot*axis
157 #ifdef PREMULT
158 VXV3(cross, axis, q.axis);
159 #else
160 VXV3(cross, q.axis, axis);
161 #endif
162 VPV3(new_axis, scaled_sum, cross); // new_axis =
163 // scaled_sum + cross
164 return SQuat (new_axis, rot * q.rot - DOT3(axis, q.axis), 1);
165 }
166
167
operator *=(const SQuat & q)168 SQuat &SQuat::operator *= (const SQuat &q)
169 {
170 return *this = *this * q;
171 }
172
GetRotation() const173 double SQuat::GetRotation() const
174 {
175 return 2.0 * acos(rot);
176 }
177
GetAxis(Vector3 & v) const178 void SQuat::GetAxis(Vector3 &v) const
179 {
180 double theta = GetRotation();
181 double f = 1.0 / sin(theta * 0.5);
182 VXS3(v, axis, f);
183 }
184
GetAxis() const185 Vector3 SQuat::GetAxis() const
186 {
187 Vector3 vec;
188 GetAxis(vec);
189 return vec;
190 }
191
GetHomoRotation() const192 double SQuat::GetHomoRotation() const
193 {
194 return rot;
195 }
196
GetHomoAxis(Vector3 & v) const197 void SQuat::GetHomoAxis(Vector3 &v) const
198 {
199 SET3(v, axis);
200 }
201
Matrix3()202 Matrix3::Matrix3 ()
203 {
204 IDENTMAT3(rows);
205 }
206
Matrix3(const Matrix3 & from)207 Matrix3::Matrix3 (const Matrix3 &from)
208 {
209 SET3(rows, from.rows);
210 }
211
Matrix3(const Vector3 & x,const Vector3 & y,const Vector3 & z)212 Matrix3::Matrix3 (const Vector3 &x, const Vector3 &y, const Vector3 &z)
213 {
214 rows[X] = x;
215 rows[Y] = y;
216 rows[Z] = z;
217 }
218
Matrix3(const SQuat & q)219 Matrix3::Matrix3 (const SQuat &q)
220 {
221 double W = q.GetHomoRotation();
222 Vector3 axis;
223 q.GetHomoAxis(axis);
224
225 double XX = axis[X] + axis[X];
226 double YY = axis[Y] + axis[Y];
227 double ZZ = axis[Z] + axis[Z];
228
229 double WXX = W * XX;
230 double XXX = axis[X] * XX;
231
232 double WYY = W * YY;
233 double XYY = axis[X] * YY;
234 double YYY = axis[Y] * YY;
235
236 double WZZ = W * ZZ;
237 double XZZ = axis[X] * ZZ;
238 double YZZ = axis[Y] * ZZ;
239 double ZZZ = axis[Z] * ZZ;
240
241 #ifdef PREMULT
242 rows[X][X] = 1.0 - (YYY + ZZZ);
243 rows[X][Y] = XYY - WZZ;
244 rows[X][Z] = XZZ + WYY;
245
246 rows[Y][X] = XYY + WZZ;
247 rows[Y][Y] = 1.0 - (XXX + ZZZ);
248 rows[Y][Z] = YZZ - WXX;
249
250 rows[Z][X] = XZZ - WYY;
251 rows[Z][Y] = YZZ + WXX;
252 rows[Z][Z] = 1.0 - (XXX + YYY);
253 #else
254 rows[X][X] = 1.0 - (YYY + ZZZ);
255 rows[Y][X] = XYY - WZZ;
256 rows[Z][X] = XZZ + WYY;
257
258 rows[X][Y] = XYY + WZZ;
259 rows[Y][Y] = 1.0 - (XXX + ZZZ);
260 rows[Z][Y] = YZZ - WXX;
261
262 rows[X][Z] = XZZ - WYY;
263 rows[Y][Z] = YZZ + WXX;
264 rows[Z][Z] = 1.0 - (XXX + YYY);
265 #endif
266 }
267
268 // Local Variables:
269 // c-basic-offset: 4
270 // c-comment-only-line-offset: 0
271 // c-file-offsets: ((defun-block-intro . +) (block-open . 0) (substatement-open . 0) (statement-cont . +) (statement-case-open . +4) (arglist-intro . +) (arglist-close . +) (inline-open . 0))
272 // indent-tabs-mode: nil
273 // End:
274