mSolver.cpp

Public Functions

mCbrt(F32 val)

mSolveCubic_c(F32 a, F32 b, F32 c, F32 d, F32 * x)

mSolveLinear(F32 a, F32 b, F32 * x)

mSolveQuadratic_c(F32 a, F32 b, F32 c, F32 * x)

mSolveQuartic_c(F32 a, F32 b, F32 c, F32 d, F32 e, F32 * x)

swap(F32 & a, F32 & b)

  1
  2//-----------------------------------------------------------------------------
  3// Copyright (c) 2012 GarageGames, LLC
  4//
  5// Permission is hereby granted, free of charge, to any person obtaining a copy
  6// of this software and associated documentation files (the "Software"), to
  7// deal in the Software without restriction, including without limitation the
  8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9// sell copies of the Software, and to permit persons to whom the Software is
 10// furnished to do so, subject to the following conditions:
 11//
 12// The above copyright notice and this permission notice shall be included in
 13// all copies or substantial portions of the Software.
 14//
 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21// IN THE SOFTWARE.
 22//-----------------------------------------------------------------------------
 23
 24#include "platform/platform.h"
 25#include "math/mMathFn.h"
 26
 27//--------------------------------------------------------------------------
 28#define EQN_EPSILON     (1e-8)
 29
 30static inline void swap(F32 & a, F32 & b)
 31{
 32   F32 t = b;
 33   b = a;
 34   a = t;
 35}
 36
 37static inline F32 mCbrt(F32 val)
 38{
 39   if(val < 0.f)
 40      return(-mPow(-val, F32(1.f/3.f)));
 41   else
 42      return(mPow(val, F32(1.f/3.f)));
 43}
 44
 45static inline U32 mSolveLinear(F32 a, F32 b, F32 * x)
 46{
 47   if(mIsZero(a))
 48      return(0);
 49
 50   x[0] = -b/<a href="/coding/file/pointer_8h/#pointer_8h_1aeeddce917cf130d62c370b8f216026dd">a</a>;
 51   return(1);
 52}
 53
 54static U32 mSolveQuadratic_c(F32 a, F32 b, F32 c, F32 * x)
 55{
 56   // really linear?
 57   if(mIsZero(a))
 58      return(mSolveLinear(b, c, x));
 59
 60   // get the descriminant:   (b^2 - 4ac)
 61   F32 desc = (b * b) - (4.f * a * c);
 62
 63   // solutions:
 64   // desc < 0:   two imaginary solutions
 65   // desc > 0:   two real solutions (b +- sqrt(desc)) / 2a
 66   // desc = 0:   one real solution (b / 2a)
 67   if(mIsZero(desc))
 68   {
 69      x[0] = b / (2.f * a);
 70      return(1);
 71   }
 72   else if(desc > 0.f)
 73   {
 74      F32 sqrdesc = mSqrt(desc);
 75      F32 den = (2.f * a);
 76      x[0] = (-b + sqrdesc) / den;
 77      x[1] = (-b - sqrdesc) / den;
 78
 79      if(x[1] < x[0])
 80         swap(x[0], x[1]);
 81
 82      return(2);
 83   }
 84   else
 85      return(0);
 86}
 87
 88//--------------------------------------------------------------------------
 89// from Graphics Gems I: pp 738-742
 90U32 mSolveCubic_c(F32 a, F32 b, F32 c, F32 d, F32 * x)
 91{
 92   if(mIsZero(a))
 93      return(mSolveQuadratic(b, c, d, x));
 94
 95   // normal form: x^3 + Ax^2 + BX + C = 0
 96   F32 A = b / a;
 97   F32 B = c / a;
 98   F32 C = d / a;
 99
100   // substitute x = y - A/3 to eliminate quadric term and depress
101   // the cubic equation to (x^3 + px + q = 0)
102   F32 A2 = A * A;
103   F32 A3 = A2 * A;
104
105   F32 p = (1.f/3.f) * (((-1.f/3.f) * A2) + B);
106   F32 q = (1.f/2.f) * (((2.f/27.f) * A3) - ((1.f/3.f) * A * B) + C);
107
108   // use Cardano's fomula to solve the depressed cubic
109   F32 p3 = p * p * p;
110   F32 q2 = q * q;
111
112   F32 D = q2 + p3;
113
114   U32 num = 0;
115
116   if(mIsZero(D))          // 1 or 2 solutions
117   {
118      if(mIsZero(q)) // 1 triple solution
119      {
120         x[0] = 0.f;
121         num = 1;
122      }
123      else // 1 single and 1 double
124      {
125         F32 u = mCbrt(-q);
126         x[0] = 2.f * u;
127         x[1] = -u;
128         num = 2;
129      }
130   }
131   else if(D < 0.f)        // 3 solutions: casus irreducibilis
132   {
133      F32 phi = (1.f/3.f) * mAcos(-q / mSqrt(-p3));
134      F32 t = 2.f * mSqrt(-p);
135
136      x[0] = t * mCos(phi);
137      x[1] = -t * mCos(phi + (M_PI / 3.f));
138      x[2] = -t * mCos(phi - (M_PI / 3.f));
139      num = 3;
140   }
141   else                    // 1 solution
142   {
143      F32 sqrtD = mSqrt(D);
144      F32 u = mCbrt(sqrtD - q);
145      F32 v = -mCbrt(sqrtD + q);
146
147      x[0] = u + v;
148      num = 1;
149   }
150
151   // resubstitute
152   F32 sub = (1.f/3.f) * A;
153   for(U32 i = 0; i < num; i++)
154      x[i] -= sub;
155
156   // sort the roots
157   for(S32 j = 0; j < (num - 1); j++)
158      for(S32 k = j + 1; k < num; k++)
159         if(x[k] < x[j])
160            swap(x[k], x[j]);
161
162   return(num);
163}
164
165//--------------------------------------------------------------------------
166// from Graphics Gems I: pp 738-742
167U32 mSolveQuartic_c(F32 a, F32 b, F32 c, F32 d, F32 e, F32 * x)
168{
169   if(mIsZero(a))
170      return(mSolveCubic(b, c, d, e, x));
171
172   // normal form: x^4 + ax^3 + bx^2 + cx + d = 0
173   F32 A = b / a;
174   F32 B = c / a;
175   F32 C = d / a;
176   F32 D = e / a;
177
178   // substitue x = y - A/4 to eliminate cubic term:
179   // x^4 + px^2 + qx + r = 0
180   F32 A2 = A * A;
181   F32 A3 = A2 * A;
182   F32 A4 = A2 * A2;
183
184   F32 p = ((-3.f/8.f) * A2) + B;
185   F32 q = ((1.f/8.f) * A3) - ((1.f/2.f) * A * B) + C;
186   F32 r = ((-3.f/256.f) * A4) + ((1.f/16.f) * A2 * B) - ((1.f/4.f) * A * C) + D;
187
188   U32 num = 0;
189   if(mIsZero(r)) // no absolute term: y(y^3 + py + q) = 0
190   {
191      num = mSolveCubic(1.f, 0.f, p, q, x);
192      x[num++] = 0.f;
193   }
194   else
195   {
196      // solve the resolvent cubic
197      F32 q2 = q * q;
198
199      a = 1.f;
200      b = (-1.f/2.f) * p;
201      c = -r;
202      d = ((1.f/2.f) * r * p) - ((1.f/8.f) * q2);
203
204      mSolveCubic(a, b, c, d, x);
205
206      F32 z = x[0];
207
208      // build 2 quadratic equations from the one solution
209      F32 u = (z * z) - r;
210      F32 v = (2.f * z) - p;
211
212      if(mIsZero(u))
213         u = 0.f;
214      else if(u > 0.f)
215         u = mSqrt(u);
216      else
217         return(0);
218
219      if(mIsZero(v))
220         v = 0.f;
221      else if(v > 0.f)
222         v = mSqrt(v);
223      else
224         return(0);
225
226      // solve the two quadratics
227      a = 1.f;
228      b = v;
229      c = z - u;
230      num = mSolveQuadratic(a, b, c, x);
231
232      a = 1.f;
233      b = -v;
234      c = z + u;
235      num += mSolveQuadratic(a, b, c, x + num);
236   }
237
238   // resubstitute
239   F32 sub = (1.f/4.f) * A;
240   for(U32 i = 0; i < num; i++)
241      x[i] -= sub;
242
243   // sort the roots
244   for(S32 j = 0; j < (num - 1); j++)
245      for(S32 k = j + 1; k < num; k++)
246         if(x[k] < x[j])
247            swap(x[k], x[j]);
248
249   return(num);
250}
251
252U32 (*mSolveQuadratic)( F32 a, F32 b, F32 c, F32* x ) = mSolveQuadratic_c;
253U32 (*mSolveCubic)( F32 a, F32 b, F32 c, F32 d, F32* x ) = mSolveCubic_c;
254U32 (*mSolveQuartic)( F32 a, F32 b, F32 c, F32 d, F32 e, F32* x ) = mSolveQuartic_c;
255