dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp

   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
 143 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 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 }