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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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"
21#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
22#include "LinearMath/btQuaternion.h"
23#include "LinearMath/btSerializer.h"
24
25btMultiSphereShape::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::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::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
btVector3::x
const btScalar & x() const
Return the x value.
Definition btVector3.h:575
btVector3::y
const btScalar & y() const
Return the y value.
Definition btVector3.h:577
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