Back to index

supertuxkart  0.5+dfsg1
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" void btBulletMathProbe () {}
00026 
00027 
00028 bool   btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar   margin)
00029 {
00030        int numbrushes = planeEquations.size();
00031        for (int i=0;i<numbrushes;i++)
00032        {
00033               const btVector3& N1 = planeEquations[i];
00034               btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
00035               if (dist>btScalar(0.))
00036               {
00037                      return false;
00038               }
00039        }
00040        return true;
00041               
00042 }
00043 
00044 
00045 bool   btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar       margin)
00046 {
00047        int numvertices = vertices.size();
00048        for (int i=0;i<numvertices;i++)
00049        {
00050               const btVector3& N1 = vertices[i];
00051               btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
00052               if (dist>btScalar(0.))
00053               {
00054                      return false;
00055               }
00056        }
00057        return true;
00058 }
00059 
00060 bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
00061 {
00062        int numbrushes = planeEquations.size();
00063        for (int i=0;i<numbrushes;i++)
00064        {
00065               const btVector3& N1 = planeEquations[i];
00066               if (planeEquation.dot(N1) > btScalar(0.999))
00067               {
00068                      return false;
00069               } 
00070        }
00071        return true;
00072 }
00073 
00074 void   btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
00075 {
00076               const int numvertices = vertices.size();
00077        // brute force:
00078        for (int i=0;i<numvertices;i++)
00079        {
00080               const btVector3& N1 = vertices[i];
00081               
00082 
00083               for (int j=i+1;j<numvertices;j++)
00084               {
00085                      const btVector3& N2 = vertices[j];
00086                             
00087                      for (int k=j+1;k<numvertices;k++)
00088                      {
00089 
00090                             const btVector3& N3 = vertices[k];
00091 
00092                             btVector3 planeEquation,edge0,edge1;
00093                             edge0 = N2-N1;
00094                             edge1 = N3-N1;
00095                             btScalar normalSign = btScalar(1.);
00096                             for (int ww=0;ww<2;ww++)
00097                             {
00098                                    planeEquation = normalSign * edge0.cross(edge1);
00099                                    if (planeEquation.length2() > btScalar(0.0001))
00100                                    {
00101                                           planeEquation.normalize();
00102                                           if (notExist(planeEquation,planeEquationsOut))
00103                                           {
00104                                                  planeEquation[3] = -planeEquation.dot(N1);
00105                                                  
00106                                                         //check if inside, and replace supportingVertexOut if needed
00107                                                         if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
00108                                                         {
00109                                                                planeEquationsOut.push_back(planeEquation);
00110                                                         }
00111                                           }
00112                                    }
00113                                    normalSign = btScalar(-1.);
00114                             }
00115                      
00116                      }
00117               }
00118        }
00119 
00120 }
00121 
00122 void   btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
00123 {
00124        const int numbrushes = planeEquations.size();
00125        // brute force:
00126        for (int i=0;i<numbrushes;i++)
00127        {
00128               const btVector3& N1 = planeEquations[i];
00129               
00130 
00131               for (int j=i+1;j<numbrushes;j++)
00132               {
00133                      const btVector3& N2 = planeEquations[j];
00134                             
00135                      for (int k=j+1;k<numbrushes;k++)
00136                      {
00137 
00138                             const btVector3& N3 = planeEquations[k];
00139 
00140                             btVector3 n2n3; n2n3 = N2.cross(N3);
00141                             btVector3 n3n1; n3n1 = N3.cross(N1);
00142                             btVector3 n1n2; n1n2 = N1.cross(N2);
00143                             
00144                             if ( ( n2n3.length2() > btScalar(0.0001) ) &&
00145                                     ( n3n1.length2() > btScalar(0.0001) ) &&
00146                                     ( n1n2.length2() > btScalar(0.0001) ) )
00147                             {
00148                                    //point P out of 3 plane equations:
00149 
00150                                    //     d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
00151                                    //P =  -------------------------------------------------------------------------  
00152                                    //   N1 . ( N2 * N3 )  
00153 
00154 
00155                                    btScalar quotient = (N1.dot(n2n3));
00156                                    if (btFabs(quotient) > btScalar(0.000001))
00157                                    {
00158                                           quotient = btScalar(-1.) / quotient;
00159                                           n2n3 *= N1[3];
00160                                           n3n1 *= N2[3];
00161                                           n1n2 *= N3[3];
00162                                           btVector3 potentialVertex = n2n3;
00163                                           potentialVertex += n3n1;
00164                                           potentialVertex += n1n2;
00165                                           potentialVertex *= quotient;
00166 
00167                                           //check if inside, and replace supportingVertexOut if needed
00168                                           if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
00169                                           {
00170                                                  verticesOut.push_back(potentialVertex);
00171                                           }
00172                                    }
00173                             }
00174                      }
00175               }
00176        }
00177 }
00178