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

implement GPU breakable constraints

Browse Source 
add GPU fixed constraint
fix performance issue with concave meshes (didn't clear the number of concave-convex pairs, so it increased every frame)
This commit is contained in:
erwincoumans
2013-07-17 16:11:54 -07:00
parent 6d7d2531b5
commit ab125fbb6d
11 changed files with 477 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Demos3/GpuDemos/constraints/ConstraintsDemo.cpp
View File

@@ -144,7 +144,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
int constraintType=0; int constraintType=0;
for (int i=0;i<ci.arraySizeZ;i++) for (int i=0;i<ci.arraySizeZ;i++)
{ {
//constraintType=(constraintType+1)&0x11; constraintType=(constraintType+1)&0x01;
for (int k=0;k<ci.arraySizeX;k++) for (int k=0;k<ci.arraySizeX;k++)
{ {
@@ -167,7 +167,9 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
} }
//b3Vector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2); //b3Vector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
//b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ); //b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ);
b3Vector3 position(1+j*2.,10+i*ci.gapX,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ); b3Vector3 position(-ci.arraySizeY/2*2+1+j*2.,
10+i*ci.gapX,
(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ);
b3Quaternion orn(0,0,0,1); b3Quaternion orn(0,0,0,1);
@@ -189,10 +191,11 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
{ {
///enable next line to force CPU constraint solving ///enable next line to force CPU constraint solving
//c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(-1.1,0,0),b3Vector3(1.1,0,0)); //c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(-1.1,0,0),b3Vector3(1.1,0,0));
// c->setBreakingImpulseThreshold(14); float breakingThreshold=14;
// c->setBreakingImpulseThreshold(breakingThreshold);
b3Vector3 pivotInA(-1.1,0,0); b3Vector3 pivotInA(-1.1,0,0);
b3Vector3 pivotInB (1.1,0,0); b3Vector3 pivotInB (1.1,0,0);
int cid = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(pid,prevBody,pivotInA,pivotInB); int cid = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(pid,prevBody,pivotInA,pivotInB,breakingThreshold);
break; break;
} }
case 1: case 1:
@@ -209,8 +212,9 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
//c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); //c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
float breakingThreshold = 15;//37.f;
//c->setBreakingImpulseThreshold(37.1); //c->setBreakingImpulseThreshold(37.1);
int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(pid,prevBody,pivotInA,pivotInB,relTargetAB); int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(pid,prevBody,pivotInA,pivotInB,relTargetAB,breakingThreshold);

4
Demos3/GpuDemos/main_opengl3core.cpp
View File

@@ -62,6 +62,7 @@ static void MyResizeCallback( float width, float height)
b3gWindowInterface* window=0; b3gWindowInterface* window=0;
GwenUserInterface* gui = 0; GwenUserInterface* gui = 0;
bool gPause = false; bool gPause = false;
bool gStep = false;
bool gReset = false; bool gReset = false;
enum enum
@@ -925,7 +926,8 @@ int main(int argc, char* argv[])
} }
if (gStep)
gPause=true;
} while (!window->requestedExit() && !gReset); } while (!window->requestedExit() && !gReset);

2
Demos3/GpuDemos/rigidbody/ConcaveScene.cpp
View File

@@ -304,7 +304,7 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci)
//float camPos[4]={1,12.5,1.5,0}; //float camPos[4]={1,12.5,1.5,0};
m_instancingRenderer->setCameraPitch(45); m_instancingRenderer->setCameraPitch(45);
m_instancingRenderer->setCameraTargetPosition(camPos); m_instancingRenderer->setCameraTargetPosition(camPos);
m_instancingRenderer->setCameraDistance(25); m_instancingRenderer->setCameraDistance(155);
} }

7
Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp
View File

@@ -209,6 +209,7 @@ void GpuRigidBodyDemo::clientMoveAndDisplay()
{ {
B3_PROFILE("stepSimulation"); B3_PROFILE("stepSimulation");
m_data->m_rigidBodyPipeline->stepSimulation(1./60.f); m_data->m_rigidBodyPipeline->stepSimulation(1./60.f);
} }
if (numObjects) if (numObjects)
@@ -319,7 +320,8 @@ bool GpuRigidBodyDemo::mouseMoveCallback(float x,float y)
dir *= m_data->m_pickDistance; dir *= m_data->m_pickDistance;
newPivotB = rayFrom + dir; newPivotB = rayFrom + dir;
m_data->m_pickPivotInB = newPivotB; m_data->m_pickPivotInB = newPivotB;
m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(m_data->m_pickBody,m_data->m_pickFixedBody,m_data->m_pickPivotInA,m_data->m_pickPivotInB); m_data->m_rigidBodyPipeline->copyConstraintsToHost();
m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(m_data->m_pickBody,m_data->m_pickFixedBody,m_data->m_pickPivotInA,m_data->m_pickPivotInB,1e30);
m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
return true; return true;
} }
@@ -398,7 +400,8 @@ bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float
pivotInB.w = 0.f; pivotInB.w = 0.f;
m_data->m_pickPivotInA = pivotInA; m_data->m_pickPivotInA = pivotInA;
m_data->m_pickPivotInB = pivotInB; m_data->m_pickPivotInB = pivotInB;
m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(hitBodyA,m_data->m_pickFixedBody,pivotInA,pivotInB);//hitResults[0].m_hitResult0 m_data->m_rigidBodyPipeline->copyConstraintsToHost();
m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(hitBodyA,m_data->m_pickFixedBody,pivotInA,pivotInB,1e30);//hitResults[0].m_hitResult0
m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
m_data->m_np->writeAllBodiesToGpu(); m_data->m_np->writeAllBodiesToGpu();
m_data->m_pickDistance = (pivotInB-camPos).length(); m_data->m_pickDistance = (pivotInB-camPos).length();

4
src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
View File

@@ -1881,6 +1881,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
int concaveCapacity=maxTriConvexPairCapacity; int concaveCapacity=maxTriConvexPairCapacity;
m_concaveSepNormals.resize(concaveCapacity); m_concaveSepNormals.resize(concaveCapacity);
m_numConcavePairsOut.resize(0);
m_numConcavePairsOut.push_back(0); m_numConcavePairsOut.push_back(0);
int compoundPairCapacity=65536*10; int compoundPairCapacity=65536*10;
@@ -1939,7 +1940,10 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
if (treeNodesGPU->size() && treeNodesGPU->size()) if (treeNodesGPU->size() && treeNodesGPU->size())
{ {
B3_PROFILE("m_bvhTraversalKernel"); B3_PROFILE("m_bvhTraversalKernel");
numConcavePairs = m_numConcavePairsOut.at(0); numConcavePairs = m_numConcavePairsOut.at(0);
b3LauncherCL launcher(m_queue, m_bvhTraversalKernel); b3LauncherCL launcher(m_queue, m_bvhTraversalKernel);
launcher.setBuffer( pairs->getBufferCL()); launcher.setBuffer( pairs->getBufferCL());
launcher.setBuffer( bodyBuf->getBufferCL()); launcher.setBuffer( bodyBuf->getBufferCL());

205
src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp
View File

@@ -19,6 +19,7 @@ bool useGpuInfo1 = true;
bool useGpuInfo2= true; bool useGpuInfo2= true;
bool useGpuSolveJointConstraintRows=true; bool useGpuSolveJointConstraintRows=true;
bool useGpuWriteBackVelocities = true; bool useGpuWriteBackVelocities = true;
bool gpuBreakConstraints = true;
#include "b3GpuPgsJacobiSolver.h" #include "b3GpuPgsJacobiSolver.h"
@@ -55,8 +56,9 @@ struct b3GpuPgsJacobiSolverInternalData
cl_kernel m_initBatchConstraintsKernel; cl_kernel m_initBatchConstraintsKernel;
cl_kernel m_getInfo2Kernel; cl_kernel m_getInfo2Kernel;
cl_kernel m_writeBackVelocitiesKernel; cl_kernel m_writeBackVelocitiesKernel;
cl_kernel m_breakViolatedConstraintsKernel;
b3OpenCLArray<unsigned int>* m_dst; b3OpenCLArray<unsigned int>* m_gpuConstraintRowOffsets;
b3OpenCLArray<b3GpuSolverBody>* m_gpuSolverBodies; b3OpenCLArray<b3GpuSolverBody>* m_gpuSolverBodies;
b3OpenCLArray<b3BatchConstraint>* m_gpuBatchConstraints; b3OpenCLArray<b3BatchConstraint>* m_gpuBatchConstraints;
@@ -67,6 +69,7 @@ struct b3GpuPgsJacobiSolverInternalData
b3AlignedObjectArray<b3BatchConstraint> m_cpuBatchConstraints; b3AlignedObjectArray<b3BatchConstraint> m_cpuBatchConstraints;
b3AlignedObjectArray<b3GpuSolverConstraint> m_cpuConstraintRows; b3AlignedObjectArray<b3GpuSolverConstraint> m_cpuConstraintRows;
b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1; b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1;
b3AlignedObjectArray<unsigned int> m_cpuConstraintRowOffsets;
b3AlignedObjectArray<b3RigidBodyCL> m_cpuBodies; b3AlignedObjectArray<b3RigidBodyCL> m_cpuBodies;
b3AlignedObjectArray<b3InertiaCL> m_cpuInertias; b3AlignedObjectArray<b3InertiaCL> m_cpuInertias;
@@ -125,7 +128,7 @@ b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device,
m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx,device,queue); m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx,device,queue);
m_gpuData->m_dst = new b3OpenCLArray<unsigned int>(m_gpuData->m_context,m_gpuData->m_queue); m_gpuData->m_gpuConstraintRowOffsets = new b3OpenCLArray<unsigned int>(m_gpuData->m_context,m_gpuData->m_queue);
m_gpuData->m_gpuSolverBodies = new b3OpenCLArray<b3GpuSolverBody>(m_gpuData->m_context,m_gpuData->m_queue); m_gpuData->m_gpuSolverBodies = new b3OpenCLArray<b3GpuSolverBody>(m_gpuData->m_context,m_gpuData->m_queue);
m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray<b3BatchConstraint>(m_gpuData->m_context,m_gpuData->m_queue); m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray<b3BatchConstraint>(m_gpuData->m_context,m_gpuData->m_queue);
@@ -134,7 +137,8 @@ b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device,
cl_int errNum=0; cl_int errNum=0;
{ {
cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,&errNum,"",B3_JOINT_SOLVER_PATH,true); cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,&errNum,"",B3_JOINT_SOLVER_PATH);
//cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,0,&errNum,"",B3_JOINT_SOLVER_PATH,true);
b3Assert(errNum==CL_SUCCESS); b3Assert(errNum==CL_SUCCESS);
m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device,solveConstraintRowsCL, "solveJointConstraintRows",&errNum,prog); m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device,solveConstraintRowsCL, "solveJointConstraintRows",&errNum,prog);
b3Assert(errNum==CL_SUCCESS); b3Assert(errNum==CL_SUCCESS);
@@ -148,6 +152,8 @@ b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device,
b3Assert(errNum==CL_SUCCESS); b3Assert(errNum==CL_SUCCESS);
m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"writeBackVelocitiesKernel",&errNum,prog); m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"writeBackVelocitiesKernel",&errNum,prog);
b3Assert(errNum==CL_SUCCESS); b3Assert(errNum==CL_SUCCESS);
m_gpuData->m_breakViolatedConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"breakViolatedConstraintsKernel",&errNum,prog);
b3Assert(errNum==CL_SUCCESS);
@@ -166,9 +172,10 @@ b3GpuPgsJacobiSolver::~b3GpuPgsJacobiSolver ()
clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel); clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel);
clReleaseKernel(m_gpuData->m_getInfo2Kernel); clReleaseKernel(m_gpuData->m_getInfo2Kernel);
clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel); clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel);
clReleaseKernel(m_gpuData->m_breakViolatedConstraintsKernel);
delete m_gpuData->m_prefixScan; delete m_gpuData->m_prefixScan;
delete m_gpuData->m_dst; delete m_gpuData->m_gpuConstraintRowOffsets;
delete m_gpuData->m_gpuSolverBodies; delete m_gpuData->m_gpuSolverBodies;
delete m_gpuData->m_gpuBatchConstraints; delete m_gpuData->m_gpuBatchConstraints;
delete m_gpuData->m_gpuConstraintRows; delete m_gpuData->m_gpuConstraintRows;
@@ -181,14 +188,8 @@ struct b3BatchConstraint
{ {
int m_bodyAPtrAndSignBit; int m_bodyAPtrAndSignBit;
int m_bodyBPtrAndSignBit; int m_bodyBPtrAndSignBit;
int m_constraintRowOffset; int m_originalConstraintIndex;
short int m_numConstraintRows; int m_batchId;
short int m_batchId;
short int& getBatchIdx()
{
return m_batchId;
}
}; };
static b3AlignedObjectArray<b3BatchConstraint> batchConstraints; static b3AlignedObjectArray<b3BatchConstraint> batchConstraints;
@@ -234,7 +235,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setConst(numBodies); launcher.setConst(numBodies);
launcher.launch1D(numBodies); launcher.launch1D(numBodies);
//clFinish(m_gpuData->m_queue); clFinish(m_gpuData->m_queue);
// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
} else } else
@@ -270,15 +271,10 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
if (useGpuInfo1) if (useGpuInfo1)
{ {
B3_PROFILE("info1 and init batchConstraint"); B3_PROFILE("info1 and init batchConstraint");
m_gpuData->m_gpuConstraintInfo1->resize(numConstraints);
if (1)
{
m_gpuData->m_gpuConstraintInfo1->resize(numConstraints);
// gpuConstraints.resize(numConstraints);
// gpuConstraints.copyFromHostPointer(gpuConstraints,numConstraints);
// m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
}
if (1) if (1)
{ {
B3_PROFILE("getInfo1Kernel"); B3_PROFILE("getInfo1Kernel");
@@ -286,37 +282,35 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo1Kernel); b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo1Kernel);
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setConst(numConstraints); launcher.setConst(numConstraints);
launcher.launch1D(numConstraints); launcher.launch1D(numConstraints);
clFinish(m_gpuData->m_queue);
} }
//clFinish(m_gpuData->m_queue);
if (batches.size()==0)
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
if (1) if (batches.size()==0)
{ {
//m_gpuData->m_gpuConstraintInfo1->copyToHost(m_tmpConstraintSizesPool); B3_PROFILE("initBatchConstraintsKernel");
m_gpuData->m_dst->resize(numConstraints);
m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints);
unsigned int total=0; unsigned int total=0;
m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1,*m_gpuData->m_dst,numConstraints,&total); m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1,*m_gpuData->m_gpuConstraintRowOffsets,numConstraints,&total);
unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints-1); unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints-1);
//b3AlignedObjectArray<unsigned int> dstHost;
//dst.copyToHost(dstHost);
totalNumRows = total+lastElem; totalNumRows = total+lastElem;
{ {
B3_PROFILE("init batch constraints"); B3_PROFILE("init batch constraints");
b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initBatchConstraintsKernel); b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initBatchConstraintsKernel);
launcher.setBuffer(m_gpuData->m_dst->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setConst(numConstraints); launcher.setConst(numConstraints);
launcher.launch1D(numConstraints); launcher.launch1D(numConstraints);
//clFinish(m_gpuData->m_queue); clFinish(m_gpuData->m_queue);
} }
if (batches.size()==0) //assume the batching happens on CPU, so copy the data
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
} }
} }
else else
@@ -336,15 +330,10 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
{ {
info1 = 0; info1 = 0;
} }
/*b3Assert(info1Prev==info1);
b3Assert(batchConstraints[i].m_numConstraintRows==info1);
b3Assert(batchConstraints[i].m_constraintRowOffset==totalNumRows);
*/
batchConstraints[i].m_numConstraintRows = info1;
batchConstraints[i].m_constraintRowOffset = totalNumRows;
totalNumRows += info1; totalNumRows += info1;
} }
m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool);
@@ -361,6 +350,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo2Kernel); b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo2Kernel);
launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL());
@@ -373,7 +363,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
launcher.setConst(infoGlobal.m_numIterations); launcher.setConst(infoGlobal.m_numIterations);
launcher.setConst(numConstraints); launcher.setConst(numConstraints);
launcher.launch1D(numConstraints); launcher.launch1D(numConstraints);
//clFinish(m_gpuData->m_queue); clFinish(m_gpuData->m_queue);
if (batches.size()==0) if (batches.size()==0)
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
@@ -397,7 +387,10 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3Rigi
if (info1) if (info1)
{ {
b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[batchConstraints[i].m_constraintRowOffset]; int constraintIndex = batchConstraints[i].m_originalConstraintIndex;
int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[constraintIndex];
b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset];
b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i]; b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i];
b3RigidBodyCL& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; b3RigidBodyCL& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()];
@@ -685,13 +678,15 @@ void b3GpuPgsJacobiSolver::averageVelocities()
} }
b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuSolverConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{ {
//only create the batches once. //only create the batches once.
//@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated //@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated
B3_PROFILE("GpuSolveGroupCacheFriendlyIterations");
bool createBatches = batches.size()==0; bool createBatches = batches.size()==0;
{ {
B3_PROFILE("GpuSolveGroupCacheFriendlyIterations");
if (createBatches) if (createBatches)
{ {
@@ -711,7 +706,12 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
} }
} else } else
{ {
m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); /*b3AlignedObjectArray<b3BatchConstraint> cpuCheckBatches;
m_gpuData->m_gpuBatchConstraints->copyToHost(cpuCheckBatches);
b3Assert(cpuCheckBatches.size()==batchConstraints.size());
printf(".\n");
*/
//>copyFromHost(batchConstraints);
} }
int maxIterations = infoGlobal.m_numIterations; int maxIterations = infoGlobal.m_numIterations;
@@ -726,6 +726,10 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool);
m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1);
m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets);
gpuConstraints1->copyToHost(m_gpuData->m_cpuConstraints);
} }
for ( int iteration = 0 ; iteration< maxIterations ; iteration++) for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
@@ -742,16 +746,30 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
if (useGpuSolveJointConstraintRows) if (useGpuSolveJointConstraintRows)
{ {
B3_PROFILE("solveJointConstraintRowsKernels"); B3_PROFILE("solveJointConstraintRowsKernels");
/*
__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
__global b3BatchConstraint* batchConstraints,
__global b3SolverConstraint* rows,
__global unsigned int* numConstraintRowsInfo1,
__global unsigned int* rowOffsets,
__global b3GpuGenericConstraint* constraints,
int batchOffset,
int numConstraintsInBatch*/
b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_solveJointConstraintRowsKernels); b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_solveJointConstraintRowsKernels);
launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(gpuConstraints1->getBufferCL());//to detect disabled constraints
launcher.setConst(batchOffset); launcher.setConst(batchOffset);
launcher.setConst(constraintOffset);
launcher.setConst(numConstraintsInBatch); launcher.setConst(numConstraintsInBatch);
launcher.launch1D(numConstraintsInBatch); launcher.launch1D(numConstraintsInBatch);
//clFinish(m_gpuData->m_queue);
} else//useGpu } else//useGpu
{ {
@@ -766,17 +784,19 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
printf("c.batchId = %d\n", c.m_batchId); printf("c.batchId = %d\n", c.m_batchId);
*/ */
b3Assert(c.m_batchId==bb); b3Assert(c.m_batchId==bb);
b3GpuGenericConstraint* constraint = &m_gpuData->m_cpuConstraints[c.m_originalConstraintIndex];
if (constraint->m_flags&B3_CONSTRAINT_FLAG_ENABLED)
//can be done in parallel...
for (int jj=0;jj<c.m_numConstraintRows;jj++)
{ {
// int numConstraintRows = m_gpuData->m_cpuConstraintInfo1[c.m_originalConstraintIndex];
b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[c.m_constraintRowOffset+jj]; int constraintOffset = m_gpuData->m_cpuConstraintRowOffsets[c.m_originalConstraintIndex];
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA],&m_tmpSolverBodyPool[constraint.m_solverBodyIdB],&constraint); for (int jj=0;jj<numConstraintRows;jj++)
{
//
b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[constraintOffset+jj];
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA],&m_tmpSolverBodyPool[constraint.m_solverBodyIdB],&constraint);
}
} }
} }
}//useGpu }//useGpu
@@ -823,6 +843,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
} }
} }
clFinish(m_gpuData->m_queue);
return 0.f; return 0.f;
} }
@@ -866,7 +887,7 @@ inline int b3GpuPgsJacobiSolver::sortConstraintByBatch3( b3BatchConstraint* cs,
#if defined(_DEBUG) #if defined(_DEBUG)
for(int i=0; i<numConstraints; i++) for(int i=0; i<numConstraints; i++)
cs[i].getBatchIdx() = -1; cs[i].m_batchId = -1;
#endif #endif
int numValidConstraints = 0; int numValidConstraints = 0;
@@ -924,7 +945,7 @@ inline int b3GpuPgsJacobiSolver::sortConstraintByBatch3( b3BatchConstraint* cs,
curUsed[curBodyUsed++]=bodyB; curUsed[curBodyUsed++]=bodyB;
} }
cs[idx].getBatchIdx() = batchIdx; cs[idx].m_batchId = batchIdx;
if (i!=numValidConstraints) if (i!=numValidConstraints)
{ {
@@ -954,7 +975,7 @@ inline int b3GpuPgsJacobiSolver::sortConstraintByBatch3( b3BatchConstraint* cs,
// debugPrintf( "nBatches: %d\n", batchIdx ); // debugPrintf( "nBatches: %d\n", batchIdx );
for(int i=0; i<numConstraints; i++) for(int i=0; i<numConstraints; i++)
{ {
b3Assert( cs[i].getBatchIdx() != -1 ); b3Assert( cs[i].m_batchId != -1 );
} }
#endif #endif
@@ -978,9 +999,9 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodie
solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal); solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal);
solveGroupCacheFriendlyIterations(m_gpuData->m_gpuConstraintRows, numConstraints,infoGlobal); solveGroupCacheFriendlyIterations(gpuConstraints, numConstraints,infoGlobal);
solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias,numBodies, infoGlobal); solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias,numBodies, gpuConstraints, numConstraints, infoGlobal);
return 0.f; return 0.f;
} }
@@ -1007,13 +1028,62 @@ void b3GpuPgsJacobiSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyC
//b3AlignedObjectArray<b3RigidBodyCL> testBodies; //b3AlignedObjectArray<b3RigidBodyCL> testBodies;
b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,const b3ContactSolverInfo& infoGlobal) b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{ {
B3_PROFILE("solveGroupCacheFriendlyFinish"); B3_PROFILE("solveGroupCacheFriendlyFinish");
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
// int i,j; // int i,j;
{
if (gpuBreakConstraints)
{
B3_PROFILE("breakViolatedConstraintsKernel");
b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_breakViolatedConstraintsKernel);
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
launcher.setConst(numConstraints);
launcher.launch1D(numConstraints);
} else
{
gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints);
m_gpuData->m_gpuConstraintRows->copyToHost(m_gpuData->m_cpuConstraintRows);
gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1);
m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets);
for (int cid=0;cid<numConstraints;cid++)
{
int originalConstraintIndex = batchConstraints[cid].m_originalConstraintIndex;
int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[originalConstraintIndex];
int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex];
if (numRows)
{
// printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold);
for (int i=0;i<numRows;i++)
{
int rowIndex =constraintRowOffset+i;
int orgConstraintIndex = (int)m_gpuData->m_cpuConstraintRows[rowIndex].m_originalContactPoint;
float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold;
// printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
if (b3Fabs((m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold))
{
m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;
}
}
}
}
gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints);
}
}
{ {
if (useGpuWriteBackVelocities) if (useGpuWriteBackVelocities)
{ {
@@ -1024,7 +1094,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3Rig
launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
launcher.setConst(numBodies); launcher.setConst(numBodies);
launcher.launch1D(numBodies); launcher.launch1D(numBodies);
//clFinish(m_gpuData->m_queue); clFinish(m_gpuData->m_queue);
// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
// m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies); // m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies);
//m_gpuData->m_gpuBodies->copyToHost(testBodies); //m_gpuData->m_gpuBodies->copyToHost(testBodies);
@@ -1075,6 +1145,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3Rig
} }
} }
clFinish(m_gpuData->m_queue);
m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);

4
src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h
View File

@@ -62,9 +62,9 @@ public:
b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs); b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs);
virtual~b3GpuPgsJacobiSolver (); virtual~b3GpuPgsJacobiSolver ();
virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuSolverConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,const b3ContactSolverInfo& infoGlobal); b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);

14
src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
View File

@@ -170,7 +170,7 @@ void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid)
} }
} }
int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB) int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold)
{ {
m_data->m_gpuSolver->recomputeBatches(); m_data->m_gpuSolver->recomputeBatches();
b3GpuGenericConstraint c; b3GpuGenericConstraint c;
@@ -181,12 +181,12 @@ int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, co
c.m_rbB = bodyB; c.m_rbB = bodyB;
c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]);
c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]);
c.m_breakingImpulseThreshold = 1e30f; c.m_breakingImpulseThreshold = breakingThreshold;
c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE; c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE;
m_data->m_cpuConstraints.push_back(c); m_data->m_cpuConstraints.push_back(c);
return c.m_uid; return c.m_uid;
} }
int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB) int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB,float breakingThreshold)
{ {
m_data->m_gpuSolver->recomputeBatches(); m_data->m_gpuSolver->recomputeBatches();
b3GpuGenericConstraint c; b3GpuGenericConstraint c;
@@ -198,8 +198,9 @@ int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const fl
c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]);
c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]);
c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]); c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]);
c.m_breakingImpulseThreshold = 1e30f; c.m_breakingImpulseThreshold = breakingThreshold;
c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE; c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE;
m_data->m_cpuConstraints.push_back(c); m_data->m_cpuConstraints.push_back(c);
return c.m_uid; return c.m_uid;
} }
@@ -476,6 +477,11 @@ void b3GpuRigidBodyPipeline::setGravity(const float* grav)
m_data->m_gravity.setValue(grav[0],grav[1],grav[2]); m_data->m_gravity.setValue(grav[0],grav[1],grav[2]);
} }
void b3GpuRigidBodyPipeline::copyConstraintsToHost()
{
m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints);
}
void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() void b3GpuRigidBodyPipeline::writeAllInstancesToGpu()
{ {
m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);

5
src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h
View File

@@ -52,11 +52,12 @@ public:
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu); int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu);
//if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered //if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered
void writeAllInstancesToGpu(); void writeAllInstancesToGpu();
void copyConstraintsToHost();
void setGravity(const float* grav); void setGravity(const float* grav);
void reset(); void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB); int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB); int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
void removeConstraintByUid(int uid); void removeConstraintByUid(int uid);
void addConstraint(class b3TypedConstraint* constraint); void addConstraint(class b3TypedConstraint* constraint);

229
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
View File

@@ -13,11 +13,12 @@ subject to the following restrictions:
*/ */
//Originally written by Erwin Coumans //Originally written by Erwin Coumans
#define B3_CONSTRAINT_FLAG_ENABLED 1
#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3 #define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3
#define B3_GPU_FIXED_CONSTRAINT_TYPE 4 #define B3_GPU_FIXED_CONSTRAINT_TYPE 4
#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails
#define B3_INFINITY 1e30f #define B3_INFINITY 1e30f
#define mymake_float4 (float4) #define mymake_float4 (float4)
@@ -148,12 +149,8 @@ typedef struct
float m_lowerLimit; float m_lowerLimit;
float m_upperLimit; float m_upperLimit;
float m_rhsPenetration; float m_rhsPenetration;
int m_originalConstraint;
union
{
void* m_originalContactPoint;
float m_unusedPadding4;
};
int m_overrideNumSolverIterations; int m_overrideNumSolverIterations;
int m_frictionIndex; int m_frictionIndex;
@@ -162,20 +159,19 @@ typedef struct
} b3SolverConstraint; } b3SolverConstraint;
typedef struct typedef struct
{ {
int m_bodyAPtrAndSignBit; int m_bodyAPtrAndSignBit;
int m_bodyBPtrAndSignBit; int m_bodyBPtrAndSignBit;
int m_constraintRowOffset; int m_originalConstraintIndex;
short int m_numConstraintRows; int m_batchId;
short int m_batchId;
} b3BatchConstraint; } b3BatchConstraint;
typedef struct typedef struct
{ {
int m_constraintType; int m_constraintType;
@@ -304,12 +300,13 @@ void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global
} }
__kernel __kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
__global b3BatchConstraint* batchConstraints, __global b3BatchConstraint* batchConstraints,
__global b3SolverConstraint* rows, __global b3SolverConstraint* rows,
__global unsigned int* numConstraintRowsInfo1,
__global unsigned int* rowOffsets,
__global b3GpuGenericConstraint* constraints,
int batchOffset, int batchOffset,
int constraintOffset,
int numConstraintsInBatch int numConstraintsInBatch
) )
{ {
@@ -318,10 +315,16 @@ void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
return; return;
__global b3BatchConstraint* c = &batchConstraints[b+batchOffset]; __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];
for (int jj=0;jj<c->m_numConstraintRows;jj++) int originalConstraintIndex = c->m_originalConstraintIndex;
if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)
{ {
__global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+jj]; int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];
resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint); int rowOffset = rowOffsets[originalConstraintIndex];
for (int jj=0;jj<numConstraintRows;jj++)
{
__global b3SolverConstraint* constraint = &rows[rowOffset+jj];
resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);
}
} }
}; };
@@ -346,7 +349,31 @@ __kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b
solverBody->m_angularVelocity = bodyCL->m_angVel; solverBody->m_angularVelocity = bodyCL->m_angVel;
} }
__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, int numConstraints) __kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)
{
int cid = get_global_id(0);
if (cid>=numConstraints)
return;
int numRows = numConstraintRows[cid];
if (numRows)
{
// printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold);
for (int i=0;i<numRows;i++)
{
int rowIndex = rowOffsets[cid]+i;
float breakingThreshold = constraints[cid].m_breakingImpulseThreshold;
// printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold)
{
constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;
}
}
}
}
__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)
{ {
int i = get_global_id(0); int i = get_global_id(0);
if (i>=numConstraints) if (i>=numConstraints)
@@ -359,13 +386,11 @@ __kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGeneric
case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:
{ {
infos[i] = 3; infos[i] = 3;
batchConstraints[i].m_numConstraintRows = 3;
break; break;
} }
case B3_GPU_FIXED_CONSTRAINT_TYPE: case B3_GPU_FIXED_CONSTRAINT_TYPE:
{ {
infos[i] = 6; infos[i] = 6;
batchConstraints[i].m_numConstraintRows = 6;
break; break;
} }
default: default:
@@ -374,13 +399,24 @@ __kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGeneric
} }
} }
__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints) __kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets,
__global b3BatchConstraint* batchConstraints,
__global b3GpuGenericConstraint* constraints,
__global b3RigidBodyCL* bodies,
int numConstraints)
{ {
int i = get_global_id(0); int i = get_global_id(0);
if (i>=numConstraints) if (i>=numConstraints)
return; return;
batchConstraints[i].m_constraintRowOffset = rowOffsets[i]; int rbA = constraints[i].m_rbA;
int rbB = constraints[i].m_rbB;
batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass? rbA : -rbA;
batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass? rbB : -rbB;
batchConstraints[i].m_batchId = -1;
batchConstraints[i].m_originalConstraintIndex = i;
} }
@@ -509,23 +545,51 @@ void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstr
} }
} }
/* Quaternion nearest( Quaternion first, Quaternion qd)
@todo: convert this code to OpenCL
void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
{ {
Quaternion orn1 = orn0.nearest(orn1a); Quaternion diff,sum;
Quaternion dorn = orn1 * orn0.inverse(); diff = first- qd;
angle = dorn.getAngle(); sum = first + qd;
axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = b3Scalar(0.); if( dot(diff,diff) < dot(sum,sum) )
//check for axis length return qd;
b3Scalar len = axis.length2(); return (-qd);
if (len < B3_EPSILON*B3_EPSILON)
axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
else
axis /= b3Sqrt(len);
} }
*/
float b3Acos(float x)
{
if (x<-1)
x=-1;
if (x>1)
x=1;
return acos(x);
}
float getAngle(Quaternion orn)
{
if (orn.w>=1.f)
orn.w=1.f;
float s = 2.f * b3Acos(orn.w);
return s;
}
void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)
{
Quaternion orn1 = nearest(orn0,orn1a);
Quaternion dorn = qtMul(orn1,qtInvert(orn0));
*angle = getAngle(dorn);
*axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);
//check for axis length
float len = dot3F4(*axis,*axis);
if (len < FLT_EPSILON*FLT_EPSILON)
*axis = (float4)(1,0,0,0);
else
*axis /= sqrt(len);
}
void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row) void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)
{ {
@@ -545,21 +609,23 @@ void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuC
info->m_J2angularAxis[start_index + s+1] = -1; info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1; info->m_J2angularAxis[start_index + s*2+2] = -1;
} }
/*
@todo
float currERP = info->erp; float currERP = info->erp;
float k = info->fps * currERP; float k = info->fps * currERP;
float4 diff; float4 diff;
float angle; float angle;
float4 qrelCur = worldOrnA *qtInvert(worldOrnB); float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));
calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,diff,angle); calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);
diff*=-angle; diff*=-angle;
for (j=0; j<3; j++)
float* resultPtr = &diff;
for (int j=0; j<3; j++)
{ {
info->m_constraintError[(start_row+j)*info->rowskip] = k * diff[j]; info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];
} }
*/
} }
@@ -572,16 +638,21 @@ __kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global
if (bodies[i].m_invMass) if (bodies[i].m_invMass)
{ {
// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity; // if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)
// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity; {
bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity; bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;
bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity; }
// if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP)
{
bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;
}
} }
} }
__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, __kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows,
__global unsigned int* infos, __global unsigned int* infos,
__global unsigned int* constraintRowOffsets,
__global b3GpuGenericConstraint* constraints, __global b3GpuGenericConstraint* constraints,
__global b3BatchConstraint* batchConstraints, __global b3BatchConstraint* batchConstraints,
__global b3RigidBodyCL* bodies, __global b3RigidBodyCL* bodies,
@@ -599,42 +670,44 @@ __kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows,
if (i>=numConstraints) if (i>=numConstraints)
return; return;
//for now, always initialize the batch info
int info1 = infos[i]; int info1 = infos[i];
if (info1) __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];
__global b3GpuGenericConstraint* constraint = &constraints[i];
__global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];
__global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];
int solverBodyIdA = constraint->m_rbA;
int solverBodyIdB = constraint->m_rbB;
__global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];
__global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];
if (rbA->m_invMass)
{
batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;
} else
{ {
__global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset];
__global b3GpuGenericConstraint* constraint = &constraints[i];
__global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];
__global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];
int solverBodyIdA = constraint->m_rbA;
int solverBodyIdB = constraint->m_rbB;
__global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];
__global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];
if (rbA->m_invMass)
{
batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;
} else
{
// if (!solverBodyIdA) // if (!solverBodyIdA)
// m_staticIdx = 0; // m_staticIdx = 0;
batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;
} }
if (rbB->m_invMass) if (rbB->m_invMass)
{ {
batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;
} else } else
{ {
// if (!solverBodyIdB) // if (!solverBodyIdB)
// m_staticIdx = 0; // m_staticIdx = 0;
batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;
} }
if (info1)
{
int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) // if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
// m_maxOverrideNumSolverIterations = overrideNumSolverIterations; // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
@@ -656,7 +729,7 @@ __kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows,
currentConstraintRow[j].m_lowerLimit = 0.f; currentConstraintRow[j].m_lowerLimit = 0.f;
currentConstraintRow[j].m_upperLimit = 0.f; currentConstraintRow[j].m_upperLimit = 0.f;
currentConstraintRow[j].m_originalContactPoint = 0; currentConstraintRow[j].m_originalConstraint = i;
currentConstraintRow[j].m_overrideNumSolverIterations = 0; currentConstraintRow[j].m_overrideNumSolverIterations = 0;
currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0); currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);
currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0); currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);

229
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
View File

@@ -15,11 +15,12 @@ static const char* solveConstraintRowsCL= \
"*/\n" "*/\n"
"//Originally written by Erwin Coumans\n" "//Originally written by Erwin Coumans\n"
"\n" "\n"
"#define B3_CONSTRAINT_FLAG_ENABLED 1\n"
"\n" "\n"
"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n" "#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n"
"#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n" "#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n"
"\n" "\n"
"\n" "#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n"
"#define B3_INFINITY 1e30f\n" "#define B3_INFINITY 1e30f\n"
"\n" "\n"
"#define mymake_float4 (float4)\n" "#define mymake_float4 (float4)\n"
@@ -150,12 +151,8 @@ static const char* solveConstraintRowsCL= \
" float m_lowerLimit;\n" " float m_lowerLimit;\n"
" float m_upperLimit;\n" " float m_upperLimit;\n"
" float m_rhsPenetration;\n" " float m_rhsPenetration;\n"
" int m_originalConstraint;\n"
"\n" "\n"
" union\n"
" {\n"
" void* m_originalContactPoint;\n"
" float m_unusedPadding4;\n"
" };\n"
"\n" "\n"
" int m_overrideNumSolverIterations;\n" " int m_overrideNumSolverIterations;\n"
" int m_frictionIndex;\n" " int m_frictionIndex;\n"
@@ -164,20 +161,19 @@ static const char* solveConstraintRowsCL= \
"\n" "\n"
"} b3SolverConstraint;\n" "} b3SolverConstraint;\n"
"\n" "\n"
"typedef struct\n" "typedef struct \n"
"{\n" "{\n"
" int m_bodyAPtrAndSignBit;\n" " int m_bodyAPtrAndSignBit;\n"
" int m_bodyBPtrAndSignBit;\n" " int m_bodyBPtrAndSignBit;\n"
" int m_constraintRowOffset;\n" " int m_originalConstraintIndex;\n"
" short int m_numConstraintRows;\n" " int m_batchId;\n"
" short int m_batchId;\n"
"\n"
"} b3BatchConstraint;\n" "} b3BatchConstraint;\n"
"\n" "\n"
"\n" "\n"
"\n" "\n"
"\n" "\n"
"\n" "\n"
"\n"
"typedef struct \n" "typedef struct \n"
"{\n" "{\n"
" int m_constraintType;\n" " int m_constraintType;\n"
@@ -306,12 +302,13 @@ static const char* solveConstraintRowsCL= \
"\n" "\n"
"}\n" "}\n"
"\n" "\n"
"__kernel\n" "__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n"
"void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n"
" __global b3BatchConstraint* batchConstraints,\n" " __global b3BatchConstraint* batchConstraints,\n"
" __global b3SolverConstraint* rows,\n" " __global b3SolverConstraint* rows,\n"
" __global unsigned int* numConstraintRowsInfo1, \n"
" __global unsigned int* rowOffsets,\n"
" __global b3GpuGenericConstraint* constraints,\n"
" int batchOffset,\n" " int batchOffset,\n"
" int constraintOffset,\n"
" int numConstraintsInBatch\n" " int numConstraintsInBatch\n"
" )\n" " )\n"
"{\n" "{\n"
@@ -320,10 +317,16 @@ static const char* solveConstraintRowsCL= \
" return;\n" " return;\n"
"\n" "\n"
" __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n" " __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n"
" for (int jj=0;jj<c->m_numConstraintRows;jj++)\n" " int originalConstraintIndex = c->m_originalConstraintIndex;\n"
" if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n"
" {\n" " {\n"
" __global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+jj];\n" " int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];\n"
" resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n" " int rowOffset = rowOffsets[originalConstraintIndex];\n"
" for (int jj=0;jj<numConstraintRows;jj++)\n"
" {\n"
" __global b3SolverConstraint* constraint = &rows[rowOffset+jj];\n"
" resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n"
" }\n"
" }\n" " }\n"
"};\n" "};\n"
"\n" "\n"
@@ -348,7 +351,31 @@ static const char* solveConstraintRowsCL= \
" solverBody->m_angularVelocity = bodyCL->m_angVel;\n" " solverBody->m_angularVelocity = bodyCL->m_angVel;\n"
"}\n" "}\n"
"\n" "\n"
"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, int numConstraints)\n" "__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n"
"{\n"
" int cid = get_global_id(0);\n"
" if (cid>=numConstraints)\n"
" return;\n"
" int numRows = numConstraintRows[cid];\n"
" if (numRows)\n"
" {\n"
" // printf(\"cid=%d, breakingThreshold =%f\n\",cid,breakingThreshold);\n"
" for (int i=0;i<numRows;i++)\n"
" {\n"
" int rowIndex = rowOffsets[cid]+i;\n"
" float breakingThreshold = constraints[cid].m_breakingImpulseThreshold;\n"
" // printf(\"rows[%d].m_appliedImpulse=%f\n\",rowIndex,rows[rowIndex].m_appliedImpulse);\n"
" if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold)\n"
" {\n"
" constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;\n"
" }\n"
" }\n"
" }\n"
"}\n"
"\n"
"\n"
"\n"
"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n"
"{\n" "{\n"
" int i = get_global_id(0);\n" " int i = get_global_id(0);\n"
" if (i>=numConstraints)\n" " if (i>=numConstraints)\n"
@@ -361,13 +388,11 @@ static const char* solveConstraintRowsCL= \
" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" " case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
" {\n" " {\n"
" infos[i] = 3;\n" " infos[i] = 3;\n"
" batchConstraints[i].m_numConstraintRows = 3;\n"
" break;\n" " break;\n"
" }\n" " }\n"
" case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" " case B3_GPU_FIXED_CONSTRAINT_TYPE:\n"
" {\n" " {\n"
" infos[i] = 6;\n" " infos[i] = 6;\n"
" batchConstraints[i].m_numConstraintRows = 6;\n"
" break;\n" " break;\n"
" }\n" " }\n"
" default:\n" " default:\n"
@@ -376,13 +401,24 @@ static const char* solveConstraintRowsCL= \
" }\n" " }\n"
"}\n" "}\n"
"\n" "\n"
"__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints)\n" "__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n"
" __global b3BatchConstraint* batchConstraints, \n"
" __global b3GpuGenericConstraint* constraints,\n"
" __global b3RigidBodyCL* bodies,\n"
" int numConstraints)\n"
"{\n" "{\n"
" int i = get_global_id(0);\n" " int i = get_global_id(0);\n"
" if (i>=numConstraints)\n" " if (i>=numConstraints)\n"
" return;\n" " return;\n"
"\n" "\n"
" batchConstraints[i].m_constraintRowOffset = rowOffsets[i];\n" " int rbA = constraints[i].m_rbA;\n"
" int rbB = constraints[i].m_rbB;\n"
"\n"
" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass? rbA : -rbA;\n"
" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass? rbB : -rbB;\n"
" batchConstraints[i].m_batchId = -1;\n"
" batchConstraints[i].m_originalConstraintIndex = i;\n"
"\n"
"}\n" "}\n"
"\n" "\n"
"\n" "\n"
@@ -511,23 +547,51 @@ static const char* solveConstraintRowsCL= \
" }\n" " }\n"
"}\n" "}\n"
"\n" "\n"
"/*\n" "Quaternion nearest( Quaternion first, Quaternion qd)\n"
"@todo: convert this code to OpenCL\n"
"void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)\n"
"{\n" "{\n"
" Quaternion orn1 = orn0.nearest(orn1a);\n" " Quaternion diff,sum;\n"
" Quaternion dorn = orn1 * orn0.inverse();\n" " diff = first- qd;\n"
" angle = dorn.getAngle();\n" " sum = first + qd;\n"
" axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());\n" " \n"
" axis[3] = b3Scalar(0.);\n" " if( dot(diff,diff) < dot(sum,sum) )\n"
" //check for axis length\n" " return qd;\n"
" b3Scalar len = axis.length2();\n" " return (-qd);\n"
" if (len < B3_EPSILON*B3_EPSILON)\n"
" axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));\n"
" else\n"
" axis /= b3Sqrt(len);\n"
"}\n" "}\n"
"*/\n" "\n"
"float b3Acos(float x) \n"
"{ \n"
" if (x<-1) \n"
" x=-1; \n"
" if (x>1) \n"
" x=1;\n"
" return acos(x); \n"
"}\n"
"\n"
"float getAngle(Quaternion orn)\n"
"{\n"
" if (orn.w>=1.f)\n"
" orn.w=1.f;\n"
" float s = 2.f * b3Acos(orn.w);\n"
" return s;\n"
"}\n"
"\n"
"void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n"
"{\n"
" Quaternion orn1 = nearest(orn0,orn1a);\n"
" \n"
" Quaternion dorn = qtMul(orn1,qtInvert(orn0));\n"
" *angle = getAngle(dorn);\n"
" *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);\n"
" \n"
" //check for axis length\n"
" float len = dot3F4(*axis,*axis);\n"
" if (len < FLT_EPSILON*FLT_EPSILON)\n"
" *axis = (float4)(1,0,0,0);\n"
" else\n"
" *axis /= sqrt(len);\n"
"}\n"
"\n"
"\n"
"\n" "\n"
"void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n" "void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n"
"{\n" "{\n"
@@ -547,21 +611,23 @@ static const char* solveConstraintRowsCL= \
" info->m_J2angularAxis[start_index + s+1] = -1;\n" " info->m_J2angularAxis[start_index + s+1] = -1;\n"
" info->m_J2angularAxis[start_index + s*2+2] = -1;\n" " info->m_J2angularAxis[start_index + s*2+2] = -1;\n"
" }\n" " }\n"
" /*\n" " \n"
" @todo\n"
" float currERP = info->erp;\n" " float currERP = info->erp;\n"
" float k = info->fps * currERP;\n" " float k = info->fps * currERP;\n"
" float4 diff;\n" " float4 diff;\n"
" float angle;\n" " float angle;\n"
" float4 qrelCur = worldOrnA *qtInvert(worldOrnB);\n" " float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));\n"
"\n" " \n"
" calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,diff,angle);\n" " calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);\n"
" diff*=-angle;\n" " diff*=-angle;\n"
" for (j=0; j<3; j++)\n" " \n"
" float* resultPtr = &diff;\n"
" \n"
" for (int j=0; j<3; j++)\n"
" {\n" " {\n"
" info->m_constraintError[(start_row+j)*info->rowskip] = k * diff[j];\n" " info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n"
" }\n" " }\n"
" */\n" " \n"
"\n" "\n"
"}\n" "}\n"
"\n" "\n"
@@ -574,16 +640,21 @@ static const char* solveConstraintRowsCL= \
"\n" "\n"
" if (bodies[i].m_invMass)\n" " if (bodies[i].m_invMass)\n"
" {\n" " {\n"
"// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity;\n" "// if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)\n"
"// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity;\n" " {\n"
" bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n" " bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n"
" bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n" " }\n"
"// if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP)\n"
" {\n"
" bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n"
" } \n"
" }\n" " }\n"
"}\n" "}\n"
"\n" "\n"
"\n" "\n"
"__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n" "__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n"
" __global unsigned int* infos, \n" " __global unsigned int* infos, \n"
" __global unsigned int* constraintRowOffsets, \n"
" __global b3GpuGenericConstraint* constraints, \n" " __global b3GpuGenericConstraint* constraints, \n"
" __global b3BatchConstraint* batchConstraints, \n" " __global b3BatchConstraint* batchConstraints, \n"
" __global b3RigidBodyCL* bodies,\n" " __global b3RigidBodyCL* bodies,\n"
@@ -601,42 +672,44 @@ static const char* solveConstraintRowsCL= \
" if (i>=numConstraints)\n" " if (i>=numConstraints)\n"
" return;\n" " return;\n"
" \n" " \n"
" //for now, always initialize the batch info\n"
" int info1 = infos[i];\n" " int info1 = infos[i];\n"
" \n" " \n"
" if (info1)\n" " __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n"
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
"\n"
" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n"
" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n"
"\n"
" int solverBodyIdA = constraint->m_rbA;\n"
" int solverBodyIdB = constraint->m_rbB;\n"
"\n"
" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n"
" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n"
"\n"
"\n"
" if (rbA->m_invMass)\n"
" {\n"
" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n"
" } else\n"
" {\n" " {\n"
" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset];\n"
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
"\n"
" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n"
" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n"
"\n"
" int solverBodyIdA = constraint->m_rbA;\n"
" int solverBodyIdB = constraint->m_rbB;\n"
"\n"
" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n"
" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n"
"\n"
" if (rbA->m_invMass)\n"
" {\n"
" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n"
" } else\n"
" {\n"
"// if (!solverBodyIdA)\n" "// if (!solverBodyIdA)\n"
"// m_staticIdx = 0;\n" "// m_staticIdx = 0;\n"
" batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n" " batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n"
" }\n" " }\n"
"\n" "\n"
" if (rbB->m_invMass)\n" " if (rbB->m_invMass)\n"
" {\n" " {\n"
" batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n" " batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n"
" } else\n" " } else\n"
" {\n" " {\n"
"// if (!solverBodyIdB)\n" "// if (!solverBodyIdB)\n"
"// m_staticIdx = 0;\n" "// m_staticIdx = 0;\n"
" batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n" " batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n"
" }\n" " }\n"
"\n" "\n"
" if (info1)\n"
" {\n"
" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n" " int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n"
"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n" "// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n"
" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n" " // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n"
@@ -658,7 +731,7 @@ static const char* solveConstraintRowsCL= \
" currentConstraintRow[j].m_lowerLimit = 0.f;\n" " currentConstraintRow[j].m_lowerLimit = 0.f;\n"
" currentConstraintRow[j].m_upperLimit = 0.f;\n" " currentConstraintRow[j].m_upperLimit = 0.f;\n"
"\n" "\n"
" currentConstraintRow[j].m_originalContactPoint = 0;\n" " currentConstraintRow[j].m_originalConstraint = i;\n"
" currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n" " currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n"
" currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n" " currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n"
" currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);\n" " currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);\n"