Add PhysicsEffects to Extras. The build is only tested on Windows and Android.
The Android/NEON optimized version of Physics Effects is thanks to Graham Rhodes and Anthony Hamilton, See Issue 587
This commit is contained in:
erwin.coumans
@@ -0,0 +1,479 @@
|
||||
/*
|
||||
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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user