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);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
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
|
||||
*/
|
||||
|
||||
#ifndef _BT_PARALLEL_DYNAMICS_WORLD_H
|
||||
#define _BT_PARALLEL_DYNAMICS_WORLD_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
#include "physics_effects/base_level/rigidbody/pfx_rigid_body.h"
|
||||
#include "physics_effects/base_level/rigidbody/pfx_rigid_state.h"
|
||||
#include "physics_effects/base_level/collision/pfx_collidable.h"
|
||||
#include "physics_effects/base_level/solver/pfx_solver_body.h"
|
||||
|
||||
//#define USE_PE_GATHER_SCATTER_SPURS_TASK 1
|
||||
|
||||
|
||||
class btThreadSupportInterface;
|
||||
class SpuPEGatherScatterTaskProcess;
|
||||
struct CellSpurs;
|
||||
class SpuBatchRaycaster;
|
||||
struct btLowLevelBroadphase;
|
||||
|
||||
#define PARALLEL_BATCH_SIZE 64
|
||||
|
||||
|
||||
///btBulletPhysicsEffectsWorld adds parallel processing for integration/motion prediction
|
||||
class btBulletPhysicsEffectsWorld : public btDiscreteDynamicsWorld
|
||||
{
|
||||
|
||||
protected:
|
||||
btAlignedObjectArray<sce::PhysicsEffects::PfxRigidState> m_lowLevelStates;
|
||||
btAlignedObjectArray<sce::PhysicsEffects::PfxRigidBody> m_lowLevelBodies;
|
||||
btAlignedObjectArray<sce::PhysicsEffects::PfxSolverBody> m_lowLevelSolverBodies;
|
||||
btAlignedObjectArray<sce::PhysicsEffects::PfxCollidable> m_lowLevelCollidables;
|
||||
|
||||
//PfxSolverBody solverBodies[NUM_RIGIDBODIES];
|
||||
|
||||
class SpuPEGatherScatterTaskProcess* m_PEGatherScatterProcess;
|
||||
|
||||
struct btLowLevelData* m_lowLevelData;
|
||||
|
||||
void createStateAndCollidable(btRigidBody* body);
|
||||
|
||||
void syncRigidBodyState(btRigidBody* body);
|
||||
public:
|
||||
|
||||
btBulletPhysicsEffectsWorld(struct btLowLevelData* lowLevelData, btDispatcher* dispatcher,btLowLevelBroadphase* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration, btThreadSupportInterface* threadSupport);
|
||||
|
||||
virtual ~btBulletPhysicsEffectsWorld();
|
||||
|
||||
virtual void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
virtual void calculateSimulationIslands();
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body, short group, short mask);
|
||||
|
||||
virtual void removeRigidBody(btRigidBody* body);
|
||||
|
||||
|
||||
};
|
||||
#endif //_BT_PARALLEL_DYNAMICS_WORLD_H
|
||||
@@ -0,0 +1,431 @@
|
||||
/*
|
||||
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 "btLowLevelBroadphase.h"
|
||||
|
||||
|
||||
#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||
#include "BulletCollision/NarrowphaseCollision/btPersistentManifold.h"
|
||||
|
||||
// Include base level headers
|
||||
#include "physics_effects/base_level/pfx_base_level_include.h"
|
||||
|
||||
// Include low level headers
|
||||
#include "physics_effects/low_level/broadphase/pfx_broadphase.h"
|
||||
|
||||
#include "physics_effects/low_level/sort/pfx_parallel_sort.h"
|
||||
|
||||
#include "BulletMultiThreaded/vectormath2bullet.h"
|
||||
|
||||
#include "physics_effects/base_level/base/pfx_vec_utils.h"
|
||||
#include "physics_effects/base_level/collision/pfx_aabb.h"
|
||||
#include "physics_effects/base_level/rigidbody/pfx_rigid_state.h"
|
||||
#include "btLowLevelData.h"
|
||||
|
||||
using namespace sce::PhysicsEffects;
|
||||
|
||||
//E Temporary buffers
|
||||
#define POOL_BYTES (5*1024*1024)
|
||||
unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];
|
||||
|
||||
//E Stack allocator for temporary buffers
|
||||
PfxHeapManager pool(poolBuff,POOL_BYTES);
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Broadphase
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
btLowLevelBroadphase::btLowLevelBroadphase(btLowLevelData* lowLevelData, btOverlappingPairCache* paircache, int maxProxies)
|
||||
:m_lowLevelData(lowLevelData)
|
||||
{
|
||||
|
||||
m_guidGenerator = 1;
|
||||
m_releasepaircache = (paircache!=0)?false:true;
|
||||
m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
|
||||
m_clientData.m_bp = this;
|
||||
m_clientData.m_dispatcher = 0;
|
||||
|
||||
|
||||
m_broadphaseAabbMin.setValue(1e30,1e30,1e30);
|
||||
m_broadphaseAabbMax.setValue(-1e30,-1e30,-1e30);
|
||||
|
||||
// allocate handles buffer and put all handles on free list
|
||||
m_pHandlesRawPtr = btAlignedAlloc(sizeof(btLowLevelBroadphaseProxy)*maxProxies,16);
|
||||
m_pHandles = new(m_pHandlesRawPtr) btLowLevelBroadphaseProxy[maxProxies];
|
||||
m_maxHandles = maxProxies;
|
||||
m_numHandles = 0;
|
||||
m_firstFreeHandle = 0;
|
||||
m_LastHandleIndex = -1;
|
||||
|
||||
|
||||
{
|
||||
for (int i = m_firstFreeHandle; i < maxProxies; i++)
|
||||
{
|
||||
m_pHandles[i].SetNextFree(i + 1);
|
||||
m_pHandles[i].m_uniqueId = i;//start from zero, so we can re-use the uid for body ID
|
||||
}
|
||||
m_pHandles[maxProxies - 1].SetNextFree(0);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//
|
||||
btLowLevelBroadphase::~btLowLevelBroadphase()
|
||||
{
|
||||
if(m_releasepaircache)
|
||||
{
|
||||
m_paircache->~btOverlappingPairCache();
|
||||
btAlignedFree(m_paircache);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btBroadphaseProxy* btLowLevelBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
|
||||
{
|
||||
if (m_numHandles >= m_maxHandles)
|
||||
{
|
||||
btAssert(0);
|
||||
return 0; //should never happen, but don't let the game crash ;-)
|
||||
}
|
||||
btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
|
||||
|
||||
int newHandleIndex = allocHandle();
|
||||
btLowLevelBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btLowLevelBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
|
||||
m_uid2ptr.insert(proxy->m_uniqueId,proxy);
|
||||
return proxy;
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
|
||||
{
|
||||
m_uid2ptr.remove(proxyOrg->m_uniqueId);
|
||||
|
||||
btLowLevelBroadphaseProxy* proxy0 = static_cast<btLowLevelBroadphaseProxy*>(proxyOrg);
|
||||
freeHandle(proxy0);
|
||||
|
||||
m_paircache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
|
||||
|
||||
//validate();
|
||||
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
|
||||
{
|
||||
const btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);
|
||||
aabbMin = sbp->m_aabbMin;
|
||||
aabbMax = sbp->m_aabbMax;
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
|
||||
{
|
||||
btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);
|
||||
sbp->m_aabbMin = aabbMin;
|
||||
sbp->m_aabbMax = aabbMax;
|
||||
}
|
||||
|
||||
|
||||
bool btLowLevelBroadphase::aabbOverlap(btLowLevelBroadphaseProxy* proxy0,btLowLevelBroadphaseProxy* proxy1)
|
||||
{
|
||||
return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
|
||||
proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
|
||||
proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
|
||||
|
||||
}
|
||||
|
||||
|
||||
PfxBroadphasePair* btLowLevelBroadphase::getCurrentPairs()
|
||||
{
|
||||
return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
|
||||
}
|
||||
|
||||
const PfxBroadphasePair* btLowLevelBroadphase::getCurrentPairs() const
|
||||
{
|
||||
return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
|
||||
}
|
||||
|
||||
int btLowLevelBroadphase::getNumCurrentPairs() const
|
||||
{
|
||||
return m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::broadphase(PfxSortData32* proxies, int numRigidBodies, int axis, btDispatcher* dispatcher)
|
||||
{
|
||||
m_lowLevelData->m_pairSwap = 1-m_lowLevelData->m_pairSwap;
|
||||
|
||||
unsigned int &numPreviousPairs = m_lowLevelData->m_numPairs[1-m_lowLevelData->m_pairSwap];
|
||||
unsigned int &numCurrentPairs = m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];
|
||||
PfxBroadphasePair *previousPairs = &m_lowLevelData->m_pairsBuff[1-m_lowLevelData->m_pairSwap][0];
|
||||
PfxBroadphasePair *currentPairs = &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
|
||||
|
||||
|
||||
//E Create broadpahse proxies
|
||||
{
|
||||
// for(int i=0;i<numRigidBodies;i++) {
|
||||
// pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);
|
||||
// }
|
||||
|
||||
int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;
|
||||
void *workBuff = pool.allocate(workBytes);
|
||||
|
||||
pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);
|
||||
|
||||
pool.deallocate(workBuff);
|
||||
}
|
||||
|
||||
//E Find overlapped pairs
|
||||
{
|
||||
PfxFindPairsParam findPairsParam;
|
||||
findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(m_lowLevelData->m_maxPairs);
|
||||
findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);
|
||||
findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(m_lowLevelData->m_maxPairs);
|
||||
findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);
|
||||
findPairsParam.proxies = proxies;
|
||||
findPairsParam.numProxies = numRigidBodies;
|
||||
findPairsParam.maxPairs = m_lowLevelData->m_maxPairs;
|
||||
findPairsParam.axis = axis;
|
||||
|
||||
PfxFindPairsResult findPairsResult;
|
||||
|
||||
|
||||
int ret = pfxFindPairs(findPairsParam,findPairsResult);
|
||||
if(ret != SCE_PFX_OK)
|
||||
SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
|
||||
|
||||
pool.deallocate(findPairsParam.workBuff);
|
||||
|
||||
//E Decompose overlapped pairs into 3 arrays
|
||||
PfxDecomposePairsParam decomposePairsParam;
|
||||
decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
|
||||
decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);
|
||||
decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
|
||||
decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);
|
||||
decomposePairsParam.previousPairs = previousPairs;
|
||||
decomposePairsParam.numPreviousPairs = numPreviousPairs;
|
||||
decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()
|
||||
decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()
|
||||
|
||||
PfxDecomposePairsResult decomposePairsResult;
|
||||
|
||||
ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
|
||||
if(ret != SCE_PFX_OK)
|
||||
SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
|
||||
|
||||
pool.deallocate(decomposePairsParam.workBuff);
|
||||
|
||||
PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;
|
||||
PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;
|
||||
PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;
|
||||
PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;
|
||||
PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;
|
||||
PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;
|
||||
|
||||
for (int i=0;i<numOutRemovePairs;i++)
|
||||
{
|
||||
int idA = pfxGetObjectIdA(outRemovePairs[i]);
|
||||
int idB = pfxGetObjectIdB(outRemovePairs[i]);
|
||||
//use m_uid2ptr to get pointer
|
||||
|
||||
btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];
|
||||
btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];
|
||||
m_paircache->removeOverlappingPair(proxyA,proxyB,dispatcher);
|
||||
|
||||
//free low level contacts
|
||||
m_lowLevelData->m_contactIdPool[m_lowLevelData->m_numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);
|
||||
|
||||
}
|
||||
|
||||
for (int i=0;i<numOutNewPairs;i++)
|
||||
{
|
||||
int idA = pfxGetObjectIdA(outNewPairs[i]);
|
||||
int idB = pfxGetObjectIdB(outNewPairs[i]);
|
||||
//use m_uid2ptr to get pointer
|
||||
|
||||
btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];
|
||||
btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];
|
||||
btBroadphasePair* btpair = m_paircache->addOverlappingPair(proxyA,proxyB);
|
||||
|
||||
//initialize low level contacts
|
||||
int cId = 0;
|
||||
if(m_lowLevelData->m_numContactIdPool > 0) {
|
||||
cId = m_lowLevelData->m_contactIdPool[--m_lowLevelData->m_numContactIdPool];
|
||||
}
|
||||
else {
|
||||
cId = m_lowLevelData->m_numContacts++;
|
||||
}
|
||||
if(cId >= m_lowLevelData->m_maxContacts) {
|
||||
cId = 0;
|
||||
}
|
||||
SCE_PFX_ASSERT(cId < m_lowLevelData->m_maxContacts);
|
||||
pfxSetContactId(outNewPairs[i],cId);
|
||||
PfxContactManifold &contact = m_lowLevelData->m_contacts[cId];
|
||||
int sz = sizeof(PfxContactManifold);
|
||||
int sz2 = sizeof(btPersistentManifold);
|
||||
int sz3 = 4*3*sizeof(btConstraintRow);
|
||||
contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));
|
||||
contact.setCompositeFriction(0.1f);
|
||||
btpair->m_internalTmpValue = cId;
|
||||
}
|
||||
|
||||
|
||||
//E Merge 'new' and 'keep' pairs
|
||||
numCurrentPairs = 0;
|
||||
for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {
|
||||
currentPairs[numCurrentPairs++] = outKeepPairs[i];
|
||||
}
|
||||
for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
|
||||
currentPairs[numCurrentPairs++] = outNewPairs[i];
|
||||
}
|
||||
|
||||
pool.deallocate(decomposePairsParam.pairBuff);
|
||||
pool.deallocate(findPairsParam.pairBuff);
|
||||
}
|
||||
|
||||
{
|
||||
int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
|
||||
void *workBuff = pool.allocate(workBytes);
|
||||
|
||||
pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
|
||||
|
||||
pool.deallocate(workBuff);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
PfxInt32 MyUpdateBroadphaseProxy(PfxBroadphaseProxy& proxy,int rigidbodyId,const btBroadphaseProxy* bulletProxy, const PfxVector3& worldCenter,const PfxVector3& worldExtent,PfxUInt32 axis)
|
||||
{
|
||||
SCE_PFX_ALWAYS_ASSERT(axis<3);
|
||||
|
||||
PfxInt32 ret = SCE_PFX_OK;
|
||||
|
||||
PfxVector3 minRig = getVmVector3(bulletProxy->m_aabbMin);
|
||||
PfxVector3 maxRig = getVmVector3(bulletProxy->m_aabbMax);
|
||||
|
||||
PfxVecInt3 aabbMin,aabbMax;
|
||||
pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);
|
||||
|
||||
pfxSetXMin(proxy,aabbMin.getX());
|
||||
pfxSetXMax(proxy,aabbMax.getX());
|
||||
pfxSetYMin(proxy,aabbMin.getY());
|
||||
pfxSetYMax(proxy,aabbMax.getY());
|
||||
pfxSetZMin(proxy,aabbMin.getZ());
|
||||
pfxSetZMax(proxy,aabbMax.getZ());
|
||||
pfxSetKey(proxy,aabbMin.get(axis));
|
||||
pfxSetObjectId(proxy,rigidbodyId);
|
||||
pfxSetMotionMask(proxy, kPfxMotionTypeActive);
|
||||
pfxSetSelf(proxy,bulletProxy->m_collisionFilterGroup);
|
||||
pfxSetTarget(proxy,bulletProxy->m_collisionFilterMask);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
//set the broadphase proxies
|
||||
|
||||
btAlignedObjectArray<PfxBroadphaseProxy> proxies;
|
||||
proxies.reserve(m_LastHandleIndex);
|
||||
|
||||
//E Find the axis along which all rigid bodies are most widely positioned
|
||||
int axis = 0;
|
||||
int i;
|
||||
PfxVector3 s(0.0f),s2(0.0f);
|
||||
|
||||
PfxVector3 worldMin(-1000);//PFX_FLT_MAX);
|
||||
PfxVector3 worldMax(1000);//-PFX_FLT_MAX);
|
||||
|
||||
int numRigidBodies = 0;
|
||||
for (i=0; i <= m_LastHandleIndex; i++)
|
||||
{
|
||||
btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];
|
||||
if(!proxy0->m_clientObject)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
PfxVector3 pe_pos = getVmVector3(0.5f*(proxy0->m_aabbMax+proxy0->m_aabbMin));
|
||||
PfxVector3 pe_min = getVmVector3(proxy0->m_aabbMin);
|
||||
PfxVector3 pe_max = getVmVector3(proxy0->m_aabbMax);
|
||||
numRigidBodies++;
|
||||
//worldMin = minPerElem(worldMin,pe_min);
|
||||
//worldMax = maxPerElem(worldMax,pe_max);
|
||||
|
||||
s += pe_pos;
|
||||
s2 += mulPerElem(pe_pos,pe_pos);
|
||||
}
|
||||
|
||||
|
||||
if (numRigidBodies)
|
||||
{
|
||||
PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
|
||||
if(v[1] > v[0])
|
||||
axis = 1;
|
||||
if(v[2] > v[axis])
|
||||
axis = 2;
|
||||
}
|
||||
|
||||
PfxVector3 worldCenter = 0.5f*(worldMax+worldMin);
|
||||
PfxVector3 worldExtent = 0.5f*(worldMax-worldMin);
|
||||
|
||||
for (i=0; i <= m_LastHandleIndex; i++)
|
||||
{
|
||||
btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];
|
||||
if(!proxy0->m_clientObject)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
PfxBroadphaseProxy& proxy = proxies.expandNonInitializing();
|
||||
MyUpdateBroadphaseProxy(proxy,proxy0->m_uniqueId,proxy0,worldCenter,worldExtent,axis);
|
||||
|
||||
}
|
||||
|
||||
//find pairs, and call 'addOverlappingPair' for new pairs and 'removeOverlappingPair' for removed pairs
|
||||
broadphase(&proxies[0],proxies.size(),axis, dispatcher);
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
btOverlappingPairCache* btLowLevelBroadphase::getOverlappingPairCache()
|
||||
{
|
||||
return(m_paircache);
|
||||
}
|
||||
|
||||
//
|
||||
const btOverlappingPairCache* btLowLevelBroadphase::getOverlappingPairCache() const
|
||||
{
|
||||
return(m_paircache);
|
||||
}
|
||||
|
||||
|
||||
void btLowLevelBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
aabbMin = m_broadphaseAabbMin;
|
||||
aabbMax = m_broadphaseAabbMax;
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::printStats()
|
||||
{
|
||||
}
|
||||
|
||||
void btLowLevelBroadphase::setNumTasks(int numTasks)
|
||||
{
|
||||
}
|
||||
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
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
|
||||
*/
|
||||
|
||||
///btLowLevelBroadphase implementation
|
||||
#ifndef BT_LOW_LEVEL_BROADPHASE_H
|
||||
#define BT_LOW_LEVEL_BROADPHASE_H
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
|
||||
|
||||
#include "physics_effects/base_level/broadphase/pfx_broadphase_pair.h"
|
||||
#include "LinearMath/btHashMap.h"
|
||||
|
||||
struct btLowLevelBroadphase;
|
||||
struct btLowLevelData;
|
||||
|
||||
namespace sce
|
||||
{
|
||||
namespace PhysicsEffects
|
||||
{
|
||||
struct PfxSortData32;
|
||||
};
|
||||
};
|
||||
|
||||
struct btMyClientData
|
||||
{
|
||||
btDispatcher* m_dispatcher;
|
||||
btLowLevelBroadphase* m_bp;
|
||||
};
|
||||
|
||||
struct btLowLevelBroadphaseProxy : public btBroadphaseProxy
|
||||
{
|
||||
int m_nextFree;
|
||||
|
||||
// int m_handleId;
|
||||
|
||||
|
||||
btLowLevelBroadphaseProxy() {};
|
||||
|
||||
btLowLevelBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
|
||||
:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
|
||||
{
|
||||
(void)shapeType;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
|
||||
SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
///btLowLevelBroadphase is a binding between Open Physics low-level broadphase and Bullet, through the btBroadphaseInterface
|
||||
|
||||
struct btLowLevelBroadphase : btBroadphaseInterface
|
||||
{
|
||||
|
||||
int m_numHandles; // number of active handles
|
||||
int m_maxHandles; // max number of handles
|
||||
int m_LastHandleIndex;
|
||||
|
||||
btLowLevelBroadphaseProxy* m_pHandles; // handles pool
|
||||
|
||||
void* m_pHandlesRawPtr;
|
||||
int m_firstFreeHandle; // free handles list
|
||||
|
||||
btOverlappingPairCache* m_paircache; // Pair cache
|
||||
bool m_releasepaircache; // Release pair cache on delete
|
||||
int m_guidGenerator;
|
||||
btVector3 m_broadphaseAabbMin;
|
||||
btVector3 m_broadphaseAabbMax;
|
||||
|
||||
btMyClientData m_clientData;
|
||||
|
||||
btLowLevelData* m_lowLevelData;
|
||||
btHashMap<btHashInt,void*> m_uid2ptr;
|
||||
|
||||
|
||||
int allocHandle()
|
||||
{
|
||||
btAssert(m_numHandles < m_maxHandles);
|
||||
int freeHandle = m_firstFreeHandle;
|
||||
m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
|
||||
m_numHandles++;
|
||||
if(freeHandle > m_LastHandleIndex)
|
||||
{
|
||||
m_LastHandleIndex = freeHandle;
|
||||
}
|
||||
return freeHandle;
|
||||
}
|
||||
|
||||
void freeHandle(btLowLevelBroadphaseProxy* proxy)
|
||||
{
|
||||
int handle = int(proxy-m_pHandles);
|
||||
btAssert(handle >= 0 && handle < m_maxHandles);
|
||||
if(handle == m_LastHandleIndex)
|
||||
{
|
||||
m_LastHandleIndex--;
|
||||
}
|
||||
proxy->SetNextFree(m_firstFreeHandle);
|
||||
m_firstFreeHandle = handle;
|
||||
|
||||
proxy->m_clientObject = 0;
|
||||
|
||||
m_numHandles--;
|
||||
}
|
||||
|
||||
inline btLowLevelBroadphaseProxy* getLowLevelProxyFromProxy(btBroadphaseProxy* proxy)
|
||||
{
|
||||
btLowLevelBroadphaseProxy* proxy0 = static_cast<btLowLevelBroadphaseProxy*>(proxy);
|
||||
return proxy0;
|
||||
}
|
||||
|
||||
inline const btLowLevelBroadphaseProxy* getLowLevelProxyFromProxy(btBroadphaseProxy* proxy) const
|
||||
{
|
||||
const btLowLevelBroadphaseProxy* proxy0 = static_cast<const btLowLevelBroadphaseProxy*>(proxy);
|
||||
return proxy0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
|
||||
{
|
||||
btLowLevelBroadphaseProxy* p0 = getLowLevelProxyFromProxy(proxy0);
|
||||
btLowLevelBroadphaseProxy* p1 = getLowLevelProxyFromProxy(proxy1);
|
||||
return aabbOverlap(p0,p1);
|
||||
}
|
||||
|
||||
static bool aabbOverlap(btLowLevelBroadphaseProxy* proxy0,btLowLevelBroadphaseProxy* proxy1);
|
||||
|
||||
void broadphase(sce::PhysicsEffects::PfxSortData32* proxies, int numRigidBodies, int axis, btDispatcher* dispatcher);
|
||||
|
||||
sce::PhysicsEffects::PfxBroadphasePair* getCurrentPairs();
|
||||
const sce::PhysicsEffects::PfxBroadphasePair* getCurrentPairs() const;
|
||||
int getNumCurrentPairs() const;
|
||||
|
||||
|
||||
|
||||
/* Methods */
|
||||
btLowLevelBroadphase(btLowLevelData* lowLevelData, btOverlappingPairCache* paircache, int maxProxies = 16384);//,class PfxAllocator* allocator,class PfxStackAllocator* stackPool,void* spursInstance);
|
||||
virtual ~btLowLevelBroadphase();
|
||||
|
||||
/* btBroadphaseInterface Implementation */
|
||||
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
|
||||
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
|
||||
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
|
||||
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0))
|
||||
{
|
||||
}
|
||||
|
||||
virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
|
||||
void calculateOverlappingPairs(btDispatcher* dispatcher);
|
||||
btOverlappingPairCache* getOverlappingPairCache();
|
||||
const btOverlappingPairCache* getOverlappingPairCache() const;
|
||||
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
virtual void printStats();
|
||||
|
||||
virtual void setNumTasks(int numTasks);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
Binary file not shown.
@@ -0,0 +1,127 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
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.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef BT_LOW_LEVEL_COLLISION__DISPATCHER_H
|
||||
#define BT_LOW_LEVEL_COLLISION__DISPATCHER_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
struct btLowLevelBroadphase;//struct instead of class?
|
||||
struct btLowLevelData;
|
||||
///Tuning value to optimized SPU utilization
|
||||
///Too small value means Task overhead is large compared to computation (too fine granularity)
|
||||
///Too big value might render some SPUs are idle, while a few other SPUs are doing all work.
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 64
|
||||
#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 512
|
||||
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024
|
||||
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
|
||||
|
||||
class btLowLevelCollisionAlgorithm : public btCollisionAlgorithm
|
||||
{
|
||||
btPersistentManifold* m_manifold;
|
||||
|
||||
public:
|
||||
|
||||
btLowLevelCollisionAlgorithm()
|
||||
:m_manifold(0)
|
||||
{
|
||||
}
|
||||
|
||||
btLowLevelCollisionAlgorithm(btPersistentManifold* manifold, btCollisionDispatcher* dispatcher)
|
||||
:m_manifold(manifold)
|
||||
{
|
||||
m_dispatcher = dispatcher;
|
||||
}
|
||||
virtual ~btLowLevelCollisionAlgorithm()
|
||||
{
|
||||
if (m_manifold)
|
||||
m_dispatcher->releaseManifold(m_manifold);
|
||||
}
|
||||
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
btAssert(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
|
||||
{
|
||||
manifoldArray.push_back(m_manifold);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
///btLowLevelCollisionDispatcher can use SPU to gather and calculate collision detection
|
||||
///Time of Impact, Closest Points and Penetration Depth.
|
||||
class btLowLevelCollisionDispatcher : public btCollisionDispatcher
|
||||
{
|
||||
|
||||
btLowLevelData* m_lowLevelData;
|
||||
|
||||
btAlignedObjectArray<btPersistentManifold> m_manifoldArray;
|
||||
btAlignedObjectArray<btLowLevelCollisionAlgorithm> m_algorithms;
|
||||
|
||||
protected:
|
||||
|
||||
void collision();
|
||||
|
||||
public:
|
||||
|
||||
|
||||
btLowLevelCollisionDispatcher (btLowLevelData* lowLevelData, btCollisionConfiguration* collisionConfiguration, int maxNumManifolds);
|
||||
|
||||
virtual ~btLowLevelCollisionDispatcher();
|
||||
|
||||
bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
|
||||
|
||||
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
|
||||
|
||||
|
||||
|
||||
virtual btPersistentManifold* getNewManifold(void* b0,void* b1)
|
||||
{
|
||||
btAssert(0);
|
||||
return 0;
|
||||
}
|
||||
virtual void releaseManifold(btPersistentManifold* manifold);
|
||||
|
||||
|
||||
virtual void* allocateCollisionAlgorithm(int size)
|
||||
{
|
||||
btAssert(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void freeCollisionAlgorithm(void* ptr)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BT_LOW_LEVEL_COLLISION__DISPATCHER_H
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
Copyright (C) 2010 Sony Computer Entertainment Inc.
|
||||
All rights reserved.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
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.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef __BT_LOW_LEVEL_CONSTRAINT_SOLVER_H
|
||||
#define __BT_LOW_LEVEL_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||
#include "physics_effects/low_level/pfx_low_level_include.h"
|
||||
#include "../src/low_level/solver/pfx_parallel_group.h"
|
||||
|
||||
using namespace sce::PhysicsEffects;
|
||||
|
||||
|
||||
class btPersistentManifold;
|
||||
|
||||
enum {
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
|
||||
};
|
||||
|
||||
|
||||
struct PfxSetupContactConstraintsIO {
|
||||
PfxConstraintPair *offsetContactPairs;
|
||||
uint32_t numContactPairs1;
|
||||
class TrbState *offsetRigStates;
|
||||
struct PfxSolverBody *offsetSolverBodies;
|
||||
uint32_t numRigidBodies;
|
||||
float separateBias;
|
||||
float timeStep;
|
||||
class btCriticalSection* criticalSection;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct PfxSolveConstraintsIO {
|
||||
PfxParallelGroup *contactParallelGroup;
|
||||
PfxParallelBatch *contactParallelBatches;
|
||||
PfxConstraintPair *contactPairs;
|
||||
uint32_t numContactPairs;
|
||||
btPersistentManifold *offsetContactManifolds;
|
||||
PfxParallelGroup *jointParallelGroup;
|
||||
PfxParallelBatch *jointParallelBatches;
|
||||
PfxConstraintPair *jointPairs;
|
||||
uint32_t numJointPairs;
|
||||
TrbState *offsetRigStates;
|
||||
PfxSolverBody *offsetSolverBodies;
|
||||
uint32_t numRigidBodies;
|
||||
uint32_t iteration;
|
||||
|
||||
uint32_t taskId;
|
||||
|
||||
class btBarrier* barrier;
|
||||
|
||||
};
|
||||
|
||||
struct PfxPostSolverIO {
|
||||
TrbState *states;
|
||||
PfxSolverBody *solverBodies;
|
||||
uint32_t numRigidBodies;
|
||||
};
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) btConstraintSolverIO {
|
||||
uint8_t cmd;
|
||||
union {
|
||||
PfxSetupContactConstraintsIO setupContactConstraints;
|
||||
PfxSolveConstraintsIO solveConstraints;
|
||||
PfxPostSolverIO postSolver;
|
||||
};
|
||||
|
||||
//SPU only
|
||||
uint32_t barrierAddr2;
|
||||
uint32_t criticalsectionAddr2;
|
||||
uint32_t maxTasks1;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
void SolverThreadFunc(void* userPtr,void* lsMemory);
|
||||
void* SolverlsMemoryFunc();
|
||||
///The btLowLevelConstraintSolver performs computations on constraint rows in parallel
|
||||
///Using the cross-platform threading it supports Windows, Linux, Mac OSX and PlayStation 3 Cell SPUs
|
||||
class btLowLevelConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
struct btParallelSolverMemoryCache* m_memoryCache;
|
||||
|
||||
class btThreadSupportInterface* m_solverThreadSupport;
|
||||
|
||||
struct btConstraintSolverIO* m_solverIO;
|
||||
class btBarrier* m_barrier;
|
||||
class btCriticalSection* m_criticalSection;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btLowLevelConstraintSolver(class btThreadSupportInterface* solverThreadSupport);
|
||||
|
||||
virtual ~btLowLevelConstraintSolver();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //__BT_LOW_LEVEL_CONSTRAINT_SOLVER_H
|
||||
@@ -0,0 +1,190 @@
|
||||
/*
|
||||
Copyright (C) 2010 Sony Computer Entertainment Inc.
|
||||
All rights reserved.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
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.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "btLowLevelConstraintSolver2.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "BulletMultiThreaded/btThreadSupportInterface.h"
|
||||
#include "BulletMultiThreaded/HeapManager.h"
|
||||
#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||
#include "BulletPhysicsEffects/btLowLevelData.h"
|
||||
#include "BulletMultiThreaded/vectormath2bullet.h"
|
||||
//#include "PfxSimdUtils.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "BulletMultiThreaded/HeapManager.h"
|
||||
|
||||
|
||||
|
||||
/////////////////
|
||||
|
||||
|
||||
#define TMP_BUFF_BYTES (15*1024*1024)
|
||||
static unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
|
||||
|
||||
|
||||
|
||||
|
||||
btLowLevelConstraintSolver2::btLowLevelConstraintSolver2(btLowLevelData* lowLevelData)
|
||||
:m_lowLevelData(lowLevelData)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btLowLevelConstraintSolver2::~btLowLevelConstraintSolver2()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
static void solveConstraints(btLowLevelData* lowLevelData, btScalar timeStep, btScalar separateBias, int iteration)
|
||||
{
|
||||
PfxPerfCounter pc;
|
||||
HeapManager pool((unsigned char*)tmp_buff,TMP_BUFF_BYTES);
|
||||
|
||||
unsigned int numCurrentPairs = lowLevelData->m_numPairs[lowLevelData->m_pairSwap];
|
||||
PfxBroadphasePair *currentPairs = lowLevelData->m_pairsBuff[lowLevelData->m_pairSwap];
|
||||
|
||||
pc.countBegin("setup solver bodies");
|
||||
{
|
||||
PfxSetupSolverBodiesParam param;
|
||||
param.states = lowLevelData->m_states;
|
||||
param.bodies = lowLevelData->m_bodies;
|
||||
param.solverBodies = lowLevelData->m_solverBodies;
|
||||
param.numRigidBodies = lowLevelData->m_numRigidBodies;
|
||||
|
||||
int ret = pfxSetupSolverBodies(param);
|
||||
if(ret != SCE_PFX_OK)
|
||||
SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);
|
||||
}
|
||||
pc.countEnd();
|
||||
|
||||
pc.countBegin("setup contact constraints");
|
||||
{
|
||||
PfxSetupContactConstraintsParam param;
|
||||
param.contactPairs = currentPairs;
|
||||
param.numContactPairs = numCurrentPairs;
|
||||
param.offsetContactManifolds = lowLevelData->m_contacts;
|
||||
param.offsetRigidStates = lowLevelData->m_states;
|
||||
param.offsetRigidBodies = lowLevelData->m_bodies;
|
||||
param.offsetSolverBodies = lowLevelData->m_solverBodies;
|
||||
param.numRigidBodies = lowLevelData->m_numRigidBodies;
|
||||
param.timeStep = timeStep;
|
||||
param.separateBias = separateBias;
|
||||
|
||||
int ret = pfxSetupContactConstraints(param);
|
||||
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
|
||||
}
|
||||
pc.countEnd();
|
||||
#if 0
|
||||
pc.countBegin("setup joint constraints");
|
||||
{
|
||||
PfxSetupJointConstraintsParam param;
|
||||
param.jointPairs = 0;//jointPairs;
|
||||
param.numJointPairs = 0;//numJoints;
|
||||
param.offsetJoints = 0;//joints;
|
||||
param.offsetRigidStates = lowLevelData->m_states;
|
||||
param.offsetRigidBodies = lowLevelData->m_bodies;
|
||||
param.offsetSolverBodies = lowLevelData->m_solverBodies;
|
||||
param.numRigidBodies = lowLevelData->m_numRigidBodies;
|
||||
param.timeStep = timeStep;
|
||||
|
||||
for(int i=0;i<numJoints;i++) {
|
||||
pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]);
|
||||
}
|
||||
|
||||
int ret = pfxSetupJointConstraints(param);
|
||||
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
|
||||
}
|
||||
pc.countEnd();
|
||||
#endif
|
||||
|
||||
pc.countBegin("solve constraints");
|
||||
{
|
||||
PfxSolveConstraintsParam param;
|
||||
param.workBytes = pfxGetWorkBytesOfSolveConstraints(lowLevelData->m_numRigidBodies,numCurrentPairs,0);//numJoints);
|
||||
param.workBuff = pool.allocate(param.workBytes);
|
||||
param.contactPairs = currentPairs;
|
||||
param.numContactPairs = numCurrentPairs;
|
||||
param.offsetContactManifolds = lowLevelData->m_contacts;
|
||||
param.jointPairs = 0;//jointPairs;
|
||||
param.numJointPairs = 0;//numJoints;
|
||||
param.offsetJoints = 0;//joints;
|
||||
param.offsetRigidStates = lowLevelData->m_states;
|
||||
param.offsetSolverBodies = lowLevelData->m_solverBodies;
|
||||
param.numRigidBodies = lowLevelData->m_numRigidBodies;
|
||||
param.iteration = iteration;
|
||||
|
||||
int ret = pfxSolveConstraints(param);
|
||||
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);
|
||||
|
||||
pool.deallocate(param.workBuff);
|
||||
}
|
||||
pc.countEnd();
|
||||
|
||||
|
||||
//pc.printCount();
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btLowLevelConstraintSolver2::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
//copy velocity from BT to PE
|
||||
{
|
||||
BT_PROFILE("copy back");
|
||||
for (int i=0;i<numRigidBodies;i++)
|
||||
{
|
||||
btCollisionObject* obj = bodies1[i];
|
||||
btRigidBody* rb = btRigidBody::upcast(obj);
|
||||
if (rb && (rb->getInvMass()>0.f))
|
||||
{
|
||||
int objectIndex = rb->getBroadphaseProxy()->m_uniqueId;
|
||||
sce::PhysicsEffects::PfxRigidState& state = m_lowLevelData->m_states[objectIndex];
|
||||
|
||||
state.setLinearVelocity(getVmVector3(rb->getLinearVelocity()));
|
||||
state.setAngularVelocity(getVmVector3(rb->getAngularVelocity()));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btScalar separateBias = 0.1f;
|
||||
solveConstraints(m_lowLevelData, infoGlobal.m_timeStep, separateBias, infoGlobal.m_numIterations);
|
||||
|
||||
|
||||
//copy resulting velocity back from PE to BT
|
||||
{
|
||||
BT_PROFILE("copy back");
|
||||
for (int i=0;i<numRigidBodies;i++)
|
||||
{
|
||||
btCollisionObject* obj = bodies1[i];
|
||||
btRigidBody* rb = btRigidBody::upcast(obj);
|
||||
if (rb && (rb->getInvMass()>0.f))
|
||||
{
|
||||
int objectIndex = rb->getBroadphaseProxy()->m_uniqueId;
|
||||
sce::PhysicsEffects::PfxRigidState& state = m_lowLevelData->m_states[objectIndex];
|
||||
|
||||
rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));
|
||||
rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
Copyright (C) 2010 Sony Computer Entertainment Inc.
|
||||
All rights reserved.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
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.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef __BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H
|
||||
#define __BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||
#include "physics_effects/low_level/pfx_low_level_include.h"
|
||||
#include "../src/low_level/solver/pfx_parallel_group.h"
|
||||
|
||||
using namespace sce::PhysicsEffects;
|
||||
|
||||
|
||||
class btPersistentManifold;
|
||||
struct btLowLevelData;
|
||||
|
||||
class btLowLevelConstraintSolver2 : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btLowLevelData* m_lowLevelData;
|
||||
|
||||
public:
|
||||
|
||||
btLowLevelConstraintSolver2(btLowLevelData* lowLevelData);
|
||||
|
||||
virtual ~btLowLevelConstraintSolver2();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //__BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H
|
||||
Binary file not shown.
Reference in New Issue
Block a user