Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
Download Patch File Download Diff File Expand all files Collapse all files

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];
}
}
}
*/
}