dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btCompoundShape.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 #include "btCompoundShape.h"
  17 #include "btCollisionShape.h"
  18 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
  19 #include "LinearMath/btSerializer.h"
  20
  21 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
  22         : m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)),
  23           m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)),
  24           m_dynamicAabbTree(0),
  25           m_updateRevision(1),
  26           m_collisionMargin(btScalar(0.)),
  27           m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.))
  28 {
  29         m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
  30
  31         if (enableDynamicAabbTree)
  32         {
  33                 void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
  34                 m_dynamicAabbTree = new (mem) btDbvt();
  35                 btAssert(mem == m_dynamicAabbTree);
  36         }
  37
  38         m_children.reserve(initialChildCapacity);
  39 }
  40
  41 btCompoundShape::~btCompoundShape()
  42 {
  43         if (m_dynamicAabbTree)
  44         {
  45                 m_dynamicAabbTree->~btDbvt();
  46                 btAlignedFree(m_dynamicAabbTree);
  47         }
  48 }
  49
  50 void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape)
  51 {
  52         m_updateRevision++;
  53         //m_childTransforms.push_back(localTransform);
  54         //m_childShapes.push_back(shape);
  55         btCompoundShapeChild child;
  56         child.m_node = 0;
  57         child.m_transform = localTransform;
  58         child.m_childShape = shape;
  59         child.m_childShapeType = shape->getShapeType();
  60         child.m_childMargin = shape->getMargin();
  61
  62         //extend the local aabbMin/aabbMax
  63         btVector3 localAabbMin, localAabbMax;
  64         shape->getAabb(localTransform, localAabbMin, localAabbMax);
  65         for (int i = 0; i < 3; i++)
  66         {
  67                 if (m_localAabbMin[i] > localAabbMin[i])
  68                 {
  69                         m_localAabbMin[i] = localAabbMin[i];
  70                 }
  71                 if (m_localAabbMax[i] < localAabbMax[i])
  72                 {
  73                         m_localAabbMax[i] = localAabbMax[i];
  74                 }
  75         }
  76         if (m_dynamicAabbTree)
  77         {
  78                 const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
  79                 size_t index = m_children.size();
  80                 child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index));
  81         }
  82
  83         m_children.push_back(child);
  84 }
  85
  86 void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb)
  87 {
  88         m_children[childIndex].m_transform = newChildTransform;
  89
  90         if (m_dynamicAabbTree)
  91         {
  92                 ///update the dynamic aabb tree
  93                 btVector3 localAabbMin, localAabbMax;
  94                 m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax);
  95                 ATTRIBUTE_ALIGNED16(btDbvtVolume)
  96                 bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
  97                 //int index = m_children.size()-1;
  98                 m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds);
  99         }
 100
 101         if (shouldRecalculateLocalAabb)
 102         {
 103                 recalculateLocalAabb();
 104         }
 105 }
 106
 107 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
 108 {
 109         m_updateRevision++;
 110         btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size());
 111         if (m_dynamicAabbTree)
 112         {
 113                 m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
 114         }
 115         m_children.swap(childShapeIndex, m_children.size() - 1);
 116         if (m_dynamicAabbTree)
 117                 m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
 118         m_children.pop_back();
 119 }
 120
 121 void btCompoundShape::removeChildShape(btCollisionShape* shape)
 122 {
 123         m_updateRevision++;
 124         // Find the children containing the shape specified, and remove those children.
 125         //note: there might be multiple children using the same shape!
 126         for (int i = m_children.size() - 1; i >= 0; i--)
 127         {
 128                 if (m_children[i].m_childShape == shape)
 129                 {
 130                         removeChildShapeByIndex(i);
 131                 }
 132         }
 133
 134         recalculateLocalAabb();
 135 }
 136
 137 void btCompoundShape::recalculateLocalAabb()
 138 {
 139         // Recalculate the local aabb
 140         // Brute force, it iterates over all the shapes left.
 141
 142         m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 143         m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 144
 145         //extend the local aabbMin/aabbMax
 146         for (int j = 0; j < m_children.size(); j++)
 147         {
 148                 btVector3 localAabbMin, localAabbMax;
 149                 m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
 150                 for (int i = 0; i < 3; i++)
 151                 {
 152                         if (m_localAabbMin[i] > localAabbMin[i])
 153                                 m_localAabbMin[i] = localAabbMin[i];
 154                         if (m_localAabbMax[i] < localAabbMax[i])
 155                                 m_localAabbMax[i] = localAabbMax[i];
 156                 }
 157         }
 158 }
 159
 160 ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
 161 void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 162 {
 163         btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin);
 164         btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin);
 165
 166         //avoid an illegal AABB when there are no children
 167         if (!m_children.size())
 168         {
 169                 localHalfExtents.setValue(0, 0, 0);
 170                 localCenter.setValue(0, 0, 0);
 171         }
 172         localHalfExtents += btVector3(getMargin(), getMargin(), getMargin());
 173
 174         btMatrix3x3 abs_b = trans.getBasis().absolute();
 175
 176         btVector3 center = trans(localCenter);
 177
 178         btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 179         aabbMin = center - extent;
 180         aabbMax = center + extent;
 181 }
 182
 183 void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 184 {
 185         //approximation: take the inertia from the aabb for now
 186         btTransform ident;
 187         ident.setIdentity();
 188         btVector3 aabbMin, aabbMax;
 189         getAabb(ident, aabbMin, aabbMax);
 190
 191         btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
 192
 193         btScalar lx = btScalar(2.) * (halfExtents.x());
 194         btScalar ly = btScalar(2.) * (halfExtents.y());
 195         btScalar lz = btScalar(2.) * (halfExtents.z());
 196
 197         inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz);
 198         inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz);
 199         inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly);
 200 }
 201
 202 void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const
 203 {
 204         int n = m_children.size();
 205
 206         btScalar totalMass = 0;
 207         btVector3 center(0, 0, 0);
 208         int k;
 209
 210         for (k = 0; k < n; k++)
 211         {
 212                 btAssert(masses[k] > 0);
 213                 center += m_children[k].m_transform.getOrigin() * masses[k];
 214                 totalMass += masses[k];
 215         }
 216
 217         btAssert(totalMass > 0);
 218
 219         center /= totalMass;
 220         principal.setOrigin(center);
 221
 222         btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
 223         for (k = 0; k < n; k++)
 224         {
 225                 btVector3 i;
 226                 m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
 227
 228                 const btTransform& t = m_children[k].m_transform;
 229                 btVector3 o = t.getOrigin() - center;
 230
 231                 //compute inertia tensor in coordinate system of compound shape
 232                 btMatrix3x3 j = t.getBasis().transpose();
 233                 j[0] *= i[0];
 234                 j[1] *= i[1];
 235                 j[2] *= i[2];
 236                 j = t.getBasis() * j;
 237
 238                 //add inertia tensor
 239                 tensor[0] += j[0];
 240                 tensor[1] += j[1];
 241                 tensor[2] += j[2];
 242
 243                 //compute inertia tensor of pointmass at o
 244                 btScalar o2 = o.length2();
 245                 j[0].setValue(o2, 0, 0);
 246                 j[1].setValue(0, o2, 0);
 247                 j[2].setValue(0, 0, o2);
 248                 j[0] += o * -o.x();
 249                 j[1] += o * -o.y();
 250                 j[2] += o * -o.z();
 251
 252                 //add inertia tensor of pointmass
 253                 tensor[0] += masses[k] * j[0];
 254                 tensor[1] += masses[k] * j[1];
 255                 tensor[2] += masses[k] * j[2];
 256         }
 257
 258         tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
 259         inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
 260 }
 261
 262 void btCompoundShape::setLocalScaling(const btVector3& scaling)
 263 {
 264         for (int i = 0; i < m_children.size(); i++)
 265         {
 266                 btTransform childTrans = getChildTransform(i);
 267                 btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
 268                 //              childScale = childScale * (childTrans.getBasis() * scaling);
 269                 childScale = childScale * scaling / m_localScaling;
 270                 m_children[i].m_childShape->setLocalScaling(childScale);
 271                 childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
 272                 updateChildTransform(i, childTrans, false);
 273         }
 274
 275         m_localScaling = scaling;
 276         recalculateLocalAabb();
 277 }
 278
 279 void btCompoundShape::createAabbTreeFromChildren()
 280 {
 281         if (!m_dynamicAabbTree)
 282         {
 283                 void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
 284                 m_dynamicAabbTree = new (mem) btDbvt();
 285                 btAssert(mem == m_dynamicAabbTree);
 286
 287                 for (int index = 0; index < m_children.size(); index++)
 288                 {
 289                         btCompoundShapeChild& child = m_children[index];
 290
 291                         //extend the local aabbMin/aabbMax
 292                         btVector3 localAabbMin, localAabbMax;
 293                         child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax);
 294
 295                         const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 296                         size_t index2 = index;
 297                         child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2));
 298                 }
 299         }
 300 }
 301
 302 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 303 const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
 304 {
 305         btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer;
 306         btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
 307
 308         shapeData->m_collisionMargin = float(m_collisionMargin);
 309         shapeData->m_numChildShapes = m_children.size();
 310         shapeData->m_childShapePtr = 0;
 311         if (shapeData->m_numChildShapes)
 312         {
 313                 btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes);
 314                 btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
 315                 shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
 316
 317                 for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++)
 318                 {
 319                         memPtr->m_childMargin = float(m_children[i].m_childMargin);
 320                         memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
 321                         //don't serialize shapes that already have been serialized
 322                         if (!serializer->findPointer(m_children[i].m_childShape))
 323                         {
 324                                 btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1);
 325                                 const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer);
 326                                 serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape);
 327                         }
 328
 329                         memPtr->m_childShapeType = m_children[i].m_childShapeType;
 330                         m_children[i].m_transform.serializeFloat(memPtr->m_transform);
 331                 }
 332                 serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr);
 333         }
 334         return "btCompoundShapeData";
 335 }