a93a661b94
The Android/NEON optimized version of Physics Effects is thanks to Graham Rhodes and Anthony Hamilton, See Issue 587
480 lines
15 KiB
C++
480 lines
15 KiB
C++
/*
|
|
Physics Effects Copyright(C) 2011 Sony Computer Entertainment Inc.
|
|
All rights reserved.
|
|
|
|
Physics Effects is open software; you can redistribute it and/or
|
|
modify it under the terms of the BSD License.
|
|
|
|
Physics Effects is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
See the BSD License for more details.
|
|
|
|
A copy of the BSD License is distributed with
|
|
Physics Effects under the filename: physics_effects_license.txt
|
|
*/
|
|
|
|
#include "btBulletPhysicsEffectsWorld.h"
|
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
|
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
|
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
|
|
#include "LinearMath/btAabbUtil2.h"
|
|
#include "BulletMultiThreaded/SequentialThreadSupport.h"
|
|
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
|
|
#include "LinearMath/btIDebugDraw.h"
|
|
#include "LinearMath/btQuickprof.h"
|
|
|
|
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
|
|
|
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
|
|
|
|
|
|
#include "BulletPhysicsEffects/btLowLevelData.h"
|
|
#include <physics_effects/low_level/pfx_low_level_include.h>
|
|
#include <physics_effects/util/pfx_util_include.h>
|
|
#include "btLowLevelBroadphase.h"
|
|
|
|
|
|
#include "BulletCollision/CollisionShapes/btCompoundShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btCylinderShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btSphereShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btBoxShape.h"//required for shape conversion
|
|
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
|
#include "BulletMultiThreaded/vectormath2bullet.h"
|
|
|
|
#ifdef _WIN32
|
|
#include "BulletMultiThreaded/Win32ThreadSupport.h"
|
|
#endif
|
|
|
|
|
|
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
#include "SpuDispatch/BulletPEGatherScatterSpursSupport.h"
|
|
#include "SpuDispatch/SpuPEGatherScatterTaskProcess.h"
|
|
#endif
|
|
|
|
|
|
btBulletPhysicsEffectsWorld::btBulletPhysicsEffectsWorld(btLowLevelData* lowLevelData, btDispatcher* dispatcher,btLowLevelBroadphase* broadphase,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration, btThreadSupportInterface* threadSupport)
|
|
:btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver,collisionConfiguration),
|
|
m_lowLevelData(lowLevelData)
|
|
{
|
|
|
|
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
int numGatherScatterSpus = threadSupport->getNumTasks();
|
|
m_PEGatherScatterProcess = new SpuPEGatherScatterTaskProcess(threadSupport,numGatherScatterSpus);
|
|
#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
|
|
m_lowLevelStates.resize(broadphase->m_maxHandles);//or use expandNonInitializing?
|
|
m_lowLevelBodies.resize(broadphase->m_maxHandles);
|
|
m_lowLevelSolverBodies.resize(broadphase->m_maxHandles);
|
|
m_lowLevelCollidables.resize(broadphase->m_maxHandles);
|
|
|
|
m_lowLevelData->m_states = &m_lowLevelStates[0];
|
|
m_lowLevelData->m_collidables = &m_lowLevelCollidables[0];
|
|
m_lowLevelData->m_bodies = &m_lowLevelBodies[0];
|
|
m_lowLevelData->m_solverBodies = &m_lowLevelSolverBodies[0];
|
|
m_lowLevelData->m_numRigidBodies = broadphase->m_maxHandles;
|
|
|
|
}
|
|
|
|
btBulletPhysicsEffectsWorld::~btBulletPhysicsEffectsWorld()
|
|
{
|
|
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
delete m_PEGatherScatterProcess;
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::integrateTransforms(btScalar timeStep)
|
|
{
|
|
///integrate transforms
|
|
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
if (getDispatchInfo().m_enableSPU)
|
|
{
|
|
BT_PROFILE("integrateTransformsSPU");
|
|
int numRemainingObjects = m_nonStaticRigidBodies.size();
|
|
|
|
int batchSize = PARALLEL_BATCH_SIZE;
|
|
int startIndex = 0;
|
|
while (numRemainingObjects>0)
|
|
{
|
|
int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
|
|
|
|
//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
|
|
m_PEGatherScatterProcess->issueTask(
|
|
CMD_SAMPLE_INTEGRATE_BODIES,
|
|
&m_nonStaticRigidBodies[0],
|
|
0,
|
|
0,
|
|
startIndex,
|
|
currentBatchSize);
|
|
numRemainingObjects -= currentBatchSize;
|
|
startIndex += currentBatchSize;
|
|
}
|
|
|
|
m_PEGatherScatterProcess->flush();
|
|
} else
|
|
#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
{
|
|
// BT_PROFILE("integrateTransforms");
|
|
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
|
|
}
|
|
}
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::predictUnconstraintMotion(btScalar timeStep)
|
|
{
|
|
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
if (getDispatchInfo().m_enableSPU)
|
|
{
|
|
BT_PROFILE("predictUnconstraintMotionSPU");
|
|
int numRemainingObjects = m_nonStaticRigidBodies.size();
|
|
|
|
int batchSize = PARALLEL_BATCH_SIZE;
|
|
int startIndex=0;
|
|
while (numRemainingObjects>0)
|
|
{
|
|
int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
|
|
|
|
//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
|
|
m_PEGatherScatterProcess->issueTask(
|
|
CMD_SAMPLE_PREDICT_MOTION_BODIES,
|
|
&m_nonStaticRigidBodies[0],
|
|
0,
|
|
0,
|
|
startIndex,
|
|
currentBatchSize);
|
|
numRemainingObjects -= currentBatchSize;
|
|
startIndex += currentBatchSize;
|
|
}
|
|
|
|
m_PEGatherScatterProcess->flush();
|
|
}
|
|
else
|
|
#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
|
|
{
|
|
btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
|
|
}
|
|
|
|
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
|
|
{
|
|
btRigidBody* body = m_nonStaticRigidBodies[i];
|
|
body->setHitFraction(1.f);
|
|
|
|
if (body->isActive() && (!body->isStaticOrKinematicObject()))
|
|
{
|
|
syncRigidBodyState(body);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
|
{
|
|
BT_PROFILE("solveConstraints");
|
|
|
|
btCollisionDispatcher* disp = (btCollisionDispatcher*) getDispatcher();
|
|
int numBodies = getNumCollisionObjects();
|
|
|
|
btPersistentManifold** manifolds = disp->getInternalManifoldPointer();
|
|
|
|
int numManifolds = disp->getNumManifolds();
|
|
|
|
|
|
if ((getNumCollisionObjects()>0) && (numManifolds + m_constraints.size()>0))
|
|
{
|
|
|
|
btCollisionObject** bodies = numBodies ? &getCollisionObjectArray()[0] : 0;
|
|
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
|
|
|
|
getConstraintSolver()->solveGroup( bodies,numBodies, disp->getInternalManifoldPointer(),numManifolds, constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,disp);
|
|
}
|
|
}
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::calculateSimulationIslands()
|
|
{
|
|
|
|
}
|
|
|
|
static void convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes)
|
|
{
|
|
switch (bulletShape->getShapeType())
|
|
{
|
|
case BOX_SHAPE_PROXYTYPE:
|
|
{
|
|
btBoxShape* boxshape = (btBoxShape*)bulletShape;
|
|
sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ());
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setBox(box);
|
|
break;
|
|
}
|
|
|
|
case TRIANGLE_MESH_SHAPE_PROXYTYPE:
|
|
{
|
|
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape;
|
|
|
|
int numSubParts = trimesh->getMeshInterface()->getNumSubParts();
|
|
btAssert(numSubParts>0);
|
|
|
|
for (int i=0;i<numSubParts;i++)
|
|
{
|
|
unsigned char* vertexBase=0;
|
|
int numVerts = 0;
|
|
PHY_ScalarType vertexType;
|
|
int vertexStride=0;
|
|
unsigned char* indexBase=0;
|
|
int indexStride=0;
|
|
int numFaces=0;
|
|
PHY_ScalarType indexType;
|
|
|
|
trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i);
|
|
|
|
sce::PhysicsEffects::PfxCreateLargeTriMeshParam param;
|
|
btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX);
|
|
unsigned short int* copyIndices = new unsigned short int[numFaces*3];
|
|
switch (indexType)
|
|
{
|
|
case PHY_UCHAR:
|
|
{
|
|
for (int p=0;p<numFaces;p++)
|
|
{
|
|
copyIndices[p*3]=indexBase[p*indexStride];
|
|
copyIndices[p*3+1]=indexBase[p*indexStride+1];
|
|
copyIndices[p*3+2]=indexBase[p*indexStride+2];
|
|
}
|
|
break;
|
|
}
|
|
//PHY_INTEGER:
|
|
//PHY_SHORT:
|
|
default:
|
|
{
|
|
btAssert(0);
|
|
}
|
|
};
|
|
|
|
param.verts = (float*)vertexBase;
|
|
param.numVerts = numVerts;
|
|
param.vertexStrideBytes = vertexStride;
|
|
|
|
param.triangles = copyIndices;
|
|
param.numTriangles = numFaces;
|
|
param.triangleStrideBytes = sizeof(unsigned short int)*3;
|
|
|
|
sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh();
|
|
|
|
sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param);
|
|
if(ret != SCE_PFX_OK) {
|
|
SCE_PFX_PRINTF("Can't create large mesh.\n");
|
|
}
|
|
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setLargeTriMesh(largeMesh);
|
|
}
|
|
|
|
break;
|
|
}
|
|
case SPHERE_SHAPE_PROXYTYPE:
|
|
{
|
|
btSphereShape* sphereshape = (btSphereShape*)bulletShape;
|
|
sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius());
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setSphere(sphere);
|
|
break;
|
|
}
|
|
case CAPSULE_SHAPE_PROXYTYPE:
|
|
{
|
|
btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now
|
|
sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius());
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setCapsule(capsule);
|
|
break;
|
|
}
|
|
case CYLINDER_SHAPE_PROXYTYPE:
|
|
{
|
|
btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now
|
|
sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius());
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setCylinder(cylinder);
|
|
break;
|
|
}
|
|
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
|
{
|
|
btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape;
|
|
|
|
sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh();
|
|
convex->m_numVerts = convexHullShape->getNumPoints();
|
|
convex->m_numIndices = 0;//todo for ray intersection test support
|
|
|
|
for (int i=0;i<convex->m_numVerts;i++)
|
|
{
|
|
convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX());
|
|
convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY());
|
|
convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ());
|
|
}
|
|
|
|
convex->updateAABB();
|
|
sce::PhysicsEffects::PfxShape& shape = shapes.expand();
|
|
shape.reset();
|
|
shape.setConvexMesh(convex);
|
|
break;
|
|
}
|
|
case COMPOUND_SHAPE_PROXYTYPE:
|
|
{
|
|
btCompoundShape* compound = (btCompoundShape*) bulletShape;
|
|
|
|
for (int s=0;s<compound->getNumChildShapes();s++)
|
|
{
|
|
convertShape(compound->getChildShape(s),shapes);
|
|
|
|
sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis());
|
|
sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin());
|
|
sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate);
|
|
shapes[shapes.size()-1].setOffsetTransform(childtransform);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
default:
|
|
{
|
|
btAssert(0);
|
|
}
|
|
};
|
|
|
|
}
|
|
|
|
void btBulletPhysicsEffectsWorld::createStateAndCollidable(btRigidBody* body)
|
|
{
|
|
|
|
int objectIndex = body->getBroadphaseProxy()->m_uniqueId;
|
|
btAssert(objectIndex>=0);
|
|
//btAssert(objectIndex<m_maxHandles);
|
|
|
|
//initialize it
|
|
sce::PhysicsEffects::PfxRigidBody* pfxbody = &m_lowLevelBodies[objectIndex];
|
|
sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];
|
|
sce::PhysicsEffects::PfxCollidable* pfxcollidable = &m_lowLevelCollidables[objectIndex];
|
|
|
|
///convert/initialize body, shape, state and collidable
|
|
pfxstate->reset();
|
|
pfxbody->reset();
|
|
pfxcollidable->reset();
|
|
|
|
|
|
|
|
pfxbody->setFriction(body->getFriction());
|
|
pfxbody->setRestitution(body->getRestitution());
|
|
|
|
if (body->getInvMass())
|
|
{
|
|
btScalar mass = 1.f/body->getInvMass();
|
|
pfxbody->setMass(mass);
|
|
Vectormath::Aos::Matrix3 inertiaInv = inertiaInv.identity();
|
|
inertiaInv.setElem(0,0,body->getInvInertiaDiagLocal().getX());
|
|
inertiaInv.setElem(1,1,body->getInvInertiaDiagLocal().getY());
|
|
inertiaInv.setElem(2,2,body->getInvInertiaDiagLocal().getZ());
|
|
pfxbody->setInertiaInv(inertiaInv);
|
|
pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeActive);
|
|
} else
|
|
{
|
|
pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeFixed);
|
|
}
|
|
|
|
btAlignedObjectArray<sce::PhysicsEffects::PfxShape> shapes;
|
|
|
|
convertShape(body->getCollisionShape(), shapes);
|
|
|
|
btAssert(shapes.size()>0);
|
|
|
|
if (shapes.size()==1)
|
|
{
|
|
pfxcollidable->addShape(shapes[0]);
|
|
pfxcollidable->finish();
|
|
} else
|
|
{
|
|
if (shapes.size()>1)
|
|
{
|
|
sce::PhysicsEffects::PfxUInt16* ints=new sce::PhysicsEffects::PfxUInt16[shapes.size()];
|
|
sce::PhysicsEffects::PfxShape* pfxshapes = new sce::PhysicsEffects::PfxShape[shapes.size()];
|
|
int p;
|
|
for (p=0;p<shapes.size();p++)
|
|
{
|
|
ints[p]=p;
|
|
pfxshapes[p] = shapes[p];
|
|
}
|
|
pfxcollidable->reset(pfxshapes,ints,shapes.size());
|
|
|
|
for (p=0;p<shapes.size();p++)
|
|
{
|
|
pfxcollidable->addShape(pfxshapes[p]);
|
|
}
|
|
|
|
pfxcollidable->finish();
|
|
}
|
|
}
|
|
pfxstate->setRigidBodyId(objectIndex);
|
|
|
|
syncRigidBodyState(body);
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::syncRigidBodyState(btRigidBody* body)
|
|
{
|
|
int objectIndex = body->getBroadphaseProxy()->m_uniqueId;
|
|
sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];
|
|
|
|
pfxstate->setPosition(sce::PhysicsEffects::PfxVector3(body->getWorldTransform().getOrigin()[0],body->getWorldTransform().getOrigin()[1],body->getWorldTransform().getOrigin()[2]));
|
|
sce::PhysicsEffects::PfxQuat rot(body->getOrientation().getX(),body->getOrientation().getY(),body->getOrientation().getZ(),body->getOrientation().getW());
|
|
pfxstate->setOrientation(rot);
|
|
|
|
}
|
|
|
|
|
|
|
|
void btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body)
|
|
{
|
|
|
|
btDiscreteDynamicsWorld::addRigidBody(body);
|
|
|
|
//create a state and collidable
|
|
|
|
createStateAndCollidable(body);
|
|
|
|
|
|
|
|
|
|
|
|
// m_lowLevelData->m_numRigidBodies++;
|
|
// btAssert(m_lowLevelData->m_numRigidBodies<m_lowLevelData->m_maxNumRigidBodies);
|
|
|
|
}
|
|
|
|
void btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body, short group, short mask)
|
|
{
|
|
btDiscreteDynamicsWorld::addRigidBody(body,group,mask);
|
|
|
|
}
|
|
|
|
void btBulletPhysicsEffectsWorld::removeRigidBody(btRigidBody* body)
|
|
{
|
|
btDiscreteDynamicsWorld::removeRigidBody(body);
|
|
|
|
// m_lowLevelData->m_numRigidBodies--;
|
|
// btAssert(m_lowLevelData->m_numRigidBodies>=0);
|
|
}
|
|
|