2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / rotation.cpp
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
32
33 #define _R(i,j) R[(i)*4+(j)]
34
35 #define SET_3x3_IDENTITY \
36   _R(0,0) = REAL(1.0); \
37   _R(0,1) = REAL(0.0); \
38   _R(0,2) = REAL(0.0); \
39   _R(0,3) = REAL(0.0); \
40   _R(1,0) = REAL(0.0); \
41   _R(1,1) = REAL(1.0); \
42   _R(1,2) = REAL(0.0); \
43   _R(1,3) = REAL(0.0); \
44   _R(2,0) = REAL(0.0); \
45   _R(2,1) = REAL(0.0); \
46   _R(2,2) = REAL(1.0); \
47   _R(2,3) = REAL(0.0);
48
49
50 void dRSetIdentity (dMatrix3 R)
51 {
52   dAASSERT (R);
53   SET_3x3_IDENTITY;
54 }
55
56
57 void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
58                          dReal angle)
59 {
60   dAASSERT (R);
61   dQuaternion q;
62   dQFromAxisAndAngle (q,ax,ay,az,angle);
63   dQtoR (q,R);
64 }
65
66
67 void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
68 {
69   dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
70   dAASSERT (R);
71   sphi = dSin(phi);
72   cphi = dCos(phi);
73   stheta = dSin(theta);
74   ctheta = dCos(theta);
75   spsi = dSin(psi);
76   cpsi = dCos(psi);
77   _R(0,0) = cpsi*ctheta;
78   _R(0,1) = spsi*ctheta;
79   _R(0,2) =-stheta;
80   _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
81   _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
82   _R(1,2) = ctheta*sphi;
83   _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
84   _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
85   _R(2,2) = ctheta*cphi;
86 }
87
88
89 void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
90                   dReal bx, dReal by, dReal bz)
91 {
92   dReal l,k;
93   dAASSERT (R);
94   l = dSqrt (ax*ax + ay*ay + az*az);
95   if (l <= REAL(0.0)) {
96     dDEBUGMSG ("zero length vector");
97     return;
98   }
99   l = dRecip(l);
100   ax *= l;
101   ay *= l;
102   az *= l;
103   k = ax*bx + ay*by + az*bz;
104   bx -= k*ax;
105   by -= k*ay;
106   bz -= k*az;
107   l = dSqrt (bx*bx + by*by + bz*bz);
108   if (l <= REAL(0.0)) {
109     dDEBUGMSG ("zero length vector");
110     return;
111   }
112   l = dRecip(l);
113   bx *= l;
114   by *= l;
115   bz *= l;
116   _R(0,0) = ax;
117   _R(1,0) = ay;
118   _R(2,0) = az;
119   _R(0,1) = bx;
120   _R(1,1) = by;
121   _R(2,1) = bz;
122   _R(0,2) = - by*az + ay*bz;
123   _R(1,2) = - bz*ax + az*bx;
124   _R(2,2) = - bx*ay + ax*by;
125 }
126
127
128 void dQSetIdentity (dQuaternion q)
129 {
130   dAASSERT (q);
131   q[0] = 1;
132   q[1] = 0;
133   q[2] = 0;
134   q[3] = 0;
135 }
136
137
138 void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
139                          dReal angle)
140 {
141   dAASSERT (q);
142   dReal l = ax*ax + ay*ay + az*az;
143   if (l > REAL(0.0)) {
144     angle *= REAL(0.5);
145     q[0] = dCos (angle);
146     l = dSin(angle) * dRecipSqrt(l);
147     q[1] = ax*l;
148     q[2] = ay*l;
149     q[3] = az*l;
150   }
151   else {
152     q[0] = 1;
153     q[1] = 0;
154     q[2] = 0;
155     q[3] = 0;
156   }
157 }
158
159
160 void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
161 {
162   dAASSERT (qa && qb && qc);
163   qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
164   qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
165   qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
166   qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
167 }
168
169
170 void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
171 {
172   dAASSERT (qa && qb && qc);
173   qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
174   qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
175   qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
176   qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
177 }
178
179
180 void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
181 {
182   dAASSERT (qa && qb && qc);
183   qa[0] =  qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
184   qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
185   qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
186   qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
187 }
188
189
190 void dQMultiply3 (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
200 // QtoR(), RtoQ() and WtoDQ() are derived from equations in "An Introduction
201 // to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
202 // Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
203 // University, 1997.
204
205 void dQtoR (const dQuaternion q, dMatrix3 R)
206 {
207   dAASSERT (q && R);
208   // q = (s,vx,vy,vz)
209   dReal qq1 = 2*q[1]*q[1];
210   dReal qq2 = 2*q[2]*q[2];
211   dReal qq3 = 2*q[3]*q[3];
212   _R(0,0) = 1 - qq2 - qq3;
213   _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
214   _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
215   _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
216   _R(1,1) = 1 - qq1 - qq3;
217   _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
218   _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
219   _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
220   _R(2,2) = 1 - qq1 - qq2;
221 }
222
223
224 void dRtoQ (const dMatrix3 R, dQuaternion q)
225 {
226   dAASSERT (q && R);
227   dReal tr,s;
228   tr = _R(0,0) + _R(1,1) + _R(2,2);
229   if (tr >= 0) {
230     s = dSqrt (tr + 1);
231     q[0] = REAL(0.5) * s;
232     s = REAL(0.5) * dRecip(s);
233     q[1] = (_R(2,1) - _R(1,2)) * s;
234     q[2] = (_R(0,2) - _R(2,0)) * s;
235     q[3] = (_R(1,0) - _R(0,1)) * s;
236   }
237   else {
238     // find the largest diagonal element and jump to the appropriate case
239     if (_R(1,1) > _R(0,0)) {
240       if (_R(2,2) > _R(1,1)) goto case_2;
241       goto case_1;
242     }
243     if (_R(2,2) > _R(0,0)) goto case_2;
244     goto case_0;
245
246     case_0:
247     s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
248     q[1] = REAL(0.5) * s;
249     s = REAL(0.5) * dRecip(s);
250     q[2] = (_R(0,1) + _R(1,0)) * s;
251     q[3] = (_R(2,0) + _R(0,2)) * s;
252     q[0] = (_R(2,1) - _R(1,2)) * s;
253     return;
254
255     case_1:
256     s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
257     q[2] = REAL(0.5) * s;
258     s = REAL(0.5) * dRecip(s);
259     q[3] = (_R(1,2) + _R(2,1)) * s;
260     q[1] = (_R(0,1) + _R(1,0)) * s;
261     q[0] = (_R(0,2) - _R(2,0)) * s;
262     return;
263
264     case_2:
265     s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
266     q[3] = REAL(0.5) * s;
267     s = REAL(0.5) * dRecip(s);
268     q[1] = (_R(2,0) + _R(0,2)) * s;
269     q[2] = (_R(1,2) + _R(2,1)) * s;
270     q[0] = (_R(1,0) - _R(0,1)) * s;
271     return;
272   }
273 }
274
275
276 void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq)
277 {
278   dAASSERT (w && q && dq);
279   dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
280   dq[1] = REAL(0.5)*(  w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
281   dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
282   dq[3] = REAL(0.5)*(  w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
283 }