Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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