← Back to branch summary

~ubuntu-branches/ubuntu/intrepid/blender/intrepid-updates

« back to all changes in this revision

Viewing changes to extern/bullet/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Cyril Brulebois
  • Date: 2008-08-08 02:45:40 UTC
  • mfrom: (12.1.14 intrepid)
  • Revision ID: james.westby@ubuntu.com-20080808024540-kkjp7ekfivzhuw3l
Tags: 2.46+dfsg-4
http://bugs.debian.org/492280
* Fix python syntax warning in import_dxf.py, which led to nasty output
  in installation/upgrade logs during byte-compilation, using a patch
  provided by the script author (Closes: #492280):
   - debian/patches/45_fix_python_syntax_warning

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp
1
 
/*
2
 
Bullet Continuous Collision Detection and Physics Library
3
 
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
 
 
5
 
This software is provided 'as-is', without any express or implied warranty.
6
 
In no event will the authors be held liable for any damages arising from the use of this software.
7
 
Permission is granted to anyone to use this software for any purpose, 
8
 
including commercial applications, and to alter it and redistribute it freely, 
9
 
subject to the following restrictions:
10
 
 
11
 
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12
 
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13
 
3. This notice may not be removed or altered from any source distribution.
14
 
*/
15
 
 
16
 
 
17
 
#include "BU_AlgebraicPolynomialSolver.h"
18
 
#include <math.h>
19
 
#include <SimdMinMax.h>
20
 
 
21
 
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) 
22
 
{ 
23
 
 
24
 
        SimdScalar basic_h_local;
25
 
        SimdScalar basic_h_local_delta;
26
 
        
27
 
        basic_h_local       = p * 0.5f; 
28
 
   basic_h_local_delta = basic_h_local * basic_h_local - q; 
29
 
   if (basic_h_local_delta > 0.0f)  { 
30
 
      basic_h_local_delta = SimdSqrt(basic_h_local_delta); 
31
 
      m_roots[0]  = - basic_h_local + basic_h_local_delta; 
32
 
      m_roots[1]  = - basic_h_local - basic_h_local_delta; 
33
 
      return 2; 
34
 
   } 
35
 
   else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { 
36
 
      m_roots[0]  = - basic_h_local; 
37
 
      return 1; 
38
 
   } 
39
 
   else { 
40
 
      return 0; 
41
 
   } 
42
 
 }
43
 
 
44
 
 
45
 
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) 
46
 
{ 
47
 
    SimdScalar radical = b * b - 4.0f * a * c;
48
 
    if(radical >= 0.f)
49
 
    {
50
 
        SimdScalar sqrtRadical = SimdSqrt(radical); 
51
 
        SimdScalar idenom = 1.0f/(2.0f * a);
52
 
        m_roots[0]=(-b + sqrtRadical) * idenom;
53
 
        m_roots[1]=(-b - sqrtRadical) * idenom;
54
 
                return 2;
55
 
        }
56
 
        return 0;
57
 
}
58
 
 
59
 
 
60
 
#define cubic_rt(x) \
61
 
 ((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
62
 
  ((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
63
 
 
64
 
 
65
 
 
66
 
/*                                                                           */
67
 
/* this function solves the following cubic equation:                        */
68
 
/*                                                                           */
69
 
/*              3          2                                                */
70
 
/*      lead * x   +  a * x   +  b * x  +  c  =  0.                          */
71
 
/*                                                                           */
72
 
/* it returns the number of different roots found, and stores the roots in   */
73
 
/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0.                */
74
 
/*                                                                           */
75
 
int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
76
 
{ 
77
 
   SimdScalar p, q, r;
78
 
   SimdScalar delta, u, phi;
79
 
   SimdScalar dummy;
80
 
 
81
 
   if (lead != 1.0) {
82
 
      /*                                                                     */
83
 
      /* transform into normal form: x^3 + a x^2 + b x + c = 0               */
84
 
      /*                                                                     */
85
 
      if (SimdEqual(lead, SIMD_EPSILON)) {
86
 
         /*                                                                  */
87
 
         /* we have  a x^2 + b x + c = 0                                     */
88
 
         /*                                                                  */
89
 
         if (SimdEqual(a, SIMD_EPSILON)) {
90
 
            /*                                                               */
91
 
            /* we have  b x + c = 0                                          */
92
 
            /*                                                               */
93
 
            if (SimdEqual(b, SIMD_EPSILON)) {
94
 
               if (SimdEqual(c, SIMD_EPSILON)) {
95
 
                  return -1;
96
 
               }
97
 
               else {
98
 
                  return 0;
99
 
               }
100
 
            }
101
 
            else {
102
 
               m_roots[0] = -c / b;
103
 
               return 1;
104
 
            }
105
 
         }
106
 
         else {
107
 
            p = c / a;
108
 
            q = b / a;
109
 
            return Solve2QuadraticFull(a,b,c);
110
 
         }
111
 
      }
112
 
      else {
113
 
         a = a / lead;
114
 
         b = b / lead;
115
 
         c = c / lead;
116
 
      }
117
 
   }
118
 
               
119
 
   /*                                                                        */
120
 
   /* we substitute  x = y - a / 3  in order to eliminate the quadric term.  */
121
 
   /* we get   x^3 + p x + q = 0                                             */
122
 
   /*                                                                        */
123
 
   a /= 3.0f;
124
 
   u  = a * a;
125
 
   p  = b / 3.0f - u;
126
 
   q  = a * (2.0f * u - b) + c;
127
 
 
128
 
   /*                                                                        */
129
 
   /* now use Cardano's formula                                              */
130
 
   /*                                                                        */
131
 
   if (SimdEqual(p, SIMD_EPSILON)) {
132
 
      if (SimdEqual(q, SIMD_EPSILON)) {
133
 
         /*                                                                  */
134
 
         /* one triple root                                                  */
135
 
         /*                                                                  */
136
 
         m_roots[0] = -a;
137
 
         return 1;
138
 
      }
139
 
      else {
140
 
         /*                                                                  */
141
 
         /* one real and two complex roots                                   */
142
 
         /*                                                                  */
143
 
         m_roots[0] = cubic_rt(-q) - a;
144
 
         return 1;
145
 
      }
146
 
   }
147
 
 
148
 
   q /= 2.0f;
149
 
   delta = p * p * p + q * q;
150
 
   if (delta > 0.0f) {
151
 
      /*                                                                     */
152
 
      /* one real and two complex roots. note that  v = -p / u.              */
153
 
      /*                                                                     */
154
 
      u = -q + SimdSqrt(delta);
155
 
      u = cubic_rt(u);
156
 
      m_roots[0] = u - p / u - a;
157
 
      return 1;
158
 
   }
159
 
   else if (delta < 0.0) {
160
 
      /*                                                                     */
161
 
      /* Casus irreducibilis: we have three real roots                       */
162
 
      /*                                                                     */
163
 
      r        = SimdSqrt(-p);
164
 
      p       *= -r;
165
 
      r       *= 2.0;
166
 
      phi      = SimdAcos(-q / p) / 3.0f;
167
 
      dummy    = SIMD_2_PI / 3.0f; 
168
 
      m_roots[0] = r * SimdCos(phi) - a;
169
 
      m_roots[1] = r * SimdCos(phi + dummy) - a;
170
 
      m_roots[2] = r * SimdCos(phi - dummy) - a;
171
 
      return 3;
172
 
   }
173
 
   else {
174
 
      /*                                                                     */
175
 
      /* one single and one SimdScalar root                                      */
176
 
      /*                                                                     */
177
 
      r = cubic_rt(-q);
178
 
      m_roots[0] = 2.0f * r - a;
179
 
      m_roots[1] = -r - a;
180
 
      return 2;
181
 
   }
182
 
}
183
 
 
184
 
 
185
 
/*                                                                           */
186
 
/* this function solves the following quartic equation:                      */
187
 
/*                                                                           */
188
 
/*             4           3          2                                      */
189
 
/*     lead * x   +  a *  x   +  b * x   +  c * x  +  d = 0.                 */
190
 
/*                                                                           */
191
 
/* it returns the number of different roots found, and stores the roots in   */
192
 
/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0.                */
193
 
/*                                                                           */
194
 
int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
195
 
{ 
196
 
   SimdScalar p, q ,r;
197
 
   SimdScalar u, v, w;
198
 
   int i, num_roots, num_tmp;
199
 
   //SimdScalar tmp[2];
200
 
 
201
 
   if (lead != 1.0) {
202
 
      /*                                                                     */
203
 
      /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0       */
204
 
      /*                                                                     */
205
 
      if (SimdEqual(lead, SIMD_EPSILON)) {
206
 
         /*                                                                  */
207
 
         /* we have  a x^3 + b x^2 + c x + d = 0                             */
208
 
         /*                                                                  */
209
 
         if (SimdEqual(a, SIMD_EPSILON)) { 
210
 
            /*                                                               */
211
 
            /* we have  b x^2 + c x + d = 0                                  */
212
 
            /*                                                               */
213
 
            if (SimdEqual(b, SIMD_EPSILON)) {
214
 
               /*                                                            */
215
 
               /* we have  c x + d = 0                                       */
216
 
               /*                                                            */
217
 
               if (SimdEqual(c, SIMD_EPSILON)) {
218
 
                  if (SimdEqual(d, SIMD_EPSILON)) {
219
 
                     return -1;
220
 
                  }
221
 
                  else {
222
 
                     return 0;
223
 
                  }
224
 
               }
225
 
               else {
226
 
                  m_roots[0] = -d / c;
227
 
                  return 1;
228
 
               }
229
 
            }
230
 
            else {
231
 
               p = c / b;
232
 
               q = d / b;
233
 
               return Solve2QuadraticFull(b,c,d);
234
 
               
235
 
            }
236
 
         }
237
 
         else { 
238
 
            return Solve3Cubic(1.0, b / a, c / a, d / a);
239
 
         }
240
 
      }
241
 
      else {
242
 
         a = a / lead;
243
 
         b = b / lead;
244
 
         c = c / lead;
245
 
         d = d / lead;
246
 
      }
247
 
   }
248
 
 
249
 
   /*                                                                        */
250
 
   /* we substitute  x = y - a / 4  in order to eliminate the cubic term.    */
251
 
   /* we get:  y^4 + p y^2 + q y + r = 0.                                    */
252
 
   /*                                                                        */
253
 
   a /= 4.0f;
254
 
   p  = b - 6.0f * a * a;
255
 
   q  = a * (8.0f * a * a - 2.0f * b) + c;
256
 
   r  = a * (a * (b - 3.f * a * a) - c) + d;
257
 
   if (SimdEqual(q, SIMD_EPSILON)) {
258
 
      /*                                                                     */
259
 
      /* biquadratic equation:  y^4 + p y^2 + r = 0.                         */
260
 
      /*                                                                     */
261
 
      num_roots = Solve2Quadratic(p, r);
262
 
      if (num_roots > 0) {                 
263
 
         if (m_roots[0] > 0.0f) {
264
 
            if (num_roots > 1)  {
265
 
               if ((m_roots[1] > 0.0f)  &&  (m_roots[1] != m_roots[0])) {
266
 
                  u        = SimdSqrt(m_roots[1]);
267
 
                  m_roots[2] =  u - a;
268
 
                  m_roots[3] = -u - a;
269
 
                  u        = SimdSqrt(m_roots[0]);
270
 
                  m_roots[0] =  u - a;
271
 
                  m_roots[1] = -u - a;
272
 
                  return 4;
273
 
               }
274
 
               else {
275
 
                  u        = SimdSqrt(m_roots[0]);
276
 
                  m_roots[0] =  u - a;
277
 
                  m_roots[1] = -u - a;
278
 
                  return 2;
279
 
               }
280
 
            }
281
 
            else {
282
 
               u        = SimdSqrt(m_roots[0]);
283
 
               m_roots[0] =  u - a;
284
 
               m_roots[1] = -u - a;
285
 
               return 2;
286
 
            }
287
 
         }
288
 
      }
289
 
      return 0;
290
 
   }
291
 
   else if (SimdEqual(r, SIMD_EPSILON)) {
292
 
      /*                                                                     */
293
 
      /* no absolute term:  y (y^3 + p y + q) = 0.                           */
294
 
      /*                                                                     */
295
 
      num_roots = Solve3Cubic(1.0, 0.0, p, q);
296
 
      for (i = 0;  i < num_roots;  ++i)  m_roots[i] -= a;
297
 
      if (num_roots != -1) {
298
 
         m_roots[num_roots] = -a;
299
 
         ++num_roots;
300
 
      }
301
 
      else {
302
 
         m_roots[0]  = -a;
303
 
         num_roots = 1;;
304
 
      }
305
 
      return num_roots;
306
 
   }
307
 
   else {
308
 
      /*                                                                     */
309
 
      /* we solve the resolvent cubic equation                               */
310
 
      /*                                                                     */
311
 
      num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
312
 
      if (num_roots == -1) {
313
 
         num_roots = 1;
314
 
         m_roots[0]  = 0.0f;
315
 
      }
316
 
 
317
 
      /*                                                                     */
318
 
      /* build two quadric equations                                         */
319
 
      /*                                                                     */
320
 
      w = m_roots[0];
321
 
      u = w * w - r;
322
 
      v = 2.0f * w - p;
323
 
 
324
 
      if (SimdEqual(u, SIMD_EPSILON))
325
 
         u = 0.0;
326
 
      else if (u > 0.0f)
327
 
         u = SimdSqrt(u);
328
 
      else
329
 
         return 0;
330
 
      
331
 
      if (SimdEqual(v, SIMD_EPSILON))
332
 
         v = 0.0;
333
 
      else if (v > 0.0f)
334
 
         v = SimdSqrt(v);
335
 
      else
336
 
         return 0;
337
 
 
338
 
      if (q < 0.0f)  v = -v;
339
 
      w -= u;
340
 
      num_roots=Solve2Quadratic(v, w);
341
 
      for (i = 0;  i < num_roots;  ++i)  
342
 
          {
343
 
                  m_roots[i] -= a;
344
 
          }
345
 
      w += 2.0f *u;
346
 
          SimdScalar tmp[2];
347
 
          tmp[0] = m_roots[0];
348
 
          tmp[1] = m_roots[1];
349
 
 
350
 
      num_tmp = Solve2Quadratic(-v, w);
351
 
      for (i = 0;  i < num_tmp;  ++i)
352
 
          {
353
 
                 m_roots[i + num_roots] = tmp[i] - a;
354
 
                 m_roots[i]=tmp[i];
355
 
          }
356
 
 
357
 
      return  (num_tmp + num_roots);
358
 
   }
359
 
}
360