summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp134
1 files changed, 59 insertions, 75 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
index 4195fa3138..c0cc55dfb0 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
@@ -13,7 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#if defined (_WIN32) || defined (__i386__)
+#if defined(_WIN32) || defined(__i386__)
#define BT_USE_SSE_IN_API
#endif
@@ -22,43 +22,41 @@ subject to the following restrictions:
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btSerializer.h"
-btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
-:btConvexInternalAabbCachingShape ()
+btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres)
+ : btConvexInternalAabbCachingShape()
{
m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
//btScalar startMargin = btScalar(BT_LARGE_FLOAT);
m_localPositionArray.resize(numSpheres);
m_radiArray.resize(numSpheres);
- for (int i=0;i<numSpheres;i++)
+ for (int i = 0; i < numSpheres; i++)
{
m_localPositionArray[i] = positions[i];
m_radiArray[i] = radi[i];
-
}
recalcLocalAabb();
-
}
#ifndef MIN
- #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b))
+#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
#endif
- btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
{
- btVector3 supVec(0,0,0);
+ btVector3 supVec(0, 0, 0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
-
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
- if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
+ if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON))
{
- vec.setValue(1,0,0);
- } else
+ vec.setValue(1, 0, 0);
+ }
+ else
{
- btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+ btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
vec *= rlen;
}
@@ -69,32 +67,30 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
const btScalar* rad = &m_radiArray[0];
int numSpheres = m_localPositionArray.size();
- for( int k = 0; k < numSpheres; k+= 128 )
+ for (int k = 0; k < numSpheres; k += 128)
{
btVector3 temp[128];
- int inner_count = MIN( numSpheres - k, 128 );
- for( long i = 0; i < inner_count; i++ )
- {
- temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
- pos++;
- rad++;
- }
- long i = vec.maxDot( temp, inner_count, newDot);
- if( newDot > maxDot )
+ int inner_count = MIN(numSpheres - k, 128);
+ for (long i = 0; i < inner_count; i++)
+ {
+ temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
+ pos++;
+ rad++;
+ }
+ long i = vec.maxDot(temp, inner_count, newDot);
+ if (newDot > maxDot)
{
maxDot = newDot;
supVec = temp[i];
}
- }
+ }
return supVec;
-
}
- void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
-
- for (int j=0;j<numVectors;j++)
+ for (int j = 0; j < numVectors; j++)
{
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
@@ -107,73 +103,63 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
const btScalar* rad = &m_radiArray[0];
int numSpheres = m_localPositionArray.size();
- for( int k = 0; k < numSpheres; k+= 128 )
- {
- btVector3 temp[128];
- int inner_count = MIN( numSpheres - k, 128 );
- for( long i = 0; i < inner_count; i++ )
- {
- temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
- pos++;
- rad++;
- }
- long i = vec.maxDot( temp, inner_count, newDot);
- if( newDot > maxDot )
- {
- maxDot = newDot;
- supportVerticesOut[j] = temp[i];
- }
- }
-
+ for (int k = 0; k < numSpheres; k += 128)
+ {
+ btVector3 temp[128];
+ int inner_count = MIN(numSpheres - k, 128);
+ for (long i = 0; i < inner_count; i++)
+ {
+ temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
+ pos++;
+ rad++;
+ }
+ long i = vec.maxDot(temp, inner_count, newDot);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supportVerticesOut[j] = temp[i];
+ }
+ }
}
}
-
-
-
-
-
-
-
-void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
//as an approximation, take the inertia of the box that bounds the spheres
- btVector3 localAabbMin,localAabbMax;
- getCachedLocalAabb(localAabbMin,localAabbMax);
- btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
+ btVector3 localAabbMin, localAabbMax;
+ getCachedLocalAabb(localAabbMin, localAabbMax);
+ btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5);
- btScalar lx=btScalar(2.)*(halfExtents.x());
- btScalar ly=btScalar(2.)*(halfExtents.y());
- btScalar lz=btScalar(2.)*(halfExtents.z());
-
- inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
- mass/(btScalar(12.0)) * (lx*lx + lz*lz),
- mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+ btScalar lx = btScalar(2.) * (halfExtents.x());
+ btScalar ly = btScalar(2.) * (halfExtents.y());
+ btScalar lz = btScalar(2.) * (halfExtents.z());
+ inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+ mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+ mass / (btScalar(12.0)) * (lx * lx + ly * ly));
}
-
///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
- btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
+ btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer;
btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
int numElem = m_localPositionArray.size();
- shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0;
-
+ shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0;
+
shapeData->m_localPositionArraySize = numElem;
if (numElem)
{
- btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
+ btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem);
btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
- for (int i=0;i<numElem;i++,memPtr++)
+ for (int i = 0; i < numElem; i++, memPtr++)
{
m_localPositionArray[i].serializeFloat(memPtr->m_pos);
memPtr->m_radius = float(m_radiArray[i]);
}
- serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
+ serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]);
}
// Fill padding with zeros to appease msan.
@@ -181,5 +167,3 @@ const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serial
return "btMultiSphereShapeData";
}
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-26 22:49:06 +0000