openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
math.cpp
Go to the documentation of this file.
1 // Copyright (C) 2006-2008 Carnegie Mellon University (rdiankov@cs.cmu.edu)
2 //
3 // This file is part of OpenRAVE.
4 // OpenRAVE is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation, either version 3 of the License, or
7 // at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program. If not, see <http://www.gnu.org/licenses/>.
16 #include "libopenrave.h"
17 
18 #include <algorithm>
19 
20 namespace OpenRAVE {
21 namespace mathextra {
22 
23 // code from MagicSoftware by Dave Eberly
24 
25 //===========================================================================
26 
27 int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2)
28 {
29  // polynomial is L^3-c2*L^2+c1*L-c0
30 
31  int maxiter = 50;
32  double discr, temp, pval, pdval, b0, b1;
33  int i;
34 
35  // find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1
36  discr = c2*c2-3*c1;
37  if ( discr >= 0.0 ) {
38  discr = (double)sqrt(discr);
39  temp = (c2+discr)/3;
40  pval = temp*(temp*(temp-c2)+c1)-c0;
41  if ( pval >= 0.0 ) {
42  // double root occurs before the positive local maximum
43  (*r0) = (c2-discr)/3 - 1; // initial guess for Newton's methods
44  pval = 2*g_fEpsilon;
45  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
46  pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
47  pdval = (*r0)*(3*(*r0)-2*c2)+c1;
48  (*r0) -= pval/pdval;
49  }
50 
51  // Other two roots are solutions to quadratic equation
52  // L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0.
53  b1 = (*r0)-c2;
54  b0 = (*r0)*((*r0)-c2)+c1;
55  discr = b1*b1-4*b0;
56  if ( discr < -g_fEpsilon )
57  {
58  // single root r0
59  return singleRoot;
60  }
61  else
62  {
63  int result = distinctRoots;
64 
65  // roots r0 <= r1 <= r2
66  discr = sqrt(fabs(discr));
67  (*r1) = 0.5f*(-b1-discr);
68  (*r2) = 0.5f*(-b1+discr);
69 
70  if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
71  {
72  (*r0) = (*r1);
73  result |= floatRoot01;
74  }
75  if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
76  {
77  (*r1) = (*r2);
78  result |= floatRoot12;
79  }
80  return result;
81  }
82  }
83  else {
84  // double root occurs after the negative local minimum
85  (*r2) = temp + 1; // initial guess for Newton's method
86  pval = 2*g_fEpsilon;
87  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
88  pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
89  pdval = (*r2)*(3*(*r2)-2*c2)+c1;
90  (*r2) -= pval/pdval;
91  }
92 
93  // Other two roots are solutions to quadratic equation
94  // L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0.
95  b1 = (*r2)-c2;
96  b0 = (*r2)*((*r2)-c2)+c1;
97  discr = b1*b1-4*b0;
98  if ( discr < -g_fEpsilon )
99  {
100  // single root
101  (*r0) = (*r2);
102  return singleRoot;
103  }
104  else
105  {
106  int result = distinctRoots;
107 
108  // roots r0 <= r1 <= r2
109  discr = sqrt(fabs(discr));
110  (*r0) = 0.5f*(-b1-discr);
111  (*r1) = 0.5f*(-b1+discr);
112 
113  if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
114  {
115  (*r0) = (*r1);
116  result |= floatRoot01;
117  }
118  if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
119  {
120  (*r1) = (*r2);
121  result |= floatRoot12;
122  }
123  return result;
124  }
125  }
126  }
127  else {
128  // p(L) has one double root
129  (*r0) = c0;
130  pval = 2*g_fEpsilon;
131  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
132  pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
133  pdval = (*r0)*(3*(*r0)-2*c2)+c1;
134  (*r0) -= pval/pdval;
135  }
136  return singleRoot;
137  }
138 }
139 
140 //----------------------------------------------------------------------------
141 template <class T>
142 bool _QLAlgorithm3 (T* m_aafEntry, T* afDiag, T* afSubDiag)
143 {
144  // QL iteration with implicit shifting to reduce matrix from tridiagonal
145  // to diagonal
146 
147  for (int i0 = 0; i0 < 3; i0++)
148  {
149  const int iMaxIter = 32;
150  int iIter;
151  for (iIter = 0; iIter < iMaxIter; iIter++)
152  {
153  int i1;
154  for (i1 = i0; i1 <= 1; i1++)
155  {
156  T fSum = geometry::MATH_FABS(afDiag[i1]) + geometry::MATH_FABS(afDiag[i1+1]);
157  if ( geometry::MATH_FABS(afSubDiag[i1]) + fSum == fSum ) {
158  break;
159  }
160  }
161  if ( i1 == i0 ) {
162  break;
163  }
164 
165  T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
166  T fTmp1 = geometry::MATH_SQRT(fTmp0*fTmp0+1.0f);
167  if ( fTmp0 < 0.0f ) {
168  fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
169  }
170  else {
171  fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
172  }
173  T fSin = 1.0f;
174  T fCos = 1.0f;
175  T fTmp2 = 0.0f;
176  for (int i2 = i1-1; i2 >= i0; i2--)
177  {
178  T fTmp3 = fSin*afSubDiag[i2];
179  T fTmp4 = fCos*afSubDiag[i2];
180  if ( geometry::MATH_FABS(fTmp3) >= geometry::MATH_FABS(fTmp0) )
181  {
182  fCos = fTmp0/fTmp3;
183  fTmp1 = geometry::MATH_SQRT(fCos*fCos+1.0f);
184  afSubDiag[i2+1] = fTmp3*fTmp1;
185  fSin = 1.0f/fTmp1;
186  fCos *= fSin;
187  }
188  else
189  {
190  fSin = fTmp3/fTmp0;
191  fTmp1 = geometry::MATH_SQRT(fSin*fSin+1.0f);
192  afSubDiag[i2+1] = fTmp0*fTmp1;
193  fCos = 1.0f/fTmp1;
194  fSin *= fCos;
195  }
196  fTmp0 = afDiag[i2+1]-fTmp2;
197  fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos;
198  fTmp2 = fSin*fTmp1;
199  afDiag[i2+1] = fTmp0+fTmp2;
200  fTmp0 = fCos*fTmp1-fTmp4;
201 
202  for (int iRow = 0; iRow < 3; iRow++)
203  {
204  fTmp3 = m_aafEntry[iRow*3+i2+1];
205  m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
206  fCos*fTmp3;
207  m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
208  fSin*fTmp3;
209  }
210  }
211  afDiag[i0] -= fTmp2;
212  afSubDiag[i0] = fTmp0;
213  afSubDiag[i1] = 0.0f;
214  }
215 
216  if ( iIter == iMaxIter )
217  {
218  // should not get here under normal circumstances
219  return false;
220  }
221  }
222 
223  return true;
224 }
225 
226 bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag)
227 {
228  return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
229 }
230 
231 bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag)
232 {
233  return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
234 }
235 
236 void EigenSymmetric3(const double* fmat, double* afEigenvalue, double* fevecs)
237 {
238  double afSubDiag[3];
239 
240  memcpy(fevecs, fmat, sizeof(double)*9);
241  Tridiagonal3(fevecs, afEigenvalue,afSubDiag);
242  QLAlgorithm3(fevecs, afEigenvalue,afSubDiag);
243 
244  // make eigenvectors form a right--handed system
245  double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) +
246  fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) +
247  fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]);
248  if ( fDet < 0.0f )
249  {
250  fevecs[0*3+2] = -fevecs[0*3+2];
251  fevecs[1*3+2] = -fevecs[1*3+2];
252  fevecs[2*3+2] = -fevecs[2*3+2];
253  }
254 }
255 /* end of MAGIC code */
256 
257 } // end namespace geometry
258 } // end namespace OpenRAVE