aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/rotation.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/rotation.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/rotation.cpp316
1 files changed, 0 insertions, 316 deletions
diff --git a/libraries/ode-0.9/ode/src/rotation.cpp b/libraries/ode-0.9/ode/src/rotation.cpp
deleted file mode 100644
index adb933b..0000000
--- a/libraries/ode-0.9/ode/src/rotation.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
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
25quaternions 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
34#define _R(i,j) R[(i)*4+(j)]
35
36#define SET_3x3_IDENTITY \
37 _R(0,0) = REAL(1.0); \
38 _R(0,1) = REAL(0.0); \
39 _R(0,2) = REAL(0.0); \
40 _R(0,3) = REAL(0.0); \
41 _R(1,0) = REAL(0.0); \
42 _R(1,1) = REAL(1.0); \
43 _R(1,2) = REAL(0.0); \
44 _R(1,3) = REAL(0.0); \
45 _R(2,0) = REAL(0.0); \
46 _R(2,1) = REAL(0.0); \
47 _R(2,2) = REAL(1.0); \
48 _R(2,3) = REAL(0.0);
49
50
51void dRSetIdentity (dMatrix3 R)
52{
53 dAASSERT (R);
54 SET_3x3_IDENTITY;
55}
56
57
58void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
59 dReal angle)
60{
61 dAASSERT (R);
62 dQuaternion q;
63 dQFromAxisAndAngle (q,ax,ay,az,angle);
64 dQtoR (q,R);
65}
66
67
68void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
69{
70 dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
71 dAASSERT (R);
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) = cpsi*ctheta;
79 _R(0,1) = spsi*ctheta;
80 _R(0,2) =-stheta;
81 _R(0,3) = REAL(0.0);
82 _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
83 _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
84 _R(1,2) = ctheta*sphi;
85 _R(1,3) = REAL(0.0);
86 _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
87 _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
88 _R(2,2) = ctheta*cphi;
89 _R(2,3) = REAL(0.0);
90}
91
92
93void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
94 dReal bx, dReal by, dReal bz)
95{
96 dReal l,k;
97 dAASSERT (R);
98 l = dSqrt (ax*ax + ay*ay + az*az);
99 if (l <= REAL(0.0)) {
100 dDEBUGMSG ("zero length vector");
101 return;
102 }
103 l = dRecip(l);
104 ax *= l;
105 ay *= l;
106 az *= l;
107 k = ax*bx + ay*by + az*bz;
108 bx -= k*ax;
109 by -= k*ay;
110 bz -= k*az;
111 l = dSqrt (bx*bx + by*by + bz*bz);
112 if (l <= REAL(0.0)) {
113 dDEBUGMSG ("zero length vector");
114 return;
115 }
116 l = dRecip(l);
117 bx *= l;
118 by *= l;
119 bz *= l;
120 _R(0,0) = ax;
121 _R(1,0) = ay;
122 _R(2,0) = az;
123 _R(0,1) = bx;
124 _R(1,1) = by;
125 _R(2,1) = bz;
126 _R(0,2) = - by*az + ay*bz;
127 _R(1,2) = - bz*ax + az*bx;
128 _R(2,2) = - bx*ay + ax*by;
129 _R(0,3) = REAL(0.0);
130 _R(1,3) = REAL(0.0);
131 _R(2,3) = REAL(0.0);
132}
133
134
135void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az)
136{
137 dVector3 n,p,q;
138 n[0] = ax;
139 n[1] = ay;
140 n[2] = az;
141 dNormalize3 (n);
142 dPlaneSpace (n,p,q);
143 _R(0,0) = p[0];
144 _R(1,0) = p[1];
145 _R(2,0) = p[2];
146 _R(0,1) = q[0];
147 _R(1,1) = q[1];
148 _R(2,1) = q[2];
149 _R(0,2) = n[0];
150 _R(1,2) = n[1];
151 _R(2,2) = n[2];
152 _R(0,3) = REAL(0.0);
153 _R(1,3) = REAL(0.0);
154 _R(2,3) = REAL(0.0);
155}
156
157
158void dQSetIdentity (dQuaternion q)
159{
160 dAASSERT (q);
161 q[0] = 1;
162 q[1] = 0;
163 q[2] = 0;
164 q[3] = 0;
165}
166
167
168void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
169 dReal angle)
170{
171 dAASSERT (q);
172 dReal l = ax*ax + ay*ay + az*az;
173 if (l > REAL(0.0)) {
174 angle *= REAL(0.5);
175 q[0] = dCos (angle);
176 l = dSin(angle) * dRecipSqrt(l);
177 q[1] = ax*l;
178 q[2] = ay*l;
179 q[3] = az*l;
180 }
181 else {
182 q[0] = 1;
183 q[1] = 0;
184 q[2] = 0;
185 q[3] = 0;
186 }
187}
188
189
190void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
191{
192 dAASSERT (qa && qb && qc);
193 qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
194 qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
195 qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
196 qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
197}
198
199
200void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
201{
202 dAASSERT (qa && qb && qc);
203 qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
204 qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
205 qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
206 qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
207}
208
209
210void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
211{
212 dAASSERT (qa && qb && qc);
213 qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
214 qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
215 qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
216 qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
217}
218
219
220void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
221{
222 dAASSERT (qa && qb && qc);
223 qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
224 qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
225 qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
226 qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
227}
228
229
230// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction
231// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
232// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
233// University, 1997.
234
235void dRfromQ (dMatrix3 R, const dQuaternion q)
236{
237 dAASSERT (q && R);
238 // q = (s,vx,vy,vz)
239 dReal qq1 = 2*q[1]*q[1];
240 dReal qq2 = 2*q[2]*q[2];
241 dReal qq3 = 2*q[3]*q[3];
242 _R(0,0) = 1 - qq2 - qq3;
243 _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
244 _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
245 _R(0,3) = REAL(0.0);
246 _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
247 _R(1,1) = 1 - qq1 - qq3;
248 _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
249 _R(1,3) = REAL(0.0);
250 _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
251 _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
252 _R(2,2) = 1 - qq1 - qq2;
253 _R(2,3) = REAL(0.0);
254}
255
256
257void dQfromR (dQuaternion q, const dMatrix3 R)
258{
259 dAASSERT (q && R);
260 dReal tr,s;
261 tr = _R(0,0) + _R(1,1) + _R(2,2);
262 if (tr >= 0) {
263 s = dSqrt (tr + 1);
264 q[0] = REAL(0.5) * s;
265 s = REAL(0.5) * dRecip(s);
266 q[1] = (_R(2,1) - _R(1,2)) * s;
267 q[2] = (_R(0,2) - _R(2,0)) * s;
268 q[3] = (_R(1,0) - _R(0,1)) * s;
269 }
270 else {
271 // find the largest diagonal element and jump to the appropriate case
272 if (_R(1,1) > _R(0,0)) {
273 if (_R(2,2) > _R(1,1)) goto case_2;
274 goto case_1;
275 }
276 if (_R(2,2) > _R(0,0)) goto case_2;
277 goto case_0;
278
279 case_0:
280 s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
281 q[1] = REAL(0.5) * s;
282 s = REAL(0.5) * dRecip(s);
283 q[2] = (_R(0,1) + _R(1,0)) * s;
284 q[3] = (_R(2,0) + _R(0,2)) * s;
285 q[0] = (_R(2,1) - _R(1,2)) * s;
286 return;
287
288 case_1:
289 s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
290 q[2] = REAL(0.5) * s;
291 s = REAL(0.5) * dRecip(s);
292 q[3] = (_R(1,2) + _R(2,1)) * s;
293 q[1] = (_R(0,1) + _R(1,0)) * s;
294 q[0] = (_R(0,2) - _R(2,0)) * s;
295 return;
296
297 case_2:
298 s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
299 q[3] = REAL(0.5) * s;
300 s = REAL(0.5) * dRecip(s);
301 q[1] = (_R(2,0) + _R(0,2)) * s;
302 q[2] = (_R(1,2) + _R(2,1)) * s;
303 q[0] = (_R(1,0) - _R(0,1)) * s;
304 return;
305 }
306}
307
308
309void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q)
310{
311 dAASSERT (w && q && dq);
312 dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
313 dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
314 dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
315 dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
316}
generated by cgit v1.1 at 2024-11-11 10:06:06 +0000