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

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

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