btGeometryUtil.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 
00017 #include "btGeometryUtil.h"
00018 
00019 
00020 /*
00021   Make sure this dummy function never changes so that it
00022   can be used by probes that are checking whether the
00023   library is actually installed.
00024 */
00025 extern "C"
00026 {       
00027         void btBulletMathProbe ();
00028 
00029         void btBulletMathProbe () {}
00030 }
00031 
00032 
00033 bool    btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar     margin)
00034 {
00035         int numbrushes = planeEquations.size();
00036         for (int i=0;i<numbrushes;i++)
00037         {
00038                 const btVector3& N1 = planeEquations[i];
00039                 btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
00040                 if (dist>btScalar(0.))
00041                 {
00042                         return false;
00043                 }
00044         }
00045         return true;
00046                 
00047 }
00048 
00049 
00050 bool    btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar  margin)
00051 {
00052         int numvertices = vertices.size();
00053         for (int i=0;i<numvertices;i++)
00054         {
00055                 const btVector3& N1 = vertices[i];
00056                 btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
00057                 if (dist>btScalar(0.))
00058                 {
00059                         return false;
00060                 }
00061         }
00062         return true;
00063 }
00064 
00065 bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
00066 
00067 bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
00068 {
00069         int numbrushes = planeEquations.size();
00070         for (int i=0;i<numbrushes;i++)
00071         {
00072                 const btVector3& N1 = planeEquations[i];
00073                 if (planeEquation.dot(N1) > btScalar(0.999))
00074                 {
00075                         return false;
00076                 } 
00077         }
00078         return true;
00079 }
00080 
00081 void    btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
00082 {
00083                 const int numvertices = vertices.size();
00084         // brute force:
00085         for (int i=0;i<numvertices;i++)
00086         {
00087                 const btVector3& N1 = vertices[i];
00088                 
00089 
00090                 for (int j=i+1;j<numvertices;j++)
00091                 {
00092                         const btVector3& N2 = vertices[j];
00093                                 
00094                         for (int k=j+1;k<numvertices;k++)
00095                         {
00096 
00097                                 const btVector3& N3 = vertices[k];
00098 
00099                                 btVector3 planeEquation,edge0,edge1;
00100                                 edge0 = N2-N1;
00101                                 edge1 = N3-N1;
00102                                 btScalar normalSign = btScalar(1.);
00103                                 for (int ww=0;ww<2;ww++)
00104                                 {
00105                                         planeEquation = normalSign * edge0.cross(edge1);
00106                                         if (planeEquation.length2() > btScalar(0.0001))
00107                                         {
00108                                                 planeEquation.normalize();
00109                                                 if (notExist(planeEquation,planeEquationsOut))
00110                                                 {
00111                                                         planeEquation[3] = -planeEquation.dot(N1);
00112                                                         
00113                                                                 //check if inside, and replace supportingVertexOut if needed
00114                                                                 if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
00115                                                                 {
00116                                                                         planeEquationsOut.push_back(planeEquation);
00117                                                                 }
00118                                                 }
00119                                         }
00120                                         normalSign = btScalar(-1.);
00121                                 }
00122                         
00123                         }
00124                 }
00125         }
00126 
00127 }
00128 
00129 void    btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
00130 {
00131         const int numbrushes = planeEquations.size();
00132         // brute force:
00133         for (int i=0;i<numbrushes;i++)
00134         {
00135                 const btVector3& N1 = planeEquations[i];
00136                 
00137 
00138                 for (int j=i+1;j<numbrushes;j++)
00139                 {
00140                         const btVector3& N2 = planeEquations[j];
00141                                 
00142                         for (int k=j+1;k<numbrushes;k++)
00143                         {
00144 
00145                                 const btVector3& N3 = planeEquations[k];
00146 
00147                                 btVector3 n2n3; n2n3 = N2.cross(N3);
00148                                 btVector3 n3n1; n3n1 = N3.cross(N1);
00149                                 btVector3 n1n2; n1n2 = N1.cross(N2);
00150                                 
00151                                 if ( ( n2n3.length2() > btScalar(0.0001) ) &&
00152                                          ( n3n1.length2() > btScalar(0.0001) ) &&
00153                                          ( n1n2.length2() > btScalar(0.0001) ) )
00154                                 {
00155                                         //point P out of 3 plane equations:
00156 
00157                                         //      d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
00158                                         //P =  -------------------------------------------------------------------------  
00159                                         //   N1 . ( N2 * N3 )  
00160 
00161 
00162                                         btScalar quotient = (N1.dot(n2n3));
00163                                         if (btFabs(quotient) > btScalar(0.000001))
00164                                         {
00165                                                 quotient = btScalar(-1.) / quotient;
00166                                                 n2n3 *= N1[3];
00167                                                 n3n1 *= N2[3];
00168                                                 n1n2 *= N3[3];
00169                                                 btVector3 potentialVertex = n2n3;
00170                                                 potentialVertex += n3n1;
00171                                                 potentialVertex += n1n2;
00172                                                 potentialVertex *= quotient;
00173 
00174                                                 //check if inside, and replace supportingVertexOut if needed
00175                                                 if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
00176                                                 {
00177                                                         verticesOut.push_back(potentialVertex);
00178                                                 }
00179                                         }
00180                                 }
00181                         }
00182                 }
00183         }
00184 }
00185