16 #include "libopenrave.h"
27 int CubicRoots (
double c0,
double c1,
double c2,
double *r0,
double *r1,
double *r2)
32 double discr, temp, pval, pdval, b0, b1;
38 discr = (double)sqrt(discr);
40 pval = temp*(temp*(temp-c2)+c1)-c0;
43 (*r0) = (c2-discr)/3 - 1;
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;
54 b0 = (*r0)*((*r0)-c2)+c1;
56 if ( discr < -g_fEpsilon )
66 discr = sqrt(fabs(discr));
67 (*r1) = 0.5f*(-b1-discr);
68 (*r2) = 0.5f*(-b1+discr);
70 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
75 if ( fabs((*r1)-(*r2)) <= 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;
96 b0 = (*r2)*((*r2)-c2)+c1;
98 if ( discr < -g_fEpsilon )
109 discr = sqrt(fabs(discr));
110 (*r0) = 0.5f*(-b1-discr);
111 (*r1) = 0.5f*(-b1+discr);
113 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
118 if ( fabs((*r1)-(*r2)) <= 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;
147 for (
int i0 = 0; i0 < 3; i0++)
149 const int iMaxIter = 32;
151 for (iIter = 0; iIter < iMaxIter; iIter++)
154 for (i1 = i0; i1 <= 1; i1++)
165 T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
167 if ( fTmp0 < 0.0f ) {
168 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
171 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
176 for (
int i2 = i1-1; i2 >= i0; i2--)
178 T fTmp3 = fSin*afSubDiag[i2];
179 T fTmp4 = fCos*afSubDiag[i2];
184 afSubDiag[i2+1] = fTmp3*fTmp1;
192 afSubDiag[i2+1] = fTmp0*fTmp1;
196 fTmp0 = afDiag[i2+1]-fTmp2;
197 fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos;
199 afDiag[i2+1] = fTmp0+fTmp2;
200 fTmp0 = fCos*fTmp1-fTmp4;
202 for (
int iRow = 0; iRow < 3; iRow++)
204 fTmp3 = m_aafEntry[iRow*3+i2+1];
205 m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
207 m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
212 afSubDiag[i0] = fTmp0;
213 afSubDiag[i1] = 0.0f;
216 if ( iIter == iMaxIter )
228 return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
231 bool QLAlgorithm3 (
double* m_aafEntry,
double* afDiag,
double* afSubDiag)
233 return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
240 memcpy(fevecs, fmat,
sizeof(
double)*9);
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]);
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];