|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 /* |
|
24 |
|
25 quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the |
|
26 "rotation axis" and s is the "rotation angle". |
|
27 |
|
28 */ |
|
29 |
|
30 #include <ode/rotation.h> |
|
31 #include <ode/odemath.h> |
|
32 |
|
33 #include <ode/lookup_tables.h> |
|
34 |
|
35 |
|
36 #define _R(i,j) R[(i)*4+(j)] |
|
37 |
|
38 #define SET_3x3_IDENTITY \ |
|
39 _R(0,0) = REAL(1.0); \ |
|
40 _R(0,1) = REAL(0.0); \ |
|
41 _R(0,2) = REAL(0.0); \ |
|
42 _R(0,3) = REAL(0.0); \ |
|
43 _R(1,0) = REAL(0.0); \ |
|
44 _R(1,1) = REAL(1.0); \ |
|
45 _R(1,2) = REAL(0.0); \ |
|
46 _R(1,3) = REAL(0.0); \ |
|
47 _R(2,0) = REAL(0.0); \ |
|
48 _R(2,1) = REAL(0.0); \ |
|
49 _R(2,2) = REAL(1.0); \ |
|
50 _R(2,3) = REAL(0.0); |
|
51 |
|
52 |
|
53 EXPORT_C void dRSetIdentity (dMatrix3 R) |
|
54 { |
|
55 SET_3x3_IDENTITY; |
|
56 } |
|
57 |
|
58 |
|
59 EXPORT_C void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, |
|
60 dReal angle) |
|
61 { |
|
62 dQuaternion q; |
|
63 dQFromAxisAndAngle (q,ax,ay,az,angle); |
|
64 dQtoR (q,R); |
|
65 } |
|
66 |
|
67 |
|
68 EXPORT_C void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi) |
|
69 { |
|
70 dReal sphi,cphi,stheta,ctheta,spsi,cpsi; |
|
71 |
|
72 sphi = dSin(phi); |
|
73 cphi = dCos(phi); |
|
74 stheta = dSin(theta); |
|
75 ctheta = dCos(theta); |
|
76 spsi = dSin(psi); |
|
77 cpsi = dCos(psi); |
|
78 _R(0,0) = dMUL(cpsi,ctheta); |
|
79 _R(0,1) = dMUL(spsi,ctheta); |
|
80 _R(0,2) =-stheta; |
|
81 _R(0,3) = REAL(0.0); |
|
82 _R(1,0) = dMUL(cpsi,dMUL(stheta,sphi)) - dMUL(spsi,cphi); |
|
83 _R(1,1) = dMUL(spsi,dMUL(stheta,sphi)) + dMUL(cpsi,cphi); |
|
84 _R(1,2) = dMUL(ctheta,sphi); |
|
85 _R(1,3) = REAL(0.0); |
|
86 _R(2,0) = dMUL(cpsi,dMUL(stheta,cphi)) + dMUL(spsi,sphi); |
|
87 _R(2,1) = dMUL(spsi,dMUL(stheta,cphi)) - dMUL(cpsi,sphi); |
|
88 _R(2,2) = dMUL(ctheta,cphi); |
|
89 _R(2,3) = REAL(0.0); |
|
90 } |
|
91 |
|
92 |
|
93 EXPORT_C void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, |
|
94 dReal bx, dReal by, dReal bz) |
|
95 { |
|
96 dReal l,k; |
|
97 |
|
98 l = dSqrt (dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az)); |
|
99 if (l <= REAL(0.0)) { |
|
100 return; |
|
101 } |
|
102 l = dRecip(l); |
|
103 ax = dMUL(ax,l); |
|
104 ay = dMUL(ay,l); |
|
105 az = dMUL(az,l); |
|
106 k = dMUL(ax,bx) + dMUL(ay,by) + dMUL(az,bz); |
|
107 bx -= dMUL(k,ax); |
|
108 by -= dMUL(k,ay); |
|
109 bz -= dMUL(k,az); |
|
110 l = dSqrt (dMUL(bx,bx) + dMUL(by,by) + dMUL(bz,bz)); |
|
111 if (l <= REAL(0.0)) { |
|
112 return; |
|
113 } |
|
114 l = dRecip(l); |
|
115 bx = dMUL(bx,l); |
|
116 by = dMUL(by,l); |
|
117 bz = dMUL(bz,l); |
|
118 _R(0,0) = ax; |
|
119 _R(1,0) = ay; |
|
120 _R(2,0) = az; |
|
121 _R(0,1) = bx; |
|
122 _R(1,1) = by; |
|
123 _R(2,1) = bz; |
|
124 _R(0,2) = - dMUL(by,az) + dMUL(ay,bz); |
|
125 _R(1,2) = - dMUL(bz,ax) + dMUL(az,bx); |
|
126 _R(2,2) = - dMUL(bx,ay) + dMUL(ax,by); |
|
127 _R(0,3) = REAL(0.0); |
|
128 _R(1,3) = REAL(0.0); |
|
129 _R(2,3) = REAL(0.0); |
|
130 } |
|
131 |
|
132 |
|
133 EXPORT_C void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az) |
|
134 { |
|
135 dVector3 n,p,q; |
|
136 n[0] = ax; |
|
137 n[1] = ay; |
|
138 n[2] = az; |
|
139 dNormalize3 (n); |
|
140 dPlaneSpace (n,p,q); |
|
141 _R(0,0) = p[0]; |
|
142 _R(1,0) = p[1]; |
|
143 _R(2,0) = p[2]; |
|
144 _R(0,1) = q[0]; |
|
145 _R(1,1) = q[1]; |
|
146 _R(2,1) = q[2]; |
|
147 _R(0,2) = n[0]; |
|
148 _R(1,2) = n[1]; |
|
149 _R(2,2) = n[2]; |
|
150 _R(0,3) = REAL(0.0); |
|
151 _R(1,3) = REAL(0.0); |
|
152 _R(2,3) = REAL(0.0); |
|
153 } |
|
154 |
|
155 |
|
156 EXPORT_C void dQSetIdentity (dQuaternion q) |
|
157 { |
|
158 q[0] = REAL(1.0); |
|
159 q[1] = 0; |
|
160 q[2] = 0; |
|
161 q[3] = 0; |
|
162 } |
|
163 |
|
164 |
|
165 EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, |
|
166 dReal angle) |
|
167 { |
|
168 dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az); |
|
169 if (l > REAL(0.0)) { |
|
170 angle = dMUL(angle,REAL(0.5)); |
|
171 q[0] = dCos (angle); |
|
172 l = dMUL(dReal(dSin(angle)),dRecipSqrt(l)); |
|
173 q[1] = dMUL(ax,l); |
|
174 q[2] = dMUL(ay,l); |
|
175 q[3] = dMUL(az,l); |
|
176 } |
|
177 else { |
|
178 q[0] = REAL(1.0); |
|
179 q[1] = 0; |
|
180 q[2] = 0; |
|
181 q[3] = 0; |
|
182 } |
|
183 } |
|
184 |
|
185 |
|
186 EXPORT_C void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) |
|
187 { |
|
188 qa[0] = dMUL(qb[0],qc[0]) - dMUL(qb[1],qc[1]) - dMUL(qb[2],qc[2]) - dMUL(qb[3],qc[3]); |
|
189 qa[1] = dMUL(qb[0],qc[1]) + dMUL(qb[1],qc[0]) + dMUL(qb[2],qc[3]) - dMUL(qb[3],qc[2]); |
|
190 qa[2] = dMUL(qb[0],qc[2]) + dMUL(qb[2],qc[0]) + dMUL(qb[3],qc[1]) - dMUL(qb[1],qc[3]); |
|
191 qa[3] = dMUL(qb[0],qc[3]) + dMUL(qb[3],qc[0]) + dMUL(qb[1],qc[2]) - dMUL(qb[2],qc[1]); |
|
192 } |
|
193 |
|
194 |
|
195 EXPORT_C void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) |
|
196 { |
|
197 qa[0] = dMUL(qb[0],qc[0]) + dMUL(qb[1],qc[1]) + dMUL(qb[2],qc[2]) + dMUL(qb[3],qc[3]); |
|
198 qa[1] = dMUL(qb[0],qc[1]) - dMUL(qb[1],qc[0]) - dMUL(qb[2],qc[3]) + dMUL(qb[3],qc[2]); |
|
199 qa[2] = dMUL(qb[0],qc[2]) - dMUL(qb[2],qc[0]) - dMUL(qb[3],qc[1]) + dMUL(qb[1],qc[3]); |
|
200 qa[3] = dMUL(qb[0],qc[3]) - dMUL(qb[3],qc[0]) - dMUL(qb[1],qc[2]) + dMUL(qb[2],qc[1]); |
|
201 } |
|
202 |
|
203 |
|
204 EXPORT_C void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) |
|
205 { |
|
206 qa[0] = dMUL(qb[0],qc[0]) + dMUL(qb[1],qc[1]) + dMUL(qb[2],qc[2]) + dMUL(qb[3],qc[3]); |
|
207 qa[1] = -dMUL(qb[0],qc[1]) + dMUL(qb[1],qc[0]) - dMUL(qb[2],qc[3]) + dMUL(qb[3],qc[2]); |
|
208 qa[2] = -dMUL(qb[0],qc[2]) + dMUL(qb[2],qc[0]) - dMUL(qb[3],qc[1]) + dMUL(qb[1],qc[3]); |
|
209 qa[3] = -dMUL(qb[0],qc[3]) + dMUL(qb[3],qc[0]) - dMUL(qb[1],qc[2]) + dMUL(qb[2],qc[1]); |
|
210 } |
|
211 |
|
212 |
|
213 EXPORT_C void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) |
|
214 { |
|
215 qa[0] = dMUL(qb[0],qc[0]) - dMUL(qb[1],qc[1]) - dMUL(qb[2],qc[2]) - dMUL(qb[3],qc[3]); |
|
216 qa[1] = -dMUL(qb[0],qc[1]) - dMUL(qb[1],qc[0]) + dMUL(qb[2],qc[3]) - dMUL(qb[3],qc[2]); |
|
217 qa[2] = -dMUL(qb[0],qc[2]) - dMUL(qb[2],qc[0]) + dMUL(qb[3],qc[1]) - dMUL(qb[1],qc[3]); |
|
218 qa[3] = -dMUL(qb[0],qc[3]) - dMUL(qb[3],qc[0]) + dMUL(qb[1],qc[2]) - dMUL(qb[2],qc[1]); |
|
219 } |
|
220 |
|
221 |
|
222 // dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction |
|
223 // to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained |
|
224 // Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon |
|
225 // University, 1997. |
|
226 |
|
227 EXPORT_C void dRfromQ (dMatrix3 R, const dQuaternion q) |
|
228 { |
|
229 |
|
230 // q = (s,vx,vy,vz) |
|
231 dReal qq1 = 2*dMUL(q[1],q[1]); |
|
232 dReal qq2 = 2*dMUL(q[2],q[2]); |
|
233 dReal qq3 = 2*dMUL(q[3],q[3]); |
|
234 _R(0,0) = REAL(1.0) - qq2 - qq3; |
|
235 _R(0,1) = 2*(dMUL(q[1],q[2]) - dMUL(q[0],q[3])); |
|
236 _R(0,2) = 2*(dMUL(q[1],q[3]) + dMUL(q[0],q[2])); |
|
237 _R(0,3) = REAL(0.0); |
|
238 _R(1,0) = 2*(dMUL(q[1],q[2]) + dMUL(q[0],q[3])); |
|
239 _R(1,1) = REAL(1.0) - qq1 - qq3; |
|
240 _R(1,2) = 2*(dMUL(q[2],q[3]) - dMUL(q[0],q[1])); |
|
241 _R(1,3) = REAL(0.0); |
|
242 _R(2,0) = 2*(dMUL(q[1],q[3]) - dMUL(q[0],q[2])); |
|
243 _R(2,1) = 2*(dMUL(q[2],q[3]) + dMUL(q[0],q[1])); |
|
244 _R(2,2) = REAL(1.0) - qq1 - qq2; |
|
245 _R(2,3) = REAL(0.0); |
|
246 } |
|
247 |
|
248 |
|
249 EXPORT_C void dQfromR (dQuaternion q, const dMatrix3 R) |
|
250 { |
|
251 |
|
252 dReal tr,s; |
|
253 tr = _R(0,0) + _R(1,1) + _R(2,2); |
|
254 if (tr >= 0) { |
|
255 s = dSqrt (tr + REAL(1.0)); |
|
256 q[0] = dMUL(REAL(0.5),s); |
|
257 s = dMUL(REAL(0.5),dRecip(s)); |
|
258 q[1] = dMUL((_R(2,1) - _R(1,2)),s); |
|
259 q[2] = dMUL((_R(0,2) - _R(2,0)),s); |
|
260 q[3] = dMUL((_R(1,0) - _R(0,1)),s); |
|
261 } |
|
262 else { |
|
263 // find the largest diagonal element and jump to the appropriate case |
|
264 if (_R(1,1) > _R(0,0)) { |
|
265 if (_R(2,2) > _R(1,1)) goto case_2; |
|
266 goto case_1; |
|
267 } |
|
268 if (_R(2,2) > _R(0,0)) goto case_2; |
|
269 goto case_0; |
|
270 |
|
271 case_0: |
|
272 s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + REAL(1.0)); |
|
273 q[1] = dMUL(REAL(0.5),s); |
|
274 s = dMUL(REAL(0.5),dRecip(s)); |
|
275 q[2] = dMUL((_R(0,1) + _R(1,0)),s); |
|
276 q[3] = dMUL((_R(2,0) + _R(0,2)),s); |
|
277 q[0] = dMUL((_R(2,1) - _R(1,2)),s); |
|
278 return; |
|
279 |
|
280 case_1: |
|
281 s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + REAL(1.0)); |
|
282 q[2] = dMUL(REAL(0.5),s); |
|
283 s = dMUL(REAL(0.5),dRecip(s)); |
|
284 q[3] = dMUL((_R(1,2) + _R(2,1)),s); |
|
285 q[1] = dMUL((_R(0,1) + _R(1,0)),s); |
|
286 q[0] = dMUL((_R(0,2) - _R(2,0)),s); |
|
287 return; |
|
288 |
|
289 case_2: |
|
290 s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + REAL(1.0)); |
|
291 q[3] = dMUL(REAL(0.5),s); |
|
292 s = dMUL(REAL(0.5),dRecip(s)); |
|
293 q[1] = dMUL((_R(2,0) + _R(0,2)),s); |
|
294 q[2] = dMUL((_R(1,2) + _R(2,1)),s); |
|
295 q[0] = dMUL((_R(1,0) - _R(0,1)),s); |
|
296 return; |
|
297 } |
|
298 } |
|
299 |
|
300 |
|
301 EXPORT_C void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q) |
|
302 { |
|
303 |
|
304 dq[0] = dMUL(REAL(0.5),(- dMUL(w[0],q[1]) - dMUL(w[1],q[2]) - dMUL(w[2],q[3]))); |
|
305 dq[1] = dMUL(REAL(0.5),( dMUL(w[0],q[0]) + dMUL(w[1],q[3]) - dMUL(w[2],q[2]))); |
|
306 dq[2] = dMUL(REAL(0.5),(- dMUL(w[0],q[3]) + dMUL(w[1],q[0]) + dMUL(w[2],q[1]))); |
|
307 dq[3] = dMUL(REAL(0.5),( dMUL(w[0],q[2]) - dMUL(w[1],q[1]) + dMUL(w[2],q[0]))); |
|
308 } |