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 recalcLocalAabb();
40 }
41
42 #ifndef MIN
43 #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
44 #endif
45 btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
46 {
47 btVector3 supVec(0, 0, 0);
48
49 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
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 {
95 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
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
126 void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
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;
147 btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
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);
156 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
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 }
MULTI_SPHERE_SHAPE_PROXYTYPE
@ MULTI_SPHERE_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:40
btCollisionMargin.h
MIN
#define MIN(_a, _b)
Definition: btMultiSphereShape.cpp:43
btMultiSphereShape.h
btQuaternion.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSerializer.h
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btChunk
Definition: btSerializer.h:48
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btConvexInternalAabbCachingShape
btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive boundi...
Definition: btConvexInternalShape.h:171
btConvexInternalAabbCachingShape::getCachedLocalAabb
void getCachedLocalAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btConvexInternalShape.h:186
btConvexInternalAabbCachingShape::recalcLocalAabb
void recalcLocalAabb()
Definition: btConvexInternalShape.cpp:93
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
btConvexInternalShape::m_localScaling
btVector3 m_localScaling
Definition: btConvexInternalShape.h:33
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btMultiSphereShape::m_localPositionArray
btAlignedObjectArray< btVector3 > m_localPositionArray
Definition: btMultiSphereShape.h:29
btMultiSphereShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
btConvexShape Interface
Definition: btMultiSphereShape.cpp:45
btMultiSphereShape::m_radiArray
btAlignedObjectArray< btScalar > m_radiArray
Definition: btMultiSphereShape.h:30
btMultiSphereShape::btMultiSphereShape
btMultiSphereShape(const btVector3 *positions, const btScalar *radi, int numSpheres)
Definition: btMultiSphereShape.cpp:25
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
btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btMultiSphereShape.cpp:91
btMultiSphereShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
CollisionShape Interface.
Definition: btMultiSphereShape.cpp:126
btSerializer
Definition: btSerializer.h:66
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
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:82
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btMultiSphereShapeData
Definition: btMultiSphereShape.h:80
btMultiSphereShapeData::m_convexInternalShapeData
btConvexInternalShapeData m_convexInternalShapeData
Definition: btMultiSphereShape.h:81
btMultiSphereShapeData::m_localPositionArraySize
int m_localPositionArraySize
Definition: btMultiSphereShape.h:84
btMultiSphereShapeData::m_padding
char m_padding[4]
Definition: btMultiSphereShape.h:85
btMultiSphereShapeData::m_localPositionArrayPtr
btPositionAndRadius * m_localPositionArrayPtr
Definition: btMultiSphereShape.h:83
btPositionAndRadius
Definition: btMultiSphereShape.h:72
btPositionAndRadius::m_radius
float m_radius
Definition: btMultiSphereShape.h:74
btPositionAndRadius::m_pos
btVector3FloatData m_pos
Definition: btMultiSphereShape.h:73