Blender  V3.3
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 
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 
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 }
typedef float(TangentPoint)[2]
SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
btConvexShape Interface
Definition: btBox2dShape.h:62
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
Definition: btConeShape.h:54
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btBox2dShape.h:71
@ MULTI_SPHERE_SHAPE_PROXYTYPE
virtual btScalar getMargin() const =0
btVector3 m_localScaling
#define MIN(_a, _b)
btMultiSphereShape(const btVector3 *positions, const btScalar *radi, int numSpheres)
btAlignedObjectArray< btScalar > m_radiArray
virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const
Data buffer MUST be 16 byte aligned.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_EPSILON
Definition: btScalar.h:543
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
void recalcLocalAabb()
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE 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
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void * m_oldPtr
Definition: btSerializer.h:52
btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive boundi...
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
uint pos
MutableSpan< float3 > positions
btConvexInternalShapeData m_convexInternalShapeData
btPositionAndRadius * m_localPositionArrayPtr
btVector3FloatData m_pos