1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4
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:
10
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.
14 */
15
16 #if defined (_WIN32) || defined (__i386__)
17 #define BT_USE_SSE_IN_API
18 #endif
19
20 #include "btMultiSphereShape.h"
21 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
22 #include "LinearMath/btQuaternion.h"
23 #include "LinearMath/btSerializer.h"
24
btMultiSphereShape(const btVector3 * positions,const btScalar * radi,int numSpheres)25 btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
26 :btConvexInternalAabbCachingShape ()
27 {
28 m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
29 //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
30
31 m_localPositionArray.resize(numSpheres);
32 m_radiArray.resize(numSpheres);
33 for (int i=0;i<numSpheres;i++)
34 {
35 m_localPositionArray[i] = positions[i];
36 m_radiArray[i] = radi[i];
37
38 }
39
40 recalcLocalAabb();
41
42 }
43
44 #ifndef MIN
45 #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b))
46 #endif
localGetSupportingVertexWithoutMargin(const btVector3 & vec0) const47 btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
48 {
49 btVector3 supVec(0,0,0);
50
51 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
52
53
54 btVector3 vec = vec0;
55 btScalar lenSqr = vec.length2();
56 if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
57 {
58 vec.setValue(1,0,0);
59 } else
60 {
61 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
62 vec *= rlen;
63 }
64
65 btVector3 vtx;
66 btScalar newDot;
67
68 const btVector3* pos = &m_localPositionArray[0];
69 const btScalar* rad = &m_radiArray[0];
70 int numSpheres = m_localPositionArray.size();
71
72 for( int k = 0; k < numSpheres; k+= 128 )
73 {
74 btVector3 temp[128];
75 int inner_count = MIN( numSpheres - k, 128 );
76 for( long i = 0; i < inner_count; i++ )
77 {
78 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
79 pos++;
80 rad++;
81 }
82 long i = vec.maxDot( temp, inner_count, newDot);
83 if( newDot > maxDot )
84 {
85 maxDot = newDot;
86 supVec = temp[i];
87 }
88 }
89
90 return supVec;
91
92 }
93
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const94 void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
95 {
96
97 for (int j=0;j<numVectors;j++)
98 {
99 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
100
101 const btVector3& vec = vectors[j];
102
103 btVector3 vtx;
104 btScalar newDot;
105
106 const btVector3* pos = &m_localPositionArray[0];
107 const btScalar* rad = &m_radiArray[0];
108 int numSpheres = m_localPositionArray.size();
109
110 for( int k = 0; k < numSpheres; k+= 128 )
111 {
112 btVector3 temp[128];
113 int inner_count = MIN( numSpheres - k, 128 );
114 for( long i = 0; i < inner_count; i++ )
115 {
116 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
117 pos++;
118 rad++;
119 }
120 long i = vec.maxDot( temp, inner_count, newDot);
121 if( newDot > maxDot )
122 {
123 maxDot = newDot;
124 supportVerticesOut[j] = temp[i];
125 }
126 }
127
128 }
129 }
130
131
132
133
134
135
136
137
calculateLocalInertia(btScalar mass,btVector3 & inertia) const138 void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
139 {
140 //as an approximation, take the inertia of the box that bounds the spheres
141
142 btVector3 localAabbMin,localAabbMax;
143 getCachedLocalAabb(localAabbMin,localAabbMax);
144 btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
145
146 btScalar lx=btScalar(2.)*(halfExtents.x());
147 btScalar ly=btScalar(2.)*(halfExtents.y());
148 btScalar lz=btScalar(2.)*(halfExtents.z());
149
150 inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
151 mass/(btScalar(12.0)) * (lx*lx + lz*lz),
152 mass/(btScalar(12.0)) * (lx*lx + ly*ly));
153
154 }
155
156
157 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer) const158 const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
159 {
160 btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
161 btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
162
163 int numElem = m_localPositionArray.size();
164 shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0;
165
166 shapeData->m_localPositionArraySize = numElem;
167 if (numElem)
168 {
169 btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
170 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
171 for (int i=0;i<numElem;i++,memPtr++)
172 {
173 m_localPositionArray[i].serializeFloat(memPtr->m_pos);
174 memPtr->m_radius = float(m_radiArray[i]);
175 }
176 serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
177 }
178
179 return "btMultiSphereShapeData";
180 }
181
182
183