Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: downloads/ogreode/src/OgreOdeEigenSolver.cpp @ 21

Last change on this file since 21 was 21, checked in by nicolasc, 17 years ago
added ogreode and Colladaplugin
File size: 9.6 KB

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

Note: See TracBrowser for help on using the repository browser.

Download in other formats: