diff --git a/build/stringify.bat b/build/stringify.bat index 8a4e0659d..af503bada 100644 --- a/build/stringify.bat +++ b/build/stringify.bat @@ -22,6 +22,8 @@ premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernel premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/solverSetup2.cl" --headerfile="../opencl/gpu_rigidbody/kernels/solverSetup2.h" --stringname="solverSetup2CL" stringify premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/batchingKernels.cl" --headerfile="../opencl/gpu_rigidbody/kernels/batchingKernels.h" --stringname="batchingKernelsCL" stringify premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/batchingKernelsNew.cl" --headerfile="../opencl/gpu_rigidbody/kernels/batchingKernelsNew.h" --stringname="batchingKernelsNewCL" stringify +premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/solverUtils.cl" --headerfile="../opencl/gpu_rigidbody/kernels/solverUtils.h" --stringname="solverUtilsCL" stringify + premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/solveContact.cl" --headerfile="../opencl/gpu_rigidbody/kernels/solveContact.h" --stringname="solveContactCL" stringify premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/solveFriction.cl" --headerfile="../opencl/gpu_rigidbody/kernels/solveFriction.h" --stringname="solveFrictionCL" stringify diff --git a/demo/gpudemo/GpuDemo.h b/demo/gpudemo/GpuDemo.h index 29301eadc..e65296d39 100644 --- a/demo/gpudemo/GpuDemo.h +++ b/demo/gpudemo/GpuDemo.h @@ -37,9 +37,9 @@ public: :useOpenCL(true), preferredOpenCLPlatformIndex(-1), preferredOpenCLDeviceIndex(-1), - arraySizeX(30), - arraySizeY(30 ), - arraySizeZ(30), + arraySizeX(41), + arraySizeY(44), + arraySizeZ(41), m_useConcaveMesh(false), gapX(14.3), gapY(14.0), diff --git a/demo/gpudemo/rigidbody/GpuConvexScene.cpp b/demo/gpudemo/rigidbody/GpuConvexScene.cpp index 5d08d48b8..9767e51d1 100644 --- a/demo/gpudemo/rigidbody/GpuConvexScene.cpp +++ b/demo/gpudemo/rigidbody/GpuConvexScene.cpp @@ -66,6 +66,7 @@ void GpuConvexScene::setupScene(const ConstructionInfo& ci) float mass = j==0? 0.f : 1.f; btVector3 position((j&1)+i*2.2,2+j*2.,(j&1)+k*2.2); + btQuaternion orn(1,0,0,0); btVector4 color = colors[curColor]; diff --git a/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp b/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp index 33878b2bd..e6e026a24 100644 --- a/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp +++ b/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp @@ -4,12 +4,46 @@ #include "parallel_primitives/host/btPrefixScanCL.h" #include "btGpuConstraint4.h" #include "BulletCommon/btQuickprof.h" +#include "../../parallel_primitives/host/btInt2.h" +#include "../../parallel_primitives/host/btFillCL.h" + + + +#include "../../parallel_primitives/host/btLauncherCL.h" + + +#include "../kernels/solverUtils.h" + +#define SOLVER_UTILS_KERNEL_PATH "opencl/gpu_rigidbody/kernels/solverUtils.cl" struct btGpuJacobiSolverInternalData { //btRadixSort32CL* m_sort32; //btBoundSearchCL* m_search; btPrefixScanCL* m_scan; + + btOpenCLArray* m_bodyCount; + btOpenCLArray* m_contactConstraintOffsets; + btOpenCLArray* m_offsetSplitBodies; + + btOpenCLArray* m_deltaLinearVelocities; + btOpenCLArray* m_deltaAngularVelocities; + + + btOpenCLArray* m_contactConstraints; + + btFillCL* m_filler; + + + cl_kernel m_countBodiesKernel; + cl_kernel m_contactToConstraintSplitKernel; + cl_kernel m_clearVelocitiesKernel; + + cl_kernel m_solveContactKernel; + cl_kernel m_solveFrictionKernel; + + + }; btGpuJacobiSolver::btGpuJacobiSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) @@ -19,10 +53,52 @@ btGpuJacobiSolver::btGpuJacobiSolver(cl_context ctx, cl_device_id device, cl_com { m_data = new btGpuJacobiSolverInternalData; m_data->m_scan = new btPrefixScanCL(m_context,m_device,m_queue); + m_data->m_bodyCount = new btOpenCLArray(m_context,m_queue); + m_data->m_filler = new btFillCL(m_context,m_device,m_queue); + m_data->m_contactConstraintOffsets = new btOpenCLArray(m_context,m_queue); + m_data->m_offsetSplitBodies = new btOpenCLArray(m_context,m_queue); + m_data->m_contactConstraints = new btOpenCLArray(m_context,m_queue); + m_data->m_deltaLinearVelocities = new btOpenCLArray(m_context,m_queue); + m_data->m_deltaAngularVelocities = new btOpenCLArray(m_context,m_queue); + + cl_int pErrNum; + const char* additionalMacros=""; + const char* solverUtilsSource = solverUtilsCL; + { + cl_program solverUtilsProg= btOpenCLUtils::compileCLProgramFromString( ctx, device, solverUtilsSource, &pErrNum,additionalMacros, SOLVER_UTILS_KERNEL_PATH); + btAssert(solverUtilsProg); + m_data->m_countBodiesKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "CountBodiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + btAssert(m_data->m_countBodiesKernel); + + m_data->m_contactToConstraintSplitKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ContactToConstraintSplitKernel", &pErrNum, solverUtilsProg,additionalMacros ); + btAssert(m_data->m_contactToConstraintSplitKernel); + m_data->m_clearVelocitiesKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ClearVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + btAssert(m_data->m_clearVelocitiesKernel); + + m_data->m_solveContactKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveContactJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + btAssert(m_data->m_solveContactKernel ); + + m_data->m_solveFrictionKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveFrictionJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + btAssert(m_data->m_solveFrictionKernel); + } + } btGpuJacobiSolver::~btGpuJacobiSolver() { + clReleaseKernel(m_data->m_solveContactKernel); + clReleaseKernel(m_data->m_solveFrictionKernel); + clReleaseKernel(m_data->m_countBodiesKernel); + clReleaseKernel(m_data->m_contactToConstraintSplitKernel); + clReleaseKernel(m_data->m_clearVelocitiesKernel ); + + delete m_data->m_deltaLinearVelocities; + delete m_data->m_deltaAngularVelocities; + delete m_data->m_contactConstraints; + delete m_data->m_offsetSplitBodies; + delete m_data->m_contactConstraintOffsets; + delete m_data->m_bodyCount; + delete m_data->m_filler; delete m_data->m_scan; delete m_data; } @@ -59,19 +135,12 @@ btVector4 make_float4(float x,float y, float z, float w) } -template -static -__inline -void solveContact(btGpuConstraint4& cs, - const btVector3& posA, btVector3& linVelA, btVector3& angVelA, float invMassA, const btMatrix3x3& invInertiaA, - const btVector3& posB, btVector3& linVelB, btVector3& angVelB, float invMassB, const btMatrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) +static __inline void solveContact(btGpuConstraint4& cs, + const btVector3& posA, const btVector3& linVelARO, const btVector3& angVelARO, float invMassA, const btMatrix3x3& invInertiaA, + const btVector3& posB, const btVector3& linVelBRO, const btVector3& angVelBRO, float invMassB, const btMatrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], btVector3& dLinVelA, btVector3& dAngVelA, btVector3& dLinVelB, btVector3& dAngVelB) { - btVector3 dLinVelA; dLinVelA.setZero(); - btVector3 dAngVelA; dAngVelA.setZero(); - btVector3 dLinVelB; dLinVelB.setZero(); - btVector3 dAngVelB; dAngVelB.setZero(); for(int ic=0; ic<4; ic++) { @@ -85,7 +154,7 @@ void solveContact(btGpuConstraint4& cs, setLinearAndAngular( (const btVector3 &)-cs.m_linear, (const btVector3 &)r0, (const btVector3 &)r1, linear, angular0, angular1 ); float rambdaDt = calcRelVel((const btVector3 &)cs.m_linear,(const btVector3 &) -cs.m_linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; + linVelARO+dLinVelA, angVelARO+dAngVelA, linVelBRO+dLinVelB, angVelBRO+dAngVelB ) + cs.m_b[ic]; rambdaDt *= cs.m_jacCoeffInv[ic]; { @@ -106,31 +175,19 @@ void solveContact(btGpuConstraint4& cs, btAssert(_finite(linImp0.x())); btAssert(_finite(linImp1.x())); #endif - if( JACOBI ) + + if (invMassA) { dLinVelA += linImp0; dAngVelA += angImp0; + } + if (invMassB) + { dLinVelB += linImp1; dAngVelB += angImp1; } - else - { - linVelA += linImp0; - angVelA += angImp0; - linVelB += linImp1; - angVelB += angImp1; - } } } - - if( JACOBI ) - { - linVelA += dLinVelA; - angVelA += dAngVelA; - linVelB += dLinVelB; - angVelB += dAngVelB; - } - } @@ -138,11 +195,16 @@ void solveContact(btGpuConstraint4& cs, static inline void solveFriction(btGpuConstraint4& cs, - const btVector3& posA, btVector3& linVelA, btVector3& angVelA, float invMassA, const btMatrix3x3& invInertiaA, - const btVector3& posB, btVector3& linVelB, btVector3& angVelB, float invMassB, const btMatrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) + const btVector3& posA, const btVector3& linVelARO, const btVector3& angVelARO, float invMassA, const btMatrix3x3& invInertiaA, + const btVector3& posB, const btVector3& linVelBRO, const btVector3& angVelBRO, float invMassB, const btMatrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], btVector3& dLinVelA, btVector3& dAngVelA, btVector3& dLinVelB, btVector3& dAngVelB) { + btVector3 linVelA = linVelARO+dLinVelA; + btVector3 linVelB = linVelBRO+dLinVelB; + btVector3 angVelA = angVelARO+dAngVelA; + btVector3 angVelB = angVelBRO+dAngVelB; + if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; const btVector3& center = (const btVector3&)cs.m_center; @@ -187,10 +249,16 @@ static inline void solveFriction(btGpuConstraint4& cs, btAssert(_finite(linImp0.x())); btAssert(_finite(linImp1.x())); #endif - linVelA += linImp0; - angVelA += angImp0; - linVelB += linImp1; - angVelB += angImp1; + if (invMassA) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + } + if (invMassB) + { + dLinVelB += linImp1; + dAngVelB += angImp1; + } } { // angular damping for point constraint @@ -201,8 +269,10 @@ static inline void solveFriction(btGpuConstraint4& cs, float angNA = btDot( n, angVelA ); float angNB = btDot( n, angVelB ); - angVelA -= (angNA*0.1f)*n; - angVelB -= (angNB*0.1f)*n; + if (invMassA) + dAngVelA -= (angNA*0.1f)*n; + if (invMassB) + dAngVelB -= (angNB*0.1f)*n; } } @@ -224,7 +294,7 @@ btVector3 mtMul3(const btVector3& a, const btMatrix3x3& b) float calcJacCoeff(const btVector3& linear0, const btVector3& linear1, const btVector3& angular0, const btVector3& angular1, - float invMass0, const btMatrix3x3* invInertia0, float invMass1, const btMatrix3x3* invInertia1) + float invMass0, const btMatrix3x3* invInertia0, float invMass1, const btMatrix3x3* invInertia1, float countA, float countB) { // linear0,1 are normlized float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; @@ -232,13 +302,13 @@ float calcJacCoeff(const btVector3& linear0, const btVector3& linear1, const btV float jmj1 = btDot(mtMul3(angular0,*invInertia0), angular0); float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; float jmj3 = btDot(mtMul3(angular1,*invInertia1), angular1); - return -1.f/(jmj0+jmj1+jmj2+jmj3); + return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB); } void setConstraint4( const btVector3& posA, const btVector3& linVelA, const btVector3& angVelA, float invMassA, const btMatrix3x3& invInertiaA, const btVector3& posB, const btVector3& linVelB, const btVector3& angVelB, float invMassB, const btMatrix3x3& invInertiaB, - btContact4* src, float dt, float positionDrift, float positionConstraintCoeff, + btContact4* src, float dt, float positionDrift, float positionConstraintCoeff, float countA, float countB, btGpuConstraint4* dstC ) { dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit); @@ -271,13 +341,16 @@ void setConstraint4( const btVector3& posA, const btVector3& linVelA, const btVe setLinearAndAngular(src->m_worldNormal, r0, r1, linear, angular0, angular1); dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); relVelN = calcRelVel(linear, -linear, angular0, angular1, linVelA, angVelA, linVelB, angVelB); float e = 0.f;//src->getRestituitionCoeff(); - if( relVelN*relVelN < 0.004f ) e = 0.f; + if( relVelN*relVelN < 0.004f ) + { + e = 0.f; + } dstC->m_b[ic] = e*relVelN; //float penetration = src->m_worldPos[ic].w; @@ -306,7 +379,7 @@ void setConstraint4( const btVector3& posA, const btVector3& linVelA, const btVe setLinearAndAngular(tangent[i], r[0], r[1], linear, angular0, angular1); dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); dstC->m_fAppliedRambdaDt[i] = 0.f; } dstC->m_center = center; @@ -330,7 +403,7 @@ void setConstraint4( const btVector3& posA, const btVector3& linVelA, const btVe void ContactToConstraintKernel(btContact4* gContact, btRigidBodyCL* gBodies, btInertiaCL* gShapes, btGpuConstraint4* gConstraintOut, int nContacts, float dt, float positionDrift, -float positionConstraintCoeff, int gIdx +float positionConstraintCoeff, int gIdx, btAlignedObjectArray& bodyCount ) { //int gIdx = 0;//GET_GLOBAL_IDX; @@ -353,9 +426,10 @@ float positionConstraintCoeff, int gIdx btMatrix3x3 invInertiaB = gShapes[bIdx].m_invInertiaWorld;//m_invInertia; btGpuConstraint4 cs; - + float countA = invMassA ? btScalar(bodyCount[aIdx]) : 1; + float countB = invMassB ? btScalar(bodyCount[bIdx]) : 1; setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, - &gContact[gIdx], dt, positionDrift, positionConstraintCoeff, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB, &cs ); @@ -367,15 +441,19 @@ float positionConstraintCoeff, int gIdx } -void btGpuJacobiSolver::solveGroup(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo) +void btGpuJacobiSolver::solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo) { BT_PROFILE("btGpuJacobiSolver::solveGroup"); - /* + btAlignedObjectArray bodyCount; bodyCount.resize(numBodies); for (int i=0;i contactConstraintOffsets; + contactConstraintOffsets.resize(numManifolds); + + for (int i=0;im_scan->executeHost(bodyCount,offsetSplitBodies,numBodies,&totalNumSplitBodies); + int numlastBody = bodyCount[numBodies-1]; + totalNumSplitBodies += numlastBody; - btAlignedObjectArray splitBodies; - //splitBodies.resize(); + - */ btAlignedObjectArray contactConstraints; @@ -416,13 +496,28 @@ void btGpuJacobiSolver::solveGroup(btRigidBodyCL* bodies,btInertiaCL* inertias,i ContactToConstraintKernel(&manifoldPtr[0],bodies,inertias,&contactConstraints[0],numManifolds, solverInfo.m_deltaTime, solverInfo.m_positionDrift, - solverInfo.m_positionConstraintCoeff,i); + solverInfo.m_positionConstraintCoeff, + i, bodyCount); } - int maxIter = 4; + int maxIter = 14; + + + btAlignedObjectArray deltaLinearVelocities; + btAlignedObjectArray deltaAngularVelocities; + deltaLinearVelocities.resize(totalNumSplitBodies); + deltaAngularVelocities.resize(totalNumSplitBodies); + for (int i=0;i( contactConstraints[i], (btVector3&)bodyA.m_pos, (btVector3&)bodyA.m_linVel, (btVector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, - (btVector3&)bodyB.m_pos, (btVector3&)bodyB.m_linVel, (btVector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); + solveContact( contactConstraints[i], (btVector3&)bodyA.m_pos, (btVector3&)bodyA.m_linVel, (btVector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, + (btVector3&)bodyB.m_pos, (btVector3&)bodyB.m_linVel, (btVector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr ); + + } } + + //easy + for (int i=0;i* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo) +{ + + BT_PROFILE("btGpuJacobiSolver::solveGroup"); + + int numBodies = bodies->size(); + int numManifolds = manifoldPtr->size(); + + m_data->m_bodyCount->resize(numBodies); + + unsigned int val=0; + btInt2 val2; + val2.x=0; + val2.y=0; + + { + BT_PROFILE("m_filler"); + m_data->m_contactConstraintOffsets->resize(numManifolds); + m_data->m_filler->execute(*m_data->m_bodyCount,val,numBodies); + + + m_data->m_filler->execute(*m_data->m_contactConstraintOffsets,val2,numManifolds); + } + + { + BT_PROFILE("m_countBodiesKernel"); + btLauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel); + launcher.setBuffer(manifoldPtr->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setConst(numManifolds); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.launch1D(numManifolds); + } + + unsigned int totalNumSplitBodies=0; + m_data->m_offsetSplitBodies->resize(numBodies); + m_data->m_scan->execute(*m_data->m_bodyCount,*m_data->m_offsetSplitBodies,numBodies,&totalNumSplitBodies); + totalNumSplitBodies+=m_data->m_bodyCount->at(numBodies-1); + + + int numContacts = manifoldPtr->size(); + m_data->m_contactConstraints->resize(numContacts); + + + { + BT_PROFILE("contactToConstraintSplitKernel"); + btLauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel); + launcher.setBuffer(manifoldPtr->getBufferCL()); + launcher.setBuffer(bodies->getBufferCL()); + launcher.setBuffer(inertias->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setConst(numContacts); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.launch1D( numContacts, 64 ); + clFinish(m_queue); + } + + + m_data->m_deltaLinearVelocities->resize(totalNumSplitBodies); + m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies); + + + + { + BT_PROFILE("m_clearVelocitiesKernel"); + btLauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel); + launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launch.setConst(totalNumSplitBodies); + launch.launch1D(totalNumSplitBodies); + } + + int maxIter = 4; + + for (int iter = 0;iterm_solveContactKernel ); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodies->getBufferCL()); + launcher.setBuffer(inertias->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + /* + for(int i=0; i* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo); }; #endif //BT_GPU_JACOBI_SOLVER_H diff --git a/opencl/gpu_rigidbody/host/btGpuRigidBodyPipeline.cpp b/opencl/gpu_rigidbody/host/btGpuRigidBodyPipeline.cpp index 38dc14e5d..98faf4f34 100644 --- a/opencl/gpu_rigidbody/host/btGpuRigidBodyPipeline.cpp +++ b/opencl/gpu_rigidbody/host/btGpuRigidBodyPipeline.cpp @@ -15,7 +15,7 @@ #include "btGpuBatchingPgsSolver.h" #include "Solver.h" #include "btGpuJacobiSolver.h" - +#include "BulletCommon/btQuickprof.h" #include "btConfig.h" btGpuRigidBodyPipeline::btGpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class btGpuNarrowPhase* narrowphase, class btGpuSapBroadphase* broadphaseSap ) @@ -111,23 +111,39 @@ void btGpuRigidBodyPipeline::stepSimulation(float deltaTime) btOpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); - bool useJacobi = false;//true; + bool useJacobi = true; if (useJacobi) { bool useGpu = true; if (useGpu) { - btAlignedObjectArray hostBodies; - gpuBodies.copyToHost(hostBodies); - btAlignedObjectArray hostInertias; - gpuInertias.copyToHost(hostInertias); - btAlignedObjectArray hostContacts; - gpuContacts.copyToHost(hostContacts); + bool forceHost = false; + if (forceHost) + { + btAlignedObjectArray hostBodies; + btAlignedObjectArray hostInertias; + btAlignedObjectArray hostContacts; + + { + BT_PROFILE("copyToHost"); + gpuBodies.copyToHost(hostBodies); + gpuInertias.copyToHost(hostInertias); + gpuContacts.copyToHost(hostContacts); + } + + { + btJacobiSolverInfo solverInfo; + m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo); + } + { + BT_PROFILE("copyFromHost"); + gpuBodies.copyFromHost(hostBodies); + } + } else { btJacobiSolverInfo solverInfo; - m_data->m_solver3->solveGroup(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo); + m_data->m_solver3->solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo); } - gpuBodies.copyFromHost(hostBodies); } else { btAlignedObjectArray hostBodies; diff --git a/opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp b/opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp index 0916cf8ee..10706407d 100644 --- a/opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp +++ b/opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp @@ -33,7 +33,7 @@ subject to the following restrictions: //#include "../../dynamics/basic_demo/Stubs/AdlContact4.h" #include "../../gpu_sat/host/btContact4.h" -bool usePgs = true; +bool usePgs = false;//true; int gNumSplitImpulseRecoveries2 = 0; #include "btRigidBody.h" @@ -42,11 +42,9 @@ int gNumSplitImpulseRecoveries2 = 0; btTransform getWorldTransform(btRigidBodyCL* rb) { - btVector3 pos(rb->m_pos[0],rb->m_pos[1],rb->m_pos[2]); - btQuaternion orn(rb->m_quat[0],rb->m_quat[1],rb->m_quat[2],rb->m_quat[3]); btTransform newTrans; - newTrans.setOrigin(pos); - newTrans.setRotation(orn); + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); return newTrans; } @@ -163,7 +161,7 @@ void btPgsJacobiSolver::solveContacts(int numBodies, btRigidBodyCL* bodies, btIn btContactSolverInfo infoGlobal; infoGlobal.m_splitImpulse = false; infoGlobal.m_timeStep = 1.f/60.f; - infoGlobal.m_numIterations = 10;//4; + infoGlobal.m_numIterations = 4;//4; // infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; //infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS; diff --git a/opencl/gpu_rigidbody/kernels/solverUtils.cl b/opencl/gpu_rigidbody/kernels/solverUtils.cl new file mode 100644 index 000000000..5fe68a5e2 --- /dev/null +++ b/opencl/gpu_rigidbody/kernels/solverUtils.cl @@ -0,0 +1,745 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +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. +*/ + + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +Matrix3x3 mtZero(); + +__inline +Matrix3x3 mtIdentity(); + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m); + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b); + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + +__inline +Matrix3x3 mtZero() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(0.f); + m.m_row[1] = (float4)(0.f); + m.m_row[2] = (float4)(0.f); + return m; +} + +__inline +Matrix3x3 mtIdentity() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(1,0,0,0); + m.m_row[1] = (float4)(0,1,0,0); + m.m_row[2] = (float4)(0,0,1,0); + return m; +} + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m) +{ + Matrix3x3 out; + out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b) +{ + Matrix3x3 transB; + transB = mtTranspose( b ); + Matrix3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + +typedef struct +{ + float4 m_worldPos[4]; + float4 m_worldNormal; + u32 m_coeffs; + int m_batchIdx; + + int m_bodyAPtrAndSignBit; + int m_bodyBPtrAndSignBit; +} Contact4; + + +__kernel void CountBodiesKernel(__global Contact4* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex) +{ + int i = GET_GLOBAL_IDX; + + if( i < numContactManifolds) + { + int pa = manifoldPtr[i].m_bodyAPtrAndSignBit; + bool isFixedA = (pa <0) || (pa == fixedBodyIndex); + int bodyIndexA = abs(pa); + if (!isFixedA) + { + AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x); + } + barrier(CLK_GLOBAL_MEM_FENCE); + int pb = manifoldPtr[i].m_bodyBPtrAndSignBit; + bool isFixedB = (pb <0) || (pb == fixedBodyIndex); + int bodyIndexB = abs(pb); + if (!isFixedB) + { + AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y); + } + } +} + +__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies) +{ + int i = GET_GLOBAL_IDX; + + if( i < numSplitBodies) + { + linearVelocities[i] = make_float4(0); + angularVelocities[i] = make_float4(0); + } +} + + + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1) +{ + *linear = -n; + *angular0 = -cross3(r0, n); + *angular1 = cross3(r1, n); +} + + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ) +{ + return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1); +} + + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/(jmj0+jmj1+jmj2+jmj3); +} + + +void btPlaneSpace1 (float4 n, float4* p, float4* q); + void btPlaneSpace1 (float4 n, float4* p, float4* q) +{ + if (fabs(n.z) > 0.70710678f) { + // choose p in y-z plane + float a = n.y*n.y + n.z*n.z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n.z*k; + p[0].z = n.y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n.x*p[0].z; + q[0].z = n.x*p[0].y; + } + else { + // choose p in x-y plane + float a = n.x*n.x + n.y*n.y; + float k = 1.f/sqrt(a); + p[0].x = -n.y*k; + p[0].y = n.x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n.z*p[0].y; + q[0].y = n.z*p[0].x; + q[0].z = a*k; + } +} + + + + + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB, + float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + float4 angular0, angular1, linear; + float4 r0 = cs->m_worldPos[ic] - posA; + float4 r1 = cs->m_worldPos[ic] - posB; + setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 ); + + float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, + *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic]; + rambdaDt *= cs->m_jacCoeffInv[ic]; + + { + float prevSum = cs->m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt ); + updated = min2( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + *linVelA += linImp0; + *angVelA += angImp0; + *linVelB += linImp1; + *angVelB += angImp1; + } +} + + +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, +__global int2* contactConstraintOffsets,__global int* offsetSplitBodies, +__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities) +{ + + //float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + float4 zero = make_float4(0); + + float4 dLinVelA = zero; + float4 dAngVelA = zero; + float4 dLinVelB = zero; + float4 dAngVelB = zero; + + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[0].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + + if (invMassA) + { + + dLinVelA = deltaLinearVelocities[splitIndexA]; + dAngVelA = deltaAngularVelocities[splitIndexA]; + } + + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[0].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + + if (invMassB) + { + dLinVelB = deltaLinearVelocities[splitIndexB]; + dAngVelB = deltaAngularVelocities[splitIndexB]; + } + + solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, + posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB); + + if (invMassA) + { + deltaLinearVelocities[splitIndexA] = dLinVelA; + deltaAngularVelocities[splitIndexA] = dAngVelA; + } + if (gBodies[bIdx].m_invMass) + { + deltaLinearVelocities[splitIndexB] = dLinVelB; + deltaAngularVelocities[splitIndexB] = dAngVelB; + } + +} + + +__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes , +__global int2* contactConstraintOffsets,__global int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, +float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds +) +{ + int i = GET_GLOBAL_IDX; + if (im_bodyA = abs(src->m_bodyAPtrAndSignBit); + dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); + + float dtInv = 1.f/dt; + for(int ic=0; ic<4; ic++) + { + dstC->m_appliedRambdaDt[ic] = 0.f; + } + dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; + + + dstC->m_linear = -src->m_worldNormal; + dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); + for(int ic=0; ic<4; ic++) + { + float4 r0 = src->m_worldPos[ic] - posA; + float4 r1 = src->m_worldPos[ic] - posB; + + if( ic >= src->m_worldNormal.w )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + float4 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) e = 0.f; + + dstC->m_b[ic] = e*relVelN; + //float penetration = src->m_worldPos[ic].w; + dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_appliedRambdaDt[ic] = 0.f; + } + } + + if( src->m_worldNormal.w > 0 )//npoints + { // prepare friction + float4 center = make_float4(0.f); + for(int i=0; im_worldNormal.w; i++) + center += src->m_worldPos[i]; + center /= (float)src->m_worldNormal.w; + + float4 tangent[2]; + btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]); + + float4 r[2]; + r[0] = center - posA; + r[1] = center - posB; + + for(int i=0; i<2; i++) + { + float4 linear, angular0, angular1; + setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1); + + dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( im_worldNormal.w ) + { + dstC->m_worldPos[i] = src->m_worldPos[i]; + } + else + { + dstC->m_worldPos[i] = make_float4(0.f); + } + } +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ContactToConstraintSplitKernel(__global const Contact4* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, +__global const unsigned int* bodyCount, +int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff +) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + Constraint4 cs; + + setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff, + &cs ); + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = cs; + } +} \ No newline at end of file diff --git a/opencl/gpu_rigidbody/kernels/solverUtils.h b/opencl/gpu_rigidbody/kernels/solverUtils.h new file mode 100644 index 000000000..c7ea815de --- /dev/null +++ b/opencl/gpu_rigidbody/kernels/solverUtils.h @@ -0,0 +1,748 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* solverUtilsCL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"\n" +"\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"\n" +"\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile global int*\n" +"#endif\n" +"\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"\n" +"\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"\n" +"\n" +"#define max2 max\n" +"#define min2 min\n" +"\n" +"\n" +"///////////////////////////////////////\n" +"// Vector\n" +"///////////////////////////////////////\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"\n" +"__inline\n" +"float fastSqrtf(float f2)\n" +"{\n" +" return native_sqrt(f2);\n" +"// return sqrt(f2);\n" +"}\n" +"\n" +"__inline\n" +"float fastRSqrt(float f2)\n" +"{\n" +" return native_rsqrt(f2);\n" +"}\n" +"\n" +"__inline\n" +"float fastLength4(float4 v)\n" +"{\n" +" return fast_length(v);\n" +"}\n" +"\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"\n" +"\n" +"__inline\n" +"float sqrtf(float a)\n" +"{\n" +"// return sqrt(a);\n" +" return native_sqrt(a);\n" +"}\n" +"\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"\n" +"__inline\n" +"float length3(const float4 a)\n" +"{\n" +" return sqrtf(dot3F4(a,a));\n" +"}\n" +"\n" +"__inline\n" +"float dot4(const float4 a, const float4 b)\n" +"{\n" +" return dot( a, b );\n" +"}\n" +"\n" +"// for height\n" +"__inline\n" +"float dot3w1(const float4 point, const float4 eqn)\n" +"{\n" +" return dot3F4(point,eqn) + eqn.w;\n" +"}\n" +"\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"// float length = sqrtf(dot3F4(a, a));\n" +"// return 1.f/length * a;\n" +"}\n" +"\n" +"__inline\n" +"float4 normalize4(const float4 a)\n" +"{\n" +" float length = sqrtf(dot4(a, a));\n" +" return 1.f/length * a;\n" +"}\n" +"\n" +"__inline\n" +"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" +"{\n" +" float4 eqn;\n" +" float4 ab = b-a;\n" +" float4 ac = c-a;\n" +" eqn = normalize3( cross3(ab, ac) );\n" +" eqn.w = -dot3F4(eqn,a);\n" +" return eqn;\n" +"}\n" +"\n" +"///////////////////////////////////////\n" +"// Matrix3x3\n" +"///////////////////////////////////////\n" +"\n" +"typedef struct\n" +"{\n" +" float4 m_row[3];\n" +"}Matrix3x3;\n" +"\n" +"__inline\n" +"Matrix3x3 mtZero();\n" +"\n" +"__inline\n" +"Matrix3x3 mtIdentity();\n" +"\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m);\n" +"\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" +"\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b);\n" +"\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b);\n" +"\n" +"__inline\n" +"Matrix3x3 mtZero()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(0.f);\n" +" m.m_row[1] = (float4)(0.f);\n" +" m.m_row[2] = (float4)(0.f);\n" +" return m;\n" +"}\n" +"\n" +"__inline\n" +"Matrix3x3 mtIdentity()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(1,0,0,0);\n" +" m.m_row[1] = (float4)(0,1,0,0);\n" +" m.m_row[2] = (float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m)\n" +"{\n" +" Matrix3x3 out;\n" +" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" +"{\n" +" Matrix3x3 transB;\n" +" transB = mtTranspose( b );\n" +" Matrix3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b)\n" +"{\n" +" float4 ans;\n" +" ans.x = dot3F4( a.m_row[0], b );\n" +" ans.y = dot3F4( a.m_row[1], b );\n" +" ans.z = dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b)\n" +"{\n" +" float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +"\n" +" float4 ans;\n" +" ans.x = dot3F4( a, colx );\n" +" ans.y = dot3F4( a, coly );\n" +" ans.z = dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"\n" +"typedef float4 Quaternion;\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"\n" +"\n" +"\n" +"\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"\n" +"\n" +"\n" +"\n" +"#define WG_SIZE 64\n" +"\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" Quaternion m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +"\n" +" u32 m_shapeIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} Body;\n" +"\n" +"typedef struct\n" +"{\n" +" Matrix3x3 m_invInertia;\n" +" Matrix3x3 m_initInvInertia;\n" +"} Shape;\n" +"\n" +"typedef struct\n" +"{\n" +" float4 m_linear;\n" +" float4 m_worldPos[4];\n" +" float4 m_center; \n" +" float m_jacCoeffInv[4];\n" +" float m_b[4];\n" +" float m_appliedRambdaDt[4];\n" +"\n" +" float m_fJacCoeffInv[2]; \n" +" float m_fAppliedRambdaDt[2]; \n" +"\n" +" u32 m_bodyA;\n" +" u32 m_bodyB;\n" +"\n" +" int m_batchIdx;\n" +" u32 m_paddings[1];\n" +"} Constraint4;\n" +"\n" +"typedef struct\n" +"{\n" +" float4 m_worldPos[4];\n" +" float4 m_worldNormal;\n" +" u32 m_coeffs;\n" +" int m_batchIdx;\n" +"\n" +" int m_bodyAPtrAndSignBit;\n" +" int m_bodyBPtrAndSignBit;\n" +"} Contact4;\n" +"\n" +"\n" +"__kernel void CountBodiesKernel(__global Contact4* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" \n" +" if( i < numContactManifolds)\n" +" {\n" +" int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;\n" +" bool isFixedA = (pa <0) || (pa == fixedBodyIndex);\n" +" int bodyIndexA = abs(pa);\n" +" if (!isFixedA)\n" +" {\n" +" AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);\n" +" }\n" +" barrier(CLK_GLOBAL_MEM_FENCE);\n" +" int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;\n" +" bool isFixedB = (pb <0) || (pb == fixedBodyIndex);\n" +" int bodyIndexB = abs(pb);\n" +" if (!isFixedB)\n" +" {\n" +" AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);\n" +" } \n" +" }\n" +"}\n" +"\n" +"__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" \n" +" if( i < numSplitBodies)\n" +" {\n" +" linearVelocities[i] = make_float4(0);\n" +" angularVelocities[i] = make_float4(0);\n" +" }\n" +"}\n" +"\n" +"\n" +"\n" +"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" +"{\n" +" *linear = -n;\n" +" *angular0 = -cross3(r0, n);\n" +" *angular1 = cross3(r1, n);\n" +"}\n" +"\n" +"\n" +"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" +"{\n" +" return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" +"}\n" +"\n" +"\n" +"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" +" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n" +"{\n" +" // linear0,1 are normlized\n" +" float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" +" float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" +" float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" +" float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" +" return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" +"}\n" +"\n" +"\n" +"void btPlaneSpace1 (float4 n, float4* p, float4* q);\n" +" void btPlaneSpace1 (float4 n, float4* p, float4* q)\n" +"{\n" +" if (fabs(n.z) > 0.70710678f) {\n" +" // choose p in y-z plane\n" +" float a = n.y*n.y + n.z*n.z;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = 0;\n" +" p[0].y = -n.z*k;\n" +" p[0].z = n.y*k;\n" +" // set q = n x p\n" +" q[0].x = a*k;\n" +" q[0].y = -n.x*p[0].z;\n" +" q[0].z = n.x*p[0].y;\n" +" }\n" +" else {\n" +" // choose p in x-y plane\n" +" float a = n.x*n.x + n.y*n.y;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = -n.y*k;\n" +" p[0].y = n.x*k;\n" +" p[0].z = 0;\n" +" // set q = n x p\n" +" q[0].x = -n.z*p[0].y;\n" +" q[0].y = n.z*p[0].x;\n" +" q[0].z = a*k;\n" +" }\n" +"}\n" +"\n" +"\n" +"\n" +"\n" +"\n" +"void solveContact(__global Constraint4* cs,\n" +" float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" +" float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,\n" +" float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)\n" +"{\n" +" float minRambdaDt = 0;\n" +" float maxRambdaDt = FLT_MAX;\n" +"\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" +"\n" +" float4 angular0, angular1, linear;\n" +" float4 r0 = cs->m_worldPos[ic] - posA;\n" +" float4 r1 = cs->m_worldPos[ic] - posB;\n" +" setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" +"\n" +" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" +" *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];\n" +" rambdaDt *= cs->m_jacCoeffInv[ic];\n" +"\n" +" {\n" +" float prevSum = cs->m_appliedRambdaDt[ic];\n" +" float updated = prevSum;\n" +" updated += rambdaDt;\n" +" updated = max2( updated, minRambdaDt );\n" +" updated = min2( updated, maxRambdaDt );\n" +" rambdaDt = updated - prevSum;\n" +" cs->m_appliedRambdaDt[ic] = updated;\n" +" }\n" +"\n" +" float4 linImp0 = invMassA*linear*rambdaDt;\n" +" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" +" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" +" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" +"\n" +" *linVelA += linImp0;\n" +" *angVelA += angImp0;\n" +" *linVelB += linImp1;\n" +" *angVelB += angImp1;\n" +" }\n" +"}\n" +"\n" +"\n" +"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, \n" +"__global int2* contactConstraintOffsets,__global int* offsetSplitBodies,\n" +"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" +"{\n" +"\n" +" //float frictionCoeff = ldsCs[0].m_linear.w;\n" +" int aIdx = ldsCs[0].m_bodyA;\n" +" int bIdx = ldsCs[0].m_bodyB;\n" +"\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +"\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +"\n" +" float4 zero = make_float4(0);\n" +" \n" +" float4 dLinVelA = zero;\n" +" float4 dAngVelA = zero;\n" +" float4 dLinVelB = zero;\n" +" float4 dAngVelB = zero;\n" +" \n" +" int bodyOffsetA = offsetSplitBodies[aIdx];\n" +" int constraintOffsetA = contactConstraintOffsets[0].x;\n" +" int splitIndexA = bodyOffsetA+constraintOffsetA;\n" +"\n" +" if (invMassA)\n" +" {\n" +" \n" +" dLinVelA = deltaLinearVelocities[splitIndexA];\n" +" dAngVelA = deltaAngularVelocities[splitIndexA];\n" +" }\n" +"\n" +" int bodyOffsetB = offsetSplitBodies[bIdx];\n" +" int constraintOffsetB = contactConstraintOffsets[0].y;\n" +" int splitIndexB= bodyOffsetB+constraintOffsetB;\n" +"\n" +" if (invMassB)\n" +" {\n" +" dLinVelB = deltaLinearVelocities[splitIndexB];\n" +" dAngVelB = deltaAngularVelocities[splitIndexB];\n" +" }\n" +"\n" +" solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" +" posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);\n" +"\n" +" if (invMassA)\n" +" {\n" +" deltaLinearVelocities[splitIndexA] = dLinVelA;\n" +" deltaAngularVelocities[splitIndexA] = dAngVelA;\n" +" } \n" +" if (gBodies[bIdx].m_invMass)\n" +" {\n" +" deltaLinearVelocities[splitIndexB] = dLinVelB;\n" +" deltaAngularVelocities[splitIndexB] = dAngVelB;\n" +" }\n" +"\n" +"}\n" +"\n" +"\n" +"__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" +"__global int2* contactConstraintOffsets,__global int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" +"float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" +")\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" if (im_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" +" dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" +"\n" +" float dtInv = 1.f/dt;\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" +"\n" +"\n" +" dstC->m_linear = -src->m_worldNormal;\n" +" dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" float4 r0 = src->m_worldPos[ic] - posA;\n" +" float4 r1 = src->m_worldPos[ic] - posB;\n" +"\n" +" if( ic >= src->m_worldNormal.w )//npoints\n" +" {\n" +" dstC->m_jacCoeffInv[ic] = 0.f;\n" +" continue;\n" +" }\n" +"\n" +" float relVelN;\n" +" {\n" +" float4 linear, angular0, angular1;\n" +" setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);\n" +"\n" +" dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" +"\n" +" relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" +" linVelA, angVelA, linVelB, angVelB);\n" +"\n" +" float e = 0.f;//src->getRestituitionCoeff();\n" +" if( relVelN*relVelN < 0.004f ) e = 0.f;\n" +"\n" +" dstC->m_b[ic] = e*relVelN;\n" +" //float penetration = src->m_worldPos[ic].w;\n" +" dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" }\n" +"\n" +" if( src->m_worldNormal.w > 0 )//npoints\n" +" { // prepare friction\n" +" float4 center = make_float4(0.f);\n" +" for(int i=0; im_worldNormal.w; i++) \n" +" center += src->m_worldPos[i];\n" +" center /= (float)src->m_worldNormal.w;\n" +"\n" +" float4 tangent[2];\n" +" btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);\n" +" \n" +" float4 r[2];\n" +" r[0] = center - posA;\n" +" r[1] = center - posB;\n" +"\n" +" for(int i=0; i<2; i++)\n" +" {\n" +" float4 linear, angular0, angular1;\n" +" setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" +"\n" +" dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" +" dstC->m_fAppliedRambdaDt[i] = 0.f;\n" +" }\n" +" dstC->m_center = center;\n" +" }\n" +"\n" +" for(int i=0; i<4; i++)\n" +" {\n" +" if( im_worldNormal.w )\n" +" {\n" +" dstC->m_worldPos[i] = src->m_worldPos[i];\n" +" }\n" +" else\n" +" {\n" +" dstC->m_worldPos[i] = make_float4(0.f);\n" +" }\n" +" }\n" +"}\n" +"\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void ContactToConstraintSplitKernel(__global const Contact4* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, \n" +"__global const unsigned int* bodyCount,\n" +"int nContacts,\n" +"float dt,\n" +"float positionDrift,\n" +"float positionConstraintCoeff\n" +")\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" \n" +" if( gIdx < nContacts )\n" +" {\n" +" int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" +" int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" +"\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +"\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +"\n" +" Constraint4 cs;\n" +"\n" +" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" +" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,\n" +" &cs );\n" +" \n" +" cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" +"\n" +" gConstraintOut[gIdx] = cs;\n" +" }\n" +"}\n" +;