btMultiSphereShape.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 
00017 
00018 #include "btMultiSphereShape.h"
00019 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
00020 #include "LinearMath/btQuaternion.h"
00021 #include "LinearMath/btSerializer.h"
00022 
00023 btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
00024 :btConvexInternalAabbCachingShape ()
00025 {
00026         m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
00027         //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
00028 
00029         m_localPositionArray.resize(numSpheres);
00030         m_radiArray.resize(numSpheres);
00031         for (int i=0;i<numSpheres;i++)
00032         {
00033                 m_localPositionArray[i] = positions[i];
00034                 m_radiArray[i] = radi[i];
00035                 
00036         }
00037 
00038         recalcLocalAabb();
00039 
00040 }
00041 
00042 #ifndef MIN
00043     #define MIN( _a, _b)    ((_a) < (_b) ? (_a) : (_b))
00044 #endif
00045  btVector3      btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
00046 {
00047         btVector3 supVec(0,0,0);
00048 
00049         btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00050 
00051 
00052         btVector3 vec = vec0;
00053         btScalar lenSqr = vec.length2();
00054         if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
00055         {
00056                 vec.setValue(1,0,0);
00057         } else
00058         {
00059                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00060                 vec *= rlen;
00061         }
00062 
00063         btVector3 vtx;
00064         btScalar newDot;
00065 
00066         const btVector3* pos = &m_localPositionArray[0];
00067         const btScalar* rad = &m_radiArray[0];
00068         int numSpheres = m_localPositionArray.size();
00069 
00070     for( int k = 0; k < numSpheres; k+= 128 )
00071     {
00072         btVector3 temp[128];
00073         int inner_count = MIN( numSpheres - k, 128 );
00074         for( long i = 0; i < inner_count; i++ )
00075         {
00076             temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
00077             pos++;
00078             rad++;
00079         }
00080         long i = vec.maxDot( temp, inner_count, newDot);
00081         if( newDot > maxDot )
00082                 {
00083                         maxDot = newDot;
00084                         supVec = temp[i];
00085                 }
00086     }
00087 
00088         return supVec;
00089 
00090 }
00091 
00092  void   btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00093 {
00094 
00095         for (int j=0;j<numVectors;j++)
00096         {
00097                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00098 
00099                 const btVector3& vec = vectors[j];
00100 
00101                 btVector3 vtx;
00102                 btScalar newDot;
00103 
00104                 const btVector3* pos = &m_localPositionArray[0];
00105                 const btScalar* rad = &m_radiArray[0];
00106                 int numSpheres = m_localPositionArray.size();
00107 
00108         for( int k = 0; k < numSpheres; k+= 128 )
00109         {
00110             btVector3 temp[128];
00111             int inner_count = MIN( numSpheres - k, 128 );
00112             for( long i = 0; i < inner_count; i++ )
00113             {
00114                 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
00115                 pos++;
00116                 rad++;
00117             }
00118             long i = vec.maxDot( temp, inner_count, newDot);
00119             if( newDot > maxDot )
00120             {
00121                 maxDot = newDot;
00122                 supportVerticesOut[j] = temp[i];
00123             }
00124         }
00125         
00126         }
00127 }
00128 
00129 
00130 
00131 
00132 
00133 
00134 
00135 
00136 void    btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00137 {
00138         //as an approximation, take the inertia of the box that bounds the spheres
00139 
00140         btVector3 localAabbMin,localAabbMax;
00141         getCachedLocalAabb(localAabbMin,localAabbMax);
00142         btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
00143 
00144         btScalar lx=btScalar(2.)*(halfExtents.x());
00145         btScalar ly=btScalar(2.)*(halfExtents.y());
00146         btScalar lz=btScalar(2.)*(halfExtents.z());
00147 
00148         inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
00149                                         mass/(btScalar(12.0)) * (lx*lx + lz*lz),
00150                                         mass/(btScalar(12.0)) * (lx*lx + ly*ly));
00151 
00152 }
00153 
00154 
00156 const char*     btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
00157 {
00158         btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
00159         btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
00160 
00161         int numElem = m_localPositionArray.size();
00162         shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]):  0;
00163         
00164         shapeData->m_localPositionArraySize = numElem;
00165         if (numElem)
00166         {
00167                 btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
00168                 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
00169                 for (int i=0;i<numElem;i++,memPtr++)
00170                 {
00171                         m_localPositionArray[i].serializeFloat(memPtr->m_pos);
00172                         memPtr->m_radius = float(m_radiArray[i]);
00173                 }
00174                 serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
00175         }
00176         
00177         return "btMultiSphereShapeData";
00178 }
00179 
00180