a93a661b94
The Android/NEON optimized version of Physics Effects is thanks to Graham Rhodes and Anthony Hamilton, See Issue 587
432 lines
14 KiB
C++
432 lines
14 KiB
C++
/*
|
|
Physics Effects Copyright(C) 2011 Sony Computer Entertainment Inc.
|
|
All rights reserved.
|
|
|
|
Physics Effects is open software; you can redistribute it and/or
|
|
modify it under the terms of the BSD License.
|
|
|
|
Physics Effects is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
See the BSD License for more details.
|
|
|
|
A copy of the BSD License is distributed with
|
|
Physics Effects under the filename: physics_effects_license.txt
|
|
*/
|
|
|
|
#include "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)
|
|
{
|
|
}
|