2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / odemath.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 #define SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE 1
24 #include <ode/common.h>
25 #include <ode/odemath.h>
26
27
28 // get some math functions under windows
29 #ifdef WIN32
30 #include <float.h>
31 #ifndef CYGWIN                  // added by andy for cygwin
32 #define copysign(a,b) ((dReal)_copysign(a,b))
33 #endif                          // added by andy for cygwin
34 #endif
35
36
37 // infinity declaration
38
39 #ifdef DINFINITY_DECL
40 DINFINITY_DECL
41 #endif
42
43
44 // this may be called for vectors `a' with extremely small magnitude, for
45 // example the result of a cross product on two nearly perpendicular vectors.
46 // we must be robust to these small vectors. to prevent numerical error,
47 // first find the component a[i] with the largest magnitude and then scale
48 // all the components by 1/a[i]. then we can compute the length of `a' and
49 // scale the components by 1/l. this has been verified to work with vectors
50 // containing the smallest representable numbers.
51
52 void dNormalize3 (dVector3 a)
53 {
54   dReal a0,a1,a2,aa0,aa1,aa2,l;
55   dAASSERT (a);
56   a0 = a[0];
57   a1 = a[1];
58   a2 = a[2];
59   aa0 = dFabs(a0);
60   aa1 = dFabs(a1);
61   aa2 = dFabs(a2);
62   if (aa1 > aa0) {
63     if (aa2 > aa1) {
64       goto aa2_largest;
65     }
66     else {              // aa1 is largest
67       a0 /= aa1;
68       a2 /= aa1;
69       l = dRecipSqrt (a0*a0 + a2*a2 + 1);
70       a[0] = a0*l;
71       a[1] = copysign(l,a1);
72       a[2] = a2*l;
73     }
74   }
75   else {
76     if (aa2 > aa0) {
77       aa2_largest:      // aa2 is largest
78       a0 /= aa2;
79       a1 /= aa2;
80       l = dRecipSqrt (a0*a0 + a1*a1 + 1);
81       a[0] = a0*l;
82       a[1] = a1*l;
83       a[2] = copysign(l,a2);
84     }
85     else {              // aa0 is largest
86       if (aa0 <= 0) {
87         dDEBUGMSG ("vector has zero size");
88         a[0] = 1;       // if all a's are zero, this is where we'll end up.
89         a[1] = 0;       // return a default unit length vector.
90         a[2] = 0;
91         return;
92       }
93       a1 /= aa0;
94       a2 /= aa0;
95       l = dRecipSqrt (a1*a1 + a2*a2 + 1);
96       a[0] = copysign(l,a0);
97       a[1] = a1*l;
98       a[2] = a2*l;
99     }
100   }
101 }
102
103
104 /* OLD VERSION */
105 /*
106 void dNormalize3 (dVector3 a)
107 {
108   dASSERT (a);
109   dReal l = dDOT(a,a);
110   if (l > 0) {
111     l = dRecipSqrt(l);
112     a[0] *= l;
113     a[1] *= l;
114     a[2] *= l;
115   }
116   else {
117     a[0] = 1;
118     a[1] = 0;
119     a[2] = 0;
120   }
121 }
122 */
123
124
125 void dNormalize4 (dVector4 a)
126 {
127   dAASSERT (a);
128   dReal l = dDOT(a,a)+a[3]*a[3];
129   if (l > 0) {
130     l = dRecipSqrt(l);
131     a[0] *= l;
132     a[1] *= l;
133     a[2] *= l;
134     a[3] *= l;
135   }
136   else {
137     dDEBUGMSG ("vector has zero size");
138     a[0] = 1;
139     a[1] = 0;
140     a[2] = 0;
141     a[3] = 0;
142   }
143 }
144
145
146 void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
147 {
148   dAASSERT (n && p && q);
149   if (dFabs(n[2]) > M_SQRT1_2) {
150     // choose p in y-z plane
151     dReal a = n[1]*n[1] + n[2]*n[2];
152     dReal k = dRecipSqrt (a);
153     p[0] = 0;
154     p[1] = -n[2]*k;
155     p[2] = n[1]*k;
156     // set q = n x p
157     q[0] = a*k;
158     q[1] = -n[0]*p[2];
159     q[2] = n[0]*p[1];
160   }
161   else {
162     // choose p in x-y plane
163     dReal a = n[0]*n[0] + n[1]*n[1];
164     dReal k = dRecipSqrt (a);
165     p[0] = -n[1]*k;
166     p[1] = n[0]*k;
167     p[2] = 0;
168     // set q = n x p
169     q[0] = -n[2]*p[1];
170     q[1] = n[2]*p[0];
171     q[2] = a*k;
172   }
173 }