1 #include "float_math.h"
8 /*----------------------------------------------------------------------
9 Copyright (c) 2004 Open Dynamics Framework Group
13 Redistribution and use in source and binary forms, with or without modification, are permitted provided
14 that the following conditions are met:
16 Redistributions of source code must retain the above copyright notice, this list of conditions
17 and the following disclaimer.
19 Redistributions in binary form must reproduce the above copyright notice,
20 this list of conditions and the following disclaimer in the documentation
21 and/or other materials provided with the distribution.
23 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
24 be used to endorse or promote products derived from this software without specific prior written permission.
26 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
27 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
29 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
31 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
32 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 -----------------------------------------------------------------------*/
35 // http://codesuppository.blogspot.com
37 // mailto: jratcliff@infiniplex.net
39 // http://www.amillionpixels.us
41 // Geometric Tools, Inc.
42 // http://www.geometrictools.com
43 // Copyright (c) 1998-2006. All Rights Reserved
45 // The Wild Magic Library (WM3) source code is supplied under the terms of
46 // the license agreement
47 // http://www.geometrictools.com/License/WildMagic3License.pdf
48 // and may not be copied or disclosed except in accordance with the terms
60 Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
63 float dot(const Vec3 &v)
65 return x*v.x + y*v.y + z*v.z; // the dot product
79 void DecrSortEigenStuff(void)
81 Tridiagonal(); //diagonalize the matrix.
87 void Tridiagonal(void)
89 float fM00 = mElement[0][0];
90 float fM01 = mElement[0][1];
91 float fM02 = mElement[0][2];
92 float fM11 = mElement[1][1];
93 float fM12 = mElement[1][2];
94 float fM22 = mElement[2][2];
98 if (fM02 != (float)0.0)
100 float fLength = sqrtf(fM01*fM01+fM02*fM02);
101 float fInvLength = ((float)1.0)/fLength;
104 float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
105 m_afDiag[1] = fM11+fM02*fQ;
106 m_afDiag[2] = fM22-fM02*fQ;
107 m_afSubd[0] = fLength;
108 m_afSubd[1] = fM12-fM01*fQ;
109 mElement[0][0] = (float)1.0;
110 mElement[0][1] = (float)0.0;
111 mElement[0][2] = (float)0.0;
112 mElement[1][0] = (float)0.0;
113 mElement[1][1] = fM01;
114 mElement[1][2] = fM02;
115 mElement[2][0] = (float)0.0;
116 mElement[2][1] = fM02;
117 mElement[2][2] = -fM01;
118 m_bIsRotation = false;
126 mElement[0][0] = (float)1.0;
127 mElement[0][1] = (float)0.0;
128 mElement[0][2] = (float)0.0;
129 mElement[1][0] = (float)0.0;
130 mElement[1][1] = (float)1.0;
131 mElement[1][2] = (float)0.0;
132 mElement[2][0] = (float)0.0;
133 mElement[2][1] = (float)0.0;
134 mElement[2][2] = (float)1.0;
135 m_bIsRotation = true;
139 bool QLAlgorithm(void)
141 const int iMaxIter = 32;
143 for (int i0 = 0; i0 <3; i0++)
146 for (i1 = 0; i1 < iMaxIter; i1++)
149 for (i2 = i0; i2 <= (3-2); i2++)
151 float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2+1]);
152 if ( fabsf(m_afSubd[i2]) + fTmp == fTmp )
160 float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
161 float fR = sqrtf(fG*fG+(float)1.0);
164 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
168 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
170 float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
171 for (int i3 = i2-1; i3 >= i0; i3--)
173 float fF = fSin*m_afSubd[i3];
174 float fB = fCos*m_afSubd[i3];
175 if (fabsf(fF) >= fabsf(fG))
178 fR = sqrtf(fCos*fCos+(float)1.0);
179 m_afSubd[i3+1] = fF*fR;
180 fSin = ((float)1.0)/fR;
186 fR = sqrtf(fSin*fSin+(float)1.0);
187 m_afSubd[i3+1] = fG*fR;
188 fCos = ((float)1.0)/fR;
191 fG = m_afDiag[i3+1]-fP;
192 fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
194 m_afDiag[i3+1] = fG+fP;
196 for (int i4 = 0; i4 < 3; i4++)
198 fF = mElement[i4][i3+1];
199 mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
200 mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
205 m_afSubd[i2] = (float)0.0;
215 void DecreasingSort(void)
217 //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
218 for (int i0 = 0, i1; i0 <= 3-2; i0++)
220 // locate maximum eigenvalue
222 float fMax = m_afDiag[i1];
224 for (i2 = i0+1; i2 < 3; i2++)
226 if (m_afDiag[i2] > fMax)
236 m_afDiag[i1] = m_afDiag[i0];
239 for (i2 = 0; i2 < 3; i2++)
241 float fTmp = mElement[i2][i0];
242 mElement[i2][i0] = mElement[i2][i1];
243 mElement[i2][i1] = fTmp;
244 m_bIsRotation = !m_bIsRotation;
251 void GuaranteeRotation(void)
255 // change sign on the first column
256 for (int iRow = 0; iRow <3; iRow++)
258 mElement[iRow][0] = -mElement[iRow][0];
263 float mElement[3][3];
272 using namespace BestFit;
275 bool getBestFitPlane(unsigned int vcount,
277 unsigned int vstride,
278 const float *weights,
279 unsigned int wstride,
290 const char *source = (const char *) points;
291 const char *wsource = (const char *) weights;
293 for (unsigned int i=0; i<vcount; i++)
296 const float *p = (const float *) source;
302 const float *ws = (const float *) wsource;
317 float recip = 1.0f / wtotal; // reciprocol of total weighting
335 const char *source = (const char *) points;
336 const char *wsource = (const char *) weights;
338 for (unsigned int i=0; i<vcount; i++)
341 const float *p = (const float *) source;
347 const float *ws = (const float *) wsource;
354 kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
355 kDiff.y = w*(p[1] - kOrigin.y);
356 kDiff.z = w*(p[2] - kOrigin.z);
358 fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
359 fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
360 fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
362 fSumYY+= kDiff.y * kDiff.y;
363 fSumYZ+= kDiff.y * kDiff.z;
364 fSumZZ+= kDiff.z * kDiff.z;
378 // setup the eigensolver
381 kES.mElement[0][0] = fSumXX;
382 kES.mElement[0][1] = fSumXY;
383 kES.mElement[0][2] = fSumXZ;
385 kES.mElement[1][0] = fSumXY;
386 kES.mElement[1][1] = fSumYY;
387 kES.mElement[1][2] = fSumYZ;
389 kES.mElement[2][0] = fSumXZ;
390 kES.mElement[2][1] = fSumYZ;
391 kES.mElement[2][2] = fSumZZ;
393 // compute eigenstuff, smallest eigenvalue is in last position
394 kES.DecrSortEigenStuff();
398 kNormal.x = kES.mElement[0][2];
399 kNormal.y = kES.mElement[1][2];
400 kNormal.z = kES.mElement[2][2];
402 // the minimum energy
403 plane[0] = kNormal.x;
404 plane[1] = kNormal.y;
405 plane[2] = kNormal.z;
407 plane[3] = 0 - kNormal.dot(kOrigin);
414 float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax) // returns the diagonal distance
417 const unsigned char *source = (const unsigned char *) points;
428 for (unsigned int i=1; i<vcount; i++)
431 const float *p = (const float *) source;
433 if ( p[0] < bmin[0] ) bmin[0] = p[0];
434 if ( p[1] < bmin[1] ) bmin[1] = p[1];
435 if ( p[2] < bmin[2] ) bmin[2] = p[2];
437 if ( p[0] > bmax[0] ) bmax[0] = p[0];
438 if ( p[1] > bmax[1] ) bmax[1] = p[1];
439 if ( p[2] > bmax[2] ) bmax[2] = p[2];
443 float dx = bmax[0] - bmin[0];
444 float dy = bmax[1] - bmin[1];
445 float dz = bmax[2] - bmin[2];
447 return sqrtf( dx*dx + dy*dy + dz*dz );
452 bool overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) // return true if the two AABB's overlap.
454 if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
455 if ( bmax2[1] < bmin1[1] ) return false;
456 if ( bmax2[2] < bmin1[2] ) return false;
458 if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
459 if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
460 if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
463 return true; // the extents overlap