2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
18 #include "btMultiSphereShape.h"
19 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
20 #include "LinearMath/btQuaternion.h"
21 #include "LinearMath/btSerializer.h"
23 btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
24 :btConvexInternalAabbCachingShape ()
26 m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
27 //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
29 m_localPositionArray.resize(numSpheres);
30 m_radiArray.resize(numSpheres);
31 for (int i=0;i<numSpheres;i++)
33 m_localPositionArray[i] = positions[i];
34 m_radiArray[i] = radi[i];
43 #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b))
45 btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
47 btVector3 supVec(0,0,0);
49 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
53 btScalar lenSqr = vec.length2();
54 if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
59 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
66 const btVector3* pos = &m_localPositionArray[0];
67 const btScalar* rad = &m_radiArray[0];
68 int numSpheres = m_localPositionArray.size();
70 for( int k = 0; k < numSpheres; k+= 128 )
73 int inner_count = MIN( numSpheres - k, 128 );
74 for( long i = 0; i < inner_count; i++ )
76 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
80 long i = vec.maxDot( temp, inner_count, newDot);
92 void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
95 for (int j=0;j<numVectors;j++)
97 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
99 const btVector3& vec = vectors[j];
104 const btVector3* pos = &m_localPositionArray[0];
105 const btScalar* rad = &m_radiArray[0];
106 int numSpheres = m_localPositionArray.size();
108 for( int k = 0; k < numSpheres; k+= 128 )
111 int inner_count = MIN( numSpheres - k, 128 );
112 for( long i = 0; i < inner_count; i++ )
114 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
118 long i = vec.maxDot( temp, inner_count, newDot);
119 if( newDot > maxDot )
122 supportVerticesOut[j] = temp[i];
136 void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
138 //as an approximation, take the inertia of the box that bounds the spheres
140 btVector3 localAabbMin,localAabbMax;
141 getCachedLocalAabb(localAabbMin,localAabbMax);
142 btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
144 btScalar lx=btScalar(2.)*(halfExtents.x());
145 btScalar ly=btScalar(2.)*(halfExtents.y());
146 btScalar lz=btScalar(2.)*(halfExtents.z());
148 inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
149 mass/(btScalar(12.0)) * (lx*lx + lz*lz),
150 mass/(btScalar(12.0)) * (lx*lx + ly*ly));
155 ///fills the dataBuffer and returns the struct name (and 0 on failure)
156 const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
158 btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
159 btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
161 int numElem = m_localPositionArray.size();
162 shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0;
164 shapeData->m_localPositionArraySize = numElem;
167 btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
168 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
169 for (int i=0;i<numElem;i++,memPtr++)
171 m_localPositionArray[i].serializeFloat(memPtr->m_pos);
172 memPtr->m_radius = float(m_radiArray[i]);
174 serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
177 return "btMultiSphereShapeData";