Extras/PhysicsEffects/include/BulletPhysicsEffects/btBulletPhysicsEffectsWorld.cpp

   1 /*\r
   2 Physics Effects Copyright(C) 2011 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #include "btBulletPhysicsEffectsWorld.h"\r
  18 #include "BulletDynamics/Dynamics/btRigidBody.h"\r
  19 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"\r
  20 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"\r
  21 #include "LinearMath/btAabbUtil2.h"\r
  22 #include "BulletMultiThreaded/SequentialThreadSupport.h"\r
  23 #include "BulletCollision/CollisionShapes/btCollisionShape.h"\r
  24 #include "LinearMath/btIDebugDraw.h"\r
  25 #include "LinearMath/btQuickprof.h"\r
  26 \r
  27 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"\r
  28 \r
  29 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"\r
  30 \r
  31 \r
  32 #include "BulletPhysicsEffects/btLowLevelData.h"\r
  33 #include <physics_effects/low_level/pfx_low_level_include.h>\r
  34 #include <physics_effects/util/pfx_util_include.h>\r
  35 #include "btLowLevelBroadphase.h"\r
  36 \r
  37 \r
  38 #include "BulletCollision/CollisionShapes/btCompoundShape.h"//required for shape conversion\r
  39 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"//required for shape conversion\r
  40 #include "BulletCollision/CollisionShapes/btCylinderShape.h"//required for shape conversion\r
  41 #include "BulletCollision/CollisionShapes/btSphereShape.h"//required for shape conversion\r
  42 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"//required for shape conversion\r
  43 #include "BulletCollision/CollisionShapes/btBoxShape.h"//required for shape conversion\r
  44 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"\r
  45 #include "BulletMultiThreaded/vectormath2bullet.h"\r
  46 \r
  47 #ifdef _WIN32\r
  48 #include "BulletMultiThreaded/Win32ThreadSupport.h"\r
  49 #endif\r
  50 \r
  51 \r
  52 #ifdef USE_PE_GATHER_SCATTER_SPURS_TASK\r
  53 #include "SpuDispatch/BulletPEGatherScatterSpursSupport.h"\r
  54 #include "SpuDispatch/SpuPEGatherScatterTaskProcess.h"\r
  55 #endif\r
  56 \r
  57 \r
  58 btBulletPhysicsEffectsWorld::btBulletPhysicsEffectsWorld(btLowLevelData* lowLevelData, btDispatcher* dispatcher,btLowLevelBroadphase* broadphase,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration, btThreadSupportInterface* threadSupport)\r
  59 :btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver,collisionConfiguration),\r
  60 m_lowLevelData(lowLevelData)\r
  61 {\r
  62 \r
  63 #ifdef USE_PE_GATHER_SCATTER_SPURS_TASK\r
  64         int numGatherScatterSpus = threadSupport->getNumTasks();\r
  65         m_PEGatherScatterProcess = new SpuPEGatherScatterTaskProcess(threadSupport,numGatherScatterSpus);\r
  66 #endif //USE_PE_GATHER_SCATTER_SPURS_TASK\r
  67 \r
  68         m_lowLevelStates.resize(broadphase->m_maxHandles);//or use expandNonInitializing?\r
  69         m_lowLevelBodies.resize(broadphase->m_maxHandles);\r
  70         m_lowLevelSolverBodies.resize(broadphase->m_maxHandles);\r
  71         m_lowLevelCollidables.resize(broadphase->m_maxHandles);\r
  72 \r
  73         m_lowLevelData->m_states = &m_lowLevelStates[0];\r
  74         m_lowLevelData->m_collidables = &m_lowLevelCollidables[0];\r
  75         m_lowLevelData->m_bodies = &m_lowLevelBodies[0];\r
  76         m_lowLevelData->m_solverBodies = &m_lowLevelSolverBodies[0];\r
  77         m_lowLevelData->m_numRigidBodies = broadphase->m_maxHandles;\r
  78 \r
  79 }\r
  80                 \r
  81 btBulletPhysicsEffectsWorld::~btBulletPhysicsEffectsWorld()\r
  82 {\r
  83 #ifdef USE_PE_GATHER_SCATTER_SPURS_TASK\r
  84         delete  m_PEGatherScatterProcess;\r
  85 #endif\r
  86 \r
  87 }\r
  88 \r
  89 \r
  90 \r
  91 void    btBulletPhysicsEffectsWorld::integrateTransforms(btScalar timeStep)\r
  92 {\r
  93                         ///integrate transforms\r
  94 #ifdef USE_PE_GATHER_SCATTER_SPURS_TASK\r
  95         if (getDispatchInfo().m_enableSPU)\r
  96         {\r
  97                 BT_PROFILE("integrateTransformsSPU");\r
  98                 int numRemainingObjects = m_nonStaticRigidBodies.size();\r
  99 \r
 100                 int batchSize = PARALLEL_BATCH_SIZE;\r
 101                 int startIndex = 0;\r
 102                 while (numRemainingObjects>0)\r
 103                 {\r
 104                         int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;\r
 105 \r
 106                         //issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)\r
 107                         m_PEGatherScatterProcess->issueTask(\r
 108                                                                                                         CMD_SAMPLE_INTEGRATE_BODIES,\r
 109                                                                                                         &m_nonStaticRigidBodies[0],\r
 110                                                                                                         0,\r
 111                                                                                                         0,\r
 112                                                                                                         startIndex,\r
 113                                                                                                         currentBatchSize);\r
 114                         numRemainingObjects -= currentBatchSize;\r
 115                         startIndex += currentBatchSize;\r
 116                 }\r
 117                 \r
 118                 m_PEGatherScatterProcess->flush();\r
 119         } else\r
 120 #endif //USE_PE_GATHER_SCATTER_SPURS_TASK\r
 121         {\r
 122 //              BT_PROFILE("integrateTransforms");\r
 123                 btDiscreteDynamicsWorld::integrateTransforms(timeStep);\r
 124         }\r
 125 }\r
 126 \r
 127 \r
 128 void    btBulletPhysicsEffectsWorld::predictUnconstraintMotion(btScalar timeStep)\r
 129 {\r
 130 #ifdef USE_PE_GATHER_SCATTER_SPURS_TASK\r
 131         if (getDispatchInfo().m_enableSPU)\r
 132         {\r
 133                 BT_PROFILE("predictUnconstraintMotionSPU");\r
 134                 int numRemainingObjects = m_nonStaticRigidBodies.size();\r
 135 \r
 136                 int batchSize = PARALLEL_BATCH_SIZE;\r
 137                 int startIndex=0;\r
 138                 while (numRemainingObjects>0)\r
 139                 {\r
 140                         int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;\r
 141 \r
 142                         //issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)\r
 143                         m_PEGatherScatterProcess->issueTask(\r
 144                                                                                                         CMD_SAMPLE_PREDICT_MOTION_BODIES,\r
 145                                                                                                         &m_nonStaticRigidBodies[0],\r
 146                                                                                                         0,\r
 147                                                                                                         0,\r
 148                                                                                                         startIndex,\r
 149                                                                                                         currentBatchSize);\r
 150                         numRemainingObjects -= currentBatchSize;\r
 151                         startIndex += currentBatchSize;\r
 152                 }\r
 153                 \r
 154                 m_PEGatherScatterProcess->flush();\r
 155         }\r
 156         else\r
 157 #endif //USE_PE_GATHER_SCATTER_SPURS_TASK\r
 158         {\r
 159                 btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);\r
 160         }\r
 161 \r
 162         for ( int i=0;i<m_nonStaticRigidBodies.size();i++)\r
 163         {\r
 164                 btRigidBody* body = m_nonStaticRigidBodies[i];\r
 165                 body->setHitFraction(1.f);\r
 166 \r
 167                 if (body->isActive() && (!body->isStaticOrKinematicObject()))\r
 168                 {\r
 169                         syncRigidBodyState(body);\r
 170                 }\r
 171         }\r
 172 }\r
 173 \r
 174 \r
 175 \r
 176 void    btBulletPhysicsEffectsWorld::solveConstraints(btContactSolverInfo& solverInfo)\r
 177 {\r
 178         BT_PROFILE("solveConstraints");\r
 179 \r
 180         btCollisionDispatcher* disp = (btCollisionDispatcher*) getDispatcher();\r
 181         int numBodies = getNumCollisionObjects();\r
 182         \r
 183         btPersistentManifold** manifolds = disp->getInternalManifoldPointer();\r
 184         \r
 185         int numManifolds = disp->getNumManifolds();\r
 186         \r
 187         \r
 188         if ((getNumCollisionObjects()>0) && (numManifolds + m_constraints.size()>0))\r
 189         {\r
 190 \r
 191                 btCollisionObject** bodies = numBodies ? &getCollisionObjectArray()[0] : 0;\r
 192                 btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;\r
 193 \r
 194                 getConstraintSolver()->solveGroup( bodies,numBodies, disp->getInternalManifoldPointer(),numManifolds, constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,disp);\r
 195         }\r
 196 }\r
 197 \r
 198 \r
 199 void    btBulletPhysicsEffectsWorld::calculateSimulationIslands()\r
 200 {\r
 201 \r
 202 }\r
 203 \r
 204 static void     convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes)\r
 205 {\r
 206         switch (bulletShape->getShapeType())\r
 207         {\r
 208         case BOX_SHAPE_PROXYTYPE:\r
 209                 {\r
 210                         btBoxShape* boxshape = (btBoxShape*)bulletShape;\r
 211                         sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ());\r
 212                         sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 213                         shape.reset();\r
 214                         shape.setBox(box);\r
 215                         break;\r
 216                 }\r
 217 \r
 218         case TRIANGLE_MESH_SHAPE_PROXYTYPE:\r
 219                 {\r
 220                         btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape;\r
 221 \r
 222                         int numSubParts = trimesh->getMeshInterface()->getNumSubParts();\r
 223                         btAssert(numSubParts>0);\r
 224                         \r
 225                         for (int i=0;i<numSubParts;i++)\r
 226                         {\r
 227                                 unsigned char* vertexBase=0;\r
 228                                 int numVerts = 0;\r
 229                                 PHY_ScalarType vertexType;\r
 230                                 int vertexStride=0;\r
 231                                 unsigned char* indexBase=0;\r
 232                                 int indexStride=0;\r
 233                                 int numFaces=0;\r
 234                                 PHY_ScalarType indexType;\r
 235 \r
 236                                 trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i);\r
 237 \r
 238                                 sce::PhysicsEffects::PfxCreateLargeTriMeshParam param;\r
 239                                 btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX);\r
 240                                 unsigned short int* copyIndices = new unsigned short int[numFaces*3];\r
 241                                 switch (indexType)\r
 242                                 {\r
 243                                 case PHY_UCHAR: \r
 244                                         {\r
 245                                                 for (int p=0;p<numFaces;p++)\r
 246                                                 {\r
 247                                                         copyIndices[p*3]=indexBase[p*indexStride];\r
 248                                                         copyIndices[p*3+1]=indexBase[p*indexStride+1];\r
 249                                                         copyIndices[p*3+2]=indexBase[p*indexStride+2];\r
 250                                                 }\r
 251                                                 break;\r
 252                                         }\r
 253                                         //PHY_INTEGER:\r
 254                                         //PHY_SHORT:\r
 255                                 default:\r
 256                                         {\r
 257                                                 btAssert(0);\r
 258                                         }\r
 259                                 };\r
 260                                 \r
 261                                 param.verts = (float*)vertexBase;\r
 262                                 param.numVerts = numVerts;\r
 263                                 param.vertexStrideBytes = vertexStride;\r
 264 \r
 265                                 param.triangles = copyIndices;\r
 266                                 param.numTriangles = numFaces;\r
 267                                 param.triangleStrideBytes = sizeof(unsigned short int)*3;\r
 268                                 \r
 269                                 sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh();\r
 270 \r
 271                                 sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param);\r
 272                                 if(ret != SCE_PFX_OK) {\r
 273                                         SCE_PFX_PRINTF("Can't create large mesh.\n");\r
 274                                 }\r
 275 \r
 276                                 sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 277                                 shape.reset();\r
 278                                 shape.setLargeTriMesh(largeMesh);\r
 279                         }\r
 280 \r
 281                         break;\r
 282                 }\r
 283         case SPHERE_SHAPE_PROXYTYPE:\r
 284                 {\r
 285                         btSphereShape* sphereshape = (btSphereShape*)bulletShape;\r
 286                         sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius());\r
 287                         sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 288                         shape.reset();\r
 289                         shape.setSphere(sphere);\r
 290                         break;\r
 291                 }\r
 292         case CAPSULE_SHAPE_PROXYTYPE:\r
 293                 {\r
 294                         btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now\r
 295                         sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius());\r
 296                         sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 297                         shape.reset();\r
 298                         shape.setCapsule(capsule);\r
 299                         break;\r
 300                 }\r
 301         case CYLINDER_SHAPE_PROXYTYPE:\r
 302                 {\r
 303                         btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now\r
 304                         sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius());\r
 305                         sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 306                         shape.reset();\r
 307                         shape.setCylinder(cylinder);\r
 308                         break;\r
 309                 }\r
 310         case CONVEX_HULL_SHAPE_PROXYTYPE:\r
 311                 {\r
 312                         btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape;\r
 313                         \r
 314                         sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh();\r
 315                         convex->m_numVerts = convexHullShape->getNumPoints();\r
 316                         convex->m_numIndices = 0;//todo for ray intersection test support\r
 317                         \r
 318                         for (int i=0;i<convex->m_numVerts;i++)\r
 319                         {\r
 320                                 convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX());\r
 321                                 convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY());\r
 322                                 convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ());\r
 323                         }\r
 324 \r
 325                         convex->updateAABB();\r
 326                         sce::PhysicsEffects::PfxShape& shape = shapes.expand();\r
 327                         shape.reset();\r
 328                         shape.setConvexMesh(convex);\r
 329                         break;\r
 330                 }\r
 331         case COMPOUND_SHAPE_PROXYTYPE:\r
 332                 {\r
 333                         btCompoundShape* compound = (btCompoundShape*) bulletShape;\r
 334                         \r
 335                         for (int s=0;s<compound->getNumChildShapes();s++)\r
 336                         {\r
 337                                 convertShape(compound->getChildShape(s),shapes);\r
 338 \r
 339                                 sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis());\r
 340                                 sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin());\r
 341                                 sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate);\r
 342                                 shapes[shapes.size()-1].setOffsetTransform(childtransform);\r
 343                         }\r
 344 \r
 345                         break;\r
 346                 }\r
 347 \r
 348 \r
 349         default:\r
 350                 {\r
 351                         btAssert(0);\r
 352                 }\r
 353         };\r
 354 \r
 355 }\r
 356 \r
 357 void    btBulletPhysicsEffectsWorld::createStateAndCollidable(btRigidBody* body)\r
 358 {\r
 359         \r
 360         int objectIndex = body->getBroadphaseProxy()->m_uniqueId;\r
 361         btAssert(objectIndex>=0);\r
 362         //btAssert(objectIndex<m_maxHandles);\r
 363 \r
 364         //initialize it\r
 365         sce::PhysicsEffects::PfxRigidBody* pfxbody = &m_lowLevelBodies[objectIndex];\r
 366         sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];\r
 367         sce::PhysicsEffects::PfxCollidable* pfxcollidable = &m_lowLevelCollidables[objectIndex];\r
 368 \r
 369         ///convert/initialize body, shape, state and collidable\r
 370         pfxstate->reset();\r
 371         pfxbody->reset();\r
 372         pfxcollidable->reset();\r
 373         \r
 374         \r
 375         \r
 376         pfxbody->setFriction(body->getFriction());\r
 377         pfxbody->setRestitution(body->getRestitution());\r
 378 \r
 379         if (body->getInvMass())\r
 380         {\r
 381                 btScalar mass = 1.f/body->getInvMass();\r
 382                 pfxbody->setMass(mass);\r
 383                 Vectormath::Aos::Matrix3 inertiaInv = inertiaInv.identity();\r
 384                 inertiaInv.setElem(0,0,body->getInvInertiaDiagLocal().getX());\r
 385                 inertiaInv.setElem(1,1,body->getInvInertiaDiagLocal().getY());\r
 386                 inertiaInv.setElem(2,2,body->getInvInertiaDiagLocal().getZ());\r
 387                 pfxbody->setInertiaInv(inertiaInv);\r
 388                 pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeActive);\r
 389         } else\r
 390         {\r
 391                 pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeFixed);\r
 392         }\r
 393 \r
 394         btAlignedObjectArray<sce::PhysicsEffects::PfxShape> shapes;\r
 395         \r
 396         convertShape(body->getCollisionShape(), shapes);\r
 397 \r
 398         btAssert(shapes.size()>0);\r
 399 \r
 400         if (shapes.size()==1)\r
 401         {\r
 402                 pfxcollidable->addShape(shapes[0]);\r
 403                 pfxcollidable->finish();\r
 404         } else\r
 405         {\r
 406                 if (shapes.size()>1)\r
 407                 {\r
 408                         sce::PhysicsEffects::PfxUInt16* ints=new sce::PhysicsEffects::PfxUInt16[shapes.size()];\r
 409                         sce::PhysicsEffects::PfxShape* pfxshapes = new sce::PhysicsEffects::PfxShape[shapes.size()];\r
 410                         int p;\r
 411                         for (p=0;p<shapes.size();p++)\r
 412                         {\r
 413                                 ints[p]=p;\r
 414                                 pfxshapes[p] = shapes[p];\r
 415                         }\r
 416                         pfxcollidable->reset(pfxshapes,ints,shapes.size());\r
 417                         \r
 418                         for (p=0;p<shapes.size();p++)\r
 419                         {\r
 420                                 pfxcollidable->addShape(pfxshapes[p]);\r
 421                         }\r
 422                         \r
 423                         pfxcollidable->finish();\r
 424                 }\r
 425         }\r
 426         pfxstate->setRigidBodyId(objectIndex);\r
 427 \r
 428         syncRigidBodyState(body);\r
 429                 \r
 430         \r
 431 }\r
 432 \r
 433 \r
 434 \r
 435 void    btBulletPhysicsEffectsWorld::syncRigidBodyState(btRigidBody* body)\r
 436 {\r
 437         int objectIndex = body->getBroadphaseProxy()->m_uniqueId;\r
 438         sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];\r
 439 \r
 440         pfxstate->setPosition(sce::PhysicsEffects::PfxVector3(body->getWorldTransform().getOrigin()[0],body->getWorldTransform().getOrigin()[1],body->getWorldTransform().getOrigin()[2]));\r
 441         sce::PhysicsEffects::PfxQuat rot(body->getOrientation().getX(),body->getOrientation().getY(),body->getOrientation().getZ(),body->getOrientation().getW());\r
 442         pfxstate->setOrientation(rot);\r
 443 \r
 444 }\r
 445         \r
 446 \r
 447 \r
 448 void    btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body)\r
 449 {\r
 450 \r
 451         btDiscreteDynamicsWorld::addRigidBody(body);\r
 452 \r
 453         //create a state and collidable\r
 454                 \r
 455         createStateAndCollidable(body);\r
 456 \r
 457 \r
 458         \r
 459 \r
 460 \r
 461 //      m_lowLevelData->m_numRigidBodies++;\r
 462 //      btAssert(m_lowLevelData->m_numRigidBodies<m_lowLevelData->m_maxNumRigidBodies);\r
 463         \r
 464 }\r
 465 \r
 466 void    btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body, short group, short mask)\r
 467 {\r
 468         btDiscreteDynamicsWorld::addRigidBody(body,group,mask);\r
 469 \r
 470 }\r
 471 \r
 472 void    btBulletPhysicsEffectsWorld::removeRigidBody(btRigidBody* body)\r
 473 {\r
 474         btDiscreteDynamicsWorld::removeRigidBody(body);\r
 475 \r
 476 //      m_lowLevelData->m_numRigidBodies--;\r
 477 //      btAssert(m_lowLevelData->m_numRigidBodies>=0);\r
 478 }\r
 479 \r