aboutsummaryrefslogtreecommitdiff
path: root/tests/bullet/Extras/ConvexDecomposition/bestfit.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tests/bullet/Extras/ConvexDecomposition/bestfit.cpp')
-rw-r--r--tests/bullet/Extras/ConvexDecomposition/bestfit.cpp466
1 files changed, 466 insertions, 0 deletions
diff --git a/tests/bullet/Extras/ConvexDecomposition/bestfit.cpp b/tests/bullet/Extras/ConvexDecomposition/bestfit.cpp
new file mode 100644
index 00000000..e6469f6f
--- /dev/null
+++ b/tests/bullet/Extras/ConvexDecomposition/bestfit.cpp
@@ -0,0 +1,466 @@
+#include "float_math.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <math.h>
+
+/*----------------------------------------------------------------------
+ Copyright (c) 2004 Open Dynamics Framework Group
+ www.physicstools.org
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided
+ that the following conditions are met:
+
+ Redistributions of source code must retain the above copyright notice, this list of conditions
+ and the following disclaimer.
+
+ Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+ Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
+ be used to endorse or promote products derived from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
+ IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-----------------------------------------------------------------------*/
+
+// http://codesuppository.blogspot.com
+//
+// mailto: jratcliff@infiniplex.net
+//
+// http://www.amillionpixels.us
+//
+// Geometric Tools, Inc.
+// http://www.geometrictools.com
+// Copyright (c) 1998-2006. All Rights Reserved
+//
+// The Wild Magic Library (WM3) source code is supplied under the terms of
+// the license agreement
+// http://www.geometrictools.com/License/WildMagic3License.pdf
+// and may not be copied or disclosed except in accordance with the terms
+// of that agreement.
+
+#include "bestfit.h"
+
+namespace BestFit
+{
+
+class Vec3
+{
+public:
+ Vec3(void) { };
+ Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
+
+
+ float dot(const Vec3 &v)
+ {
+ return x*v.x + y*v.y + z*v.z; // the dot product
+ }
+
+ float x;
+ float y;
+ float z;
+};
+
+
+class Eigen
+{
+public:
+
+
+ void DecrSortEigenStuff(void)
+ {
+ Tridiagonal(); //diagonalize the matrix.
+ QLAlgorithm(); //
+ DecreasingSort();
+ GuaranteeRotation();
+ }
+
+ void Tridiagonal(void)
+ {
+ float fM00 = mElement[0][0];
+ float fM01 = mElement[0][1];
+ float fM02 = mElement[0][2];
+ float fM11 = mElement[1][1];
+ float fM12 = mElement[1][2];
+ float fM22 = mElement[2][2];
+
+ m_afDiag[0] = fM00;
+ m_afSubd[2] = 0;
+ if (fM02 != (float)0.0)
+ {
+ float fLength = sqrtf(fM01*fM01+fM02*fM02);
+ float fInvLength = ((float)1.0)/fLength;
+ fM01 *= fInvLength;
+ fM02 *= fInvLength;
+ float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
+ m_afDiag[1] = fM11+fM02*fQ;
+ m_afDiag[2] = fM22-fM02*fQ;
+ m_afSubd[0] = fLength;
+ m_afSubd[1] = fM12-fM01*fQ;
+ mElement[0][0] = (float)1.0;
+ mElement[0][1] = (float)0.0;
+ mElement[0][2] = (float)0.0;
+ mElement[1][0] = (float)0.0;
+ mElement[1][1] = fM01;
+ mElement[1][2] = fM02;
+ mElement[2][0] = (float)0.0;
+ mElement[2][1] = fM02;
+ mElement[2][2] = -fM01;
+ m_bIsRotation = false;
+ }
+ else
+ {
+ m_afDiag[1] = fM11;
+ m_afDiag[2] = fM22;
+ m_afSubd[0] = fM01;
+ m_afSubd[1] = fM12;
+ mElement[0][0] = (float)1.0;
+ mElement[0][1] = (float)0.0;
+ mElement[0][2] = (float)0.0;
+ mElement[1][0] = (float)0.0;
+ mElement[1][1] = (float)1.0;
+ mElement[1][2] = (float)0.0;
+ mElement[2][0] = (float)0.0;
+ mElement[2][1] = (float)0.0;
+ mElement[2][2] = (float)1.0;
+ m_bIsRotation = true;
+ }
+ }
+
+ bool QLAlgorithm(void)
+ {
+ const int iMaxIter = 32;
+
+ for (int i0 = 0; i0 <3; i0++)
+ {
+ int i1;
+ for (i1 = 0; i1 < iMaxIter; i1++)
+ {
+ int i2;
+ for (i2 = i0; i2 <= (3-2); i2++)
+ {
+ float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2+1]);
+ if ( fabsf(m_afSubd[i2]) + fTmp == fTmp )
+ break;
+ }
+ if (i2 == i0)
+ {
+ break;
+ }
+
+ float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
+ float fR = sqrtf(fG*fG+(float)1.0);
+ if (fG < (float)0.0)
+ {
+ fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
+ }
+ else
+ {
+ fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
+ }
+ float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
+ for (int i3 = i2-1; i3 >= i0; i3--)
+ {
+ float fF = fSin*m_afSubd[i3];
+ float fB = fCos*m_afSubd[i3];
+ if (fabsf(fF) >= fabsf(fG))
+ {
+ fCos = fG/fF;
+ fR = sqrtf(fCos*fCos+(float)1.0);
+ m_afSubd[i3+1] = fF*fR;
+ fSin = ((float)1.0)/fR;
+ fCos *= fSin;
+ }
+ else
+ {
+ fSin = fF/fG;
+ fR = sqrtf(fSin*fSin+(float)1.0);
+ m_afSubd[i3+1] = fG*fR;
+ fCos = ((float)1.0)/fR;
+ fSin *= fCos;
+ }
+ fG = m_afDiag[i3+1]-fP;
+ fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
+ fP = fSin*fR;
+ m_afDiag[i3+1] = fG+fP;
+ fG = fCos*fR-fB;
+ for (int i4 = 0; i4 < 3; i4++)
+ {
+ fF = mElement[i4][i3+1];
+ mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
+ mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
+ }
+ }
+ m_afDiag[i0] -= fP;
+ m_afSubd[i0] = fG;
+ m_afSubd[i2] = (float)0.0;
+ }
+ if (i1 == iMaxIter)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void DecreasingSort(void)
+ {
+ //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
+ for (int i0 = 0, i1; i0 <= 3-2; i0++)
+ {
+ // locate maximum eigenvalue
+ i1 = i0;
+ float fMax = m_afDiag[i1];
+ int i2;
+ for (i2 = i0+1; i2 < 3; i2++)
+ {
+ if (m_afDiag[i2] > fMax)
+ {
+ i1 = i2;
+ fMax = m_afDiag[i1];
+ }
+ }
+
+ if (i1 != i0)
+ {
+ // swap eigenvalues
+ m_afDiag[i1] = m_afDiag[i0];
+ m_afDiag[i0] = fMax;
+ // swap eigenvectors
+ for (i2 = 0; i2 < 3; i2++)
+ {
+ float fTmp = mElement[i2][i0];
+ mElement[i2][i0] = mElement[i2][i1];
+ mElement[i2][i1] = fTmp;
+ m_bIsRotation = !m_bIsRotation;
+ }
+ }
+ }
+ }
+
+
+ void GuaranteeRotation(void)
+ {
+ if (!m_bIsRotation)
+ {
+ // change sign on the first column
+ for (int iRow = 0; iRow <3; iRow++)
+ {
+ mElement[iRow][0] = -mElement[iRow][0];
+ }
+ }
+ }
+
+ float mElement[3][3];
+ float m_afDiag[3];
+ float m_afSubd[3];
+ bool m_bIsRotation;
+};
+
+}
+
+
+using namespace BestFit;
+
+
+bool getBestFitPlane(unsigned int vcount,
+ const float *points,
+ unsigned int vstride,
+ const float *weights,
+ unsigned int wstride,
+ float *plane)
+{
+ bool ret = false;
+
+ Vec3 kOrigin(0,0,0);
+
+ float wtotal = 0;
+
+ if ( 1 )
+ {
+ const char *source = (const char *) points;
+ const char *wsource = (const char *) weights;
+
+ for (unsigned int i=0; i<vcount; i++)
+ {
+
+ const float *p = (const float *) source;
+
+ float w = 1;
+
+ if ( wsource )
+ {
+ const float *ws = (const float *) wsource;
+ w = *ws; //
+ wsource+=wstride;
+ }
+
+ kOrigin.x+=p[0]*w;
+ kOrigin.y+=p[1]*w;
+ kOrigin.z+=p[2]*w;
+
+ wtotal+=w;
+
+ source+=vstride;
+ }
+ }
+
+ float recip = 1.0f / wtotal; // reciprocol of total weighting
+
+ kOrigin.x*=recip;
+ kOrigin.y*=recip;
+ kOrigin.z*=recip;
+
+
+ float fSumXX=0;
+ float fSumXY=0;
+ float fSumXZ=0;
+
+ float fSumYY=0;
+ float fSumYZ=0;
+ float fSumZZ=0;
+
+
+ if ( 1 )
+ {
+ const char *source = (const char *) points;
+ const char *wsource = (const char *) weights;
+
+ for (unsigned int i=0; i<vcount; i++)
+ {
+
+ const float *p = (const float *) source;
+
+ float w = 1;
+
+ if ( wsource )
+ {
+ const float *ws = (const float *) wsource;
+ w = *ws; //
+ wsource+=wstride;
+ }
+
+ Vec3 kDiff;
+
+ kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
+ kDiff.y = w*(p[1] - kOrigin.y);
+ kDiff.z = w*(p[2] - kOrigin.z);
+
+ fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
+ fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
+ fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
+
+ fSumYY+= kDiff.y * kDiff.y;
+ fSumYZ+= kDiff.y * kDiff.z;
+ fSumZZ+= kDiff.z * kDiff.z;
+
+
+ source+=vstride;
+ }
+ }
+
+ fSumXX *= recip;
+ fSumXY *= recip;
+ fSumXZ *= recip;
+ fSumYY *= recip;
+ fSumYZ *= recip;
+ fSumZZ *= recip;
+
+ // setup the eigensolver
+ Eigen kES;
+
+ kES.mElement[0][0] = fSumXX;
+ kES.mElement[0][1] = fSumXY;
+ kES.mElement[0][2] = fSumXZ;
+
+ kES.mElement[1][0] = fSumXY;
+ kES.mElement[1][1] = fSumYY;
+ kES.mElement[1][2] = fSumYZ;
+
+ kES.mElement[2][0] = fSumXZ;
+ kES.mElement[2][1] = fSumYZ;
+ kES.mElement[2][2] = fSumZZ;
+
+ // compute eigenstuff, smallest eigenvalue is in last position
+ kES.DecrSortEigenStuff();
+
+ Vec3 kNormal;
+
+ kNormal.x = kES.mElement[0][2];
+ kNormal.y = kES.mElement[1][2];
+ kNormal.z = kES.mElement[2][2];
+
+ // the minimum energy
+ plane[0] = kNormal.x;
+ plane[1] = kNormal.y;
+ plane[2] = kNormal.z;
+
+ plane[3] = 0 - kNormal.dot(kOrigin);
+
+ return ret;
+}
+
+
+
+float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax) // returns the diagonal distance
+{
+
+ const unsigned char *source = (const unsigned char *) points;
+
+ bmin[0] = points[0];
+ bmin[1] = points[1];
+ bmin[2] = points[2];
+
+ bmax[0] = points[0];
+ bmax[1] = points[1];
+ bmax[2] = points[2];
+
+
+ for (unsigned int i=1; i<vcount; i++)
+ {
+ source+=pstride;
+ const float *p = (const float *) source;
+
+ if ( p[0] < bmin[0] ) bmin[0] = p[0];
+ if ( p[1] < bmin[1] ) bmin[1] = p[1];
+ if ( p[2] < bmin[2] ) bmin[2] = p[2];
+
+ if ( p[0] > bmax[0] ) bmax[0] = p[0];
+ if ( p[1] > bmax[1] ) bmax[1] = p[1];
+ if ( p[2] > bmax[2] ) bmax[2] = p[2];
+
+ }
+
+ float dx = bmax[0] - bmin[0];
+ float dy = bmax[1] - bmin[1];
+ float dz = bmax[2] - bmin[2];
+
+ return sqrtf( dx*dx + dy*dy + dz*dz );
+
+}
+
+
+bool overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) // return true if the two AABB's overlap.
+{
+ if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
+ if ( bmax2[1] < bmin1[1] ) return false;
+ if ( bmax2[2] < bmin1[2] ) return false;
+
+ if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
+ if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
+ if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
+
+
+ return true; // the extents overlap
+}
+
+
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-25 14:39:56 +0000