Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/ogreode/OgreOdeEigenSolver.cpp @ 1963

Last change on this file since 1963 was 1923, checked in by rgrieder, 16 years ago

Cleaned up the heavy mess with header file includes in OgreOde. It should now compile a lot faster.
  • Property svn:eol-style set to native
File size: 9.6 KB
Line 
1#include "OgreOdePrecompiledHeaders.h"
2#include "OgreOdeEigenSolver.h"
3
4using namespace OgreOde;
5using namespace Ogre;
6
7void EigenSolver::DecrSortEigenStuff3 ()
8{
9        Tridiagonal3();
10        QLAlgorithm();
11        DecreasingSort();
12        GuaranteeRotation();
13}
14
15void EigenSolver::Tridiagonal3 ()
16{
17        const Ogre::Real fM00 = m_kMat[0][0];
18   Ogre::Real fM01 = m_kMat[0][1];
19        Real fM02 = m_kMat[0][2];
20        const Ogre::Real fM11 = m_kMat[1][1];
21        const Ogre::Real fM12 = m_kMat[1][2];
22        const Ogre::Real fM22 = m_kMat[2][2];
23
24        m_afDiag[0] = fM00;
25        m_afSubd[2] = (Real)0.0;
26        if ( fM02 != (Real)0.0 )
27        {
28                const Ogre::Real fLength = sqrtf(fM01*fM01+fM02*fM02);
29                const Ogre::Real fInvLength = ((Real)1.0)/fLength;
30                fM01 *= fInvLength;
31                fM02 *= fInvLength;
32                const Ogre::Real fQ = ((Real)2.0)*fM01*fM12+fM02*(fM22-fM11);
33                m_afDiag[1] = fM11+fM02*fQ;
34                m_afDiag[2] = fM22-fM02*fQ;
35                m_afSubd[0] = fLength;
36                m_afSubd[1] = fM12-fM01*fQ;
37                m_kMat[0][0] = (Real)1.0;
38                m_kMat[0][1] = (Real)0.0;
39                m_kMat[0][2] = (Real)0.0;
40                m_kMat[1][0] = (Real)0.0;
41                m_kMat[1][1] = fM01;
42                m_kMat[1][2] = fM02;
43                m_kMat[2][0] = (Real)0.0;
44                m_kMat[2][1] = fM02;
45                m_kMat[2][2] = -fM01;
46        }
47        else
48        {
49                m_afDiag[1] = fM11;
50                m_afDiag[2] = fM22;
51                m_afSubd[0] = fM01;
52                m_afSubd[1] = fM12;
53                m_kMat[0][0] = (Real)1.0;
54                m_kMat[0][1] = (Real)0.0;
55                m_kMat[0][2] = (Real)0.0;
56                m_kMat[1][0] = (Real)0.0;
57                m_kMat[1][1] = (Real)1.0;
58                m_kMat[1][2] = (Real)0.0;
59                m_kMat[2][0] = (Real)0.0;
60                m_kMat[2][1] = (Real)0.0;
61                m_kMat[2][2] = (Real)1.0;
62        }
63}
64
65bool EigenSolver::QLAlgorithm ()
66{
67        const int iMaxIter = 32;
68
69        for (int i0 = 0; i0 < m_iSize; i0++)
70        {
71                int i1;
72                for (i1 = 0; i1 < iMaxIter; i1++)
73                {
74                        int i2;
75                        for (i2 = i0; i2 <= m_iSize-2; i2++)
76                        {
77                                Real fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
78                                if ( fabs(m_afSubd[i2]) + fTmp == fTmp ) break;
79                        }
80                        if ( i2 == i0 ) break;
81
82                        Real fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((Real)2.0) * m_afSubd[i0]);
83                        Real fR = sqrtf(fG*fG+(Real)1.0);
84                        if ( fG < (Real)0.0 ) fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
85                        else fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
86                        Real fSin = (Real)1.0, fCos = (Real)1.0, fP = (Real)0.0;
87                        for (int i3 = i2-1; i3 >= i0; i3--)
88                        {
89                                Real fF = fSin*m_afSubd[i3];
90               Ogre::Real fB = fCos*m_afSubd[i3];
91                    if ( fabs(fF) >= fabs(fG) )
92                        {
93                                fCos = fG/fF;
94                                    fR = sqrtf(fCos*fCos+(Real)1.0);
95                                        m_afSubd[i3+1] = fF*fR;
96                    fSin = ((Real)1.0)/fR;
97                        fCos *= fSin;
98                        }
99                            else
100                                {
101                                        fSin = fF/fG;
102                                        fR = sqrtf(fSin*fSin+(Real)1.0);
103                                        m_afSubd[i3+1] = fG*fR;
104                                        fCos = ((Real)1.0)/fR;
105                                        fSin *= fCos;
106                                }
107                                fG = m_afDiag[i3+1]-fP;
108                fR = (m_afDiag[i3]-fG)*fSin+((Real)2.0)*fB*fCos;
109                    fP = fSin*fR;
110                        m_afDiag[i3+1] = fG+fP;
111                            fG = fCos*fR-fB;
112
113                                for (int i4 = 0; i4 < m_iSize; i4++)
114                                {
115                                        fF = m_kMat[i4][i3+1];
116                    m_kMat[i4][i3+1] = fSin*m_kMat[i4][i3]+fCos*fF;
117                        m_kMat[i4][i3] = fCos*m_kMat[i4][i3]-fSin*fF;
118                        }
119                        }
120                        m_afDiag[i0] -= fP;
121                        m_afSubd[i0] = fG;
122                        m_afSubd[i2] = (Real)0.0;
123                }
124                if ( i1 == iMaxIter ) return false;
125        }
126        return true;
127}
128
129void EigenSolver::DecreasingSort ()
130{
131        // sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
132        for (int i0 = 0, i1; i0 <= m_iSize-2; i0++)
133        {
134                // locate maximum eigenvalue
135                i1 = i0;
136                Real fMax = m_afDiag[i1];
137                int i2;
138                for (i2 = i0+1; i2 < m_iSize; i2++)
139                {
140                        if ( m_afDiag[i2] > fMax )
141                        {
142                                i1 = i2;
143                                fMax = m_afDiag[i1];
144                        }
145                }
146
147                if ( i1 != i0 )
148                {
149                        // swap eigenvalues
150                        m_afDiag[i1] = m_afDiag[i0];
151                        m_afDiag[i0] = fMax;
152
153                        // swap eigenvectors
154                        for (i2 = 0; i2 < m_iSize; i2++)
155                        {
156                                Real fTmp = m_kMat[i2][i0];
157                                m_kMat[i2][i0] = m_kMat[i2][i1];
158                                m_kMat[i2][i1] = fTmp;
159                                m_bIsRotation = !m_bIsRotation;
160                        }
161                }
162        }
163}
164
165void EigenSolver::GuaranteeRotation ()
166{
167        if ( !m_bIsRotation )
168        {
169                // change sign on the first column
170                for (int iRow = 0; iRow < m_iSize; iRow++) 
171            m_kMat[iRow][0] = -m_kMat[iRow][0];
172        }
173}
174
175void EigenSolver::orthogonalLineFit(unsigned int vertex_count, const Ogre::Vector3* vertices,Vector3& origin,Vector3& direction)
176{
177        unsigned int i;
178
179    // compute average of points
180        origin = vertices[0];
181    for(i = 1; i < vertex_count; ++i) 
182        origin += vertices[i];
183
184        const Ogre::Real fInvQuantity = 1.0 / vertex_count;
185        origin *= fInvQuantity;
186
187        // compute sums of products
188        Real fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
189   Ogre::Real fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
190        for (i = 0; i < vertex_count; i++) 
191        {
192                const Ogre::Vector3 kDiff (vertices[i] - origin);
193            fSumXX += kDiff.x*kDiff.x;
194        fSumXY += kDiff.x*kDiff.y;
195                fSumXZ += kDiff.x*kDiff.z;
196            fSumYY += kDiff.y*kDiff.y;
197        fSumYZ += kDiff.y*kDiff.z;
198                fSumZZ += kDiff.z*kDiff.z;
199        }
200        fSumXX *= fInvQuantity;
201        fSumXY *= fInvQuantity;
202        fSumXZ *= fInvQuantity;
203        fSumYY *= fInvQuantity;
204        fSumYZ *= fInvQuantity;
205        fSumZZ *= fInvQuantity;
206
207        // setup the eigensolver
208        EigenSolver kES(3);
209        kES(0,0) = fSumYY+fSumZZ;
210        kES(0,1) = -fSumXY;
211        kES(0,2) = -fSumXZ;
212        kES(1,0) = kES(0,1);
213        kES(1,1) = fSumXX+fSumZZ;
214        kES(1,2) = -fSumYZ;
215        kES(2,0) = kES(0,2);
216        kES(2,1) = kES(1,2);
217    kES(2,2) = fSumXX+fSumYY;
218
219        // compute eigenstuff, smallest eigenvalue is in last position
220        kES.DecrSortEigenStuff3();
221
222    // unit-length direction for best-fit line
223        kES.GetEigenvector(2,direction);
224}
225
226Real EigenSolver::SqrDistance(const Ogre::Vector3& rkPoint,const Ogre::Vector3& origin,const Ogre::Vector3& direction)
227{
228        Vector3 kDiff(rkPoint - origin);
229    const Ogre::Real fSqrLen = direction.squaredLength();
230        const Ogre::Real fT = kDiff.dotProduct(direction) / fSqrLen;
231
232    kDiff -= fT*direction;
233
234        return kDiff.squaredLength();
235}
236
237void EigenSolver::GenerateOrthonormalBasis (Vector3& rkU, Ogre::Vector3& rkV, Ogre::Vector3& rkW, bool bUnitLengthW)
238{
239        if ( !bUnitLengthW ) rkW.normalise();
240
241        Real fInvLength;
242
243        if ( fabs(rkW[0]) >= fabs(rkW[1]) )
244        {
245                // W.x or W.z is the largest magnitude component, swap them
246                fInvLength = 1.0 / sqrtf(rkW[0]*rkW[0] + rkW[2]*rkW[2]);
247
248            rkU[0] = -rkW[2]*fInvLength;
249        rkU[1] = (Real)0.0;
250                rkU[2] = +rkW[0]*fInvLength;
251        }
252    else
253        {
254        // W.y or W.z is the largest magnitude component, swap them
255                fInvLength = 1.0 / sqrtf(rkW[1]*rkW[1] + rkW[2]*rkW[2]);
256
257            rkU[0] = (Real)0.0;
258        rkU[1] = +rkW[2]*fInvLength;
259                rkU[2] = -rkW[1]*fInvLength;
260        }
261
262        rkV = rkW.crossProduct(rkU);
263}
264
265EigenSolver::EigenSolver(int iSize)
266{
267        assert( iSize >= 2 );
268    m_iSize = iSize;
269        m_afDiag = new Ogre::Real[m_iSize];
270    m_afSubd = new Ogre::Real[m_iSize];
271
272        // set according to the parity of the number of Householder reflections
273        m_bIsRotation = ((iSize % 2) == 0);
274}
275
276Ogre::Real& EigenSolver::operator() (int iRow, int iCol)
277{
278        return m_kMat[iRow][iCol];
279}
280
281EigenSolver::~EigenSolver()
282{
283    delete[] m_afSubd;
284        delete[] m_afDiag;
285}
286
287void EigenSolver::GetEigenvector (int i, Ogre::Vector3& rkV) const
288{
289        assert( m_iSize == 3 );
290        if ( m_iSize == 3 )
291        {
292                for (int iRow = 0; iRow < m_iSize; iRow++) 
293            rkV[iRow] = m_kMat[iRow][i];
294        }
295        else
296        {
297            rkV = Ogre::Vector3::ZERO;
298    }
299}
300
301void EigenSolver::GaussPointsFit(unsigned int iQuantity,const Ogre::Vector3* akPoint,Vector3 &rkCenter,Vector3 akAxis[3],Real afExtent[3])
302{
303    // compute mean of points
304    rkCenter = akPoint[0];
305    unsigned int i;
306    for (i = 1; i < iQuantity; i++)
307        rkCenter += akPoint[i];
308    const Ogre::Real fInvQuantity = ((Real)1.0)/iQuantity;
309    rkCenter *= fInvQuantity;
310
311    // compute covariances of points
312   Ogre::Real fSumXX = (Real)0.0, fSumXY = (Real)0.0, fSumXZ = (Real)0.0;
313   Ogre::Real fSumYY = (Real)0.0, fSumYZ = (Real)0.0, fSumZZ = (Real)0.0;
314    for (i = 0; i < iQuantity; i++)
315    {
316        const Ogre::Vector3 kDiff (akPoint[i] - rkCenter);
317        fSumXX += kDiff.x*kDiff.x;
318        fSumXY += kDiff.x*kDiff.y;
319        fSumXZ += kDiff.x*kDiff.z;
320        fSumYY += kDiff.y*kDiff.y;
321        fSumYZ += kDiff.y*kDiff.z;
322        fSumZZ += kDiff.z*kDiff.z;
323    }
324    fSumXX *= fInvQuantity;
325    fSumXY *= fInvQuantity;
326    fSumXZ *= fInvQuantity;
327    fSumYY *= fInvQuantity;
328    fSumYZ *= fInvQuantity;
329    fSumZZ *= fInvQuantity;
330
331    // compute eigenvectors for covariance matrix
332    EigenSolver kES(3);
333    kES(0,0) = fSumXX;
334    kES(0,1) = fSumXY;
335    kES(0,2) = fSumXZ;
336    kES(1,0) = fSumXY;
337    kES(1,1) = fSumYY;
338    kES(1,2) = fSumYZ;
339    kES(2,0) = fSumXZ;
340    kES(2,1) = fSumYZ;
341    kES(2,2) = fSumZZ;
342    kES.IncrSortEigenStuff3();
343
344    for (i = 0; i < 3; i++)
345    {
346        afExtent[i] = kES.GetEigenvalue(i);
347        kES.GetEigenvector(i,akAxis[i]);
348    }
349}
350
351Real EigenSolver::GetEigenvalue (int i) const
352{
353    return m_afDiag[i];
354}
355
356void EigenSolver::IncrSortEigenStuff3 ()
357{
358    Tridiagonal3();
359    QLAlgorithm();
360    IncreasingSort();
361    GuaranteeRotation();
362}
363
364void EigenSolver::IncreasingSort ()
365{
366    // sort eigenvalues in increasing order, e[0] <= ... <= e[iSize-1]
367    for (int i0 = 0, i1; i0 <= m_iSize-2; i0++)
368    {
369        // locate minimum eigenvalue
370        i1 = i0;
371       Ogre::Real fMin = m_afDiag[i1];
372        int i2;
373        for (i2 = i0+1; i2 < m_iSize; i2++)
374        {
375            if ( m_afDiag[i2] < fMin )
376            {
377                i1 = i2;
378                fMin = m_afDiag[i1];
379            }
380        }
381
382        if ( i1 != i0 )
383        {
384            // swap eigenvalues
385            m_afDiag[i1] = m_afDiag[i0];
386            m_afDiag[i0] = fMin;
387
388            // swap eigenvectors
389            for (i2 = 0; i2 < m_iSize; i2++)
390            {
391               Ogre::Real fTmp = m_kMat[i2][i0];
392                m_kMat[i2][i0] = m_kMat[i2][i1];
393                m_kMat[i2][i1] = fTmp;
394                m_bIsRotation = !m_bIsRotation;
395            }
396        }
397    }
398}
Note: See TracBrowser for help on using the repository browser.