btConvexHullShape.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btConvexHullShape.h"
00017 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
00018 
00019 #include "LinearMath/btQuaternion.h"
00020 #include "LinearMath/btSerializer.h"
00021 
00022 btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
00023 {
00024         m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
00025         m_unscaledPoints.resize(numPoints);
00026 
00027         unsigned char* pointsAddress = (unsigned char*)points;
00028 
00029         for (int i=0;i<numPoints;i++)
00030         {
00031                 btScalar* point = (btScalar*)pointsAddress;
00032                 m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
00033                 pointsAddress += stride;
00034         }
00035 
00036         recalcLocalAabb();
00037 
00038 }
00039 
00040 
00041 
00042 void btConvexHullShape::setLocalScaling(const btVector3& scaling)
00043 {
00044         m_localScaling = scaling;
00045         recalcLocalAabb();
00046 }
00047 
00048 void btConvexHullShape::addPoint(const btVector3& point)
00049 {
00050         m_unscaledPoints.push_back(point);
00051         recalcLocalAabb();
00052 
00053 }
00054 
00055 btVector3       btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
00056 {
00057         btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
00058         btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
00059 
00060     // Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically. 
00061     if( 0 < m_unscaledPoints.size() )
00062     {
00063         btVector3 scaled = vec * m_localScaling;
00064         int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints
00065         return m_unscaledPoints[index] * m_localScaling;
00066     }
00067 
00068     return supVec;
00069 }
00070 
00071 void    btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00072 {
00073         btScalar newDot;
00074         //use 'w' component of supportVerticesOut?
00075         {
00076                 for (int i=0;i<numVectors;i++)
00077                 {
00078                         supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
00079                 }
00080         }
00081 
00082     for (int j=0;j<numVectors;j++)
00083     {
00084         btVector3 vec = vectors[j] * m_localScaling;        // dot(a*b,c) = dot(a,b*c)
00085         if( 0 <  m_unscaledPoints.size() )
00086         {
00087             int i = (int) vec.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
00088             supportVerticesOut[j] = getScaledPoint(i);
00089             supportVerticesOut[j][3] = newDot;        
00090         }
00091         else
00092             supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
00093     }
00094 
00095 
00096 
00097 }
00098         
00099 
00100 
00101 btVector3       btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
00102 {
00103         btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
00104 
00105         if ( getMargin()!=btScalar(0.) )
00106         {
00107                 btVector3 vecnorm = vec;
00108                 if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
00109                 {
00110                         vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
00111                 } 
00112                 vecnorm.normalize();
00113                 supVertex+= getMargin() * vecnorm;
00114         }
00115         return supVertex;
00116 }
00117 
00118 
00119 
00120 
00121 
00122 
00123 
00124 
00125 
00126 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
00127 //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
00128 int     btConvexHullShape::getNumVertices() const
00129 {
00130         return m_unscaledPoints.size();
00131 }
00132 
00133 int btConvexHullShape::getNumEdges() const
00134 {
00135         return m_unscaledPoints.size();
00136 }
00137 
00138 void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
00139 {
00140 
00141         int index0 = i%m_unscaledPoints.size();
00142         int index1 = (i+1)%m_unscaledPoints.size();
00143         pa = getScaledPoint(index0);
00144         pb = getScaledPoint(index1);
00145 }
00146 
00147 void btConvexHullShape::getVertex(int i,btVector3& vtx) const
00148 {
00149         vtx = getScaledPoint(i);
00150 }
00151 
00152 int     btConvexHullShape::getNumPlanes() const
00153 {
00154         return 0;
00155 }
00156 
00157 void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
00158 {
00159 
00160         btAssert(0);
00161 }
00162 
00163 //not yet
00164 bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
00165 {
00166         btAssert(0);
00167         return false;
00168 }
00169 
00171 const char*     btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
00172 {
00173         //int szc = sizeof(btConvexHullShapeData);
00174         btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer;
00175         btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
00176 
00177         int numElem = m_unscaledPoints.size();
00178         shapeData->m_numUnscaledPoints = numElem;
00179 #ifdef BT_USE_DOUBLE_PRECISION
00180         shapeData->m_unscaledPointsFloatPtr = 0;
00181         shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
00182 #else
00183         shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
00184         shapeData->m_unscaledPointsDoublePtr = 0;
00185 #endif
00186         
00187         if (numElem)
00188         {
00189                 int sz = sizeof(btVector3Data);
00190         //      int sz2 = sizeof(btVector3DoubleData);
00191         //      int sz3 = sizeof(btVector3FloatData);
00192                 btChunk* chunk = serializer->allocate(sz,numElem);
00193                 btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr;
00194                 for (int i=0;i<numElem;i++,memPtr++)
00195                 {
00196                         m_unscaledPoints[i].serialize(*memPtr);
00197                 }
00198                 serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
00199         }
00200         
00201         return "btConvexHullShapeData";
00202 }
00203 
00204 void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
00205 {
00206 #if 1
00207         minProj = FLT_MAX;
00208         maxProj = -FLT_MAX;
00209 
00210         int numVerts = m_unscaledPoints.size();
00211         for(int i=0;i<numVerts;i++)
00212         {
00213                 btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
00214                 btVector3 pt = trans * vtx;
00215                 btScalar dp = pt.dot(dir);
00216                 if(dp < minProj)        
00217                 {
00218                         minProj = dp;
00219                         witnesPtMin = pt;
00220                 }
00221                 if(dp > maxProj)        
00222                 {
00223                         maxProj = dp;
00224                         witnesPtMax=pt;
00225                 }
00226         }
00227 #else
00228         btVector3 localAxis = dir*trans.getBasis();
00229         witnesPtMin  = trans(localGetSupportingVertex(localAxis));
00230         witnesPtMax = trans(localGetSupportingVertex(-localAxis));
00231 
00232         minProj = witnesPtMin.dot(dir);
00233         maxProj = witnesPtMax.dot(dir);
00234 #endif
00235 
00236         if(minProj>maxProj)
00237         {
00238                 btSwap(minProj,maxProj);
00239                 btSwap(witnesPtMin,witnesPtMax);
00240         }
00241 
00242 
00243 }
00244 
00245