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

fix GPU solver (need to clear .w component because "m_linear" contains friction coefficient

Browse Source 
added a mixed solver to find bugs like that
This commit is contained in:
erwin coumans
2013-04-02 14:53:30 -07:00
parent 1ebcc78280
commit e38c032280
13 changed files with 456 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
demo/gpudemo/GpuDemo.h
View File

@@ -39,7 +39,7 @@ public:
preferredOpenCLPlatformIndex(-1),
preferredOpenCLDeviceIndex(-1),
arraySizeX(1),
arraySizeY(5),
arraySizeY(2),
arraySizeZ(1),
m_useConcaveMesh(false),
gapX(14.3),

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

@@ -123,7 +123,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
mass=0.f;
//btVector3 position((j&1)+i*2.2,2+j*2.,(j&1)+k*2.2);
btVector3 position(i*2.2,2+j*2.,k*2.2);
btVector3 position(i*2.2,2+j*4.,k*2.2);
btQuaternion orn(0,0,0,1);

2
opencl/gpu_rigidbody/host/btGpuBatchingPgsSolver.cpp
View File

@@ -38,7 +38,7 @@ enum
bool gpuBatchContacts = true;//true;
bool gpuSolveConstraint = false;//true;//true;
bool gpuSolveConstraint = true;//true;
struct btGpuBatchingPgsSolverInternalData

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

@@ -921,4 +921,438 @@ void btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenC
}
void btGpuJacobiSolver::solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodiesGPU,btOpenCLArray<btInertiaCL>* inertiasGPU,btOpenCLArray<btContact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo)
{
btAlignedObjectArray<btRigidBodyCL> bodiesCPU;
bodiesGPU->copyToHost(bodiesCPU);
btAlignedObjectArray<btInertiaCL> inertiasCPU;
inertiasGPU->copyToHost(inertiasCPU);
btAlignedObjectArray<btContact4> manifoldPtrCPU;
manifoldPtrGPU->copyToHost(manifoldPtrCPU);
int numBodiesCPU = bodiesGPU->size();
int numManifoldsCPU = manifoldPtrGPU->size();
BT_PROFILE("btGpuJacobiSolver::solveGroupMixed");
btAlignedObjectArray<unsigned int> bodyCount;
bodyCount.resize(numBodiesCPU);
for (int i=0;i<numBodiesCPU;i++)
bodyCount[i] = 0;
btAlignedObjectArray<btInt2> contactConstraintOffsets;
contactConstraintOffsets.resize(numManifoldsCPU);
for (int i=0;i<numManifoldsCPU;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]++;
}
}
btAlignedObjectArray<unsigned int> offsetSplitBodies;
offsetSplitBodies.resize(numBodiesCPU);
unsigned int totalNumSplitBodiesCPU;
m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodiesCPU,&totalNumSplitBodiesCPU);
int numlastBody = bodyCount[numBodiesCPU-1];
totalNumSplitBodiesCPU += numlastBody;
int numBodies = bodiesGPU->size();
int numManifolds = manifoldPtrGPU->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(manifoldPtrGPU->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);
if (totalNumSplitBodies != totalNumSplitBodiesCPU)
{
printf("error in totalNumSplitBodies!\n");
}
int numContacts = manifoldPtrGPU->size();
m_data->m_contactConstraints->resize(numContacts);
{
BT_PROFILE("contactToConstraintSplitKernel");
btLauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel);
launcher.setBuffer(manifoldPtrGPU->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);
}
btAlignedObjectArray<btGpuConstraint4> contactConstraints;
contactConstraints.resize(numManifoldsCPU);
for (int i=0;i<numManifoldsCPU;i++)
{
ContactToConstraintKernel(&manifoldPtrCPU[0],&bodiesCPU[0],&inertiasCPU[0],&contactConstraints[0],numManifoldsCPU,
solverInfo.m_deltaTime,
solverInfo.m_positionDrift,
solverInfo.m_positionConstraintCoeff,
i, bodyCount);
}
int maxIter = solverInfo.m_numIterations;
btAlignedObjectArray<btVector3> deltaLinearVelocities;
btAlignedObjectArray<btVector3> deltaAngularVelocities;
deltaLinearVelocities.resize(totalNumSplitBodiesCPU);
deltaAngularVelocities.resize(totalNumSplitBodiesCPU);
for (int i=0;i<totalNumSplitBodiesCPU;i++)
{
deltaLinearVelocities[i].setZero();
deltaAngularVelocities[i].setZero();
}
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);
}
///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
m_data->m_contactConstraints->copyToHost(contactConstraints);
m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodies);
m_data->m_contactConstraintOffsets->copyToHost(contactConstraintOffsets);
m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
for (int iter = 0;iter<maxIter;iter++)
{
{
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);
}
int i=0;
for( i=0; i<numManifoldsCPU; 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];
}
{
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 );
}
}
{
BT_PROFILE("average velocities");
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);
}
//easy
for (int i=0;i<numBodiesCPU;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;
}
}
}
// m_data->m_deltaAngularVelocities->copyFromHost(deltaAngularVelocities);
//m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities);
m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
#if 0
{
BT_PROFILE("m_solveFrictionKernel");
btLauncherCL launcher( m_queue, m_data->m_solveFrictionKernel);
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);
}
//solve friction
for(int i=0; i<numManifoldsCPU; 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 = 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];
}
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,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);
}
{
BT_PROFILE("average velocities");
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);
}
//easy
for (int i=0;i<numBodiesCPU;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;
}
}
}
#endif
}
{
BT_PROFILE("update body velocities");
btLauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel);
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);
}
//easy
for (int i=0;i<numBodiesCPU;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];
}
}
}
// bodiesGPU->copyFromHost(bodiesCPU);
}

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

@@ -46,6 +46,7 @@ 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 solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
};
#endif //BT_GPU_JACOBI_SOLVER_H

2
opencl/gpu_rigidbody/kernels/solveContact.cl
View File

@@ -237,7 +237,7 @@ void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4
void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
{
*linear = -n;
*linear = mymake_float4(-n.xyz,0.f);
*angular0 = -cross3(r0, n);
*angular1 = cross3(r1, n);
}

2
opencl/gpu_rigidbody/kernels/solveContact.h
View File

@@ -239,7 +239,7 @@ static const char* solveContactCL= \
"\n"
"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
"{\n"
" *linear = -n;\n"
" *linear = mymake_float4(-n.xyz,0.f);\n"
" *angular0 = -cross3(r0, n);\n"
" *angular1 = cross3(r1, n);\n"
"}\n"

2
opencl/gpu_rigidbody/kernels/solveFriction.cl
View File

@@ -237,7 +237,7 @@ void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4
void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
{
*linear = -n;
*linear = mymake_float4(-n.xyz,0.f);
*angular0 = -cross3(r0, n);
*angular1 = cross3(r1, n);
}

2
opencl/gpu_rigidbody/kernels/solveFriction.h
View File

@@ -239,7 +239,7 @@ static const char* solveFrictionCL= \
"\n"
"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
"{\n"
" *linear = -n;\n"
" *linear = mymake_float4(-n.xyz,0.f);\n"
" *angular0 = -cross3(r0, n);\n"
" *angular1 = cross3(r1, n);\n"
"}\n"

2
opencl/gpu_rigidbody/kernels/solverSetup.cl
View File

@@ -435,7 +435,7 @@ typedef struct
void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
{
*linear = -n;
*linear = make_float4(-n.xyz,0.f);
*angular0 = -cross3(r0, n);
*angular1 = cross3(r1, n);
}

2
opencl/gpu_rigidbody/kernels/solverSetup.h
View File

@@ -437,7 +437,7 @@ static const char* solverSetupCL= \
"\n"
"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
"{\n"
" *linear = -n;\n"
" *linear = make_float4(-n.xyz,0.f);\n"
" *angular0 = -cross3(r0, n);\n"
" *angular1 = cross3(r1, n);\n"
"}\n"

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

@@ -462,7 +462,7 @@ __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
{
*linear = -n;
*linear = make_float4(-n.xyz,0.f);
*angular0 = -cross3(r0, n);
*angular1 = cross3(r1, n);
}
@@ -537,10 +537,12 @@ void solveContact(__global Constraint4* cs,
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];
rambdaDt *= cs->m_jacCoeffInv[ic];
{
float prevSum = cs->m_appliedRambdaDt[ic];
float updated = prevSum;
@@ -550,12 +552,14 @@ 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;
float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
if (invMassA)
{
*dLinVelA += linImp0;

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

@@ -464,7 +464,7 @@ static const char* solverUtilsCL= \
"\n"
"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
"{\n"
" *linear = -n;\n"
" *linear = make_float4(-n.xyz,0.f);\n"
" *angular0 = -cross3(r0, n);\n"
" *angular1 = cross3(r1, n);\n"
"}\n"
@@ -539,10 +539,12 @@ static const char* solverUtilsCL= \
" setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n"
" \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"
" rambdaDt *= cs->m_jacCoeffInv[ic];\n"
"\n"
" \n"
" {\n"
" float prevSum = cs->m_appliedRambdaDt[ic];\n"
" float updated = prevSum;\n"
@@ -552,12 +554,14 @@ static const char* solverUtilsCL= \
" rambdaDt = updated - prevSum;\n"
" cs->m_appliedRambdaDt[ic] = updated;\n"
" }\n"
" \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"
" \n"
" if (invMassA)\n"
" {\n"
" *dLinVelA += linImp0;\n"