Bullet Collision Detection & Physics Library
btMultiSphereShape.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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"
24
25btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
27{
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
41
42}
43
44#ifndef MIN
45 #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b))
46#endif
48{
49 btVector3 supVec(0,0,0);
50
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)*m_localScaling +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
94 void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
95{
96
97 for (int j=0;j<numVectors;j++)
98 {
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)*m_localScaling +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
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
158const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
159{
160 btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
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);
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 // Fill padding with zeros to appease msan.
180 memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
181
182 return "btMultiSphereShapeData";
183}
184
185
@ MULTI_SPHERE_SHAPE_PROXYTYPE
#define MIN(_a, _b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define SIMD_EPSILON
Definition: btScalar.h:521
#define BT_ARRAY_CODE
Definition: btSerializer.h:126
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void * m_oldPtr
Definition: btSerializer.h:56
btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive boundi...
void getCachedLocalAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
virtual btScalar getMargin() const
btAlignedObjectArray< btVector3 > m_localPositionArray
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
btConvexShape Interface
btAlignedObjectArray< btScalar > m_radiArray
btMultiSphereShape(const btVector3 *positions, const btScalar *radi, int numSpheres)
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
CollisionShape Interface.
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
long maxDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of maximum dot product between this and vectors in array[]
Definition: btVector3.h:1013
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btConvexInternalShapeData m_convexInternalShapeData
btPositionAndRadius * m_localPositionArrayPtr
btVector3FloatData m_pos