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"
24 
25 btMultiSphereShape::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 
40 }
41 
42 #ifndef MIN
43 #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
44 #endif
46 {
47  btVector3 supVec(0, 0, 0);
48 
50 
51  btVector3 vec = vec0;
52  btScalar lenSqr = vec.length2();
53  if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON))
54  {
55  vec.setValue(1, 0, 0);
56  }
57  else
58  {
59  btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
60  vec *= rlen;
61  }
62 
63  btVector3 vtx;
64  btScalar newDot;
65 
66  const btVector3* pos = &m_localPositionArray[0];
67  const btScalar* rad = &m_radiArray[0];
68  int numSpheres = m_localPositionArray.size();
69 
70  for (int k = 0; k < numSpheres; k += 128)
71  {
72  btVector3 temp[128];
73  int inner_count = MIN(numSpheres - k, 128);
74  for (long i = 0; i < inner_count; i++)
75  {
76  temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
77  pos++;
78  rad++;
79  }
80  long i = vec.maxDot(temp, inner_count, newDot);
81  if (newDot > maxDot)
82  {
83  maxDot = newDot;
84  supVec = temp[i];
85  }
86  }
87 
88  return supVec;
89 }
90 
91 void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
92 {
93  for (int j = 0; j < numVectors; j++)
94  {
96 
97  const btVector3& vec = vectors[j];
98 
99  btVector3 vtx;
100  btScalar newDot;
101 
102  const btVector3* pos = &m_localPositionArray[0];
103  const btScalar* rad = &m_radiArray[0];
104  int numSpheres = m_localPositionArray.size();
105 
106  for (int k = 0; k < numSpheres; k += 128)
107  {
108  btVector3 temp[128];
109  int inner_count = MIN(numSpheres - k, 128);
110  for (long i = 0; i < inner_count; i++)
111  {
112  temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
113  pos++;
114  rad++;
115  }
116  long i = vec.maxDot(temp, inner_count, newDot);
117  if (newDot > maxDot)
118  {
119  maxDot = newDot;
120  supportVerticesOut[j] = temp[i];
121  }
122  }
123  }
124 }
125 
127 {
128  //as an approximation, take the inertia of the box that bounds the spheres
129 
130  btVector3 localAabbMin, localAabbMax;
131  getCachedLocalAabb(localAabbMin, localAabbMax);
132  btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5);
133 
134  btScalar lx = btScalar(2.) * (halfExtents.x());
135  btScalar ly = btScalar(2.) * (halfExtents.y());
136  btScalar lz = btScalar(2.) * (halfExtents.z());
137 
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));
141 }
142 
144 const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
145 {
146  btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer;
148 
149  int numElem = m_localPositionArray.size();
150  shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0;
151 
152  shapeData->m_localPositionArraySize = numElem;
153  if (numElem)
154  {
155  btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem);
157  for (int i = 0; i < numElem; i++, memPtr++)
158  {
159  m_localPositionArray[i].serializeFloat(memPtr->m_pos);
160  memPtr->m_radius = float(m_radiArray[i]);
161  }
162  serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]);
163  }
164 
165  // Fill padding with zeros to appease msan.
166  memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
167 
168  return "btMultiSphereShapeData";
169 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
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:144
btConvexInternalAabbCachingShape
btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive boundi...
Definition: btConvexInternalShape.h:170
btConvexInternalShape::m_localScaling
btVector3 m_localScaling
Definition: btConvexInternalShape.h:33
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultiSphereShape::m_radiArray
btAlignedObjectArray< btScalar > m_radiArray
Definition: btMultiSphereShape.h:30
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btChunk
Definition: btSerializer.h:47
btMultiSphereShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
CollisionShape Interface.
Definition: btMultiSphereShape.cpp:126
btCollisionMargin.h
btPositionAndRadius::m_pos
btVector3FloatData m_pos
Definition: btMultiSphereShape.h:73
btMultiSphereShapeData::m_localPositionArraySize
int m_localPositionArraySize
Definition: btMultiSphereShape.h:84
btMultiSphereShapeData::m_padding
char m_padding[4]
Definition: btMultiSphereShape.h:85
btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btMultiSphereShape.cpp:91
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btMultiSphereShape::btMultiSphereShape
btMultiSphereShape(const btVector3 *positions, const btScalar *radi, int numSpheres)
Definition: btMultiSphereShape.cpp:25
MULTI_SPHERE_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:40
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
btMultiSphereShapeData::m_localPositionArrayPtr
btPositionAndRadius * m_localPositionArrayPtr
Definition: btMultiSphereShape.h:83
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btMultiSphereShape::m_localPositionArray
btAlignedObjectArray< btVector3 > m_localPositionArray
Definition: btMultiSphereShape.h:29
btMultiSphereShapeData
Definition: btMultiSphereShape.h:79
btConvexInternalAabbCachingShape::getCachedLocalAabb
void getCachedLocalAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btConvexInternalShape.h:186
btSerializer.h
btQuaternion.h
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btPositionAndRadius
Definition: btMultiSphereShape.h:71
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btConvexInternalAabbCachingShape::recalcLocalAabb
void recalcLocalAabb()
Definition: btConvexInternalShape.cpp:93
btSerializer
Definition: btSerializer.h:65
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btMultiSphereShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
btConvexShape Interface
Definition: btMultiSphereShape.cpp:45
MIN
#define MIN(_a, _b)
Definition: btMultiSphereShape.cpp:43
btMultiSphereShapeData::m_convexInternalShapeData
btConvexInternalShapeData m_convexInternalShapeData
Definition: btMultiSphereShape.h:81
btPositionAndRadius::m_radius
float m_radius
Definition: btMultiSphereShape.h:74
btMultiSphereShape.h
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:446
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
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:154
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:998