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,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)
|
||||
{
|
||||
}
|
||||
Reference in New Issue
Block a user