AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 22:20:12 +00:00
Code Issues Releases Wiki Activity

more work towards GPU Jacobi solver.

Browse Source 
Not fully working yet, GPU version shows explosion at high iteration count.
This commit is contained in:
erwin coumans 2013-03-27 21:40:26 -07:00
parent 2133712207
commit a1aa281622
7 changed files with 441 additions and 661 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
demo/gpudemo/GpuDemo.h
View File

@ -37,9 +37,9 @@ public:
:useOpenCL(true),
preferredOpenCLPlatformIndex(-1),
preferredOpenCLDeviceIndex(-1),
arraySizeX(1),
arraySizeY(3),
arraySizeZ(1),
arraySizeX(10),
arraySizeY(20),
arraySizeZ(10),
m_useConcaveMesh(false),
gapX(14.3),
gapY(14.0),

2
demo/gpudemo/rigidbody/GpuConvexScene.cpp
View File

@ -84,6 +84,6 @@ void GpuConvexScene::setupScene(const ConstructionInfo& ci)
float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
//float camPos[4]={1,12.5,1.5,0};
m_instancingRenderer->setCameraTargetPosition(camPos);
m_instancingRenderer->setCameraDistance(20);
m_instancingRenderer->setCameraDistance(120);
}

673
opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp
View File

@ -39,6 +39,7 @@ struct btGpuJacobiSolverInternalData
cl_kernel m_contactToConstraintSplitKernel;
cl_kernel m_clearVelocitiesKernel;
cl_kernel m_averageVelocitiesKernel;
cl_kernel m_updateBodyVelocitiesKernel;
cl_kernel m_solveContactKernel;
cl_kernel m_solveFrictionKernel;
@ -65,7 +66,7 @@ btGpuJacobiSolver::btGpuJacobiSolver(cl_context ctx, cl_device_id device, cl_com
const char* additionalMacros="";
const char* solverUtilsSource = solverUtilsCL;
{
cl_program solverUtilsProg= btOpenCLUtils::compileCLProgramFromString( ctx, device, solverUtilsSource, &pErrNum,additionalMacros, SOLVER_UTILS_KERNEL_PATH);
cl_program solverUtilsProg= btOpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, SOLVER_UTILS_KERNEL_PATH,true);
btAssert(solverUtilsProg);
m_data->m_countBodiesKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "CountBodiesKernel", &pErrNum, solverUtilsProg,additionalMacros );
btAssert(m_data->m_countBodiesKernel);
@ -78,7 +79,8 @@ btGpuJacobiSolver::btGpuJacobiSolver(cl_context ctx, cl_device_id device, cl_com
m_data->m_averageVelocitiesKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "AverageVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros );
btAssert(m_data->m_averageVelocitiesKernel);
m_data->m_updateBodyVelocitiesKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "UpdateBodyVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros );
btAssert(m_data->m_updateBodyVelocitiesKernel);
m_data->m_solveContactKernel = btOpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveContactJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros );
@ -97,6 +99,7 @@ btGpuJacobiSolver::~btGpuJacobiSolver()
clReleaseKernel(m_data->m_countBodiesKernel);
clReleaseKernel(m_data->m_contactToConstraintSplitKernel);
clReleaseKernel(m_data->m_averageVelocitiesKernel);
clReleaseKernel(m_data->m_updateBodyVelocitiesKernel);
clReleaseKernel(m_data->m_clearVelocitiesKernel );
delete m_data->m_deltaLinearVelocities;
@ -556,7 +559,7 @@ void btGpuJacobiSolver::solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inerti
solverInfo.m_positionConstraintCoeff,
i, bodyCount);
}
int maxIter = 1;
int maxIter = solverInfo.m_numIterations;
btAlignedObjectArray<btVector3> deltaLinearVelocities;
@ -648,9 +651,6 @@ void btGpuJacobiSolver::solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inerti
}
}
#if 0
//solve friction
for(int i=0; i<numManifolds; i++)
@ -731,7 +731,6 @@ void btGpuJacobiSolver::solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inerti
}
}
#endif
}
@ -754,422 +753,6 @@ void btGpuJacobiSolver::solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inerti
}
void btGpuJacobiSolver::solveGroupMixedHost(btRigidBodyCL* bodiesCPU,btInertiaCL* inertiasCPU,int numBodies,btContact4* manifoldPtrCpu, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo)
{
BT_PROFILE("btGpuJacobiSolver::solveGroup");
btAlignedObjectArray<unsigned int> bodyCount;
bool useHost = true;
btAlignedObjectArray<btInt2> contactConstraintOffsets;
btOpenCLArray<btContact4> manifoldGPU(m_context,m_queue);
manifoldGPU.resize(numManifolds);
manifoldGPU.copyFromHostPointer(manifoldPtrCpu,numManifolds);
btAlignedObjectArray<unsigned int> offsetSplitBodies;
unsigned int totalNumSplitBodies;
btAlignedObjectArray<btGpuConstraint4> contactConstraints;
contactConstraints.resize(numManifolds);
btOpenCLArray<btRigidBodyCL> bodiesGPU(m_context,m_queue);
bodiesGPU.resize(numBodies);
btOpenCLArray<btInertiaCL> inertiasGPU(m_context,m_queue);
inertiasGPU.resize(numBodies);
if (useHost)
{
bodyCount.resize(numBodies);
for (int i=0;i<numBodies;i++)
bodyCount[i] = 0;
contactConstraintOffsets.resize(numManifolds);
for (int i=0;i<numManifolds;i++)
{
int pa = manifoldPtrCpu[i].m_bodyAPtrAndSignBit;
int pb = manifoldPtrCpu[i].m_bodyBPtrAndSignBit;
bool isFixedA = (pa <0) || (pa == solverInfo.m_fixedBodyIndex);
bool isFixedB = (pb <0) || (pb == solverInfo.m_fixedBodyIndex);
int bodyIndexA = manifoldPtrCpu[i].getBodyA();
int bodyIndexB = manifoldPtrCpu[i].getBodyB();
if (!isFixedA)
{
contactConstraintOffsets[i].x = bodyCount[bodyIndexA];
bodyCount[bodyIndexA]++;
}
if (!isFixedB)
{
contactConstraintOffsets[i].y = bodyCount[bodyIndexB];
bodyCount[bodyIndexB]++;
}
}
offsetSplitBodies.resize(numBodies);
m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodies,&totalNumSplitBodies);
int numlastBody = bodyCount[numBodies-1];
totalNumSplitBodies += numlastBody;
for (int i=0;i<numManifolds;i++)
{
ContactToConstraintKernel(&manifoldPtrCpu[0],bodiesCPU,inertiasCPU,&contactConstraints[0],numManifolds,
solverInfo.m_deltaTime,
solverInfo.m_positionDrift,
solverInfo.m_positionConstraintCoeff,
i, bodyCount);
}
} else
{
// 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(manifoldGPU.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);
}
m_data->m_contactConstraintOffsets->copyToHost(contactConstraintOffsets);
m_data->m_bodyCount->copyToHost(bodyCount);
// 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);
m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodies);
int numContacts = manifoldGPU.size();
m_data->m_contactConstraints->resize(numContacts);
bodiesGPU.copyFromHostPointer(bodiesCPU,numBodies);
inertiasGPU.copyFromHostPointer(inertiasCPU,numBodies);
{
BT_PROFILE("contactToConstraintSplitKernel");
btLauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel);
launcher.setBuffer(manifoldGPU.getBufferCL());
launcher.setBuffer(bodiesGPU.getBufferCL());
launcher.setBuffer(inertiasGPU.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_contactConstraints->copyToHost(contactConstraints);
}
int maxIter = 1;
btAlignedObjectArray<btVector3> deltaLinearVelocities;
btAlignedObjectArray<btVector3> deltaAngularVelocities;
deltaLinearVelocities.resize(totalNumSplitBodies);
deltaAngularVelocities.resize(totalNumSplitBodies);
for (int i=0;i<totalNumSplitBodies;i++)
{
deltaLinearVelocities[i].setZero();
deltaAngularVelocities[i].setZero();
}
m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyFromHost(deltaAngularVelocities);
for (int iter = 0;iter<maxIter;iter++)
{
bool solveHost=true;
if (solveHost)
{
int i=0;
for( i=0; i<numManifolds; i++)
{
float frictionCoeff = contactConstraints[i].getFrictionCoeff();
int aIdx = (int)contactConstraints[i].m_bodyA;
int bIdx = (int)contactConstraints[i].m_bodyB;
btRigidBodyCL& bodyA = bodiesCPU[aIdx];
btRigidBodyCL& bodyB = bodiesCPU[bIdx];
btVector3 zero(0,0,0);
btVector3* dlvAPtr=&zero;
btVector3* davAPtr=&zero;
btVector3* dlvBPtr=&zero;
btVector3* davBPtr=&zero;
if (bodyA.getInvMass())
{
int bodyOffsetA = offsetSplitBodies[aIdx];
int constraintOffsetA = contactConstraintOffsets[i].x;
int splitIndexA = bodyOffsetA+constraintOffsetA;
dlvAPtr = &deltaLinearVelocities[splitIndexA];
davAPtr = &deltaAngularVelocities[splitIndexA];
}
if (bodyB.getInvMass())
{
int bodyOffsetB = offsetSplitBodies[bIdx];
int constraintOffsetB = contactConstraintOffsets[i].y;
int splitIndexB= bodyOffsetB+constraintOffsetB;
dlvBPtr =&deltaLinearVelocities[splitIndexB];
davBPtr = &deltaAngularVelocities[splitIndexB];
}
{
bool test=false;
if (test)
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
solveContact( contactConstraints[i], (btVector3&)bodyA.m_pos, (btVector3&)bodyA.m_linVel, (btVector3&)bodyA.m_angVel, bodyA.m_invMass, inertiasCPU[aIdx].m_invInertiaWorld,
(btVector3&)bodyB.m_pos, (btVector3&)bodyB.m_linVel, (btVector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr );
} else
{
solveContact3(&contactConstraints[i], &bodyA.m_pos, &bodyA.m_linVel, &bodyA.m_angVel, bodyA.m_invMass, inertiasCPU[aIdx].m_invInertiaWorld,
&bodyB.m_pos, &bodyB.m_linVel, &bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
dlvAPtr,davAPtr,dlvBPtr,davBPtr );
}
printf("!");
}
}
}else
{
{
//__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
BT_PROFILE("m_solveContactKernel");
btLauncherCL launcher( m_queue, m_data->m_solveContactKernel );
launcher.setBuffer(m_data->m_contactConstraints->getBufferCL());
launcher.setBuffer(bodiesGPU.getBufferCL());
launcher.setBuffer(inertiasGPU.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);
}
m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
}
bool useHostAverage=false;
if (useHostAverage)
{
//easy
for (int i=0;i<numBodies;i++)
{
if (bodiesCPU[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
float factor = 1.f/float(count);
btVector3 averageLinVel;
averageLinVel.setZero();
btVector3 averageAngVel;
averageAngVel.setZero();
for (int j=0;j<count;j++)
{
averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
}
for (int j=0;j<count;j++)
{
deltaLinearVelocities[bodyOffset+j] = averageLinVel;
deltaAngularVelocities[bodyOffset+j] = averageAngVel;
}
}
}
} else
{
// bodiesGPU.copyFromHostPointer(bodiesCPU,numBodies);
m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyFromHost(deltaAngularVelocities);
BT_PROFILE("average velocities");
//__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,
//__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)
btLauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel);
launcher.setBuffer(bodiesGPU.getBufferCL());
launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
launcher.setConst(numBodies);
launcher.launch1D(numBodies);
clFinish(m_queue);
m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
}
#if 0
//solve friction
for(int i=0; i<numManifolds; i++)
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
float sum = 0;
for(int j=0; j<4; j++)
{
sum +=contactConstraints[i].m_appliedRambdaDt[j];
}
float frictionCoeff = contactConstraints[i].getFrictionCoeff();
int aIdx = (int)contactConstraints[i].m_bodyA;
int bIdx = (int)contactConstraints[i].m_bodyB;
btRigidBodyCL& bodyA = bodies[aIdx];
btRigidBodyCL& bodyB = bodies[bIdx];
btVector3 zero(0,0,0);
btVector3* dlvAPtr=&zero;
btVector3* davAPtr=&zero;
btVector3* dlvBPtr=&zero;
btVector3* davBPtr=&zero;
if (bodyA.getInvMass())
{
int bodyOffsetA = offsetSplitBodies[aIdx];
int constraintOffsetA = contactConstraintOffsets[i].x;
int splitIndexA = bodyOffsetA+constraintOffsetA;
dlvAPtr = &deltaLinearVelocities[splitIndexA];
davAPtr = &deltaAngularVelocities[splitIndexA];
}
if (bodyB.getInvMass())
{
int bodyOffsetB = offsetSplitBodies[bIdx];
int constraintOffsetB = contactConstraintOffsets[i].y;
int splitIndexB= bodyOffsetB+constraintOffsetB;
dlvBPtr =&deltaLinearVelocities[splitIndexB];
davBPtr = &deltaAngularVelocities[splitIndexB];
}
for(int j=0; j<4; j++)
{
maxRambdaDt[j] = frictionCoeff*sum;
minRambdaDt[j] = -maxRambdaDt[j];
}
solveFriction( 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<numBodies;i++)
{
if (bodies[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
float factor = 1.f/float(count);
btVector3 averageLinVel;
averageLinVel.setZero();
btVector3 averageAngVel;
averageAngVel.setZero();
for (int j=0;j<count;j++)
{
averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
}
for (int j=0;j<count;j++)
{
deltaLinearVelocities[bodyOffset+j] = averageLinVel;
deltaAngularVelocities[bodyOffset+j] = averageAngVel;
}
}
}
#endif
}
//easy
for (int i=0;i<numBodies;i++)
{
if (bodiesCPU[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
if (count)
{
bodiesCPU[i].m_linVel += deltaLinearVelocities[bodyOffset];
bodiesCPU[i].m_angVel += deltaAngularVelocities[bodyOffset];
}
}
}
}
void btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo)
{
@ -1247,17 +830,11 @@ void btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenC
launch.launch1D(totalNumSplitBodies);
}
int maxIter = 14;
int maxIter = solverInfo.m_numIterations;
for (int iter = 0;iter<maxIter;iter++)
{
{
//__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
BT_PROFILE("m_solveContactKernel");
btLauncherCL launcher( m_queue, m_data->m_solveContactKernel );
launcher.setBuffer(m_data->m_contactConstraints->getBufferCL());
@ -1277,10 +854,9 @@ void btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenC
clFinish(m_queue);
}
{
BT_PROFILE("average velocities");
//__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,
//__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)
btLauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel);
launcher.setBuffer(bodies->getBufferCL());
launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
@ -1291,213 +867,58 @@ void btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenC
launcher.launch1D(numBodies);
clFinish(m_queue);
}
/*
for(int i=0; i<numManifolds; i++)
{
BT_PROFILE("m_solveFrictionKernel");
btLauncherCL launcher( m_queue, m_data->m_solveFrictionKernel);
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);
float frictionCoeff = contactConstraints[i].getFrictionCoeff();
int aIdx = (int)contactConstraints[i].m_bodyA;
int bIdx = (int)contactConstraints[i].m_bodyB;
btRigidBodyCL& bodyA = bodies[aIdx];
btRigidBodyCL& bodyB = bodies[bIdx];
launcher.launch1D(numManifolds);
clFinish(m_queue);
}
btVector3 zero(0,0,0);
btVector3* dlvAPtr=&zero;
btVector3* davAPtr=&zero;
btVector3* dlvBPtr=&zero;
btVector3* davBPtr=&zero;
if (bodyA.getInvMass())
{
int bodyOffsetA = offsetSplitBodies[aIdx];
int constraintOffsetA = contactConstraintOffsets[i].x;
int splitIndexA = bodyOffsetA+constraintOffsetA;
dlvAPtr = &deltaLinearVelocities[splitIndexA];
davAPtr = &deltaAngularVelocities[splitIndexA];
}
if (bodyB.getInvMass())
{
int bodyOffsetB = offsetSplitBodies[bIdx];
int constraintOffsetB = contactConstraintOffsets[i].y;
int splitIndexB= bodyOffsetB+constraintOffsetB;
dlvBPtr =&deltaLinearVelocities[splitIndexB];
davBPtr = &deltaAngularVelocities[splitIndexB];
}
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
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 );
}
{
BT_PROFILE("average velocities");
btLauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel);
launcher.setBuffer(bodies->getBufferCL());
launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
launcher.setConst(numBodies);
launcher.launch1D(numBodies);
clFinish(m_queue);
}
//easy
for (int i=0;i<numBodies;i++)
{
if (bodies[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
float factor = 1.f/float(count);
btVector3 averageLinVel;
averageLinVel.setZero();
btVector3 averageAngVel;
averageAngVel.setZero();
for (int j=0;j<count;j++)
{
averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
}
for (int j=0;j<count;j++)
{
deltaLinearVelocities[bodyOffset+j] = averageLinVel;
deltaAngularVelocities[bodyOffset+j] = averageAngVel;
}
}
}
//solve friction
for(int i=0; i<numManifolds; i++)
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
float sum = 0;
for(int j=0; j<4; j++)
{
sum +=contactConstraints[i].m_appliedRambdaDt[j];
}
float frictionCoeff = contactConstraints[i].getFrictionCoeff();
int aIdx = (int)contactConstraints[i].m_bodyA;
int bIdx = (int)contactConstraints[i].m_bodyB;
btRigidBodyCL& bodyA = bodies[aIdx];
btRigidBodyCL& bodyB = bodies[bIdx];
btVector3 zero(0,0,0);
btVector3* dlvAPtr=&zero;
btVector3* davAPtr=&zero;
btVector3* dlvBPtr=&zero;
btVector3* davBPtr=&zero;
if (bodyA.getInvMass())
{
int bodyOffsetA = offsetSplitBodies[aIdx];
int constraintOffsetA = contactConstraintOffsets[i].x;
int splitIndexA = bodyOffsetA+constraintOffsetA;
dlvAPtr = &deltaLinearVelocities[splitIndexA];
davAPtr = &deltaAngularVelocities[splitIndexA];
}
if (bodyB.getInvMass())
{
int bodyOffsetB = offsetSplitBodies[bIdx];
int constraintOffsetB = contactConstraintOffsets[i].y;
int splitIndexB= bodyOffsetB+constraintOffsetB;
dlvBPtr =&deltaLinearVelocities[splitIndexB];
davBPtr = &deltaAngularVelocities[splitIndexB];
}
for(int j=0; j<4; j++)
{
maxRambdaDt[j] = frictionCoeff*sum;
minRambdaDt[j] = -maxRambdaDt[j];
}
solveFriction( 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<numBodies;i++)
{
if (bodies[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
float factor = 1.f/float(count);
btVector3 averageLinVel;
averageLinVel.setZero();
btVector3 averageAngVel;
averageAngVel.setZero();
for (int j=0;j<count;j++)
{
averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
}
for (int j=0;j<count;j++)
{
deltaLinearVelocities[bodyOffset+j] = averageLinVel;
deltaAngularVelocities[bodyOffset+j] = averageAngVel;
}
}
}
*/
}
btAlignedObjectArray<btVector3> dLinVel;
btAlignedObjectArray<btVector3> dAngVel;
m_data->m_deltaLinearVelocities->copyToHost(dLinVel);
m_data->m_deltaAngularVelocities->copyToHost(dAngVel);
btAlignedObjectArray<btRigidBodyCL> bodiesCPU;
bodies->copyToHost(bodiesCPU);
btAlignedObjectArray<unsigned int> bodyCountCPU;
m_data->m_bodyCount->copyToHost(bodyCountCPU);
btAlignedObjectArray<unsigned int> offsetSplitBodiesCPU;
m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodiesCPU);
for (int i=0;i<numBodies;i++)
{
if (bodiesCPU[i].getInvMass())
{
int bodyOffset = offsetSplitBodiesCPU[i];
int count = bodyCountCPU[i];
if (count)
{
bodiesCPU[i].m_linVel += dLinVel[bodyOffset];
bodiesCPU[i].m_angVel += dAngVel[bodyOffset];
}
BT_PROFILE("update body velocities");
btLauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel);
launcher.setBuffer(bodies->getBufferCL());
launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
launcher.setConst(numBodies);
launcher.launch1D(numBodies);
clFinish(m_queue);
}
}
bodies->copyFromHost(bodiesCPU);
printf(".");
/*
//easy
for (int i=0;i<numBodies;i++)
{
if (bodies[i].getInvMass())
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
if (count)
{
bodies[i].m_linVel += deltaLinearVelocities[bodyOffset];
bodies[i].m_angVel += deltaAngularVelocities[bodyOffset];
}
}
}
*/
}

6
opencl/gpu_rigidbody/host/btGpuJacobiSolver.h
View File

@ -16,13 +16,14 @@ struct btJacobiSolverInfo
float m_deltaTime;
float m_positionDrift;
float m_positionConstraintCoeff;
int m_numIterations;
btJacobiSolverInfo()
:m_fixedBodyIndex(0),
m_deltaTime(1./60.f),
m_positionDrift( 0.005f ),
m_positionConstraintCoeff( 0.99f )
m_positionConstraintCoeff( 0.99f ),
m_numIterations(14)
{
}
};
@ -45,7 +46,6 @@ public:
void solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo);
void solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
void solveGroupMixedHost(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo);
};
#endif //BT_GPU_JACOBI_SOLVER_H

7
opencl/gpu_rigidbody/host/btGpuRigidBodyPipeline.cpp
View File

@ -111,13 +111,13 @@ void btGpuRigidBodyPipeline::stepSimulation(float deltaTime)
btOpenCLArray<btContact4> 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 = true;
bool useJacobi = false;
if (useJacobi)
{
bool useGpu = true;
if (useGpu)
{
bool forceHost = true;
bool forceHost = false;
if (forceHost)
{
btAlignedObjectArray<btRigidBodyCL> hostBodies;
@ -133,8 +133,7 @@ void btGpuRigidBodyPipeline::stepSimulation(float deltaTime)
{
btJacobiSolverInfo solverInfo;
// m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo);
m_data->m_solver3->solveGroupMixedHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo);
m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo);
}

204
opencl/gpu_rigidbody/kernels/solverUtils.cl
View File

@ -109,8 +109,13 @@ float sqrtf(float a)
}
__inline
float4 cross3(float4 a, float4 b)
float4 cross3(float4 a1, float4 b1)
{
float4 a=make_float4(a1.xyz,0.f);
float4 b=make_float4(b1.xyz,0.f);
//float4 a=a1;
//float4 b=b1;
return cross(a,b);
}
@ -530,6 +535,7 @@ void solveContact(__global Constraint4* cs,
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+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];
@ -544,7 +550,7 @@ void solveContact(__global Constraint4* cs,
rambdaDt = updated - prevSum;
cs->m_appliedRambdaDt[ic] = updated;
}
float4 linImp0 = invMassA*linear*rambdaDt;
float4 linImp1 = invMassB*(-linear)*rambdaDt;
float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
@ -588,20 +594,18 @@ __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)
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;
float4 dLinVelA = make_float4(0,0,0,0);
float4 dAngVelA = make_float4(0,0,0,0);
float4 dLinVelB = make_float4(0,0,0,0);
float4 dAngVelB = make_float4(0,0,0,0);
int bodyOffsetA = offsetSplitBodies[aIdx];
int constraintOffsetA = contactConstraintOffsets[0].x;
int splitIndexA = bodyOffsetA+constraintOffsetA;
if (invMassA)
{
dLinVelA = deltaLinearVelocities[splitIndexA];
dAngVelA = deltaAngularVelocities[splitIndexA];
}
@ -641,13 +645,189 @@ float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBo
int i = GET_GLOBAL_IDX;
if (i<numManifolds)
{
solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
}
}
__kernel void SolveFrictionJacobiKernel()
{
void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,
__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,
__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)
{
float frictionCoeff = 0.7f;//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 dLinVelA = make_float4(0,0,0,0);
float4 dAngVelA = make_float4(0,0,0,0);
float4 dLinVelB = make_float4(0,0,0,0);
float4 dAngVelB = make_float4(0,0,0,0);
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];
}
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
float sum = 0;
for(int j=0; j<4; j++)
{
sum +=ldsCs[0].m_appliedRambdaDt[j];
}
frictionCoeff = 0.7f;
for(int j=0; j<4; j++)
{
maxRambdaDt[j] = frictionCoeff*sum;
minRambdaDt[j] = -maxRambdaDt[j];
}
// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,
// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );
{
__global Constraint4* cs = ldsCs;
if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;
const float4 center = cs->m_center;
float4 n = -cs->m_linear;
float4 tangent[2];
btPlaneSpace1(n,&tangent[0],&tangent[1]);
float4 angular0, angular1, linear;
float4 r0 = center - posA;
float4 r1 = center - posB;
for(int i=0; i<2; i++)
{
setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );
float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );
rambdaDt *= cs->m_fJacCoeffInv[i];
{
float prevSum = cs->m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = max2( updated, minRambdaDt[i] );
updated = min2( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs->m_fAppliedRambdaDt[i] = updated;
}
float4 linImp0 = invMassA*linear*rambdaDt;
float4 linImp1 = invMassB*(-linear)*rambdaDt;
float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
dLinVelA += linImp0;
dAngVelA += angImp0;
dLinVelB += linImp1;
dAngVelB += angImp1;
}
{ // angular damping for point constraint
float4 ab = normalize3( posB - posA );
float4 ac = normalize3( center - posA );
if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = dot3F4( n, angVelA );
float angNB = dot3F4( n, angVelB );
dAngVelA -= (angNA*0.1f)*n;
dAngVelB -= (angNB*0.1f)*n;
}
}
}
}
if (invMassA)
{
deltaLinearVelocities[splitIndexA] = dLinVelA;
deltaAngularVelocities[splitIndexA] = dAngVelA;
}
if (invMassB)
{
deltaLinearVelocities[splitIndexB] = dLinVelB;
deltaAngularVelocities[splitIndexB] = dAngVelB;
}
}
__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,
__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,
__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds
)
{
int i = GET_GLOBAL_IDX;
if (i<numManifolds)
{
solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
}
}
__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,
__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)
{
int i = GET_GLOBAL_IDX;
if (i<numBodies)
{
if (gBodies[i].m_invMass)
{
int bodyOffset = offsetSplitBodies[i];
int count = bodyCount[i];
if (count)
{
gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];
gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];
}
}
}
}

204
opencl/gpu_rigidbody/kernels/solverUtils.h
View File

@ -111,8 +111,13 @@ static const char* solverUtilsCL= \
"}\n"
"\n"
"__inline\n"
"float4 cross3(float4 a, float4 b)\n"
"float4 cross3(float4 a1, float4 b1)\n"
"{\n"
"\n"
" float4 a=make_float4(a1.xyz,0.f);\n"
" float4 b=make_float4(b1.xyz,0.f);\n"
" //float4 a=a1;\n"
" //float4 b=b1;\n"
" return cross(a,b);\n"
"}\n"
"\n"
@ -532,6 +537,7 @@ static const char* solverUtilsCL= \
" 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"
"\n"
" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n"
" *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n"
@ -546,7 +552,7 @@ static const char* solverUtilsCL= \
" rambdaDt = updated - prevSum;\n"
" cs->m_appliedRambdaDt[ic] = updated;\n"
" }\n"
"\n"
" \n"
" float4 linImp0 = invMassA*linear*rambdaDt;\n"
" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
@ -590,20 +596,18 @@ static const char* solverUtilsCL= \
" 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"
" float4 dLinVelA = make_float4(0,0,0,0);\n"
" float4 dAngVelA = make_float4(0,0,0,0);\n"
" float4 dLinVelB = make_float4(0,0,0,0);\n"
" float4 dAngVelB = make_float4(0,0,0,0);\n"
" \n"
" int bodyOffsetA = offsetSplitBodies[aIdx];\n"
" int constraintOffsetA = contactConstraintOffsets[0].x;\n"
" int splitIndexA = bodyOffsetA+constraintOffsetA;\n"
"\n"
" \n"
" if (invMassA)\n"
" {\n"
" \n"
" dLinVelA = deltaLinearVelocities[splitIndexA];\n"
" dAngVelA = deltaAngularVelocities[splitIndexA];\n"
" }\n"
@ -643,13 +647,189 @@ static const char* solverUtilsCL= \
" int i = GET_GLOBAL_IDX;\n"
" if (i<numManifolds)\n"
" {\n"
" solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
" solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
" }\n"
"}\n"
"\n"
"__kernel void SolveFrictionJacobiKernel()\n"
"{\n"
"\n"
"\n"
"\n"
"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,\n"
" __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n"
" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n"
"{\n"
" float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w;\n"
" int aIdx = ldsCs[0].m_bodyA;\n"
" int bIdx = ldsCs[0].m_bodyB;\n"
"\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"
"\n"
" float4 dLinVelA = make_float4(0,0,0,0);\n"
" float4 dAngVelA = make_float4(0,0,0,0);\n"
" float4 dLinVelB = make_float4(0,0,0,0);\n"
" float4 dAngVelB = make_float4(0,0,0,0);\n"
" \n"
" int bodyOffsetA = offsetSplitBodies[aIdx];\n"
" int constraintOffsetA = contactConstraintOffsets[0].x;\n"
" int splitIndexA = bodyOffsetA+constraintOffsetA;\n"
" \n"
" if (invMassA)\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"
"\n"
"\n"
"\n"
" {\n"
" float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n"
" float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n"
"\n"
" float sum = 0;\n"
" for(int j=0; j<4; j++)\n"
" {\n"
" sum +=ldsCs[0].m_appliedRambdaDt[j];\n"
" }\n"
" frictionCoeff = 0.7f;\n"
" for(int j=0; j<4; j++)\n"
" {\n"
" maxRambdaDt[j] = frictionCoeff*sum;\n"
" minRambdaDt[j] = -maxRambdaDt[j];\n"
" }\n"
"\n"
" \n"
"// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n"
"// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n"
" \n"
" \n"
" {\n"
" \n"
" __global Constraint4* cs = ldsCs;\n"
" \n"
" if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n"
" const float4 center = cs->m_center;\n"
" \n"
" float4 n = -cs->m_linear;\n"
" \n"
" float4 tangent[2];\n"
" btPlaneSpace1(n,&tangent[0],&tangent[1]);\n"
" float4 angular0, angular1, linear;\n"
" float4 r0 = center - posA;\n"
" float4 r1 = center - posB;\n"
" for(int i=0; i<2; i++)\n"
" {\n"
" setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n"
" float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n"
" linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );\n"
" rambdaDt *= cs->m_fJacCoeffInv[i];\n"
" \n"
" {\n"
" float prevSum = cs->m_fAppliedRambdaDt[i];\n"
" float updated = prevSum;\n"
" updated += rambdaDt;\n"
" updated = max2( updated, minRambdaDt[i] );\n"
" updated = min2( updated, maxRambdaDt[i] );\n"
" rambdaDt = updated - prevSum;\n"
" cs->m_fAppliedRambdaDt[i] = 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"
" dLinVelA += linImp0;\n"
" dAngVelA += angImp0;\n"
" dLinVelB += linImp1;\n"
" dAngVelB += angImp1;\n"
" }\n"
" { // angular damping for point constraint\n"
" float4 ab = normalize3( posB - posA );\n"
" float4 ac = normalize3( center - posA );\n"
" if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n"
" {\n"
" float angNA = dot3F4( n, angVelA );\n"
" float angNB = dot3F4( n, angVelB );\n"
" \n"
" dAngVelA -= (angNA*0.1f)*n;\n"
" dAngVelB -= (angNB*0.1f)*n;\n"
" }\n"
" }\n"
" }\n"
"\n"
" \n"
" \n"
" }\n"
"\n"
" if (invMassA)\n"
" {\n"
" deltaLinearVelocities[splitIndexA] = dLinVelA;\n"
" deltaAngularVelocities[splitIndexA] = dAngVelA;\n"
" } \n"
" if (invMassB)\n"
" {\n"
" deltaLinearVelocities[splitIndexB] = dLinVelB;\n"
" deltaAngularVelocities[splitIndexB] = dAngVelB;\n"
" }\n"
" \n"
"\n"
"}\n"
"\n"
"\n"
"__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n"
" __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n"
" __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 (i<numManifolds)\n"
" {\n"
" solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
" }\n"
"}\n"
"\n"
"\n"
"__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n"
" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n"
"{\n"
" int i = GET_GLOBAL_IDX;\n"
" if (i<numBodies)\n"
" {\n"
" if (gBodies[i].m_invMass)\n"
" {\n"
" int bodyOffset = offsetSplitBodies[i];\n"
" int count = bodyCount[i];\n"
" if (count)\n"
" {\n"
" gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];\n"
" gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];\n"
" }\n"
" }\n"
" }\n"
"}\n"
"\n"
"\n"