Bullet Collision Detection & Physics Library: btMultiSphereShape.cpp Source File

Bullet Collision Detection & Physics Library

btMultiSphereShape.cpp
Go to the documentation of this file.
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
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
47 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)*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 {
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)*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
138 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
158 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 // Fill padding with zeros to appease msan.
180 memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
181
182 return "btMultiSphereShapeData";
183 }
184
185
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btChunk
Definition: btSerializer.h:51
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btMultiSphereShape::serialize
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btMultiSphereShape.cpp:158
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btMultiSphereShapeData::m_localPositionArraySize
int m_localPositionArraySize
Definition: btMultiSphereShape.h:88
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btConvexInternalShape::m_localScaling
btVector3 m_localScaling
Definition: btConvexInternalShape.h:35
MIN
#define MIN(_a, _b)
Definition: btMultiSphereShape.cpp:45
btMultiSphereShape::m_radiArray
btAlignedObjectArray< btScalar > m_radiArray
Definition: btMultiSphereShape.h:32
btCollisionMargin.h
btSerializer
Definition: btSerializer.h:68
btVector3::maxDot
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
btConvexInternalShape::serialize
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btConvexInternalShape.h:166
btQuaternion.h
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:113
btMultiSphereShapeData::m_padding
char m_padding[4]
Definition: btMultiSphereShape.h:89
btMultiSphereShapeData::m_localPositionArrayPtr
btPositionAndRadius * m_localPositionArrayPtr
Definition: btMultiSphereShape.h:87
MULTI_SPHERE_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:41
btPositionAndRadius::m_pos
btVector3FloatData m_pos
Definition: btMultiSphereShape.h:79
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btConvexInternalAabbCachingShape
btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive boundi...
Definition: btConvexInternalShape.h:185
btPositionAndRadius::m_radius
float m_radius
Definition: btMultiSphereShape.h:80
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:126
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultiSphereShape.h
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btConvexInternalAabbCachingShape::getCachedLocalAabb
void getCachedLocalAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btConvexInternalShape.h:202
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btMultiSphereShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
btConvexShape Interface
Definition: btMultiSphereShape.cpp:47
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btMultiSphereShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
CollisionShape Interface.
Definition: btMultiSphereShape.cpp:138
btConvexInternalAabbCachingShape::recalcLocalAabb
void recalcLocalAabb()
Definition: btConvexInternalShape.cpp:105
btMultiSphereShapeData::m_convexInternalShapeData
btConvexInternalShapeData m_convexInternalShapeData
Definition: btMultiSphereShape.h:85
btMultiSphereShape::btMultiSphereShape
btMultiSphereShape(const btVector3 *positions, const btScalar *radi, int numSpheres)
Definition: btMultiSphereShape.cpp:25
btPositionAndRadius
Definition: btMultiSphereShape.h:77
btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btMultiSphereShape.cpp:94
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:56
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btMultiSphereShape::m_localPositionArray
btAlignedObjectArray< btVector3 > m_localPositionArray
Definition: btMultiSphereShape.h:31
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btSerializer.h
btMultiSphereShapeData
Definition: btMultiSphereShape.h:83