SCL  1.0
Standard Control Library : Control, dynamics, physics, and simulation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Macros Groups Pages
TaoDeQuaternionf.h
1 /* Copyright (c) 2005 Arachi, Inc. and Stanford University. All rights reserved.
2  *
3  * Permission is hereby granted, free of charge, to any person obtaining
4  * a copy of this software and associated documentation files (the
5  * "Software"), to deal in the Software without restriction, including
6  * without limitation the rights to use, copy, modify, merge, publish,
7  * distribute, sublicense, and/or sell copies of the Software, and to
8  * permit persons to whom the Software is furnished to do so, subject
9  * to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included
12  * in all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
17  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
18  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
20  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21  */
22 
23 #ifndef _deQuaternionf_h
24 #define _deQuaternionf_h
25 
26 #ifdef __cplusplus
27 extern "C" {
28 #endif
29 
30 /*
31  * q = [w,v] = [cos(theta/2);, axis*sin(theta/2);] <--- axisAngle(axis,theta);
32  * q = [vx,vy,vz,w] --> float[4];
33  */
34 
35 extern void deColumnV3Q4S1(deFloat* res, const deFloat* q1, const int col);
36 extern void deConsistentSignQ4Q4Q4(deFloat* res, const deFloat* q1, const deFloat* q2);
37 extern void deSlerpQ4Q4Q4S2(deFloat* res, const deFloat* q1, const deFloat* q2, const deFloat t, const deFloat addedSpins);
38 extern void deAxisAngleV3S1Q4(deFloat* axis, deFloat *angle, const deFloat* q1);
39 extern void deSetQ4zyxV3(deFloat* res, const deFloat *v);
40 
41 /* inlines */
42 
43 DE_MATH_API void deSetQ4S4(deFloat* res, const deFloat x, const deFloat y, const deFloat z, const deFloat w)
44 {
45  res[0] = x; res[1] = y; res[2] = z; res[3] = w;
46 }
47 
48 DE_MATH_API void deIdentityQ4(deFloat* res)
49 {
50  res[0] = res[1] = res[2] = 0; res[3] = 1;
51 }
52 
53 DE_MATH_API void deZeroQ4(deFloat* res)
54 {
55  res[0] = res[1] = res[2] = res[3] = 0;
56 }
57 
58 DE_MATH_API void deNegateQ4Q4(deFloat* res, const deFloat* q1)
59 {
60  res[0] = -q1[0];
61  res[1] = -q1[1];
62  res[2] = -q1[2];
63  res[3] = -q1[3];
64 }
65 
66 DE_MATH_API void deInvertQ4Q4(deFloat* res, const deFloat* q1)
67 {
68  res[0] = -q1[0];
69  res[1] = -q1[1];
70  res[2] = -q1[2];
71  res[3] = q1[3];
72 }
73 
74 DE_MATH_API void deSetQ4Q4(deFloat* res, const deFloat* q1)
75 {
76  res[0] = q1[0];
77  res[1] = q1[1];
78  res[2] = q1[2];
79  res[3] = q1[3];
80 }
81 
82 DE_MATH_API void deAddQ4Q4Q4(deFloat* res, const deFloat* q1, const deFloat* q2)
83 {
84  res[0] = q1[0] + q2[0];
85  res[1] = q1[1] + q2[1];
86  res[2] = q1[2] + q2[2];
87  res[3] = q1[3] + q2[3];
88 }
89 
90 DE_MATH_API void deSubQ4Q4Q4(deFloat* res, const deFloat* q1, const deFloat* q2)
91 {
92  res[0] = q1[0] - q2[0];
93  res[1] = q1[1] - q2[1];
94  res[2] = q1[2] - q2[2];
95  res[3] = q1[3] - q2[3];
96 }
97 
98 DE_MATH_API deFloat deDotQ4Q4(const deFloat* q1, const deFloat* q2)
99 {
100  return (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]);
101 }
102 
103 /* res = (w1*w2 - v1.v2, v1*w2 + v2*w1 + v1xv2) */
104 DE_MATH_API void deMulQ4Q4Q4(deFloat* res, const deFloat* q1, const deFloat* q2)
105 {
106  res[0] = q1[0] * q2[3] + q2[0] * q1[3] + (q1[1] * q2[2] - q1[2] * q2[1]);
107  res[1] = q1[1] * q2[3] + q2[1] * q1[3] + (q1[2] * q2[0] - q1[0] * q2[2]);
108  res[2] = q1[2] * q2[3] + q2[2] * q1[3] + (q1[0] * q2[1] - q1[1] * q2[0]);
109  res[3] = q1[3] * q2[3] - (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2]);
110 }
111 
112 DE_MATH_API void deMulQ4Q4iQ4(deFloat* res, const deFloat* q1, const deFloat* q2)
113 {
114  /* w_ = w_, v_ = - v_ */
115  res[0] = -q1[0] * q2[3] + q2[0] * q1[3] - (q1[1] * q2[2] - q1[2] * q2[1]);
116  res[1] = -q1[1] * q2[3] + q2[1] * q1[3] - (q1[2] * q2[0] - q1[0] * q2[2]);
117  res[2] = -q1[2] * q2[3] + q2[2] * q1[3] - (q1[0] * q2[1] - q1[1] * q2[0]);
118  res[3] = q1[3] * q2[3] + (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2]);
119 }
120 
121 DE_MATH_API void deMulQ4Q4Q4i(deFloat* res, const deFloat* q1, const deFloat* q2)
122 {
123  /* w_ = w_, v_ = - v_ */
124  res[0] = q1[0] * q2[3] - q2[0] * q1[3] - (q1[1] * q2[2] - q1[2] * q2[1]);
125  res[1] = q1[1] * q2[3] - q2[1] * q1[3] - (q1[2] * q2[0] - q1[0] * q2[2]);
126  res[2] = q1[2] * q2[3] - q2[2] * q1[3] - (q1[0] * q2[1] - q1[1] * q2[0]);
127  res[3] = q1[3] * q2[3] + (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2]);
128 }
129 
130 DE_MATH_API void deMulQ4S1(deFloat* res, const deFloat s)
131 {
132  res[0] *= s;
133  res[1] *= s;
134  res[2] *= s;
135  res[3] *= s;
136 }
137 
138 DE_MATH_API void deAddQ4Q4(deFloat* res, const deFloat* q1)
139 {
140  res[0] += q1[0];
141  res[1] += q1[1];
142  res[2] += q1[2];
143  res[3] += q1[3];
144 }
145 
146 DE_MATH_API void deSubQ4Q4(deFloat* res, const deFloat* q1)
147 {
148  res[0] -= q1[0];
149  res[1] -= q1[1];
150  res[2] -= q1[2];
151  res[3] -= q1[3];
152 }
153 
154 /*
155  * [w,v'] = q * [0,v] * ~q <==> v' = R v
156  * = [w1,v1]*[0,v2]*[w1,-v1] = [ -v1.v2, w1v2+ v1 x v2]*[w1,-v1]
157  * = [(-v1.v2)(w1)+(w1v2+ v1 x v2).v1,
158  * (v1.v2)v1 + w1(w1v2+ v1 x v2) + v1 x (w1v2+ v1 x v2)]
159  * since A x (B + C) = A x B + A x C
160  * = [w, (w1*w1)v2 + 2*w1(v1 x v2) + (v1.v2)v1 + v1 x (v1 x v2)]
161  * since A x (B x C) = (A dot C)B - (A dot B)C
162  * = [w, (w1*w1)v2 + 2*w1(v1 x v2) + (v1.v2)v1 + (v1.v2)v1 - (v1.v1)v2]
163  * = [w, (w1*w1 - v1.v1)v2 + 2*w1(v1 x v2) + 2*(v1.v2)v1]
164  * v' = [(w1*w1 - v1.v1)v2 + 2*w1(v1 x v2) + 2*(v1.v2)v1]
165  */
166 DE_MATH_API void deMulV3Q4V3(deFloat* res, const deFloat* q1, const deFloat* v2)
167 {
168  deFloat m11 = q1[3] * q1[3] - (q1[0] * q1[0] + q1[1] * q1[1] + q1[2] * q1[2]);
169  deFloat m12 = 2 * (q1[0] * v2[0] + q1[1] * v2[1] + q1[2] * v2[2]);
170  deFloat w2 = q1[3] + q1[3];
171 
172  res[0] = q1[0] * m12 + v2[0] * m11 + (q1[1] * v2[2] - q1[2] * v2[1]) * w2;
173  res[1] = q1[1] * m12 + v2[1] * m11 + (q1[2] * v2[0] - q1[0] * v2[2]) * w2;
174  res[2] = q1[2] * m12 + v2[2] * m11 + (q1[0] * v2[1] - q1[1] * v2[0]) * w2;
175 }
176 
177 /* [w,v'] = ~q * [0,v] * q <==> v' = R v
178  * = [w1,-v1]*[0,v2]*[w1,v1] = [ v1.v2, w1v2 - v1 x v2]*[w1,v1]
179  * v' = [(w1*w1 - v1.v1)v2 - 2*w1(v1 x v2) + 2*(v1.v2)v1]
180  */
181 DE_MATH_API void deMulV3Q4iV3(deFloat* res, const deFloat* q1, const deFloat* v2)
182 {
183  // w_ = w_, v_ = - v_
184  deFloat m11 = q1[3] * q1[3] - (q1[0] * q1[0] + q1[1] * q1[1] + q1[2] * q1[2]);
185  deFloat m12 = 2 * (q1[0] * v2[0] + q1[1] * v2[1] + q1[2] * v2[2]);
186  deFloat w2 = -(q1[3] + q1[3]);
187 
188  res[0] = q1[0] * m12 + v2[0] * m11 + (q1[1] * v2[2] - q1[2] * v2[1]) * w2;
189  res[1] = q1[1] * m12 + v2[1] * m11 + (q1[2] * v2[0] - q1[0] * v2[2]) * w2;
190  res[2] = q1[2] * m12 + v2[2] * m11 + (q1[0] * v2[1] - q1[1] * v2[0]) * w2;
191 }
192 
193 DE_MATH_API int deIsEqualQ4Q4(const deFloat* q1, const deFloat* q2)
194 {
195  deFloat mag2 = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
196  return (mag2 > DE_QUATERNION_COS_THRESHHOLD || mag2 < -DE_QUATERNION_COS_THRESHHOLD);
197 }
198 
199 /* [w1,v1]*[w2,v2] = [(w1*w2 - v1 dot v2), (w1*v2+w2*v1+ v1 cross v2)] */
200 DE_MATH_API void deNormalizeQ4(deFloat* res)
201 {
202  deFloat mag = 1 / deSqrt(res[0] * res[0] + res[1] * res[1] + res[2] * res[2] + res[3] * res[3]);
203  res[0] *= mag;
204  res[1] *= mag;
205  res[2] *= mag;
206  res[3] *= mag;
207 }
208 
209 /* axis angle */
210 DE_MATH_API void deSetQ4V3S1(deFloat* res, const deFloat* axis, const deFloat angle)
211 {
212  deFloat s = deSin(0.5 * angle);
213  res[0] = axis[0] * s;
214  res[1] = axis[1] * s;
215  res[2] = axis[2] * s;
216  res[3] = deCos(0.5 * angle);
217 }
218 
219 /* axis angle */
220 DE_MATH_API void deSetQ4S2(deFloat* res, const int axis, const deFloat angle)
221 {
222  /* also, see Intro. to Robotics by J. Craig: p. 51 */
223  res[0] = res[1] = res[2] = 0;
224  res[axis] = deSin(0.5 * angle);
225  res[3] = deCos(0.5 * angle);
226 }
227 
228 /*
229  * dPhi = Einv ( q - qd) = -2*q_tilde^T qd <-- since (q_tilde^T q) = 0
230  * E = 0.5*q_tilde
231  * Einv = (EtE)inv Et = 4Et = 2*qtilde^T <-- since (q_tilde^T q_tilde) = I
232  *
233  * q = [w x y z]
234  * Khatib
235  * q_tilde = [ -x -y -z
236  * w z -y
237  * -z w x
238  * y -x w ]
239  * q_tilde^T = [ -x w -z y
240  * -y z w -x
241  * -z -y x w ]
242  */
243 DE_MATH_API void deAngularErrorV3Q4Q4(deFloat* res, const deFloat* q1, const deFloat* q2)
244 {
245  res[0] = -2 * ( q1[3] * q2[0] - q1[2] * q2[1] + q1[1] * q2[2] - q1[0] * q2[3]);
246  res[1] = -2 * ( q1[2] * q2[0] + q1[3] * q2[1] - q1[0] * q2[2] - q1[1] * q2[3]);
247  res[2] = -2 * (-q1[1] * q2[0] + q1[0] * q2[1] + q1[3] * q2[2] - q1[2] * q2[3]);
248 }
249 
250 /*
251  * dq = E v2
252  * dq = 0.5 q_tilde v2
253  */
254 DE_MATH_API void deVelocityQ4Q4V3(deFloat* res, const deFloat* q1, const deFloat* v2)
255 {
256  res[0] = (deFloat)0.5 * ( q1[3] * v2[0] + q1[2] * v2[1] - q1[1] * v2[2]);
257  res[1] = (deFloat)0.5 * (-q1[2] * v2[0] + q1[3] * v2[1] + q1[0] * v2[2]);
258  res[2] = (deFloat)0.5 * ( q1[1] * v2[0] - q1[0] * v2[1] + q1[3] * v2[2]);
259  res[3] = (deFloat)0.5 * (-q1[0] * v2[0] - q1[1] * v2[1] - q1[2] * v2[2]);
260 }
261 
262 /*
263  * res = q1 + t * (q2 - q1)
264  */
265 DE_MATH_API void deLerpQ4Q4Q4S1(deFloat* res, const deFloat* q1, const deFloat* q2, const deFloat t)
266 {
267  res[0] = q1[0] + t * (q2[0] - q1[0]);
268  res[1] = q1[1] + t * (q2[1] - q1[1]);
269  res[2] = q1[2] + t * (q2[2] - q1[2]);
270  res[3] = q1[3] + t * (q2[3] - q1[3]);
271 }
272 
273 /*
274  * res = [x, y, z] = Z-Y-X Euler angles
275  */
276 DE_MATH_API void deSetV3Q4zyx(deFloat* res, const deFloat* q)
277 {
278  deFloat sqx = q[0] * q[0];
279  deFloat sqy = q[1] * q[1];
280  deFloat sqz = q[2] * q[2];
281  deFloat sqw = q[3] * q[3];
282 
283  res[0] = deAtan2(2 * (q[1] * q[2] + q[0] * q[3]), (-sqx - sqy + sqz + sqw));
284  res[1] = deAsin(-2 * (q[0] * q[2] - q[1] * q[3]));
285  res[2] = deAtan2(2 * (q[0] * q[1] + q[2] * q[3]), (sqx - sqy - sqz + sqw));
286 }
287 
288 #ifdef __cplusplus
289 }
290 #endif
291 
292 #endif /* _deQuaternion_h */
Generated on Sun Sep 14 2014 02:35:55 for SCL by   doxygen 1.8.6