1140 lines
32 KiB
C++
1140 lines
32 KiB
C++
|
|
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "particles_kernel.cuh"
|
|
#include "particleSystem.cuh"
|
|
#include "radixsort.cuh"
|
|
#include "vector_functions.h"
|
|
#include <stdio.h>
|
|
|
|
#ifdef WIN32//for glut.h
|
|
#include <windows.h>
|
|
#endif
|
|
|
|
#include <GL/glew.h>
|
|
//think different
|
|
#if defined(__APPLE__) && !defined (VMDMESA)
|
|
#include <OpenGL/gl.h>
|
|
#include <OpenGL/glu.h>
|
|
#include <GLUT/glut.h>
|
|
#else
|
|
#include <GL/glut.h>
|
|
#endif
|
|
|
|
|
|
|
|
#define USE_SORT 1
|
|
|
|
#include "btCudaBroadphase.h"
|
|
#include "LinearMath/btAlignedAllocator.h"
|
|
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
|
|
|
|
btCudaBroadphase::btCudaBroadphase(SimParams& simParams,int maxProxies) :
|
|
btSimpleBroadphase(maxProxies,
|
|
new (btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16)) btSortedOverlappingPairCache),
|
|
m_bInitialized(false),
|
|
m_numParticles(simParams.numBodies),
|
|
m_hPos(0),
|
|
m_hVel(0),
|
|
m_currentPosRead(0),
|
|
m_currentVelRead(0),
|
|
m_currentPosWrite(1),
|
|
m_currentVelWrite(1),
|
|
m_maxParticlesPerCell(4),
|
|
m_simParams(simParams)
|
|
{
|
|
m_ownsPairCache = true;
|
|
|
|
m_dPos[0] = m_dPos[1] = 0;
|
|
m_dVel[0] = m_dVel[1] = 0;
|
|
|
|
m_simParams.gridSize.x = 64;
|
|
m_simParams.gridSize.y = 64;
|
|
m_simParams.gridSize.z = 64;
|
|
|
|
|
|
m_simParams.numCells = m_simParams.gridSize.x*m_simParams.gridSize.y*m_simParams.gridSize.z;
|
|
m_simParams.worldSize = make_float3(2.0f, 2.0f, 2.0f);
|
|
|
|
// set simulation parameters
|
|
|
|
m_simParams.numBodies = m_numParticles;
|
|
m_simParams.maxParticlesPerCell = m_maxParticlesPerCell;
|
|
|
|
m_simParams.worldOrigin = make_float3(-1.0f, -1.0f, -1.0f);
|
|
m_simParams.cellSize = make_float3(m_simParams.worldSize.x / m_simParams.gridSize.x, m_simParams.worldSize.y / m_simParams.gridSize.y, m_simParams.worldSize.z / m_simParams.gridSize.z);
|
|
|
|
m_simParams.particleRadius = m_simParams.cellSize.x * 0.5f;
|
|
m_simParams.colliderPos = make_float4(-1.2f, -0.8f, 0.8f, 1.0f);
|
|
m_simParams.colliderRadius = 0.2f;
|
|
|
|
m_simParams.spring = 0.5f;
|
|
m_simParams.damping = 0.02f;
|
|
m_simParams.shear = 0.1f;
|
|
m_simParams.attraction = 0.0f;
|
|
m_simParams.boundaryDamping = -0.5f;
|
|
|
|
m_simParams.gravity = make_float3(0.0f, -0.0003f, 0.0f);
|
|
m_simParams.globalDamping = 1.0f;
|
|
|
|
_initialize(m_numParticles);
|
|
|
|
}
|
|
|
|
static inline float lerp(float a, float b, float t)
|
|
{
|
|
return a + t*(b-a);
|
|
}
|
|
|
|
static void colorRamp(float t, float *r)
|
|
{
|
|
const int ncolors = 7;
|
|
float c[ncolors][3] = {
|
|
{ 1.0, 0.0, 0.0, },
|
|
{ 1.0, 0.5, 0.0, },
|
|
{ 1.0, 1.0, 0.0, },
|
|
{ 0.0, 1.0, 0.0, },
|
|
{ 0.0, 1.0, 1.0, },
|
|
{ 0.0, 0.0, 1.0, },
|
|
{ 1.0, 0.0, 1.0, },
|
|
};
|
|
t = t * (ncolors-1);
|
|
int i = (int) t;
|
|
float u = t - floor(t);
|
|
r[0] = lerp(c[i][0], c[i+1][0], u);
|
|
r[1] = lerp(c[i][1], c[i+1][1], u);
|
|
r[2] = lerp(c[i][2], c[i+1][2], u);
|
|
}
|
|
|
|
|
|
unsigned int btCudaBroadphase::createVBO(unsigned int size)
|
|
{
|
|
GLuint vbo;
|
|
glGenBuffers(1, &vbo);
|
|
glBindBuffer(GL_ARRAY_BUFFER, vbo);
|
|
glBufferData(GL_ARRAY_BUFFER, size, 0, GL_DYNAMIC_DRAW);
|
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
|
registerGLBufferObject(vbo);
|
|
return vbo;
|
|
}
|
|
|
|
|
|
void btCudaBroadphase::_initialize(int numParticles)
|
|
{
|
|
assert(!m_bInitialized);
|
|
|
|
// allocate host storage
|
|
m_hPos = new float[numParticles*4];
|
|
m_hVel = new float[numParticles*4];
|
|
m_hSortedPos = new float[numParticles*4];
|
|
memset(m_hPos, 0, numParticles*4*sizeof(float));
|
|
memset(m_hVel, 0, numParticles*4*sizeof(float));
|
|
memset(m_hSortedPos, 0, numParticles*4*sizeof(float));
|
|
|
|
m_hGridCounters = new uint[m_simParams.numCells];
|
|
m_hGridCells = new uint[m_simParams.numCells*m_maxParticlesPerCell];
|
|
memset(m_hGridCounters, 0, m_simParams.numCells*sizeof(uint));
|
|
memset(m_hGridCells, 0, m_simParams.numCells*m_maxParticlesPerCell*sizeof(uint));
|
|
|
|
m_hParticleHash = new uint[numParticles*2];
|
|
memset(m_hParticleHash, 0, numParticles*2*sizeof(uint));
|
|
|
|
m_hCellStart = new uint[m_simParams.numCells];
|
|
memset(m_hCellStart, 0, m_simParams.numCells*sizeof(uint));
|
|
|
|
// allocate GPU data
|
|
unsigned int memSize = sizeof(float) * 4 * m_numParticles;
|
|
|
|
m_posVbo[0] = createVBO(memSize);
|
|
m_posVbo[1] = createVBO(memSize);
|
|
|
|
allocateArray((void**)&m_dVel[0], memSize);
|
|
allocateArray((void**)&m_dVel[1], memSize);
|
|
|
|
allocateArray((void**)&m_dSortedPos, memSize);
|
|
allocateArray((void**)&m_dSortedVel, memSize);
|
|
|
|
#if USE_SORT
|
|
allocateArray((void**)&m_dParticleHash[0], m_numParticles*2*sizeof(uint));
|
|
allocateArray((void**)&m_dParticleHash[1], m_numParticles*2*sizeof(uint));
|
|
allocateArray((void**)&m_dCellStart, m_simParams.numCells*sizeof(uint));
|
|
#else
|
|
allocateArray((void**)&m_dGridCounters, m_numGridCells*sizeof(uint));
|
|
allocateArray((void**)&m_dGridCells, m_numGridCells*m_maxParticlesPerCell*sizeof(uint));
|
|
#endif
|
|
|
|
m_colorVBO = createVBO(m_numParticles*4*sizeof(float));
|
|
|
|
#if 1
|
|
// fill color buffer
|
|
glBindBufferARB(GL_ARRAY_BUFFER, m_colorVBO);
|
|
float *data = (float *) glMapBufferARB(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
|
|
float *ptr = data;
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
float t = i / (float) m_numParticles;
|
|
#if 0
|
|
*ptr++ = rand() / (float) RAND_MAX;
|
|
*ptr++ = rand() / (float) RAND_MAX;
|
|
*ptr++ = rand() / (float) RAND_MAX;
|
|
#else
|
|
colorRamp(t, ptr);
|
|
ptr+=3;
|
|
#endif
|
|
*ptr++ = 1.0f;
|
|
}
|
|
glUnmapBufferARB(GL_ARRAY_BUFFER);
|
|
#endif
|
|
|
|
|
|
setParameters(&m_simParams);
|
|
|
|
m_bInitialized = true;
|
|
}
|
|
|
|
|
|
|
|
void btCudaBroadphase::_finalize()
|
|
{
|
|
assert(m_bInitialized);
|
|
|
|
delete [] m_hPos;
|
|
delete [] m_hVel;
|
|
delete [] m_hSortedPos;
|
|
|
|
delete [] m_hGridCounters;
|
|
delete [] m_hGridCells;
|
|
|
|
freeArray(m_dVel[0]);
|
|
freeArray(m_dVel[1]);
|
|
|
|
freeArray(m_dSortedPos);
|
|
freeArray(m_dSortedVel);
|
|
|
|
#if USE_SORT
|
|
freeArray(m_dParticleHash[0]);
|
|
freeArray(m_dParticleHash[1]);
|
|
freeArray(m_dCellStart);
|
|
#else
|
|
freeArray(m_dGridCounters);
|
|
freeArray(m_dGridCells);
|
|
#endif
|
|
|
|
unregisterGLBufferObject(m_posVbo[0]);
|
|
unregisterGLBufferObject(m_posVbo[1]);
|
|
glDeleteBuffers(2, (const GLuint*)m_posVbo);
|
|
|
|
glDeleteBuffers(1, (const GLuint*)&m_colorVBO);
|
|
}
|
|
|
|
btCudaBroadphase::~btCudaBroadphase()
|
|
{
|
|
//btSimpleBroadphase will free memory of btSortedOverlappingPairCache, because m_ownsPairCache
|
|
assert(m_bInitialized);
|
|
|
|
_finalize();
|
|
|
|
}
|
|
|
|
/*
|
|
int btCudaBroadphase::myCollideCell2(int3 gridPos,
|
|
uint index,
|
|
unsigned int* particleHash,
|
|
unsigned int* cellStart)
|
|
{
|
|
int numOverlap = 0;
|
|
|
|
|
|
if ((gridPos.x < 0) || (gridPos.x > params.gridSize.x-1) ||
|
|
(gridPos.y < 0) || (gridPos.y > params.gridSize.y-1) ||
|
|
(gridPos.z < 0) || (gridPos.z > params.gridSize.z-1)) {
|
|
return force;
|
|
}
|
|
|
|
uint gridHash = calcGridHash(gridPos);
|
|
|
|
// get start of bucket for this cell
|
|
uint bucketStart = FETCH(cellStart, gridHash);
|
|
if (bucketStart == 0xffffffff)
|
|
return force; // cell empty
|
|
|
|
// iterate over particles in this cell
|
|
for(uint i=0; i<params.maxParticlesPerCell; i++) {
|
|
uint index2 = bucketStart + i;
|
|
uint2 cellData = FETCH(particleHash, index2);
|
|
if (cellData.x != gridHash) break; // no longer in same bucket
|
|
|
|
if (index2 != index) { // check not colliding with self
|
|
float4 pos2 = FETCH(oldPos, index2);
|
|
float4 vel2 = FETCH(oldVel, index2);
|
|
|
|
// collide two spheres
|
|
float3 projVec = collideSpheres(pos, pos2, vel, vel2, params.particleRadius, params.particleRadius, params.attraction);
|
|
force += projVec;
|
|
}
|
|
}
|
|
|
|
return force;
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
void btCudaBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
|
|
{
|
|
//first check for new overlapping pairs
|
|
int j;
|
|
static int frameCount = 0;
|
|
//printf("framecount=%d\n",frameCount++);
|
|
|
|
|
|
int numRejected=0;
|
|
|
|
if (m_numHandles >= 0)
|
|
{
|
|
|
|
//#define _USE_BRUTEFORCE_N 1
|
|
#ifdef _USE_BRUTEFORCE_N
|
|
|
|
int i;
|
|
for (i=0;i<m_numHandles;i++)
|
|
{
|
|
btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
|
|
|
|
for (j=i+1;j<m_numHandles;j++)
|
|
{
|
|
btSimpleBroadphaseProxy* proxy1 = &m_pHandles[i];
|
|
|
|
if (proxy0 != proxy1)
|
|
{
|
|
btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
|
|
btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
|
|
|
|
if (aabbOverlap(p0,p1))
|
|
{
|
|
if ( !m_pairCache->findPair(proxy0,proxy1))
|
|
{
|
|
m_pairCache->addOverlappingPair(proxy0,proxy1);
|
|
}
|
|
} else
|
|
{
|
|
if (!m_pairCache->hasDeferredRemoval())
|
|
{
|
|
if ( m_pairCache->findPair(proxy0,proxy1))
|
|
{
|
|
m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
proxy1 = &m_pHandles[proxy1->GetNextAllocated()];
|
|
|
|
}
|
|
proxy0 = &m_pHandles[proxy0->GetNextAllocated()];
|
|
|
|
}
|
|
#else //_USE_BRUTEFORCE_N
|
|
|
|
// update constants
|
|
setParameters(&m_simParams);
|
|
|
|
float deltaTime = 1./60.f;
|
|
|
|
/*
|
|
|
|
// integrate
|
|
integrateSystem(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
deltaTime,
|
|
m_numParticles);
|
|
|
|
|
|
|
|
|
|
btSwap(m_currentPosRead, m_currentPosWrite);
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
*/
|
|
|
|
#if USE_SORT
|
|
// sort and search method
|
|
|
|
// calculate hash
|
|
calcHash(m_posVbo[m_currentPosRead],
|
|
m_dParticleHash[0],
|
|
m_numParticles);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
// sort particles based on hash
|
|
RadixSort((KeyValuePair *) m_dParticleHash[0], (KeyValuePair *) m_dParticleHash[1], m_numParticles, 32);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash sorted:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
|
|
// reorder particle arrays into sorted order and
|
|
// find start of each cell
|
|
reorderDataAndFindCellStart(m_dParticleHash[0],
|
|
m_posVbo[m_currentPosRead],
|
|
m_dVel[m_currentVelRead],
|
|
m_dSortedPos,
|
|
m_dSortedVel,
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells);
|
|
|
|
//#define DEBUG_GRID2
|
|
#ifdef DEBUG_GRID2
|
|
copyArrayFromDevice((void *) m_hCellStart, (void *) m_dCellStart, 0, sizeof(uint)*m_simParams.numCells);
|
|
printf("cell start:\n");
|
|
for(uint i=0; i<16; i++) {
|
|
printf("%d: %d//", i, m_hCellStart[i]);
|
|
}
|
|
#endif
|
|
|
|
#else
|
|
// update grid using atomics
|
|
updateGrid(m_posVbo[m_currentPosRead],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_numParticles,
|
|
m_numGridCells);
|
|
#endif
|
|
|
|
/*
|
|
dsadsa
|
|
*/
|
|
|
|
|
|
/*
|
|
int m_solverIterations = 1;
|
|
|
|
// process collisions
|
|
for(uint i=0; i<m_solverIterations; i++) {
|
|
collide(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dSortedPos, m_dSortedVel,
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_dParticleHash[0],
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells,
|
|
m_maxParticlesPerCell
|
|
);
|
|
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
|
|
}
|
|
*/
|
|
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
copyArrayFromDevice((void *) m_hCellStart, (void *) m_dCellStart, 0, sizeof(uint)*m_simParams.numCells);
|
|
copyArrayFromDevice((void *) m_hSortedPos, (void*) m_dSortedPos,0 , sizeof(float)*4*m_numParticles);
|
|
|
|
//#define DEBUG_INDICES 1
|
|
#ifdef DEBUG_INDICES
|
|
{
|
|
printf("cell start:\n");
|
|
for(uint i=0; i<16; i++) {
|
|
printf("%d: %d\n", i, m_hCellStart[i]);
|
|
}
|
|
}
|
|
{
|
|
printf("particle hash sorted:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
}
|
|
#endif //DEBUG_INDICES
|
|
|
|
{
|
|
// printf("cell start:\n");
|
|
// for(uint i=0; i<m_simParams.numCells; i++) {
|
|
// printf("%d: %d\n", i, m_hCellStart[i]);
|
|
// }
|
|
}
|
|
|
|
//printf("particle hash sorted:\n");
|
|
for(uint pi=0; pi<m_numParticles; pi++)
|
|
{
|
|
int index = m_hParticleHash[pi*2+1];
|
|
|
|
//printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
//perform an AABB check?
|
|
// examine only neighbouring cells
|
|
|
|
|
|
|
|
btSimpleBroadphaseProxy* proxy0 = &m_pHandles[index];
|
|
btVector3 mypos = (proxy0->m_min+proxy0->m_max)*0.5f;
|
|
|
|
// float4* p = (float4*)&m_hSortedPos[index*4];
|
|
|
|
|
|
int3 particleGridPos;
|
|
particleGridPos.x = floor((mypos.x() - m_simParams.worldOrigin.x) / m_simParams.cellSize.x);
|
|
particleGridPos.y = floor((mypos.y() - m_simParams.worldOrigin.y) / m_simParams.cellSize.y);
|
|
particleGridPos.z = floor((mypos.z() - m_simParams.worldOrigin.z) / m_simParams.cellSize.z);
|
|
|
|
|
|
//for(int z=0; z<1; z++)
|
|
for(int z=-1; z<=1; z++)
|
|
{
|
|
// for(int y=0; y<1; y++)
|
|
for(int y=-1; y<=1; y++)
|
|
{
|
|
// for(int x=0; x<1; x++)
|
|
for(int x=-1; x<=1; x++)
|
|
{
|
|
int3 gridPos;
|
|
gridPos.x = particleGridPos.x + x;
|
|
gridPos.y = particleGridPos.y + y;
|
|
gridPos.z = particleGridPos.z + z;
|
|
|
|
if ((gridPos.x < 0) || (gridPos.x > m_simParams.gridSize.x-1) ||
|
|
(gridPos.y < 0) || (gridPos.y > m_simParams.gridSize.y-1) ||
|
|
(gridPos.z < 0) || (gridPos.z > m_simParams.gridSize.z-1))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
|
|
gridPos.x = max(0, min(gridPos.x, m_simParams.gridSize.x-1));
|
|
gridPos.y = max(0, min(gridPos.y, m_simParams.gridSize.y-1));
|
|
gridPos.z = max(0, min(gridPos.z, m_simParams.gridSize.z-1));
|
|
uint gridHash = ((gridPos.z*m_simParams.gridSize.y)* m_simParams.gridSize.x) + (gridPos.y* m_simParams.gridSize.x) + gridPos.x;
|
|
|
|
// get start of bucket for this cell
|
|
unsigned int bucketStart = m_hCellStart[gridHash];
|
|
if (bucketStart == 0xffffffff)
|
|
continue;
|
|
|
|
// iterate over particles in this cell
|
|
for(uint q=0; q<m_simParams.maxParticlesPerCell; q++)
|
|
{
|
|
///add overlap with planes
|
|
|
|
|
|
uint cellIndex2 = bucketStart + q;
|
|
int cellData = m_hParticleHash[cellIndex2*2];
|
|
if (cellData != gridHash)
|
|
break; // no longer in same bucket
|
|
|
|
int particleIndex2 = m_hParticleHash[cellIndex2*2+1];
|
|
if (particleIndex2!= index && particleIndex2<index)
|
|
{ // check not colliding with self
|
|
//add an overlapping pair
|
|
//printf("add pair (%d,%d)\n",particleIndex2,index);
|
|
btSimpleBroadphaseProxy* proxy1 = &m_pHandles[particleIndex2];
|
|
|
|
//do a more exact AABB overlap test before adding the pair
|
|
bool hasOverlap = testAabbOverlap(proxy0,proxy1);
|
|
if (hasOverlap)
|
|
m_pairCache->addOverlappingPair(proxy0,proxy1);
|
|
else
|
|
{
|
|
numRejected++;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//int numOverlap += myCollideCell2(gridPos + make_int3(x, y, z), index, pos, vel, oldPos, oldVel, particleHash, cellStart);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
#endif //_USE_BRUTEFORCE_N
|
|
|
|
///if this broadphase is used in a btMultiSapBroadphase, we shouldn't sort the overlapping paircache
|
|
if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
|
|
{
|
|
|
|
btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
|
|
|
|
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
|
|
//overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
|
|
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
|
//printf("A) overlappingPairArray.size()=%d\n",overlappingPairArray.size());
|
|
|
|
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
|
m_invalidPair = 0;
|
|
|
|
|
|
btBroadphasePair previousPair;
|
|
previousPair.m_pProxy0 = 0;
|
|
previousPair.m_pProxy1 = 0;
|
|
previousPair.m_algorithm = 0;
|
|
|
|
|
|
int i;
|
|
for (i=0;i<overlappingPairArray.size();i++)
|
|
{
|
|
|
|
btBroadphasePair& pair = overlappingPairArray[i];
|
|
|
|
bool isDuplicate = (pair == previousPair);
|
|
|
|
previousPair = pair;
|
|
|
|
bool needsRemoval = false;
|
|
|
|
if (!isDuplicate)
|
|
{
|
|
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
|
|
|
|
if (hasOverlap)
|
|
{
|
|
needsRemoval = false;//callback->processOverlap(pair);
|
|
} else
|
|
{
|
|
needsRemoval = true;
|
|
}
|
|
} else
|
|
{
|
|
//remove duplicate
|
|
needsRemoval = true;
|
|
//should have no algorithm
|
|
btAssert(!pair.m_algorithm);
|
|
}
|
|
|
|
if (needsRemoval)
|
|
{
|
|
m_pairCache->cleanOverlappingPair(pair,dispatcher);
|
|
|
|
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
|
|
// m_overlappingPairArray.pop_back();
|
|
pair.m_pProxy0 = 0;
|
|
pair.m_pProxy1 = 0;
|
|
m_invalidPair++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
///if you don't like to skip the invalid pairs in the array, execute following code:
|
|
#define CLEAN_INVALID_PAIRS 1
|
|
#ifdef CLEAN_INVALID_PAIRS
|
|
|
|
//perform a sort, to sort 'invalid' pairs to the end
|
|
//overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
|
|
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
|
//printf("B) overlappingPairArray.size()=%d\n",overlappingPairArray.size());
|
|
|
|
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
|
// printf("C) overlappingPairArray.size()=%d\n",overlappingPairArray.size());
|
|
m_invalidPair = 0;
|
|
#endif//CLEAN_INVALID_PAIRS
|
|
|
|
}
|
|
}
|
|
|
|
//printf("numRejected=%d\n",numRejected);
|
|
}
|
|
|
|
static inline float frand()
|
|
{
|
|
return rand() / (float) RAND_MAX;
|
|
}
|
|
|
|
|
|
void btCudaBroadphase::initGrid(unsigned int* size, float spacing, float jitter, unsigned int numParticles)
|
|
{
|
|
srand(1973);
|
|
#ifdef CONTROLLED_START
|
|
float extra=0.01f;
|
|
for(uint z=0; z<size[2]; z++) {
|
|
for(uint y=0; y<size[1]; y++) {
|
|
for(uint x=0; x<size[0]; x++) {
|
|
uint i = (z*size[1]*size[0]) + (y*size[0]) + x;
|
|
if (i < numParticles) {
|
|
m_hPos[i*4] = (spacing * x) + m_simParams.particleRadius - 1.0f+extra;//+ (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+1] = (spacing * y) + m_simParams.particleRadius - 1.0f;//+ (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+2] = (spacing * z) + m_simParams.particleRadius - 1.0f;//+ (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+3] = 1.0f;
|
|
extra=0.f;
|
|
|
|
m_hVel[i*4] = 0.0f;
|
|
m_hVel[i*4+1] = 0.0f;
|
|
m_hVel[i*4+2] = 0.0f;
|
|
m_hVel[i*4+3] = 0.0f;
|
|
}
|
|
}
|
|
extra=0.f;
|
|
}
|
|
}
|
|
#else
|
|
for(uint z=0; z<size[2]; z++) {
|
|
for(uint y=0; y<size[1]; y++) {
|
|
for(uint x=0; x<size[0]; x++) {
|
|
uint i = (z*size[1]*size[0]) + (y*size[0]) + x;
|
|
if (i < numParticles) {
|
|
m_hPos[i*4] = (spacing * x) + m_simParams.particleRadius - 1.0f + (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+1] = (spacing * y) + m_simParams.particleRadius - 1.0f + (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+2] = (spacing * z) + m_simParams.particleRadius - 1.0f + (frand()*2.0f-1.0f)*jitter;
|
|
m_hPos[i*4+3] = 1.0f;
|
|
|
|
m_hVel[i*4] = 0.0f;
|
|
m_hVel[i*4+1] = 0.0f;
|
|
m_hVel[i*4+2] = 0.0f;
|
|
m_hVel[i*4+3] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void btCudaBroadphase::reset(ParticleConfig config)
|
|
{
|
|
switch(config)
|
|
{
|
|
default:
|
|
case CONFIG_RANDOM:
|
|
{
|
|
int p = 0, v = 0;
|
|
for(uint i=0; i < m_numParticles; i++)
|
|
{
|
|
float point[3];
|
|
point[0] = frand();
|
|
point[1] = frand();
|
|
point[2] = frand();
|
|
m_hPos[p++] = 2 * (point[0] - 0.5f);
|
|
m_hPos[p++] = 2 * (point[1] - 0.5f);
|
|
m_hPos[p++] = 2 * (point[2] - 0.5f);
|
|
m_hPos[p++] = 1.0f; // radius
|
|
m_hVel[v++] = 0.0f;
|
|
m_hVel[v++] = 0.0f;
|
|
m_hVel[v++] = 0.0f;
|
|
m_hVel[v++] = 0.0f;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case CONFIG_GRID:
|
|
{
|
|
float jitter = m_simParams.particleRadius*0.01f;
|
|
uint s = (int) ceilf(powf((float) m_numParticles, 1.0f / 3.0f));
|
|
uint gridSize[3];
|
|
gridSize[0] = gridSize[1] = gridSize[2] = s;
|
|
initGrid(gridSize, m_simParams.particleRadius*2.0f, jitter, m_numParticles);
|
|
}
|
|
break;
|
|
}
|
|
|
|
setArray(POSITION, m_hPos, 0, m_numParticles);
|
|
setArray(VELOCITY, m_hVel, 0, m_numParticles);
|
|
|
|
}
|
|
|
|
|
|
|
|
void btCudaBroadphase::addSphere(int start, float *pos, float *vel, int r, float spacing)
|
|
{
|
|
uint index = start;
|
|
for(int z=-r; z<=r; z++) {
|
|
for(int y=-r; y<=r; y++) {
|
|
for(int x=-r; x<=r; x++) {
|
|
float dx = x*spacing;
|
|
float dy = y*spacing;
|
|
float dz = z*spacing;
|
|
float l = sqrtf(dx*dx + dy*dy + dz*dz);
|
|
if ((l <= m_simParams.particleRadius*2.0f*r) && (index < m_numParticles)) {
|
|
m_hPos[index*4] = pos[0] + dx;
|
|
m_hPos[index*4+1] = pos[1] + dy;
|
|
m_hPos[index*4+2] = pos[2] + dz;
|
|
m_hPos[index*4+3] = pos[3];
|
|
|
|
m_hVel[index*4] = vel[0];
|
|
m_hVel[index*4+1] = vel[1];
|
|
m_hVel[index*4+2] = vel[2];
|
|
m_hVel[index*4+3] = vel[3];
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
setArray(POSITION, m_hPos, start, index);
|
|
setArray(VELOCITY, m_hVel, start, index);
|
|
}
|
|
|
|
|
|
void btCudaBroadphase::setArray(ParticleArray array, const float* data, int start, int count)
|
|
{
|
|
assert(m_bInitialized);
|
|
|
|
switch (array)
|
|
{
|
|
default:
|
|
case POSITION:
|
|
{
|
|
unregisterGLBufferObject(m_posVbo[m_currentPosRead]);
|
|
glBindBuffer(GL_ARRAY_BUFFER, m_posVbo[m_currentPosRead]);
|
|
glBufferSubData(GL_ARRAY_BUFFER, start*4*sizeof(float), count*4*sizeof(float), data);
|
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
|
registerGLBufferObject(m_posVbo[m_currentPosRead]);
|
|
}
|
|
break;
|
|
case VELOCITY:
|
|
copyArrayToDevice(m_dVel[m_currentVelRead], data, start*4*sizeof(float), count*4*sizeof(float));
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
float* btCudaBroadphase::getArray(ParticleArray array)
|
|
{
|
|
assert(m_bInitialized);
|
|
|
|
float* hdata = 0;
|
|
float* ddata = 0;
|
|
|
|
unsigned int vbo = 0;
|
|
|
|
switch (array)
|
|
{
|
|
default:
|
|
case POSITION:
|
|
hdata = m_hPos;
|
|
ddata = m_dPos[m_currentPosRead];
|
|
vbo = m_posVbo[m_currentPosRead];
|
|
break;
|
|
case VELOCITY:
|
|
hdata = m_hVel;
|
|
ddata = m_dVel[m_currentVelRead];
|
|
break;
|
|
}
|
|
|
|
copyArrayFromDevice(hdata, ddata, vbo, m_numParticles*4*sizeof(float));
|
|
return hdata;
|
|
}
|
|
|
|
void btCudaBroadphase::dumpGrid()
|
|
{
|
|
// debug
|
|
copyArrayFromDevice(m_hGridCounters, m_dGridCounters, 0, sizeof(uint)*m_simParams.numCells);
|
|
copyArrayFromDevice(m_hGridCells, m_dGridCells, 0, sizeof(uint)*m_simParams.numCells*m_maxParticlesPerCell);
|
|
uint total = 0;
|
|
uint maxPerCell = 0;
|
|
for(uint i=0; i<m_simParams.numCells; i++) {
|
|
if (m_hGridCounters[i] > maxPerCell)
|
|
maxPerCell = m_hGridCounters[i];
|
|
if (m_hGridCounters[i] > 0) {
|
|
printf("%d (%d): ", i, m_hGridCounters[i]);
|
|
for(uint j=0; j<m_hGridCounters[i]; j++) {
|
|
printf("%d ", m_hGridCells[i*m_maxParticlesPerCell + j]);
|
|
}
|
|
total += m_hGridCounters[i];
|
|
printf("\n");
|
|
}
|
|
}
|
|
printf("max per cell = %d\n", maxPerCell);
|
|
printf("total = %d\n", total);
|
|
}
|
|
|
|
void btCudaBroadphase::dumpParticles(unsigned int start, unsigned int count)
|
|
{
|
|
// debug
|
|
copyArrayFromDevice(m_hPos, 0, m_posVbo[m_currentPosRead], sizeof(float)*4*count);
|
|
copyArrayFromDevice(m_hVel, m_dVel[m_currentVelRead], 0, sizeof(float)*4*count);
|
|
|
|
for(uint i=start; i<start+count; i++) {
|
|
// printf("%d: ", i);
|
|
printf("pos: (%.4f, %.4f, %.4f, %.4f)\n", m_hPos[i*4+0], m_hPos[i*4+1], m_hPos[i*4+2], m_hPos[i*4+3]);
|
|
printf("vel: (%.4f, %.4f, %.4f, %.4f)\n", m_hVel[i*4+0], m_hVel[i*4+1], m_hVel[i*4+2], m_hVel[i*4+3]);
|
|
}
|
|
}
|
|
|
|
float* btCudaBroadphase::copyBuffersFromDeviceToHost()
|
|
{
|
|
// copyArrayFromDevice(m_hPos, 0, m_posVbo[m_currentPosRead], sizeof(float)*4*m_numParticles);
|
|
copyArrayFromDevice(m_hVel, m_dVel[m_currentVelRead], 0, sizeof(float)*4*m_numParticles);
|
|
// fill color buffer
|
|
glBindBufferARB(GL_ARRAY_BUFFER, m_posVbo[m_currentPosRead]);
|
|
float* hPosData = (float *) glMapBufferARB(GL_ARRAY_BUFFER, GL_READ_WRITE);//GL_WRITE_ONLY);
|
|
return hPosData;
|
|
}
|
|
|
|
void btCudaBroadphase::copyBuffersFromHostToDevice()
|
|
{
|
|
glUnmapBufferARB(GL_ARRAY_BUFFER);
|
|
copyArrayToDevice(m_dVel[m_currentVelRead],m_hVel, 0, sizeof(float)*4*m_numParticles);
|
|
}
|
|
|
|
float* btCudaBroadphase::getHvelPtr()
|
|
{
|
|
return m_hVel;
|
|
}
|
|
|
|
float* btCudaBroadphase::getHposPtr()
|
|
{
|
|
return m_hPos;
|
|
}
|
|
|
|
void btCudaBroadphase::quickHack(float deltaTime)
|
|
{
|
|
// update constants
|
|
setParameters(&m_simParams);
|
|
|
|
|
|
|
|
|
|
// integrate
|
|
integrateSystem(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
deltaTime,
|
|
m_numParticles);
|
|
|
|
|
|
|
|
|
|
btSwap(m_currentPosRead, m_currentPosWrite);
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
|
|
#if USE_SORT
|
|
// sort and search method
|
|
|
|
// calculate hash
|
|
calcHash(m_posVbo[m_currentPosRead],
|
|
m_dParticleHash[0],
|
|
m_numParticles);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
// sort particles based on hash
|
|
RadixSort((KeyValuePair *) m_dParticleHash[0], (KeyValuePair *) m_dParticleHash[1], m_numParticles, 32);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash sorted:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
// reorder particle arrays into sorted order and
|
|
// find start of each cell
|
|
reorderDataAndFindCellStart(m_dParticleHash[0],
|
|
m_posVbo[m_currentPosRead],
|
|
m_dVel[m_currentVelRead],
|
|
m_dSortedPos,
|
|
m_dSortedVel,
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells);
|
|
|
|
//#define DEBUG_GRID2
|
|
#ifdef DEBUG_GRID2
|
|
copyArrayFromDevice((void *) m_hCellStart, (void *) m_dCellStart, 0, sizeof(uint)*m_simParams.numCells);
|
|
printf("cell start:\n");
|
|
for(uint i=0; i<m_simParams.numCells; i++) {
|
|
printf("%d: %d\n", i, m_hCellStart[i]);
|
|
}
|
|
#endif
|
|
|
|
#else
|
|
// update grid using atomics
|
|
updateGrid(m_posVbo[m_currentPosRead],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_numParticles,
|
|
m_numGridCells);
|
|
#endif
|
|
|
|
/*
|
|
dsadsa
|
|
*/
|
|
|
|
|
|
|
|
int m_solverIterations = 1;
|
|
|
|
// process collisions
|
|
for(uint i=0; i<m_solverIterations; i++) {
|
|
collide(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dSortedPos, m_dSortedVel,
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_dParticleHash[0],
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells,
|
|
m_maxParticlesPerCell
|
|
);
|
|
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void btCudaBroadphase::integrate()
|
|
{
|
|
// update constants
|
|
setParameters(&m_simParams);
|
|
|
|
float deltaTime = 1./60.f;
|
|
|
|
|
|
// integrate
|
|
integrateSystem(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
deltaTime,
|
|
m_numParticles);
|
|
|
|
btSwap(m_currentPosRead, m_currentPosWrite);
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
}
|
|
|
|
void btCudaBroadphase::quickHack2()
|
|
{
|
|
// update constants
|
|
setParameters(&m_simParams);
|
|
|
|
|
|
// integrate
|
|
integrateSystem(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
0.f,
|
|
m_numParticles);
|
|
|
|
|
|
|
|
|
|
|
|
btSwap(m_currentPosRead, m_currentPosWrite);
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
|
|
#if USE_SORT
|
|
// sort and search method
|
|
|
|
// calculate hash
|
|
calcHash(m_posVbo[m_currentPosRead],
|
|
m_dParticleHash[0],
|
|
m_numParticles);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
// sort particles based on hash
|
|
RadixSort((KeyValuePair *) m_dParticleHash[0], (KeyValuePair *) m_dParticleHash[1], m_numParticles, 32);
|
|
|
|
#if DEBUG_GRID
|
|
copyArrayFromDevice((void *) m_hParticleHash, (void *) m_dParticleHash[0], 0, sizeof(uint)*2*m_numParticles);
|
|
printf("particle hash sorted:\n");
|
|
for(uint i=0; i<m_numParticles; i++) {
|
|
printf("%d: %d, %d\n", i, m_hParticleHash[i*2], m_hParticleHash[i*2+1]);
|
|
}
|
|
#endif
|
|
|
|
// reorder particle arrays into sorted order and
|
|
// find start of each cell
|
|
reorderDataAndFindCellStart(m_dParticleHash[0],
|
|
m_posVbo[m_currentPosRead],
|
|
m_dVel[m_currentVelRead],
|
|
m_dSortedPos,
|
|
m_dSortedVel,
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells);
|
|
|
|
//#define DEBUG_GRID2
|
|
#ifdef DEBUG_GRID2
|
|
copyArrayFromDevice((void *) m_hCellStart, (void *) m_dCellStart, 0, sizeof(uint)*m_simParams.numCells);
|
|
printf("cell start:\n");
|
|
for(uint i=0; i<m_simParams.numCells; i++) {
|
|
printf("%d: %d\n", i, m_hCellStart[i]);
|
|
}
|
|
#endif
|
|
|
|
#else
|
|
// update grid using atomics
|
|
updateGrid(m_posVbo[m_currentPosRead],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_numParticles,
|
|
m_numGridCells);
|
|
#endif
|
|
|
|
/*
|
|
dsadsa
|
|
*/
|
|
|
|
|
|
/*
|
|
int m_solverIterations = 1;
|
|
|
|
// process collisions
|
|
for(uint i=0; i<m_solverIterations; i++) {
|
|
collide(m_posVbo[m_currentPosRead], m_posVbo[m_currentPosWrite],
|
|
m_dSortedPos, m_dSortedVel,
|
|
m_dVel[m_currentVelRead], m_dVel[m_currentVelWrite],
|
|
m_dGridCounters,
|
|
m_dGridCells,
|
|
m_dParticleHash[0],
|
|
m_dCellStart,
|
|
m_numParticles,
|
|
m_simParams.numCells,
|
|
m_maxParticlesPerCell
|
|
);
|
|
|
|
btSwap(m_currentVelRead, m_currentVelWrite);
|
|
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
}
|