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.
16 #if defined(_WIN32) || defined(__i386__)
17 #define BT_USE_SSE_IN_API
20 #include "btMultiSphereShape.h"
21 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
22 #include "LinearMath/btQuaternion.h"
23 #include "LinearMath/btSerializer.h"
25 btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres)
26 : btConvexInternalAabbCachingShape()
28 m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
29 //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
31 m_localPositionArray.resize(numSpheres);
32 m_radiArray.resize(numSpheres);
33 for (int i = 0; i < numSpheres; i++)
35 m_localPositionArray[i] = positions[i];
36 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));
52 btScalar lenSqr = vec.length2();
53 if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON))
55 vec.setValue(1, 0, 0);
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) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
80 long i = vec.maxDot(temp, inner_count, newDot);
91 void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
93 for (int j = 0; j < numVectors; j++)
95 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
97 const btVector3& vec = vectors[j];
102 const btVector3* pos = &m_localPositionArray[0];
103 const btScalar* rad = &m_radiArray[0];
104 int numSpheres = m_localPositionArray.size();
106 for (int k = 0; k < numSpheres; k += 128)
109 int inner_count = MIN(numSpheres - k, 128);
110 for (long i = 0; i < inner_count; i++)
112 temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
116 long i = vec.maxDot(temp, inner_count, newDot);
120 supportVerticesOut[j] = temp[i];
126 void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
128 //as an approximation, take the inertia of the box that bounds the spheres
130 btVector3 localAabbMin, localAabbMax;
131 getCachedLocalAabb(localAabbMin, localAabbMax);
132 btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5);
134 btScalar lx = btScalar(2.) * (halfExtents.x());
135 btScalar ly = btScalar(2.) * (halfExtents.y());
136 btScalar lz = btScalar(2.) * (halfExtents.z());
138 inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
139 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
140 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
143 ///fills the dataBuffer and returns the struct name (and 0 on failure)
144 const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
146 btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer;
147 btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
149 int numElem = m_localPositionArray.size();
150 shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0;
152 shapeData->m_localPositionArraySize = numElem;
155 btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem);
156 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
157 for (int i = 0; i < numElem; i++, memPtr++)
159 m_localPositionArray[i].serializeFloat(memPtr->m_pos);
160 memPtr->m_radius = float(m_radiArray[i]);
162 serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]);
165 // Fill padding with zeros to appease msan.
166 memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
168 return "btMultiSphereShapeData";