Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add PhysicsEffects to Extras. The build is only tested on Windows and Android.

Browse Source 
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
2012-03-05 04:59:58 +00:00
parent 6cf8dfc202
commit a93a661b94
462 changed files with 86626 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
Extras/PhysicsEffects/src/low_level/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
INCLUDE_DIRECTORIES( ${PHYSICS_EFFECTS_SOURCE_DIR}/include )
SET(PfxLowLevel_SRCS
broadphase/pfx_broadphase_single.cpp
collision/pfx_batched_ray_cast_single.cpp
collision/pfx_collision_detection_single.cpp
collision/pfx_detect_collision_func.cpp
collision/pfx_intersect_ray_func.cpp
collision/pfx_island_generation.cpp
collision/pfx_ray_cast.cpp
collision/pfx_refresh_contacts_single.cpp
solver/pfx_constraint_solver_single.cpp
solver/pfx_joint_constraint_func.cpp
solver/pfx_update_rigid_states_single.cpp
sort/pfx_parallel_sort_single.cpp
)
SET(PfxLowLevel_HDRS
collision/pfx_detect_collision_func.h
collision/pfx_intersect_ray_func.h
)
ADD_LIBRARY(PfxLowLevel ${PfxLowLevel_SRCS} ${PfxLowLevel_HDRS})
SET_TARGET_PROPERTIES(PfxLowLevel PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(PfxLowLevel PROPERTIES SOVERSION ${BULLET_VERSION})

294
Extras/PhysicsEffects/src/low_level/broadphase/pfx_broadphase_single.cpp Normal file
View File

@@ -0,0 +1,294 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
#include "../../../include/physics_effects/base_level/broadphase/pfx_update_broadphase_proxy.h"
#include "../../../include/physics_effects/low_level/broadphase/pfx_broadphase.h"
#include "../../base_level/broadphase/pfx_check_collidable.h"
namespace sce {
namespace PhysicsEffects {
PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies)
{
return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphaseProxy) * numRigidBodies);
}
PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies,PfxUInt32 maxTasks)
{
(void)maxTasks;
return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphaseProxy) * numRigidBodies) * 6;
}
PfxUInt32 pfxGetWorkBytesOfFindPairs(PfxUInt32 maxPairs,PfxUInt32 maxTasks)
{
return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair) * maxPairs) * maxTasks;
}
PfxUInt32 pfxGetPairBytesOfFindPairs(PfxUInt32 maxPairs)
{
return SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair) * maxPairs);
}
PfxUInt32 pfxGetWorkBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs,int maxTasks)
{
return 128 +
SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numPreviousPairs) +
SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numPreviousPairs) * maxTasks +
SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numCurrentPairs) * maxTasks;
}
PfxInt32 pfxCheckParamOfUpdateBroadphaseProxies(const PfxUpdateBroadphaseProxiesParam &param)
{
if(!param.workBuff ||
!param.proxiesX || !param.proxiesY || !param.proxiesZ ||
!param.proxiesXb || !param.proxiesYb || !param.proxiesZb ||
!param.offsetRigidStates || !param.offsetCollidables ) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb) ) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfUpdateBroadphaseProxies(param.numRigidBodies) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
PfxInt32 pfxCheckParamOfFindPairs(const PfxFindPairsParam &param,int maxTasks)
{
if(!param.workBuff || !param.pairBuff || !param.proxies || param.axis > 2 || param.axis < 0) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.proxies) || !SCE_PFX_PTR_IS_ALIGNED16(param.pairBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfFindPairs(param.maxPairs,maxTasks) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.pairBuff,param.pairBytes) < pfxGetPairBytesOfFindPairs(param.maxPairs) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
PfxUInt32 pfxGetPairBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs)
{
return sizeof(PfxBroadphasePair)*(numPreviousPairs*2+numCurrentPairs);
}
PfxInt32 pfxCheckParamOfDecomposePairs(const PfxDecomposePairsParam &param,int maxTasks)
{
if(!param.workBuff || !param.pairBuff || !param.previousPairs || !param.currentPairs) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.previousPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.currentPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.pairBuff) ) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfDecomposePairs(param.numPreviousPairs,param.numCurrentPairs,maxTasks)) return SCE_PFX_ERR_OUT_OF_BUFFER;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.pairBuff,param.pairBytes) < pfxGetPairBytesOfDecomposePairs(param.numPreviousPairs,param.numCurrentPairs)) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
PfxInt32 pfxUpdateBroadphaseProxies(PfxUpdateBroadphaseProxiesParam &param,PfxUpdateBroadphaseProxiesResult &result)
{
PfxInt32 ret = pfxCheckParamOfUpdateBroadphaseProxies(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxUpdateBroadphaseProxies")
result.numOutOfWorldProxies = 0;
for(int i=0;i<(int)param.numRigidBodies;i++) {
PfxInt32 chk = pfxUpdateBroadphaseProxy(
param.proxiesX[i],
param.proxiesY[i],
param.proxiesZ[i],
param.proxiesXb[i],
param.proxiesYb[i],
param.proxiesZb[i],
param.offsetRigidStates[i],
param.offsetCollidables[i],
param.worldCenter,
param.worldExtent);
if(chk == SCE_PFX_ERR_OUT_OF_WORLD) {
result.numOutOfWorldProxies++;
if(param.outOfWorldBehavior & SCE_PFX_OUT_OF_WORLD_BEHAVIOR_FIX_MOTION) {
PfxRigidState &state = param.offsetRigidStates[i];
state.setMotionType(kPfxMotionTypeFixed);
pfxSetMotionMask(param.proxiesX[i],state.getMotionMask());
pfxSetMotionMask(param.proxiesY[i],state.getMotionMask());
pfxSetMotionMask(param.proxiesZ[i],state.getMotionMask());
pfxSetMotionMask(param.proxiesXb[i],state.getMotionMask());
pfxSetMotionMask(param.proxiesYb[i],state.getMotionMask());
pfxSetMotionMask(param.proxiesZb[i],state.getMotionMask());
}
if(param.outOfWorldBehavior & SCE_PFX_OUT_OF_WORLD_BEHAVIOR_REMOVE_PROXY) {
pfxSetKey(param.proxiesX[i],SCE_PFX_SENTINEL_KEY);
pfxSetKey(param.proxiesY[i],SCE_PFX_SENTINEL_KEY);
pfxSetKey(param.proxiesZ[i],SCE_PFX_SENTINEL_KEY);
pfxSetKey(param.proxiesXb[i],SCE_PFX_SENTINEL_KEY);
pfxSetKey(param.proxiesYb[i],SCE_PFX_SENTINEL_KEY);
pfxSetKey(param.proxiesZb[i],SCE_PFX_SENTINEL_KEY);
}
}
}
PfxHeapManager pool((unsigned char*)param.workBuff,param.workBytes);
PfxBroadphaseProxy *workProxies = (PfxBroadphaseProxy*)pool.allocate(sizeof(PfxBroadphaseProxy)*param.numRigidBodies,PfxHeapManager::ALIGN128);
pfxSort(param.proxiesX,workProxies,param.numRigidBodies);
pfxSort(param.proxiesY,workProxies,param.numRigidBodies);
pfxSort(param.proxiesZ,workProxies,param.numRigidBodies);
pfxSort(param.proxiesXb,workProxies,param.numRigidBodies);
pfxSort(param.proxiesYb,workProxies,param.numRigidBodies);
pfxSort(param.proxiesZb,workProxies,param.numRigidBodies);
pool.deallocate(workProxies);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxFindPairs(PfxFindPairsParam &param,PfxFindPairsResult &result)
{
PfxInt32 ret = pfxCheckParamOfFindPairs(param,0);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxFindPairs")
void *workBuff = param.workBuff;
PfxUInt32 workBytes = param.workBytes;
PfxBroadphaseProxy *proxies = param.proxies;
PfxUInt32 numProxies = param.numProxies;
PfxUInt32 maxPairs = param.maxPairs;
int axis = param.axis;
(void) workBytes;
PfxBroadphasePair *pairs = (PfxBroadphasePair*)SCE_PFX_PTR_ALIGN16(param.pairBuff);
PfxUInt32 numPairs = 0;
for(PfxUInt32 i=0;i<numProxies;i++) {
for(PfxUInt32 j=i+1;j<numProxies;j++) {
PfxBroadphaseProxy proxyA,proxyB;
if(pfxGetObjectId(proxies[i]) < pfxGetObjectId(proxies[j])) {
proxyA = proxies[i];
proxyB = proxies[j];
}
else {
proxyA = proxies[j];
proxyB = proxies[i];
}
if(pfxGetXYZMax(proxyA,axis) < pfxGetXYZMin(proxyB,axis)) {
break;
}
if( pfxCheckCollidableInBroadphase(proxyA,proxyB) ) {
if(numPairs >= maxPairs)
return SCE_PFX_ERR_OUT_OF_MAX_PAIRS;
PfxBroadphasePair &pair = pairs[numPairs++];
pfxSetActive(pair,true);
pfxSetObjectIdA(pair,pfxGetObjectId(proxyA));
pfxSetObjectIdB(pair,pfxGetObjectId(proxyB));
pfxSetMotionMaskA(pair,pfxGetMotionMask(proxyA));
pfxSetMotionMaskB(pair,pfxGetMotionMask(proxyB));
pfxSetKey(pair,pfxCreateUniqueKey(pfxGetObjectId(proxyA),pfxGetObjectId(proxyB)));
}
}
}
pfxSort(pairs,(PfxBroadphasePair*)workBuff,numPairs);
result.pairs = pairs;
result.numPairs = numPairs;
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxDecomposePairs(PfxDecomposePairsParam &param,PfxDecomposePairsResult &result)
{
PfxInt32 ret = pfxCheckParamOfDecomposePairs(param,0);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxDecomposePairs")
PfxBroadphasePair *previousPairs = param.previousPairs;
PfxUInt32 numPreviousPairs = param.numPreviousPairs;
PfxBroadphasePair *currentPairs = param.currentPairs;
PfxUInt32 numCurrentPairs = param.numCurrentPairs;
PfxBroadphasePair *outNewPairs = (PfxBroadphasePair*)SCE_PFX_PTR_ALIGN16(param.pairBuff);
PfxBroadphasePair *outKeepPairs = outNewPairs + numCurrentPairs;
PfxBroadphasePair *outRemovePairs = outKeepPairs + numPreviousPairs;
PfxUInt32 nNew = 0;
PfxUInt32 nKeep = 0;
PfxUInt32 nRemove = 0;
PfxUInt32 oldId = 0,newId = 0;
while(oldId<numPreviousPairs&&newId<numCurrentPairs) {
if(pfxGetKey(currentPairs[newId]) > pfxGetKey(previousPairs[oldId])) {
// remove
SCE_PFX_ASSERT(nRemove<=numPreviousPairs);
outRemovePairs[nRemove] = previousPairs[oldId];
nRemove++;
oldId++;
}
else if(pfxGetKey(currentPairs[newId]) == pfxGetKey(previousPairs[oldId])) {
// keep
SCE_PFX_ASSERT(nKeep<=numPreviousPairs);
outKeepPairs[nKeep] = currentPairs[newId];
pfxSetContactId(outKeepPairs[nKeep],pfxGetContactId(previousPairs[oldId]));
nKeep++;
oldId++;
newId++;
}
else {
// new
SCE_PFX_ASSERT(nNew<=numCurrentPairs);
outNewPairs[nNew] = currentPairs[newId];
nNew++;
newId++;
}
};
if(newId<numCurrentPairs) {
// all new
for(;newId<numCurrentPairs;newId++,nNew++) {
SCE_PFX_ASSERT(nNew<=numCurrentPairs);
outNewPairs[nNew] = currentPairs[newId];
}
}
else if(oldId<numPreviousPairs) {
// all remove
for(;oldId<numPreviousPairs;oldId++,nRemove++) {
SCE_PFX_ASSERT(nRemove<=numPreviousPairs);
outRemovePairs[nRemove] = previousPairs[oldId];
}
}
result.outNewPairs = outNewPairs;
result.outKeepPairs = outKeepPairs;
result.outRemovePairs = outRemovePairs;
result.numOutNewPairs = nNew;
result.numOutKeepPairs = nKeep;
result.numOutRemovePairs = nRemove;
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

95
Extras/PhysicsEffects/src/low_level/collision/pfx_batched_ray_cast_parallel.cpp Normal file
View File

@@ -0,0 +1,95 @@
/*
Applied Research Associates Inc. (c)2011
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/low_level/collision/pfx_batched_ray_cast.h"
#include "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// MULTIPLE THREADS
//----------------------------------------------------------------------------
// pfxCastRaysStartTaskEntry
//
/// The thread PfxTaskEntry function used to cast rays in parallel.
//----------------------------------------------------------------------------
void pfxCastRaysStartTaskEntry(PfxTaskArg *arg)
{
PfxRayCastParam &param = *((PfxRayCastParam*)arg->io);
PfxRayInput *rayInputs = (PfxRayInput*)arg->data[0];
PfxRayOutput *rayOutputs = (PfxRayOutput*)arg->data[1];
PfxUInt32 iFirstRay = arg->data[2];
PfxUInt32 iEndRay = arg->data[3];
for(PfxUInt32 i = iFirstRay; i < iEndRay; i++)
{
pfxCastSingleRay(rayInputs[i], rayOutputs[i], param);
}
}
//----------------------------------------------------------------------------
// pfxCastRays
//
/// Perform cast rays in parallel using a task manager.
///
/// @param rayInputs [in] Array of rays to cast
/// @param rayOutputs [out] On return contains output of ray casts
/// @param param Information about ray cast
/// @param taskManager Pointer to the thread task manager to use
//----------------------------------------------------------------------------
void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,
PfxRayCastParam &param,PfxTaskManager *taskManager)
{
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
SCE_PFX_PUSH_MARKER("pfxCastRays");
PfxUInt32 maxBatchSize = numRays / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxCastRaysStartTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), (PfxUInt32)rayInputs, (PfxUInt32)rayOutputs, iStart, iEnd);
}
// send final task
iEnd = numRays;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
}
} //namespace PhysicsEffects
} //namespace sce

47
Extras/PhysicsEffects/src/low_level/collision/pfx_batched_ray_cast_single.cpp Normal file
View File

@@ -0,0 +1,47 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/low_level/collision/pfx_batched_ray_cast.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
void pfxCastRaysStart(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param)
{
for(int i=0;i<numRays;i++) {
pfxCastSingleRay(rayInputs[i],rayOutputs[i],param);
}
}
void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param)
{
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
pfxCastRaysStart(rayInputs,rayOutputs,numRays,param);
}
} //namespace PhysicsEffects
} //namespace sce

166
Extras/PhysicsEffects/src/low_level/collision/pfx_collision_detection_parallel.cpp Normal file
View File

@@ -0,0 +1,166 @@
/*
Applied Research Associates Inc. (c)2011
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
#include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"
#include "../../base_level/broadphase/pfx_check_collidable.h"
#include "../../base_level/collision/pfx_contact_cache.h"
#include "pfx_detect_collision_func.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// This function is implemented in pfx_collision_detection_single.cpp
extern int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param);
///////////////////////////////////////////////////////////////////////////////
// MULTIPLE THREADS
#define SCE_PFX_CONTACT_THRESHOLD 0.0f
//----------------------------------------------------------------------------
// pfxDetectCollisionTaskEntry
//
/// The thread PfxTaskEntry function used to perform narrow phase collision
/// detection in parallel.
//----------------------------------------------------------------------------
void pfxDetectCollisionTaskEntry(PfxTaskArg *arg)
{
PfxDetectCollisionParam &param = *((PfxDetectCollisionParam*)arg->io);
PfxUInt32 iFirstContactPair = arg->data[0];
PfxUInt32 iEndContactPair = arg->data[1];
PfxConstraintPair *contactPairs = param.contactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxCollidable *offsetCollidables = param.offsetCollidables;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
{
const PfxBroadphasePair &pair = contactPairs[i];
if(!pfxCheckCollidableInCollision(pair))
continue;
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxCollidable &collA = offsetCollidables[iA];
PfxCollidable &collB = offsetCollidables[iB];
PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
PfxContactCache contactCache;
PfxShapeIterator itrShapeA(collA);
for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA)
{
const PfxShape &shapeA = *itrShapeA;
PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
PfxTransform3 worldTrA = tA0 * offsetTrA;
PfxShapeIterator itrShapeB(collB);
for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB)
{
const PfxShape &shapeB = *itrShapeB;
PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
PfxTransform3 worldTrB = tB0 * offsetTrB;
if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) &&
(shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) )
{
pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
contactCache,
shapeA,offsetTrA,worldTrA,j,
shapeB,offsetTrB,worldTrB,k,
SCE_PFX_CONTACT_THRESHOLD);
}
}
}
for(int j=0;j<contactCache.getNumContacts();j++)
{
const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
contact.addContactPoint(
cp.m_distance,
cp.m_normal,
cp.m_localPointA,
cp.m_localPointB,
cp.m_subData
);
}
}
}
//----------------------------------------------------------------------------
// pfxDetectCollision
//
/// Perform narrow phase collision detection in parallel using a task
/// manager.
///
/// @param param Information about pairs that may be colliding
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param, PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
if(ret != SCE_PFX_OK)
return ret;
SCE_PFX_PUSH_MARKER("pfxDetectCollision");
PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxDetectCollisionTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numContactPairs;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

125
Extras/PhysicsEffects/src/low_level/collision/pfx_collision_detection_single.cpp Normal file
View File

@@ -0,0 +1,125 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
#include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"
#include "../../base_level/broadphase/pfx_check_collidable.h"
#include "../../base_level/collision/pfx_contact_cache.h"
#include "pfx_detect_collision_func.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param)
{
if(!param.contactPairs || !param.offsetContactManifolds || !param.offsetRigidStates|| !param.offsetCollidables ) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
#define SCE_PFX_CONTACT_THRESHOLD 0.0f
PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param)
{
PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
if(ret != SCE_PFX_OK)
return ret;
SCE_PFX_PUSH_MARKER("pfxDetectCollision");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxCollidable *offsetCollidables = param.offsetCollidables;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++) {
const PfxBroadphasePair &pair = contactPairs[i];
if(!pfxCheckCollidableInCollision(pair)) {
continue;
}
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxCollidable &collA = offsetCollidables[iA];
PfxCollidable &collB = offsetCollidables[iB];
PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
PfxContactCache contactCache;
PfxShapeIterator itrShapeA(collA);
for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA) {
const PfxShape &shapeA = *itrShapeA;
PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
PfxTransform3 worldTrA = tA0 * offsetTrA;
PfxShapeIterator itrShapeB(collB);
for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB) {
const PfxShape &shapeB = *itrShapeB;
PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
PfxTransform3 worldTrB = tB0 * offsetTrB;
if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) &&
(shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) ) {
pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
contactCache,
shapeA,offsetTrA,worldTrA,j,
shapeB,offsetTrB,worldTrB,k,
SCE_PFX_CONTACT_THRESHOLD);
}
}
}
for(int j=0;j<contactCache.getNumContacts();j++) {
const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
contact.addContactPoint(
cp.m_distance,
cp.m_normal,
cp.m_localPointA,
cp.m_localPointB,
cp.m_subData
);
}
}
SCE_PFX_POP_MARKER();
(void) numRigidBodies;
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

449
Extras/PhysicsEffects/src/low_level/collision/pfx_detect_collision_func.cpp Normal file
View File

@@ -0,0 +1,449 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/collision/pfx_shape.h"
#include "../../base_level/collision/pfx_contact_box_box.h"
#include "../../base_level/collision/pfx_contact_box_capsule.h"
#include "../../base_level/collision/pfx_contact_box_sphere.h"
#include "../../base_level/collision/pfx_contact_capsule_capsule.h"
#include "../../base_level/collision/pfx_contact_capsule_sphere.h"
#include "../../base_level/collision/pfx_contact_sphere_sphere.h"
#include "../../base_level/collision/pfx_gjk_solver.h"
#include "../../base_level/collision/pfx_contact_large_tri_mesh.h"
#include "../../base_level/collision/pfx_gjk_support_func.h"
#include "pfx_detect_collision_func.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// Collision Detection Function
void detectCollisionDummy(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)contacts;
(void)shapeA,(void)offsetTransformA,(void)worldTransformA,(void)shapeIdA;
(void)shapeB,(void)offsetTransformB,(void)worldTransformB,(void)shapeIdB;
(void)contactThreshold;
}
void detectCollisionBoxBox(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxBox boxA = shapeA.getBox();
PfxBox boxB = shapeB.getBox();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactBoxBox(nml,pA,pB,&boxA,worldTransformA,&boxB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionBoxCapsule(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxBox boxA = shapeA.getBox();
PfxCapsule capsuleB = shapeB.getCapsule();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactBoxCapsule(nml,pA,pB,&boxA,worldTransformA,&capsuleB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionBoxSphere(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxBox boxA = shapeA.getBox();
PfxSphere sphereB = shapeB.getSphere();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactBoxSphere(nml,pA,pB,&boxA,worldTransformA,&sphereB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionCapsuleBox(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxCapsule capsuleA = shapeA.getCapsule();
PfxBox boxB = shapeB.getBox();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactBoxCapsule(nml,pB,pA,&boxB,worldTransformB,&capsuleA,worldTransformA);
if(d < contactThreshold) {
contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionCapsuleCapsule(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxCapsule capsuleA = shapeA.getCapsule();
PfxCapsule capsuleB = shapeB.getCapsule();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactCapsuleCapsule(nml,pA,pB,&capsuleA,worldTransformA,&capsuleB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionCapsuleSphere(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxCapsule capsuleA = shapeA.getCapsule();
PfxSphere sphereB = shapeB.getSphere();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactCapsuleSphere(nml,pA,pB,&capsuleA,worldTransformA,&sphereB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionSphereBox(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxSphere sphereA = shapeA.getSphere();
PfxBox boxB = shapeB.getBox();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactBoxSphere(nml,pB,pA,&boxB,worldTransformB,&sphereA,worldTransformA);
if(d < contactThreshold) {
contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionSphereCapsule(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxSphere sphereA = shapeA.getSphere();
PfxCapsule capsuleB = shapeB.getCapsule();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactCapsuleSphere(nml,pB,pA,&capsuleB,worldTransformB,&sphereA,worldTransformA);
if(d < contactThreshold) {
contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionSphereSphere(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxSphere sphereA = shapeA.getSphere();
PfxSphere sphereB = shapeB.getSphere();
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxFloat d = pfxContactSphereSphere(nml,pA,pB,&sphereA,worldTransformA,&sphereB,worldTransformB);
if(d < contactThreshold) {
contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionGjk(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
PfxFloat d = SCE_PFX_FLT_MAX;
PfxVector3 nml;
PfxPoint3 pA,pB;
PfxGjkSolver gjk;
if(shapeA.getType() == kPfxShapeCylinder) {
PfxCylinder cylinderA = shapeA.getCylinder();
if(shapeB.getType() == kPfxShapeSphere) {
PfxSphere sphereB = shapeB.getSphere();
gjk.setup((void*)&cylinderA,(void*)&sphereB,pfxGetSupportVertexCylinder,pfxGetSupportVertexSphere);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeBox) {
PfxBox boxB = shapeB.getBox();
gjk.setup((void*)&cylinderA,(void*)&boxB,pfxGetSupportVertexCylinder,pfxGetSupportVertexBox);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeCapsule) {
PfxCapsule capsuleB = shapeB.getCapsule();
gjk.setup((void*)&cylinderA,(void*)&capsuleB,pfxGetSupportVertexCylinder,pfxGetSupportVertexCapsule);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeCylinder) {
PfxCylinder cylinderB = shapeB.getCylinder();
gjk.setup((void*)&cylinderA,(void*)&cylinderB,pfxGetSupportVertexCylinder,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexB = shapeB.getConvexMesh();
gjk.setup((void*)&cylinderA,(void*)convexB,pfxGetSupportVertexCylinder,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
}
else if(shapeB.getType() == kPfxShapeCylinder) {
PfxCylinder cylinderB = shapeB.getCylinder();
if(shapeA.getType() == kPfxShapeSphere) {
PfxSphere sphereA = shapeA.getSphere();
gjk.setup((void*)&sphereA,(void*)&cylinderB,pfxGetSupportVertexSphere,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeBox) {
PfxBox boxA = shapeA.getBox();
gjk.setup((void*)&boxA,(void*)&cylinderB,pfxGetSupportVertexBox,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeCapsule) {
PfxCapsule capsuleA = shapeA.getCapsule();
gjk.setup((void*)&capsuleA,(void*)&cylinderB,pfxGetSupportVertexCapsule,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexA = shapeA.getConvexMesh();
gjk.setup((void*)convexA,(void*)&cylinderB,pfxGetSupportVertexConvex,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
}
else if(shapeA.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexA = shapeA.getConvexMesh();
if(shapeB.getType() == kPfxShapeSphere) {
PfxSphere sphereB = shapeB.getSphere();
gjk.setup((void*)convexA,(void*)&sphereB,pfxGetSupportVertexConvex,pfxGetSupportVertexSphere);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeBox) {
PfxBox boxB = shapeB.getBox();
gjk.setup((void*)convexA,(void*)&boxB,pfxGetSupportVertexConvex,pfxGetSupportVertexBox);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeCapsule) {
PfxCapsule capsuleB = shapeB.getCapsule();
gjk.setup((void*)convexA,(void*)&capsuleB,pfxGetSupportVertexConvex,pfxGetSupportVertexCapsule);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeCylinder) {
PfxCylinder cylinderB = shapeB.getCylinder();
gjk.setup((void*)convexA,(void*)&cylinderB,pfxGetSupportVertexConvex,pfxGetSupportVertexCylinder);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeB.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexB = shapeB.getConvexMesh();
gjk.setup((void*)convexA,(void*)convexB,pfxGetSupportVertexConvex,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
}
else if(shapeB.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexB = shapeB.getConvexMesh();
if(shapeA.getType() == kPfxShapeSphere) {
PfxSphere sphereA = shapeA.getSphere();
gjk.setup((void*)&sphereA,(void*)convexB,pfxGetSupportVertexSphere,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeBox) {
PfxBox boxA = shapeA.getBox();
gjk.setup((void*)&boxA,(void*)convexB,pfxGetSupportVertexBox,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeCapsule) {
PfxCapsule capsuleA = shapeA.getCapsule();
gjk.setup((void*)&capsuleA,(void*)convexB,pfxGetSupportVertexCapsule,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
else if(shapeA.getType() == kPfxShapeConvexMesh) {
const PfxConvexMesh *convexA = shapeA.getConvexMesh();
gjk.setup((void*)convexA,(void*)convexB,pfxGetSupportVertexConvex,pfxGetSupportVertexConvex);
d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
}
}
if(d < contactThreshold) {
contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
}
}
void detectCollisionLargeTriMesh(
PfxContactCache &contacts,
const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold)
{
(void)shapeIdA,(void)shapeIdB;
if(shapeA.getType() == kPfxShapeLargeTriMesh) {
const PfxLargeTriMesh *lmeshA = shapeA.getLargeTriMesh();
PfxContactCache localContacts;
pfxContactLargeTriMesh(localContacts,
lmeshA,worldTransformA,
shapeB,worldTransformB,
contactThreshold);
for(int i=0;i<localContacts.getNumContacts();i++) {
contacts.addContactPoint(
localContacts.getDistance(i),
localContacts.getNormal(i),
offsetTransformA * localContacts.getLocalPointA(i),
offsetTransformB * localContacts.getLocalPointB(i),
localContacts.getSubData(i));
}
}
else if(shapeB.getType() == kPfxShapeLargeTriMesh) {
const PfxLargeTriMesh *lmeshB = shapeB.getLargeTriMesh();
PfxContactCache localContacts;
pfxContactLargeTriMesh(localContacts,
lmeshB,worldTransformB,
shapeA,worldTransformA,
contactThreshold);
for(int i=0;i<localContacts.getNumContacts();i++) {
contacts.addContactPoint(
localContacts.getDistance(i),
-localContacts.getNormal(i),
offsetTransformA * localContacts.getLocalPointB(i),
offsetTransformB * localContacts.getLocalPointA(i),
localContacts.getSubData(i));
}
}
}
///////////////////////////////////////////////////////////////////////////////
// Collision Detection Function Table
pfx_detect_collision_func funcTbl_detectCollision[kPfxShapeCount][kPfxShapeCount] = {
{detectCollisionSphereSphere ,detectCollisionSphereBox ,detectCollisionSphereCapsule ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionLargeTriMesh, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionBoxSphere ,detectCollisionBoxBox ,detectCollisionBoxCapsule ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionLargeTriMesh, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionCapsuleSphere ,detectCollisionCapsuleBox ,detectCollisionCapsuleCapsule ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionLargeTriMesh, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionLargeTriMesh, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionGjk ,detectCollisionLargeTriMesh, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionLargeTriMesh ,detectCollisionLargeTriMesh,detectCollisionLargeTriMesh ,detectCollisionLargeTriMesh,detectCollisionLargeTriMesh ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
{detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy ,detectCollisionDummy, detectCollisionDummy},
};
///////////////////////////////////////////////////////////////////////////////
// Collision Detection Function Table Interface
pfx_detect_collision_func pfxGetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB)
{
SCE_PFX_ASSERT(shapeTypeA<kPfxShapeCount);
SCE_PFX_ASSERT(shapeTypeB<kPfxShapeCount);
return funcTbl_detectCollision[shapeTypeA][shapeTypeB];
}
int pfxSetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB,pfx_detect_collision_func func)
{
if(shapeTypeA >= kPfxShapeCount || shapeTypeB >= kPfxShapeCount) {
return SCE_PFX_ERR_OUT_OF_RANGE;
}
funcTbl_detectCollision[shapeTypeA][shapeTypeB] = func;
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

37
Extras/PhysicsEffects/src/low_level/collision/pfx_detect_collision_func.h Normal file
View File

@@ -0,0 +1,37 @@
/*
Physics Effects Copyright(C) 2010 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 _SCE_PFX_DETECT_COLLISION_FUNC_H
#define _SCE_PFX_DETECT_COLLISION_FUNC_H
#include "../../base_level/collision/pfx_contact_cache.h"
namespace sce {
namespace PhysicsEffects {
typedef void (*pfx_detect_collision_func)(
PfxContactCache &contacts,
const PfxShape & shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
const PfxShape & shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
float contactThreshold);
pfx_detect_collision_func pfxGetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB);
int pfxSetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB,pfx_detect_collision_func func);
} //namespace PhysicsEffects
} //namespace sce
#endif // _SCE_PFX_DETECT_COLLISION_FUNC_H

129
Extras/PhysicsEffects/src/low_level/collision/pfx_intersect_ray_func.cpp Normal file
View File

@@ -0,0 +1,129 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/collision/pfx_shape.h"
#include "../../base_level/collision/pfx_intersect_ray_box.h"
#include "../../base_level/collision/pfx_intersect_ray_sphere.h"
#include "../../base_level/collision/pfx_intersect_ray_capsule.h"
#include "../../base_level/collision/pfx_intersect_ray_cylinder.h"
#include "../../base_level/collision/pfx_intersect_ray_convex.h"
#include "../../base_level/collision/pfx_intersect_ray_large_tri_mesh.h"
#include "pfx_intersect_ray_func.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// Ray Intersection Function Table
PfxBool intersectRayFuncDummy(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
(void)ray,(void)out,(void)shape,(void)transform;
return false;
}
PfxBool intersectRayFuncBox(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
return pfxIntersectRayBox(ray,out,shape.getBox(),transform);
}
PfxBool intersectRayFuncSphere(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
return pfxIntersectRaySphere(ray,out,shape.getSphere(),transform);
}
PfxBool intersectRayFuncCapsule(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
return pfxIntersectRayCapsule(ray,out,shape.getCapsule(),transform);
}
PfxBool intersectRayFuncCylinder(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
return pfxIntersectRayCylinder(ray,out,shape.getCylinder(),transform);
}
PfxBool intersectRayFuncConvex(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
const PfxConvexMesh *convex = shape.getConvexMesh();
PfxBool ret = pfxIntersectRayConvex(ray,out,(const void*)convex,transform);
return ret;
}
PfxBool intersectRayFuncLargeTriMesh(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform)
{
const PfxLargeTriMesh *lmesh = shape.getLargeTriMesh();
PfxBool ret = pfxIntersectRayLargeTriMesh(ray,out,(const void*)lmesh,transform);
return ret;
}
PfxIntersectRayFunc funcTbl_intersectRay[kPfxShapeCount] = {
intersectRayFuncSphere,
intersectRayFuncBox,
intersectRayFuncCapsule,
intersectRayFuncCylinder,
intersectRayFuncConvex,
intersectRayFuncLargeTriMesh,
intersectRayFuncDummy,
intersectRayFuncDummy,
intersectRayFuncDummy,
intersectRayFuncDummy,
intersectRayFuncDummy,
intersectRayFuncDummy,
};
///////////////////////////////////////////////////////////////////////////////
// Ray Intersection Function Table Interface
PfxIntersectRayFunc pfxGetIntersectRayFunc(PfxUInt8 shapeType)
{
return funcTbl_intersectRay[shapeType];
}
PfxInt32 pfxSetIntersectRayFunc(PfxUInt8 shapeType,PfxIntersectRayFunc func)
{
if(shapeType >= kPfxShapeCount) {
return SCE_PFX_ERR_OUT_OF_RANGE;
}
funcTbl_intersectRay[shapeType] = func;
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

36
Extras/PhysicsEffects/src/low_level/collision/pfx_intersect_ray_func.h Normal file
View File

@@ -0,0 +1,36 @@
/*
Physics Effects Copyright(C) 2010 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 _SCE_PFX_INTERSECT_RAY_FUNC_H
#define _SCE_PFX_INTERSECT_RAY_FUNC_H
#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
namespace sce {
namespace PhysicsEffects {
typedef PfxBool (*PfxIntersectRayFunc)(
const PfxRayInput &ray,PfxRayOutput &out,
const PfxShape &shape,const PfxTransform3 &transform);
PfxIntersectRayFunc pfxGetIntersectRayFunc(PfxUInt8 shapeType);
PfxInt32 pfxSetIntersectRayFunc(PfxUInt8 shapeType,PfxIntersectRayFunc func);
} //namespace PhysicsEffects
} //namespace sce
#endif /* _SCE_PFX_INTERSECT_RAY_FUNC_H */

231
Extras/PhysicsEffects/src/low_level/collision/pfx_island_generation.cpp Normal file
View File

@@ -0,0 +1,231 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/rigidbody/pfx_rigid_state.h"
#include "../../../include/physics_effects/low_level/collision/pfx_island_generation.h"
namespace sce {
namespace PhysicsEffects {
struct PfxIslandNode {
PfxUInt32 rootId;
PfxUInt32 rank;
PfxUInt32 islandId;
PfxUInt32 isRoot;
};
struct PfxIslandUnit
{
PfxUInt32 id;
PfxIslandUnit *next;
};
struct PfxIsland
{
PfxUInt32 numNodes;
PfxUInt32 numIslands;
PfxIslandNode *nodes;
PfxIslandUnit *islandsUnits;
PfxIslandUnit **islandsHeads;
};
PfxUInt32 pfxIslandNodeFind(PfxUInt32 i,PfxIsland *island)
{
if( i != island->nodes[i].rootId ) {
island->nodes[i].rootId = pfxIslandNodeFind(island->nodes[i].rootId,island);
}
return island->nodes[i].rootId;
}
void pfxIslandNodeLink(PfxUInt32 iA,PfxUInt32 iB,PfxIsland *island)
{
if(island->nodes[iA].rank > island->nodes[iB].rank) {
island->nodes[iB].rootId = iA;
}
else if(island->nodes[iA].rank == island->nodes[iB].rank) {
island->nodes[iA].rootId = iB;
island->nodes[iB].rank++;
}
else {
island->nodes[iA].rootId = iB;
}
}
void pfxIslandNodeUnion(PfxUInt32 iA,PfxUInt32 iB,PfxIsland *island)
{
SCE_PFX_ALWAYS_ASSERT(iA<island->numNodes);
SCE_PFX_ALWAYS_ASSERT(iB<island->numNodes);
pfxIslandNodeLink(pfxIslandNodeFind(iA,island),pfxIslandNodeFind(iB,island),island);
}
PfxUInt32 pfxGetIslandBytesOfGenerateIsland(PfxUInt32 numObjects)
{
return 16 +
SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIsland)) +
SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandNode)*numObjects) +
SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandUnit*)*numObjects) +
SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandUnit)*numObjects);
}
SCE_PFX_FORCE_INLINE int pfxCheckParamOfGenerateIsland(const PfxGenerateIslandParam &param)
{
if(!param.islandBuff || !param.pairs) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.pairs)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.islandBuff,param.islandBytes) < pfxGetIslandBytesOfGenerateIsland(param.numObjects) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
PfxInt32 pfxGenerateIsland(PfxGenerateIslandParam &param,PfxGenerateIslandResult &result)
{
int ret = pfxCheckParamOfGenerateIsland(param);
if(ret != SCE_PFX_OK) return ret;
PfxConstraintPair *pairs = param.pairs;
PfxUInt32 numPairs = param.numPairs;
PfxUInt32 numUnits = param.numObjects;
memset(param.islandBuff,0,param.islandBytes);
PfxHeapManager pool((unsigned char*)param.islandBuff,param.islandBytes);
PfxIsland *island = (PfxIsland*)pool.allocate(sizeof(PfxIsland));
island->numIslands = 0;
island->numNodes = numUnits;
island->nodes = (PfxIslandNode*)pool.allocate(sizeof(PfxIslandNode)*numUnits);
island->islandsHeads = (PfxIslandUnit**)pool.allocate(sizeof(PfxIslandUnit*)*numUnits);
island->islandsUnits = (PfxIslandUnit*)pool.allocate(sizeof(PfxIslandUnit)*numUnits);
result.island = island;
// 初期化
for(PfxUInt32 i=0;i<island->numNodes;i++) {
island->nodes[i].rootId = i;
island->nodes[i].rank = 0;
}
return pfxAppendPairs(island,pairs,numPairs);
}
PfxUInt32 pfxGetNumIslands(const PfxIsland *islands)
{
SCE_PFX_ALWAYS_ASSERT(islands);
return islands->numIslands;
}
PfxIslandUnit *pfxGetFirstUnitInIsland(const PfxIsland *islands,PfxUInt32 islandId)
{
SCE_PFX_ALWAYS_ASSERT(islands);
SCE_PFX_ALWAYS_ASSERT(islandId < islands->numIslands);
return islands->islandsHeads[islandId];
}
PfxIslandUnit *pfxGetNextUnitInIsland(const PfxIslandUnit *islandUnit)
{
SCE_PFX_ALWAYS_ASSERT(islandUnit);
return islandUnit->next;
}
PfxUInt32 pfxGetUnitId(const PfxIslandUnit *islandUnit)
{
SCE_PFX_ALWAYS_ASSERT(islandUnit);
return islandUnit->id;
}
PfxUInt32 pfxGetIslandId(const PfxIsland *islands,PfxUInt32 unitId)
{
SCE_PFX_ALWAYS_ASSERT(islands&&unitId<islands->numNodes);
return islands->nodes[unitId].islandId;
}
PfxInt32 pfxAppendPairs(PfxIsland *island,PfxConstraintPair *pairs,PfxUInt32 numPairs)
{
if(numPairs == 0) {
return SCE_PFX_OK;
}
if(!island || !pairs) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(island) || !SCE_PFX_PTR_IS_ALIGNED16(pairs)) return SCE_PFX_ERR_INVALID_ALIGN;
// 統合
for(PfxUInt32 i=0;i<numPairs;i++) {
PfxConstraintPair &pair = pairs[i];
if(pfxGetActive(pair)) {
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
SCE_PFX_ALWAYS_ASSERT(iA<island->numNodes);
SCE_PFX_ALWAYS_ASSERT(iB<island->numNodes);
if( (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_TYPE)) &&
(SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_TYPE)) ) {
pfxIslandNodeUnion(iA,iB,island);
}
}
}
// アイランド生成のための初期化
for(PfxUInt32 i=0;i<island->numNodes;i++) {
island->nodes[i].islandId = 0;
island->nodes[i].isRoot = 0;
island->islandsHeads[i] = NULL;
}
// 親へ直結
PfxUInt32 id = 0;
for(PfxUInt32 i=0;i<island->numNodes;i++) {
PfxUInt32 rootId = pfxIslandNodeFind(i,island);
if( island->nodes[rootId].isRoot == 0 ) {
island->nodes[rootId].islandId = id++;
island->nodes[rootId].isRoot = 1;
}
island->nodes[i].islandId = island->nodes[rootId].islandId;
}
// アイランド作成
PfxUInt32 n = 0;
for(PfxUInt32 i=0;i<island->numNodes;i++) {
PfxUInt32 islandId = island->nodes[i].islandId;
PfxIslandUnit *newUnit = &island->islandsUnits[n++];
newUnit->id = i;
if(!island->islandsHeads[islandId]) {
island->islandsHeads[islandId] = newUnit;
continue;
}
PfxIslandUnit *unit=island->islandsHeads[islandId];
island->islandsHeads[islandId] = newUnit;
newUnit->next = unit;
}
island->numIslands = id;
return SCE_PFX_OK;
}
void pfxResetIsland(PfxIsland *island)
{
SCE_PFX_ALWAYS_ASSERT(island);
island->numIslands = 0;
for(PfxUInt32 i=0;i<island->numNodes;i++) {
island->nodes[i].rootId = i;
island->nodes[i].rank = 0;
}
}
} //namespace PhysicsEffects
} //namespace sce

228
Extras/PhysicsEffects/src/low_level/collision/pfx_ray_cast.cpp Normal file
View File

@@ -0,0 +1,228 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
#include "../../../include/physics_effects/low_level/collision/pfx_ray_cast.h"
#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
#include "pfx_intersect_ray_func.h"
#include "../../base_level/collision/pfx_intersect_common.h"
namespace sce {
namespace PhysicsEffects {
void pfxRayTraverseForward(
const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,
PfxBroadphaseProxy *proxies,int numProxies,
PfxRigidState *offsetRigidStates,
PfxCollidable *offsetCollidables,
int axis,const PfxVector3 &center,const PfxVector3 &half)
{
#ifdef SCE_PFX_USE_GEOMETRY
PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);
#endif
for(int i=0;i<numProxies;i++) {
PfxBroadphaseProxy &proxy = proxies[i];
// 終了条件のチェック
if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {
return;
}
PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;
PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);
PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);
if(boundOnRay[axis] < AABBmin[axis]) {
return;
}
// スキップ
if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {
continue;
}
PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);
PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);
PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);
#ifdef SCE_PFX_USE_GEOMETRY
PfxFloatInVec t_(1.0f);
PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {
#else
float t_=1.0f;
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {
#endif
PfxRigidState &state = offsetRigidStates[rigidbodyId];
PfxCollidable &coll = offsetCollidables[rigidbodyId];
PfxTransform3 transform(state.getOrientation(), state.getPosition());
PfxRayOutput tout = out;
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 shapeTr = transform * shape.getOffsetTransform();
if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {
out = tout;
out.m_shapeId = j;
out.m_objectId = rigidbodyId;
}
}
}
}
}
void pfxRayTraverseBackward(
const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,
PfxBroadphaseProxy *proxies,int numProxies,
PfxRigidState *offsetRigidStates,
PfxCollidable *offsetCollidables,
int axis,const PfxVector3 &center,const PfxVector3 &half)
{
#ifdef SCE_PFX_USE_GEOMETRY
PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);
#endif
for(int i=numProxies-1;i>=0;i--) {
PfxBroadphaseProxy &proxy = proxies[i];
// 終了条件のチェック
if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {
return;
}
PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;
PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);
PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);
if(AABBmax[axis] < boundOnRay[axis]) {
return;
}
// スキップ
if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {
continue;
}
PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);
PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);
PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);
#ifdef SCE_PFX_USE_GEOMETRY
PfxFloatInVec t_(1.0f);
PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {
#else
float t_=1.0f;
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {
#endif
PfxRigidState &state = offsetRigidStates[rigidbodyId];
PfxCollidable &coll = offsetCollidables[rigidbodyId];
PfxTransform3 transform(state.getOrientation(), state.getPosition());
PfxRayOutput tout = out;
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 shapeTr = transform * shape.getOffsetTransform();
if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {
out = tout;
out.m_shapeId = j;
out.m_objectId = rigidbodyId;
}
}
}
}
}
void pfxCastSingleRay(const PfxRayInput &ray,PfxRayOutput &out,const PfxRayCastParam &param)
{
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
PfxBroadphaseProxy *proxies[] = {
param.proxiesX,
param.proxiesY,
param.proxiesZ,
param.proxiesXb,
param.proxiesYb,
param.proxiesZb,
};
out.m_variable = 1.0f;
out.m_contactFlag = false;
// 探索軸
PfxVector3 chkAxisVec = absPerElem(ray.m_direction);
int axis = 0;
if(chkAxisVec[1] < chkAxisVec[0]) axis = 1;
if(chkAxisVec[2] < chkAxisVec[axis]) axis = 2;
// レイのAABB作成
PfxVector3 p1 = ray.m_startPosition;
PfxVector3 p2 = ray.m_startPosition + ray.m_direction;
PfxVecInt3 rayMin,rayMax;
pfxConvertCoordWorldToLocal(param.rangeCenter,param.rangeExtent,minPerElem(p1,p2),maxPerElem(p1,p2),rayMin,rayMax);
PfxAabb16 rayAABB;
pfxSetXMin(rayAABB,rayMin.getX());
pfxSetXMax(rayAABB,rayMax.getX());
pfxSetYMin(rayAABB,rayMin.getY());
pfxSetYMax(rayAABB,rayMax.getY());
pfxSetZMin(rayAABB,rayMin.getZ());
pfxSetZMax(rayAABB,rayMax.getZ());
// AABB探索開始
int sign = ray.m_direction[axis] < 0.0f ? -1 : 1; // 探索方向
if(sign > 0) {
pfxRayTraverseForward(
ray,out,rayAABB,
proxies[axis],param.numProxies,
param.offsetRigidStates,param.offsetCollidables,
axis,param.rangeCenter,param.rangeExtent);
}
else {
pfxRayTraverseBackward(
ray,out,rayAABB,
proxies[axis+3],param.numProxies,
param.offsetRigidStates,param.offsetCollidables,
axis,param.rangeCenter,param.rangeExtent);
}
}
} //namespace PhysicsEffects
} //namespace sce

115
Extras/PhysicsEffects/src/low_level/collision/pfx_refresh_contacts_parallel.cpp Normal file
View File

@@ -0,0 +1,115 @@
/*
Applied Research Associates Inc. (c)2011
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
#include "../../../include/physics_effects/low_level/collision/pfx_refresh_contacts.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// This function is implemented in pfx_refresh_contacts_single.cpp
extern int pfxCheckParamOfRefreshContacts(PfxRefreshContactsParam &param);
///////////////////////////////////////////////////////////////////////////////
// MULTIPLE THREADS
//----------------------------------------------------------------------------
// pfxRefreshContactsTaskEntry
//
/// The thread PfxTaskEntry function used to perform refresh contacts in
/// parallel
//----------------------------------------------------------------------------
void pfxRefreshContactsTaskEntry(PfxTaskArg *arg)
{
PfxRefreshContactsParam &param = *((PfxRefreshContactsParam*)arg->io);
PfxUInt32 iFirstContactPair = arg->data[0];
PfxUInt32 iEndContactPair = arg->data[1];
PfxConstraintPair *contactPairs = param.contactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
{
PfxBroadphasePair &pair = contactPairs[i];
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &instA = offsetRigidStates[iA];
PfxRigidState &instB = offsetRigidStates[iB];
contact.refresh(
instA.getPosition(),instA.getOrientation(),
instB.getPosition(),instB.getOrientation() );
}
}
//----------------------------------------------------------------------------
// pfxRefreshContacts
//
/// Perform refresh contacts in parallel using a task manager.
///
/// @param param Information about contact pairs
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param, PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfRefreshContacts(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxRefreshContacts");
PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxRefreshContactsTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numContactPairs;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

76
Extras/PhysicsEffects/src/low_level/collision/pfx_refresh_contacts_single.cpp Normal file
View File

@@ -0,0 +1,76 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
#include "../../../include/physics_effects/low_level/collision/pfx_refresh_contacts.h"
namespace sce {
namespace PhysicsEffects {
int pfxCheckParamOfRefreshContacts(PfxRefreshContactsParam &param)
{
if(!param.contactPairs || !param.offsetContactManifolds || !param.offsetRigidStates ) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param)
{
PfxInt32 ret = pfxCheckParamOfRefreshContacts(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxRefreshContacts");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxBroadphasePair &pair = contactPairs[i];
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &instA = offsetRigidStates[iA];
PfxRigidState &instB = offsetRigidStates[iB];
contact.refresh(
instA.getPosition(),instA.getOrientation(),
instB.getPosition(),instB.getOrientation() );
}
SCE_PFX_POP_MARKER();
(void) numRigidBodies;
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

12
Extras/PhysicsEffects/src/low_level/premake4.lua Normal file
View File

@@ -0,0 +1,12 @@
project "physicseffects2_lowlevel"
kind "StaticLib"
targetdir "../../build/lib"
includedirs {
".",
}
files {
"**.cpp",
"../../include/physics_effects/low_level/**.h"
}

795
Extras/PhysicsEffects/src/low_level/solver/pfx_constraint_solver_parallel.cpp Normal file
View File

@@ -0,0 +1,795 @@
/*
Applied Research Associates Inc. (c)2011
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/solver/pfx_contact_constraint.h"
#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
#include "../../../include/physics_effects/low_level/solver/pfx_constraint_solver.h"
#include "../../base_level/solver/pfx_check_solver.h"
#include "pfx_parallel_group.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// These functions are implemented in pfx_constraint_solver_single.cpp
extern PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks);
extern PfxInt32 pfxCheckParamOfSetupSolverBodies(const PfxSetupSolverBodiesParam &param);
extern PfxInt32 pfxCheckParamOfSetupContactConstraints(const PfxSetupContactConstraintsParam &param);
extern PfxInt32 pfxCheckParamOfSetupJointConstraints(const PfxSetupJointConstraintsParam &param);
extern PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param);
//----------------------------------------------------------------------------
// pfxCheckParamOfSolveConstraints
//
/// This function verifies that the input parameter for solving constraints
/// in parallel is good.
//----------------------------------------------------------------------------
PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param,
PfxTaskManager *taskManager)
{
if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) ||
(param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || !param.offsetSolverBodies) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfSolveConstraints(param.numRigidBodies,param.numContactPairs,param.numJointPairs, taskManager->getNumTasks()) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// MULTIPLE THREADS
//----------------------------------------------------------------------------
// pfxSetupSolverBodiesTaskEntry
//
/// The thread PfxTaskEntry function used to setup solver bodies in parallel.
//----------------------------------------------------------------------------
void pfxSetupSolverBodiesTaskEntry(PfxTaskArg *arg)
{
PfxSetupSolverBodiesParam &param = *((PfxSetupSolverBodiesParam*)arg->io);
PfxUInt32 iFirstBody = arg->data[0];
PfxUInt32 iEndBody = arg->data[1];
PfxRigidState *states = param.states;
PfxRigidBody *bodies = param.bodies;
PfxSolverBody *solverBodies = param.solverBodies;
for(PfxUInt32 i = iFirstBody; i < iEndBody; i++)
{
PfxRigidState &state = states[i];
PfxRigidBody &body = bodies[i];
PfxSolverBody &solverBody = solverBodies[i];
solverBody.m_orientation = state.getOrientation();
solverBody.m_deltaLinearVelocity = PfxVector3(0.0f);
solverBody.m_deltaAngularVelocity = PfxVector3(0.0f);
solverBody.m_motionType = state.getMotionMask();
if(SCE_PFX_MOTION_MASK_DYNAMIC(state.getMotionType())) {
PfxMatrix3 ori(solverBody.m_orientation);
solverBody.m_massInv = body.getMassInv();
solverBody.m_inertiaInv = ori * body.getInertiaInv() * transpose(ori);
}
else {
solverBody.m_massInv = 0.0f;
solverBody.m_inertiaInv = PfxMatrix3(0.0f);
}
}
SCE_PFX_POP_MARKER();
}
//----------------------------------------------------------------------------
// pfxSetupSolverBodies
//
/// Perform setup solver bodies in parallel using a task manager.
///
/// @param param Information about rigid bodies
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param,
PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfSetupSolverBodies(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupSolverBodies");
PfxUInt32 maxBatchSize = param.numRigidBodies / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxSetupSolverBodiesTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numRigidBodies;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
//----------------------------------------------------------------------------
// pfxSetupContactConstraintsTaskEntry
//
/// The thread PfxTaskEntry function used to setup contact constraints in
/// parallel.
//----------------------------------------------------------------------------
void pfxSetupContactConstraintsTaskEntry(PfxTaskArg *arg)
{
PfxSetupContactConstraintsParam &param = *((PfxSetupContactConstraintsParam*)arg->io);
PfxUInt32 iFirstContactPair = arg->data[0];
PfxUInt32 iEndContactPair = arg->data[1];
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
{
PfxConstraintPair &pair = contactPairs[i];
if(!pfxCheckSolver(pair))
{
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxContactManifold &contact = offsetContactManifolds[iConstraint];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidBody &bodyA = offsetRigidBodies[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxRigidBody &bodyB = offsetRigidBodies[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
contact.setInternalFlag(0);
PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
if(contact.getDuration() > 1) restitution = 0.0f;
PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
for(int j=0;j<contact.getNumContacts();j++)
{
PfxContactPoint &cp = contact.getContactPoint(j);
pfxSetupContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
cp.m_distance,
restitution,
friction,
pfxReadVector3(cp.m_constraintRow[0].m_normal),
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
stateA,
stateB,
solverBodyA,
solverBodyB,
param.separateBias,
param.timeStep
);
}
contact.setCompositeFriction(friction);
}
}
//----------------------------------------------------------------------------
// pfxSetupContactConstraints
//
/// Perform setup contact constraints in parallel using a task manager.
///
/// @param param Information about contact constraints
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param,
PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxSetupContactConstraintsTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numContactPairs;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
//----------------------------------------------------------------------------
// pfxSetupJointConstraintsTaskEntry
//
/// The thread PfxTaskEntry function used to setup joint constraints in
/// parallel.
//----------------------------------------------------------------------------
void pfxSetupJointConstraintsTaskEntry(PfxTaskArg *arg)
{
PfxSetupJointConstraintsParam &param = *((PfxSetupJointConstraintsParam*)arg->io);
PfxUInt32 iFirstJointPair = arg->data[0];
PfxUInt32 iEndJointPair = arg->data[1];
PfxConstraintPair *jointPairs = param.jointPairs;
PfxUInt32 numJointPairs = param.numJointPairs;
PfxJoint *offsetJoints = param.offsetJoints;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for(PfxUInt32 i = iFirstJointPair; i < iEndJointPair; i++)
{
PfxConstraintPair &pair = jointPairs[i];
if(!pfxCheckSolver(pair))
{
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxJoint &joint = offsetJoints[iConstraint];
SCE_PFX_ALWAYS_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ALWAYS_ASSERT(iB==joint.m_rigidBodyIdB);
PfxRigidState &stateA = offsetRigidStates[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetSetupJointConstraintFunc(joint.m_type)(
joint,
stateA,
stateB,
solverBodyA,
solverBodyB,
param.timeStep);
}
}
//----------------------------------------------------------------------------
// pfxSetupJointConstraints
//
/// Perform setup joint constraints in parallel using a task manager.
///
/// @param param Information about joint constraints
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param,
PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfSetupJointConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupJointConstraints");
PfxUInt32 maxBatchSize = param.numJointPairs / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxSetupJointConstraintsTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numJointPairs;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
//----------------------------------------------------------------------------
// pfxSplitConstraints
//
/// Given a set of constraints to be solved, split the constraints into
/// a collection of phases, with each phase having one or more independent
/// batches that can be solved in parallel. The phases must be solved
/// sequentially.
///
/// @param numRigidBodies [in] Total number of rigid bodies referenced
/// in the given set of constraints
/// @param constraintpairs [in] Pointer to array of constraints to split.
/// For clarity, note that the pairs always stay
/// together. Some pairs are split to be solved
/// in parallel with other pairs.
/// @param numConstraints [in] Number of constraints to split
/// @param taskManager [in] Pointer to the thread task manager that will
/// eventually be used to solve the constraints.
/// @param group [out] On return, contains information about
/// the phases and batches that define the splitting
/// @param batches [out] Caller should pass a pointer to a pre-
/// allocated array of SCE_PFX_MAX_SOLVER_BATCHES
/// PfxParallelBatch objects. On output, these will
/// be populated with the correct number of pairs
/// for each batch.
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
void pfxSplitConstraints(PfxUInt32 numRigidBodies, PfxConstraintPair *constraintPairs,
PfxUInt32 numConstraints, PfxTaskManager *taskManager, PfxParallelGroup *group,
PfxParallelBatch *batches)
{
SCE_PFX_PUSH_MARKER("pfxSplitConstraints");
// allocate a table that will be used to indicate, for a given phase being
// populated, which batch a given body belongs to.
PfxInt32 bufSize = sizeof(PfxUInt8) * numRigidBodies;
bufSize = ((bufSize + 127) >> 7) << 7; // 128 bytes alignment
PfxUInt8 *bodyTable = (PfxUInt8*)taskManager->allocate(bufSize);
// allocate a table that will be used to indicate, for a given phase being
// populated, which batch a given pair of bodies belongs to.
PfxUInt32 *pairTable;
size_t allocSize = sizeof(PfxUInt32)*((numConstraints + 31) / 32);
pairTable = (PfxUInt32*)taskManager->allocate(allocSize);
memset(pairTable, 0, allocSize);
//
PfxUInt32 numTasks = taskManager->getNumTasks();
PfxUInt32 targetCount = SCE_PFX_MAX(PfxUInt32(SCE_PFX_MIN_SOLVER_PAIRS),
SCE_PFX_MIN(numConstraints / (numTasks * 2), PfxUInt32(SCE_PFX_MAX_SOLVER_PAIRS)));
PfxUInt32 startIndex = 0;
PfxUInt32 phaseId;
PfxUInt32 batchId;
PfxUInt32 totalCount = 0;
PfxUInt32 maxBatches = SCE_PFX_MIN(numTasks, PfxUInt32(SCE_PFX_MAX_SOLVER_BATCHES));
// accumulate phases and batches until group resources are exhausted or all incoming
// pairs are accounted for.
for (phaseId = 0; phaseId < SCE_PFX_MAX_SOLVER_PHASES && totalCount < numConstraints; phaseId++)
{
bool startIndexCheck = true;
group->numBatches[phaseId] = 0;
PfxUInt32 i = startIndex;
// Initialize body table such that no body is assigned to any batch. (0xff is explicitly assumed to
// mean no batch assigned)
memset(bodyTable, 0xff, bufSize);
// accumulate batches within the current phase. This code creates batches that are
// independent, e.g., no batch on this phase will touch the same bodies as any other
// batch on the phase. Batches within a phase can be solved in parallel on shared memory
// multiprocessor hardware.
for (batchId = 0; i < numConstraints && totalCount < numConstraints && batchId < maxBatches; batchId++)
{
PfxUInt32 pairCount=0;
PfxParallelBatch &batch = batches[(phaseId * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
PfxUInt32 pairId = 0;
// iterate through pairs, and assigns the pairs to batches
for (; i < numConstraints && pairCount < targetCount; i++)
{
PfxUInt32 idxP = i >> 5;
PfxUInt32 maskP = 1L << (i & 31);
if(pairTable[idxP] & maskP) // pair is already assigned to a phase/batch
continue;
PfxUInt16 idxA = pfxGetObjectIdA(constraintPairs[i]);
PfxUInt16 idxB = pfxGetObjectIdB(constraintPairs[i]);
// It is possible an incoming constraint pair can be skipped. For example, if the pair is inactive,
// or if both its objects are static, unmoving objects and therefore would be unaffected by constraints.
// This conditional statement addresses constraints to be skipped.
if (!pfxGetActive(constraintPairs[i]) ||
(SCE_PFX_MOTION_MASK_STATIC(pfxGetMotionMaskA(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE) &&
SCE_PFX_MOTION_MASK_STATIC(pfxGetMotionMaskB(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE)) )
{
if (startIndexCheck)
startIndex++;
//assign pair -> skip it because it has no constraints
pairTable[idxP] |= maskP;
totalCount++;
continue;
}
// If either body of the current pair belongs to another batch already, we cannot add the current
// pair to the current batch. Must defer to another phase
if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) ||
(bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) )
{
startIndexCheck = false; // so we will revisit this during allocation of next phase
continue;
}
// Dynamic bodies for current pair are assigned to the current batch in this phase.
// Static bodies are not assigned. Since they never move, and their corresponding solver
// bodies are therefore never touched, they can actually be used by any batch.
if (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskA(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE))
bodyTable[idxA] = batchId;
if (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskB(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE))
bodyTable[idxB] = batchId;
if(startIndexCheck)
startIndex++;
pairTable[idxP] |= maskP; // pair has been handled
//add the pair 'i' to the current batch
batch.pairIndices[pairId++] = i;
pairCount++;
}
group->numPairs[(phaseId * SCE_PFX_MAX_SOLVER_BATCHES) + batchId] = (PfxUInt16)pairId;
totalCount += pairCount;
}
group->numBatches[phaseId] = batchId;
}
group->numPhases = phaseId;
taskManager->deallocate(bodyTable);
taskManager->deallocate(pairTable);
SCE_PFX_POP_MARKER();
}
//----------------------------------------------------------------------------
// pfxSolveConstraintsTaskEntry
//
/// The thread PfxTaskEntry function used to solve constraints in parallel.
//----------------------------------------------------------------------------
void pfxSolveConstraintsTaskEntry(PfxTaskArg *arg)
{
PfxSolveConstraintsParam &param = *((PfxSolveConstraintsParam*)arg->io);
PfxConstraintPair *contactPairs = param.contactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxConstraintPair *jointPairs = param.jointPairs;
PfxJoint *offsetJoints = param.offsetJoints;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
PfxParallelGroup *jointgroup = (PfxParallelGroup*)arg->data[0];
PfxParallelBatch *jointbatches = (PfxParallelBatch*)arg->data[1];
PfxParallelGroup *contactgroup = (PfxParallelGroup*)arg->data[2];
PfxParallelBatch *contactbatches = (PfxParallelBatch*)arg->data[3];
// Warm Starting
{
// Joints
for (PfxUInt16 phase = 0; phase < jointgroup->numPhases; phase++)
{
for (PfxUInt16 batchId = 0; batchId < jointgroup->numBatches[phase]; batchId++)
{
PfxUInt16 numJointPairs = jointgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
if ((arg->taskId == (batchId % arg->maxTasks)) && numJointPairs > 0) // only spend time on batches meant for this task
{
const PfxParallelBatch &batch = jointbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
for (PfxUInt16 i = 0; i < numJointPairs; i++)
{
PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
if(!pfxCheckSolver(pair))
continue;
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetWarmStartJointConstraintFunc(joint.m_type)(
joint,
solverBodyA,
solverBodyB);
}
}
arg->barrier->sync(); // block until all threads are ready to go to next phase
}
}
// Contacts
for (PfxUInt16 phase = 0; phase < contactgroup->numPhases; phase++)
{
for (PfxUInt16 batchId = 0; batchId < contactgroup->numBatches[phase]; batchId++)
{
PfxUInt16 numContactPairs = contactgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
if ((arg->taskId == (batchId % arg->maxTasks)) && numContactPairs > 0) // only spend time on batches meant for this task
{
const PfxParallelBatch &batch = contactbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
for (PfxUInt16 i = 0; i < numContactPairs; i++)
{
PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
if(!pfxCheckSolver(pair))
{
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
PfxFloat massInvA = solverBodyA.m_massInv;
PfxFloat massInvB = solverBodyB.m_massInv;
PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
if(solverBodyA.m_motionType == kPfxMotionTypeOneWay)
{
massInvB = 0.0f;
inertiaInvB = PfxMatrix3(0.0f);
}
if(solverBodyB.m_motionType == kPfxMotionTypeOneWay)
{
massInvA = 0.0f;
inertiaInvA = PfxMatrix3(0.0f);
}
for(int j = 0; j < contact.getNumContacts(); j++)
{
PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 rA = rotate(solverBodyA.m_orientation,pfxReadVector3(cp.m_localPointA));
PfxVector3 rB = rotate(solverBodyB.m_orientation,pfxReadVector3(cp.m_localPointB));
for(int k = 0; k < 3; k++)
{
PfxVector3 normal = pfxReadVector3(cp.m_constraintRow[k].m_normal);
PfxFloat deltaImpulse = cp.m_constraintRow[k].m_accumImpulse;
solverBodyA.m_deltaLinearVelocity += deltaImpulse * massInvA * normal;
solverBodyA.m_deltaAngularVelocity += deltaImpulse * inertiaInvA * cross(rA,normal);
solverBodyB.m_deltaLinearVelocity -= deltaImpulse * massInvB * normal;
solverBodyB.m_deltaAngularVelocity -= deltaImpulse * inertiaInvB * cross(rB,normal);
}
}
}
}
}
arg->barrier->sync(); // block until all threads are ready to go to next phase
}
}
// Solver
for(PfxUInt32 iteration = 0; iteration < param.iteration; iteration++)
{
// Joints
for (PfxUInt16 phase = 0; phase < jointgroup->numPhases; phase++)
{
for (PfxUInt16 batchId = 0; batchId < jointgroup->numBatches[phase]; batchId++)
{
PfxUInt16 numJointPairs = jointgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
if ((arg->taskId == (batchId % arg->maxTasks)) && numJointPairs > 0) // only spend time on batches meant for this task
{
const PfxParallelBatch &batch = jointbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
for(PfxUInt16 i = 0; i < numJointPairs; i++)
{
PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
if(!pfxCheckSolver(pair))
continue;
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetSolveJointConstraintFunc(joint.m_type)(
joint,
solverBodyA,
solverBodyB);
}
}
}
arg->barrier->sync(); // block until all threads are ready to go to next phase
}
// Contacts
for (PfxUInt32 phase = 0; phase < contactgroup->numPhases; phase++)
{
for (PfxUInt32 batchId = 0; batchId < contactgroup->numBatches[phase]; batchId++)
{
PfxUInt32 numContactPairs = contactgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
if ((arg->taskId == (batchId % arg->maxTasks)) && numContactPairs > 0) // only spend time on batches meant for this task
{
const PfxParallelBatch &batch = contactbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
for(PfxUInt32 i = 0; i < numContactPairs; i++)
{
PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
if(!pfxCheckSolver(pair))
continue;
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
for(int j = 0; j < contact.getNumContacts(); j++)
{
PfxContactPoint &cp = contact.getContactPoint(j);
pfxSolveContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
solverBodyA,
solverBodyB,
contact.getCompositeFriction()
);
}
}
}
}
arg->barrier->sync(); // block until all threads are ready to go to next phase
}
}
}
//----------------------------------------------------------------------------
// pfxSolveConstraints
//
/// Perform setup joint constraints in parallel using a task manager.
///
/// @param param Information about constraints
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param,
PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfSolveConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSolveConstraints");
PfxParallelGroup *contactgroup = (PfxParallelGroup*)taskManager->allocate(sizeof(PfxParallelGroup));
PfxParallelBatch *contactbatches = (PfxParallelBatch*)taskManager->allocate(sizeof(PfxParallelBatch) * (SCE_PFX_MAX_SOLVER_PHASES * SCE_PFX_MAX_SOLVER_BATCHES));
PfxParallelGroup *jointgroup = (PfxParallelGroup*)taskManager->allocate(sizeof(PfxParallelGroup));
PfxParallelBatch *jointbatches = (PfxParallelBatch*)taskManager->allocate(sizeof(PfxParallelBatch) * (SCE_PFX_MAX_SOLVER_PHASES * SCE_PFX_MAX_SOLVER_BATCHES));
// split constraints into independent phases and batches. Phases allow
// a set of non-independent constraints to be solved in parallel. One
// phase may have dependencies within another phase, but the phases are
// solved sequentially. Within a phases, there are multiple batches, and
// the batches are independent. Since they are independent, they can be
// distributed to different processors and solved in parallel.
pfxSplitConstraints(param.numRigidBodies, param.contactPairs, param.numContactPairs,
taskManager, contactgroup, contactbatches);
pfxSplitConstraints(param.numRigidBodies, param.jointPairs, param.numJointPairs,
taskManager, jointgroup, jointbatches);
// parallel solve
taskManager->setTaskEntry((void*)pfxSolveConstraintsTaskEntry);
int task = 0;
for (; task < taskManager->getNumTasks(); task++)
{
taskManager->startTask(task, static_cast<void*>(&param), (PfxUInt32)jointgroup, (PfxUInt32)jointbatches,
(PfxUInt32)contactgroup, (PfxUInt32)contactbatches);
}
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
// post solve
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for (PfxUInt32 i = 0; i < param.numRigidBodies; i++)
{
param.offsetRigidStates[i].setLinearVelocity(param.offsetRigidStates[i].getLinearVelocity() +
param.offsetSolverBodies[i].m_deltaLinearVelocity);
param.offsetRigidStates[i].setAngularVelocity(param.offsetRigidStates[i].getAngularVelocity() +
param.offsetSolverBodies[i].m_deltaAngularVelocity);
}
taskManager->clearPool();
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

399
Extras/PhysicsEffects/src/low_level/solver/pfx_constraint_solver_single.cpp Normal file
View File

@@ -0,0 +1,399 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/solver/pfx_contact_constraint.h"
#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
#include "../../../include/physics_effects/low_level/solver/pfx_constraint_solver.h"
#include "../../base_level/solver/pfx_check_solver.h"
#include "pfx_parallel_group.h"
namespace sce {
namespace PhysicsEffects {
PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks)
{
(void)maxTasks;
PfxUInt32 workBytes = SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt8) * numRigidBodies) +
SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt32)*((SCE_PFX_MAX(numContactPairs,numJointPairs)+31)/32));
workBytes += 128 + (SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxParallelGroup)) +
SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxParallelBatch)*(SCE_PFX_MAX_SOLVER_PHASES*SCE_PFX_MAX_SOLVER_BATCHES))) * 2;
return workBytes;
}
PfxInt32 pfxCheckParamOfSetupSolverBodies(const PfxSetupSolverBodiesParam &param)
{
if(!param.states || !param.bodies || !param.solverBodies ) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.states) || !SCE_PFX_PTR_IS_ALIGNED16(param.bodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.solverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
PfxInt32 pfxCheckParamOfSetupContactConstraints(const PfxSetupContactConstraintsParam &param)
{
if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || !param.offsetRigidStates ||
!param.offsetRigidBodies || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
PfxInt32 pfxCheckParamOfSetupJointConstraints(const PfxSetupJointConstraintsParam &param)
{
if((param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates ||
!param.offsetRigidBodies || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param)
{
if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) ||
(param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || !param.offsetSolverBodies) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
!SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfSolveConstraints(param.numRigidBodies,param.numContactPairs,param.numJointPairs) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param)
{
PfxInt32 ret = pfxCheckParamOfSetupSolverBodies(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupSolverBodies");
PfxRigidState *states = param.states;
PfxRigidBody *bodies = param.bodies;
PfxSolverBody *solverBodies = param.solverBodies;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i=0;i<numRigidBodies;i++) {
PfxRigidState &state = states[i];
PfxRigidBody &body = bodies[i];
PfxSolverBody &solverBody = solverBodies[i];
solverBody.m_orientation = state.getOrientation();
solverBody.m_deltaLinearVelocity = PfxVector3(0.0f);
solverBody.m_deltaAngularVelocity = PfxVector3(0.0f);
solverBody.m_motionType = state.getMotionMask();
if(SCE_PFX_MOTION_MASK_DYNAMIC(state.getMotionType())) {
PfxMatrix3 ori(solverBody.m_orientation);
solverBody.m_massInv = body.getMassInv();
solverBody.m_inertiaInv = ori * body.getInertiaInv() * transpose(ori);
}
else {
solverBody.m_massInv = 0.0f;
solverBody.m_inertiaInv = PfxMatrix3(0.0f);
}
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param)
{
PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxConstraintPair &pair = contactPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxContactManifold &contact = offsetContactManifolds[iConstraint];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidBody &bodyA = offsetRigidBodies[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxRigidBody &bodyB = offsetRigidBodies[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
contact.setInternalFlag(0);
PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
if(contact.getDuration() > 1) restitution = 0.0f;
PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
for(int j=0;j<contact.getNumContacts();j++) {
PfxContactPoint &cp = contact.getContactPoint(j);
pfxSetupContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
cp.m_distance,
restitution,
friction,
pfxReadVector3(cp.m_constraintRow[0].m_normal),
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
stateA,
stateB,
solverBodyA,
solverBodyB,
param.separateBias,
param.timeStep
);
}
contact.setCompositeFriction(friction);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param)
{
PfxInt32 ret = pfxCheckParamOfSetupJointConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupJointConstraints");
PfxConstraintPair *jointPairs = param.jointPairs;
PfxUInt32 numJointPairs = param.numJointPairs;
PfxJoint *offsetJoints = param.offsetJoints;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for(PfxUInt32 i=0;i<numJointPairs;i++) {
PfxConstraintPair &pair = jointPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxJoint &joint = offsetJoints[iConstraint];
SCE_PFX_ALWAYS_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ALWAYS_ASSERT(iB==joint.m_rigidBodyIdB);
PfxRigidState &stateA = offsetRigidStates[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetSetupJointConstraintFunc(joint.m_type)(
joint,
stateA,
stateB,
solverBodyA,
solverBodyB,
param.timeStep);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param)
{
PfxInt32 ret = pfxCheckParamOfSolveConstraints(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSolveConstraints");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxConstraintPair *jointPairs = param.jointPairs;
PfxUInt32 numJointPairs = param.numJointPairs;
PfxJoint *offsetJoints = param.offsetJoints;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
PfxUInt32 numRigidBodies = param.numRigidBodies;
// Warm Starting
{
for(PfxUInt32 i=0;i<numJointPairs;i++) {
PfxConstraintPair &pair = jointPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetWarmStartJointConstraintFunc(joint.m_type)(
joint,
solverBodyA,
solverBodyB);
}
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxConstraintPair &pair = contactPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
PfxFloat massInvA = solverBodyA.m_massInv;
PfxFloat massInvB = solverBodyB.m_massInv;
PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
massInvB = 0.0f;
inertiaInvB = PfxMatrix3(0.0f);
}
if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
massInvA = 0.0f;
inertiaInvA = PfxMatrix3(0.0f);
}
for(int j=0;j<contact.getNumContacts();j++) {
PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 rA = rotate(solverBodyA.m_orientation,pfxReadVector3(cp.m_localPointA));
PfxVector3 rB = rotate(solverBodyB.m_orientation,pfxReadVector3(cp.m_localPointB));
for(int k=0;k<3;k++) {
PfxVector3 normal = pfxReadVector3(cp.m_constraintRow[k].m_normal);
PfxFloat deltaImpulse = cp.m_constraintRow[k].m_accumImpulse;
solverBodyA.m_deltaLinearVelocity += deltaImpulse * massInvA * normal;
solverBodyA.m_deltaAngularVelocity += deltaImpulse * inertiaInvA * cross(rA,normal);
solverBodyB.m_deltaLinearVelocity -= deltaImpulse * massInvB * normal;
solverBodyB.m_deltaAngularVelocity -= deltaImpulse * inertiaInvB * cross(rB,normal);
}
}
}
}
// Solver
for(PfxUInt32 iteration=0;iteration<param.iteration;iteration++) {
for(PfxUInt32 i=0;i<numJointPairs;i++) {
PfxConstraintPair &pair = jointPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
pfxGetSolveJointConstraintFunc(joint.m_type)(
joint,
solverBodyA,
solverBodyB);
}
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxConstraintPair &pair = contactPairs[i];
if(!pfxCheckSolver(pair)) {
continue;
}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
for(int j=0;j<contact.getNumContacts();j++) {
PfxContactPoint &cp = contact.getContactPoint(j);
pfxSolveContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
solverBodyA,
solverBodyB,
contact.getCompositeFriction()
);
}
}
}
for(PfxUInt32 i=0;i<numRigidBodies;i++) {
offsetRigidStates[i].setLinearVelocity(
offsetRigidStates[i].getLinearVelocity()+offsetSolverBodies[i].m_deltaLinearVelocity);
offsetRigidStates[i].setAngularVelocity(
offsetRigidStates[i].getAngularVelocity()+offsetSolverBodies[i].m_deltaAngularVelocity);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

180
Extras/PhysicsEffects/src/low_level/solver/pfx_joint_constraint_func.cpp Normal file
View File

@@ -0,0 +1,180 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/solver/pfx_joint.h"
#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_ball.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_swing_twist.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_hinge.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_slider.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_fix.h"
#include "../../../include/physics_effects/base_level/solver/pfx_joint_universal.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// Setup Joint Constraint Function Table
void setupJointConstraintDummy(
PfxJoint &joint,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat timeStep)
{
(void)joint,(void)stateA,(void)stateB,(void)solverBodyA,(void)solverBodyB,(void)timeStep;
}
PfxSetupJointConstraintFunc funcTbl_setupJointConstraint[kPfxJointCount] = {
pfxSetupBallJoint,
pfxSetupSwingTwistJoint,
pfxSetupSwingTwistJoint,
pfxSetupSwingTwistJoint,
pfxSetupSwingTwistJoint,
pfxSetupUniversalJoint,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
setupJointConstraintDummy,
};
///////////////////////////////////////////////////////////////////////////////
// Setup Joint Constraint Function Table Interface
PfxSetupJointConstraintFunc pfxGetSetupJointConstraintFunc(PfxUInt8 jointType)
{
SCE_PFX_ASSERT(jointType<kPfxJointCount);
return funcTbl_setupJointConstraint[jointType];
}
int pfxSetSetupJointConstraintFunc(PfxUInt8 jointType,PfxSetupJointConstraintFunc func)
{
if(jointType >= kPfxJointCount) {
return SCE_PFX_ERR_OUT_OF_RANGE;
}
funcTbl_setupJointConstraint[jointType] = func;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// Warm Start Joint Constraint Function Table
void warmStartJointConstraintDummy(
PfxJoint &joint,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB)
{
(void)joint,(void)solverBodyA,(void)solverBodyB;
}
PfxWarmStartJointConstraintFunc funcTbl_warmStartJointConstraint[kPfxJointCount] = {
pfxWarmStartBallJoint,
pfxWarmStartSwingTwistJoint,
pfxWarmStartSwingTwistJoint,
pfxWarmStartSwingTwistJoint,
pfxWarmStartSwingTwistJoint,
pfxWarmStartSwingTwistJoint,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
warmStartJointConstraintDummy,
};
///////////////////////////////////////////////////////////////////////////////
// Warm Start Joint Constraint Function Table Interface
PfxWarmStartJointConstraintFunc pfxGetWarmStartJointConstraintFunc(PfxUInt8 jointType)
{
SCE_PFX_ASSERT(jointType<kPfxJointCount);
return funcTbl_warmStartJointConstraint[jointType];
}
int pfxSetWarmStartJointConstraintFunc(PfxUInt8 jointType,PfxWarmStartJointConstraintFunc func)
{
if(jointType >= kPfxJointCount) {
return SCE_PFX_ERR_OUT_OF_RANGE;
}
funcTbl_warmStartJointConstraint[jointType] = func;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// Solve Joint Constraint Function Table
void solveJointConstraintDummy(
PfxJoint &joint,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB)
{
(void)joint,(void)solverBodyA,(void)solverBodyB;
}
PfxSolveJointConstraintFunc funcTbl_solveJointConstraint[kPfxJointCount] = {
pfxSolveBallJoint,
pfxSolveSwingTwistJoint,
pfxSolveSwingTwistJoint,
pfxSolveSwingTwistJoint,
pfxSolveSwingTwistJoint,
pfxSolveSwingTwistJoint,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
solveJointConstraintDummy,
};
///////////////////////////////////////////////////////////////////////////////
// Solve Joint Constraint Function Table Interface
PfxSolveJointConstraintFunc pfxGetSolveJointConstraintFunc(PfxUInt8 jointType)
{
SCE_PFX_ASSERT(jointType<kPfxJointCount);
return funcTbl_solveJointConstraint[jointType];
}
int pfxSetSolveJointConstraintFunc(PfxUInt8 jointType,PfxSolveJointConstraintFunc func)
{
if(jointType >= kPfxJointCount) {
return SCE_PFX_ERR_OUT_OF_RANGE;
}
funcTbl_solveJointConstraint[jointType] = func;
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

44
Extras/PhysicsEffects/src/low_level/solver/pfx_parallel_group.h Normal file
View File

@@ -0,0 +1,44 @@
/*
Physics Effects Copyright(C) 2010 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 _SCE_PFX_PARALLEL_GROUP_H
#define _SCE_PFX_PARALLEL_GROUP_H
///////////////////////////////////////////////////////////////////////////////
// Parallel Group
#define SCE_PFX_MAX_SOLVER_PHASES 64 // 最大フェーズ数
#define SCE_PFX_MAX_SOLVER_BATCHES 32 // １フェーズに含まれる最大並列処理バッチ
#define SCE_PFX_MAX_SOLVER_PAIRS 64 // １バッチに含まれる最大ペア数
#define SCE_PFX_MIN_SOLVER_PAIRS 16 // １バッチに含まれる最小ペア数
namespace sce {
namespace PhysicsEffects {
struct SCE_PFX_ALIGNED(128) PfxParallelBatch {
PfxUInt16 pairIndices[SCE_PFX_MAX_SOLVER_PAIRS];
};
struct SCE_PFX_ALIGNED(128) PfxParallelGroup {
PfxUInt16 numPhases;
PfxUInt16 numBatches[SCE_PFX_MAX_SOLVER_PHASES]; // 各フェーズの保持する並列実行可能なバッチの数
PfxUInt16 numPairs[SCE_PFX_MAX_SOLVER_PHASES*SCE_PFX_MAX_SOLVER_BATCHES]; // 各バッチの保持するペアの数
SCE_PFX_PADDING(1,126)
};
} //namespace PhysicsEffects
} //namespace sce
#endif // _SCE_PFX_PARALLEL_GROUP_H

94
Extras/PhysicsEffects/src/low_level/solver/pfx_update_rigid_states_parallel.cpp Normal file
View File

@@ -0,0 +1,94 @@
/*
Applied Research Associates Inc. (c)2011
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/solver/pfx_integrate.h"
#include "../../../include/physics_effects/low_level/solver/pfx_update_rigid_states.h"
namespace sce {
namespace PhysicsEffects {
///////////////////////////////////////////////////////////////////////////////
// This function is implemented in pfx_update_rigid_states_single.cpp
extern PfxInt32 pfxCheckParamOfUpdateRigidStates(const PfxUpdateRigidStatesParam &param);
///////////////////////////////////////////////////////////////////////////////
// MULTIPLE THREADS
//----------------------------------------------------------------------------
// pfxUpdateRigidStatesTaskEntry
//
/// The thread PfxTaskEntry function used to update rigid body states in
/// parallel.
//----------------------------------------------------------------------------
void pfxUpdateRigidStatesTaskEntry(PfxTaskArg *arg)
{
PfxUpdateRigidStatesParam &param = *((PfxUpdateRigidStatesParam*)arg->io);
PfxUInt32 iFirstBody = arg->data[0];
PfxUInt32 iEndBody = arg->data[1];
for(PfxUInt32 i = iFirstBody; i < iEndBody; i++)
{
pfxIntegrate(param.states[i],param.bodies[i],param.timeStep);
}
}
//----------------------------------------------------------------------------
// pfxUpdateRigidStates
//
/// Perform update rigid states in parallel using a task manager.
///
/// @param param Information about rigid bodies
/// @param taskManager Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param, PfxTaskManager *taskManager)
{
PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");
PfxUInt32 maxBatchSize = param.numRigidBodies / (PfxUInt32)(taskManager->getNumTasks());
PfxUInt32 iEnd = maxBatchSize, iStart = 0;
int task = 0;
taskManager->setTaskEntry((void*)pfxUpdateRigidStatesTaskEntry);
for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
{
taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numRigidBodies;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
// wait for tasks to complete
PfxUInt32 data1, data2, data3, data4;
for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
taskManager->waitTask(task, data1, data2, data3, data4);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

51
Extras/PhysicsEffects/src/low_level/solver/pfx_update_rigid_states_single.cpp Normal file
View File

@@ -0,0 +1,51 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/base_level/solver/pfx_integrate.h"
#include "../../../include/physics_effects/low_level/solver/pfx_update_rigid_states.h"
namespace sce {
namespace PhysicsEffects {
PfxInt32 pfxCheckParamOfUpdateRigidStates(const PfxUpdateRigidStatesParam &param)
{
if(!param.states || !param.bodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
if(!SCE_PFX_PTR_IS_ALIGNED16(param.states) || !SCE_PFX_PTR_IS_ALIGNED16(param.bodies)) return SCE_PFX_ERR_INVALID_ALIGN;
return SCE_PFX_OK;
}
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param)
{
PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");
for(PfxUInt32 i=0;i<param.numRigidBodies;i++) {
pfxIntegrate(param.states[i],param.bodies[i],param.timeStep);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

56
Extras/PhysicsEffects/src/low_level/sort/pfx_parallel_sort_single.cpp Normal file
View File

@@ -0,0 +1,56 @@
/*
Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
#include "../../../include/physics_effects/low_level/sort/pfx_parallel_sort.h"
#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
///////////////////////////////////////////////////////////////////////////////
// SINGLE THREAD
namespace sce {
namespace PhysicsEffects {
PfxInt32 pfxParallelSort(
PfxSortData16 *data,PfxUInt32 numData,
void *workBuff,PfxUInt32 workBytes)
{
if(!SCE_PFX_PTR_IS_ALIGNED16(workBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(workBuff,workBytes) < sizeof(PfxSortData16) * numData) return SCE_PFX_ERR_OUT_OF_BUFFER;
SCE_PFX_PUSH_MARKER("pfxParallelSort");
pfxSort(data,(PfxSortData16*)workBuff,numData);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
PfxInt32 pfxParallelSort(
PfxSortData32 *data,PfxUInt32 numData,
void *workBuff,PfxUInt32 workBytes)
{
if(!SCE_PFX_PTR_IS_ALIGNED16(workBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(workBuff,workBytes) < sizeof(PfxSortData32) * numData) return SCE_PFX_ERR_OUT_OF_BUFFER;
SCE_PFX_PUSH_MARKER("pfxParallelSort");
pfxSort(data,(PfxSortData32*)workBuff,numData);
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
} //namespace PhysicsEffects
} //namespace sce

237
Extras/PhysicsEffects/src/low_level/task/pfx_sync_components_pthreads.cpp Normal file
View File

@@ -0,0 +1,237 @@
/*
Applied Research Associates Inc. (c)2011
Redistribution and use in source and binary forms,
with or without modification, are permitted provided that the
following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Applied Research Associates Inc nor the names
of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifdef __ANDROID__
#include "../../../include/physics_effects/base_level/base/pfx_common.h"
#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
#include "pfx_sync_components_pthreads.h"
namespace sce {
namespace PhysicsEffects {
//----------------------------------------------------------------------------
// PfxPthreadsBarrier::PfxPthreadsBarrier
//
/// Default constructor
//----------------------------------------------------------------------------
PfxPthreadsBarrier::PfxPthreadsBarrier() :
m_maxThreads(0), m_called(0)
{
}
//----------------------------------------------------------------------------
// PfxPthreadsBarrier::~PfxPthreadsBarrier
//
/// Destructor
//----------------------------------------------------------------------------
PfxPthreadsBarrier::~PfxPthreadsBarrier()
{
if (m_maxThreads > 0)
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_destroy(&m_cond));
}
}
//----------------------------------------------------------------------------
// PfxPthreadsBarrier::sync
//
/// This function is used to sync m_numThreads worker threads. Each worker
/// should call sync() when it finishes a task. All workers will block until
/// the last worker also calls sync().
//----------------------------------------------------------------------------
void PfxPthreadsBarrier::sync()
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_lock(&m_mutex));
m_called++;
if (m_called == m_maxThreads)
{
// last thread to join broadcasts a condition that will release
// all the threads waiting at the barrier. The barrier is reset
// to be ready for the next sync point.
m_called = 0;
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_broadcast(&m_cond));
}
else
{
// First m_numThreads - 1 worker threads block on the condition.
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_wait(&m_cond, &m_mutex));
}
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_unlock(&m_mutex));
}
//----------------------------------------------------------------------------
// PfxPthreadsBarrier::setMaxCount
//
/// Set the number of threads that the barrier should wait for. This also
/// initializes a mutex and condition variable that are used to implement
/// the barrier.
///
/// @param n Number of threads that should wait at the barrier.
//----------------------------------------------------------------------------
void PfxPthreadsBarrier::setMaxCount(int n)
{
if (m_maxThreads > 0)
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_destroy(&m_cond));
}
m_called = 0;
if (0 < n)
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_init(&m_mutex,NULL));
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_init(&m_cond,NULL));
}
m_maxThreads = n;
}
//----------------------------------------------------------------------------
// PfxPthreadsBarrier::getMaxCount
//
/// Get the number of threads that the barrier will wait for.
///
/// @return The number of threads the barrier waits for
//----------------------------------------------------------------------------
int PfxPthreadsBarrier::getMaxCount()
{
return m_maxThreads;
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::PfxPthreadsCriticalSection
//
/// Default constructor
//----------------------------------------------------------------------------
PfxPthreadsCriticalSection::PfxPthreadsCriticalSection()
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_init(&m_mutex,NULL));
}
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::~PfxPthreadsCriticalSection
//
/// Destructor
//----------------------------------------------------------------------------
PfxPthreadsCriticalSection::~PfxPthreadsCriticalSection()
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
}
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::getSharedParam
//
/// Get the value of a shared parameter. Note that user must lock the
/// critical section before performing this action.
///
/// @param i index of shared parameter to retrieve. Must have value
/// between 0 and 31
///
/// @return Shared parameter value
//----------------------------------------------------------------------------
PfxUInt32 PfxPthreadsCriticalSection::getSharedParam(int i)
{
return(m_commonBuff[i]);
}
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::setSharedParam
//
/// Set the value of a shared parameter. Note that user must lock the
/// critical section before performing this action.
///
/// @param i index of shared parameter to set. Must have value
/// between 0 and 31
/// @param p Value to assign to shared parameter
//----------------------------------------------------------------------------
void PfxPthreadsCriticalSection::setSharedParam(int i,PfxUInt32 p)
{
m_commonBuff[i] = p;
}
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::lock
//
/// Lock the critical section
//----------------------------------------------------------------------------
void PfxPthreadsCriticalSection::lock()
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_lock(&m_mutex));
}
//----------------------------------------------------------------------------
// PfxPthreadsCriticalSection::lock
//
/// Unlock the critical section
//----------------------------------------------------------------------------
void PfxPthreadsCriticalSection::unlock()
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_unlock(&m_mutex));
}
//----------------------------------------------------------------------------
// Factory functions
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// PfxCreateBarrierPthreads
//
/// Factory function to create a pthreads-based barrier
///
/// @param n Max number of tasks
//----------------------------------------------------------------------------
PfxBarrier *PfxCreateBarrierPthreads(int n)
{
PfxPthreadsBarrier *barrier = new PfxPthreadsBarrier;
barrier->setMaxCount(n);
return(barrier);
}
//----------------------------------------------------------------------------
// PfxCreateCriticalSectionPthreads
//
/// Factory function to create a pthreads-based critical section
//----------------------------------------------------------------------------
PfxCriticalSection *PfxCreateCriticalSectionPthreads()
{
PfxPthreadsCriticalSection *cs = new PfxPthreadsCriticalSection;
return(cs);
}
} //namespace PhysicsEffects
} //namespace sce
#endif //__ANDROID__

93
Extras/PhysicsEffects/src/low_level/task/pfx_sync_components_pthreads.h Normal file
View File

@@ -0,0 +1,93 @@
/*
Applied Research Associates Inc. (c)2011
Redistribution and use in source and binary forms,
with or without modification, are permitted provided that the
following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Applied Research Associates Inc nor the names
of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H
#define _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H
#include "../../../include/physics_effects/base_level/base/pfx_common.h"
#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
#include <pthread.h>
#include <semaphore.h>
namespace sce {
namespace PhysicsEffects {
//----------------------------------------------------------------------------
// PfxPthreadsBarrier
//
/// Implementation of a barrier using pthreads. This version uses a mutex
/// and condition variable rather than using native pthreads barrier, which
/// enables it to be used on platforms that don't support pthreads barriers
/// (such as Android 2.3x)
//----------------------------------------------------------------------------
class PfxPthreadsBarrier : public PfxBarrier
{
public:
PfxPthreadsBarrier();
virtual ~PfxPthreadsBarrier();
// from PfxBarrier
virtual void sync();
virtual void setMaxCount(int n);
virtual int getMaxCount();
private:
pthread_mutex_t m_mutex; ///< Mutex used to block worker threads
pthread_cond_t m_cond; ///< Condition variable
int m_maxThreads; ///< Maximum number of worker threads
int m_called; ///< Number of worker threads waiting at barrier
};
//----------------------------------------------------------------------------
// PfxPthreadsBarrier
//
/// Implementation of a critical section using pthreads.
//----------------------------------------------------------------------------
class PfxPthreadsCriticalSection : public PfxCriticalSection
{
public:
PfxPthreadsCriticalSection();
virtual ~PfxPthreadsCriticalSection();
// from PfxCriticalSection
virtual PfxUInt32 getSharedParam(int i);
virtual void setSharedParam(int i,PfxUInt32 p);
virtual void lock();
virtual void unlock();
private:
pthread_mutex_t m_mutex; ///< Mutex used to implement lock
};
} //namespace PhysicsEffects
} //namespace sce
#endif // _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H

311
Extras/PhysicsEffects/src/low_level/task/pfx_task_manager_pthreads.cpp Normal file
View File

@@ -0,0 +1,311 @@
/*
Applied Research Associates Inc. (c)2011
Redistribution and use in source and binary forms,
with or without modification, are permitted provided that the
following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Applied Research Associates Inc nor the names
of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifdef __ANDROID__
#include "../../../include/physics_effects/base_level/base/pfx_common.h"
#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
#include "../../../include/physics_effects/low_level/task/pfx_task_manager.h"
#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
#include "pfx_sync_components_pthreads.h"
namespace sce {
namespace PhysicsEffects {
//----------------------------------------------------------------------------
// Standalone functions and structs
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// PfxPthreadsThreadData
//
/// Struct to store information needed by worker threads
//----------------------------------------------------------------------------
struct PfxPthreadsThreadData
{
// task runner information
PfxTaskArg *taskargument; ///< Pointer to argument for the task entry function
PfxTaskEntry taskEntry; ///< Pointer to current task entry function
// pthreads synchronization and thread info
pthread_t thread; ///< Current thread
sem_t semaphore; ///< Semaphore used to wake the thread
sem_t *taskmanagersemaphore; ///< Semaphore used to notify parent thread
};
//----------------------------------------------------------------------------
// PfxPthreadsThreadFunction
//
/// The thread function used for threads created and managed using a
/// PfxPthreadsThreadPool
//----------------------------------------------------------------------------
void *PfxPthreadsThreadFunction(void *argument)
{
PfxPthreadsThreadData *threaddata = (PfxPthreadsThreadData*)argument;
while (1)
{
// wait until a task is available
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&threaddata->semaphore));
// do work
if (threaddata->taskEntry)
threaddata->taskEntry(threaddata->taskargument);
// notify threadpool that task is done
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(threaddata->taskmanagersemaphore));
// If no task, then exit
if (!threaddata->taskEntry)
pthread_exit(0);
}
return 0;
}
//----------------------------------------------------------------------------
// Class definitions
//----------------------------------------------------------------------------
class PfxPthreadsTaskManager : public PfxTaskManager
{
public:
PfxPthreadsTaskManager(PfxUInt32 numTasks, PfxUInt32 maxTasks, void *workBuff, PfxUInt32 workBytes) :
PfxTaskManager(numTasks, maxTasks, workBuff, workBytes),
m_threads(NULL) {}
// from PfxTaskManager
virtual PfxUInt32 getSharedParam(int i);
virtual void setSharedParam(int i, PfxUInt32 p);
virtual void startTask(int taskId, void *io, PfxUInt32 data1, PfxUInt32 data2,
PfxUInt32 data3, PfxUInt32 data4);
virtual void waitTask(int &taskId, PfxUInt32 &data1, PfxUInt32 &data2,
PfxUInt32 &data3, PfxUInt32 &data4);
virtual void initialize();
virtual void finalize();
protected:
PfxPthreadsTaskManager() : PfxTaskManager(), m_threads(NULL) {}
private:
PfxPthreadsThreadData *m_threads; ///< Pointer to array of running threads (count is m_numThreads);
PfxPthreadsBarrier m_barrier; ///< Barrier used to sync task groups
PfxPthreadsCriticalSection m_cs; ///< Critical section used to manage shared parameters
sem_t m_taskmanagersemaphore; ///< Synchronization semaphore for task manager
};
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager
//
/// Implementation of a task manager using pthreads.
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::initialize
//
/// Initialize the task manager
//----------------------------------------------------------------------------
void PfxPthreadsTaskManager::initialize()
{
if (0 == m_maxTasks)
return;
if (m_threads) // already started
{
SCE_PFX_PRINTF("PfxPthreadsThreadPool attempt to start threads when they are already started, line %i, file %s\n", __LINE__, __FILE__);
return;
}
m_threads = (PfxPthreadsThreadData*)m_pool.allocate(sizeof(PfxPthreadsThreadData)*m_maxTasks);
if (!m_threads)
{
SCE_PFX_PRINTF("PfxPthreadsThreadPool unable to allocate threads at line %i in file %s\n", __LINE__, __FILE__);
return;
}
m_barrier.setMaxCount(m_maxTasks);
// Initialize sync semaphore
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_init(&m_taskmanagersemaphore, 0, 0));
// Allocate and start the threads
for (unsigned int i = 0; i < m_maxTasks; i++)
{
// Prepare argument data structure for task entry functions. Mostly, this is
// setting parameters that are fixed until the simulation ends
m_taskArg[i].taskId = i;
m_taskArg[i].maxTasks = m_maxTasks;
m_taskArg[i].barrier = &m_barrier;
m_taskArg[i].criticalSection = &m_cs;
m_taskArg[i].io = NULL;
// Prepare other per-thread data
m_threads[i].taskEntry = NULL;
m_threads[i].taskargument = &m_taskArg[i];
m_threads[i].taskmanagersemaphore = &m_taskmanagersemaphore;
// Now create the thread's semaphore, then start the thread
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_init(&m_threads[i].semaphore, 0, 0));
SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_create(&m_threads[i].thread, NULL, PfxPthreadsThreadFunction, (void*)&m_threads[i]));
}
}
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::finalize
//
/// Finalize the task manager
//----------------------------------------------------------------------------
void PfxPthreadsTaskManager::finalize()
{
// stop the threads
for (unsigned int i = 0; i < m_maxTasks; i++)
{
m_threads[i].taskEntry = NULL; // NULL task tells thread to exit
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(&m_threads[i].semaphore));
}
// wait for them all to exit
for (unsigned int i = 0; i < m_maxTasks; i++)
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&m_taskmanagersemaphore));
// destroy per-thread semaphores
for (unsigned int i = 0; i < m_maxTasks; i++)
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_destroy(&m_threads[i].semaphore));
// delete thread pool
delete [] m_threads;
m_threads = NULL;
// destroy task manager semaphore and reset barrier
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_destroy(&m_taskmanagersemaphore));
m_barrier.setMaxCount(0);
}
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::startTask
//
/// Start a task
///
/// @param taskId [in] task thread identifier/index
/// @param io [in, out] task input and output buffer
/// @param data1 [in] first of four user parameter data values for the task
/// @param data2 [in] second of four user parameter data values for the task
/// @param data3 [in] third of four user parameter data values for the task
/// @param data4 [in] fourth of four user parameter data values for the task
//----------------------------------------------------------------------------
void PfxPthreadsTaskManager::startTask(int taskId,void *io,PfxUInt32 data1,
PfxUInt32 data2,PfxUInt32 data3,PfxUInt32 data4)
{
m_threads[taskId].taskEntry = m_taskEntry;
m_taskArg[taskId].io = io;
m_taskArg[taskId].data[0] = data1;
m_taskArg[taskId].data[1] = data2;
m_taskArg[taskId].data[2] = data3;
m_taskArg[taskId].data[3] = data4;
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(&m_threads[taskId].semaphore));
}
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::waitTask
//
/// Wait for a task to finish
///
/// @param taskId [out] task thread identifier/index
/// @param data1 [out] first of four data values from the task
/// @param data2 [out] second of four data values from the task
/// @param data3 [out] third of four data values from the task
/// @param data4 [out] fourth of four data values from the task
//----------------------------------------------------------------------------
void PfxPthreadsTaskManager::waitTask(int &taskId,PfxUInt32 &data1,PfxUInt32 &data2,
PfxUInt32 &data3,PfxUInt32 &data4)
{
SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&m_taskmanagersemaphore));
}
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::getSharedParam
//
/// Get the value of a shared parameter
///
/// @param i index of shared parameter to retrieve. Must have value
/// between 0 and 31
///
/// @return Shared parameter value
//----------------------------------------------------------------------------
PfxUInt32 PfxPthreadsTaskManager::getSharedParam(int i)
{
m_cs.lock();
PfxUInt32 paramval = m_cs.getSharedParam(i);
m_cs.unlock();
return(paramval);
}
//----------------------------------------------------------------------------
// PfxPthreadsTaskManager::setSharedParam
//
/// Set the value of a shared parameter
///
/// @param i index of shared parameter to set. Must have value
/// between 0 and 31
/// @param p Value to assign to shared parameter
//----------------------------------------------------------------------------
void PfxPthreadsTaskManager::setSharedParam(int i, PfxUInt32 p)
{
m_cs.lock();
PfxUInt32 paramval = m_cs.getSharedParam(i);
m_cs.setSharedParam(i, p);
m_cs.unlock();
}
//----------------------------------------------------------------------------
// Factory functions
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// PfxCreateTaskManagerPthreads
//
/// Factory function to create a pthreads-based task manager
///
/// @param numTasks number of tasks
/// @param maxTasks max number of tasks
/// @param workBuff work buffer
/// @param workBytes size of work buffer, in bytes
//----------------------------------------------------------------------------
PfxTaskManager *PfxCreateTaskManagerPthreads(PfxUInt32 numTasks,PfxUInt32 maxTasks,
void *workBuff,PfxUInt32 workBytes)
{
PfxTaskManager *taskmanager = new PfxPthreadsTaskManager(numTasks, maxTasks,
workBuff, workBytes);
return(taskmanager);
}
} //namespace PhysicsEffects
} //namespace sce
#endif //__ANDROID__