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