Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/ogreode/OgreOdeEigenSolver.cpp @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 15 years ago

Merged physics branch into physics_new branch.
  • 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.