Пример #1
0
    void run(int tIdx)
    {


        for(int ic=0; ic<m_nConstraints; ic++)
        {
            int i = m_start + ic;

            float frictionCoeff = m_constraints[i].getFrictionCoeff();
            int aIdx = (int)m_constraints[i].m_bodyA;
            int bIdx = (int)m_constraints[i].m_bodyB;
            b3RigidBodyCL& bodyA = m_bodies[aIdx];
            b3RigidBodyCL& bodyB = m_bodies[bIdx];

            if( !m_solveFriction )
            {
                float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
                float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

                solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld,
                                     (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
                                     maxRambdaDt, minRambdaDt );

            }
            else
            {
                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 +=m_constraints[i].m_appliedRambdaDt[j];
                }
                frictionCoeff = 0.7f;
                for(int j=0; j<4; j++)
                {
                    maxRambdaDt[j] = frictionCoeff*sum;
                    minRambdaDt[j] = -maxRambdaDt[j];
                }

                solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld,
                               (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
                               maxRambdaDt, minRambdaDt );

            }
        }


    }
Пример #2
0
void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo)
{
	B3_PROFILE("b3GpuJacobiContactSolver::solveGroup");

	b3AlignedObjectArray<unsigned int> bodyCount;
	bodyCount.resize(numBodies);
	for (int i=0;i<numBodies;i++)
		bodyCount[i] = 0;

	b3AlignedObjectArray<b3Int2> contactConstraintOffsets;
	contactConstraintOffsets.resize(numManifolds);


	for (int i=0;i<numManifolds;i++)
	{
		int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;
		int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;

		bool isFixedA = (pa <0) || (pa == solverInfo.m_fixedBodyIndex);
		bool isFixedB = (pb <0) || (pb == solverInfo.m_fixedBodyIndex);

		int bodyIndexA = manifoldPtr[i].getBodyA();
		int bodyIndexB = manifoldPtr[i].getBodyB();

		if (!isFixedA)
		{
			contactConstraintOffsets[i].x = bodyCount[bodyIndexA];
			bodyCount[bodyIndexA]++;
		}
		if (!isFixedB)
		{
			contactConstraintOffsets[i].y = bodyCount[bodyIndexB];
			bodyCount[bodyIndexB]++;
		} 
	}

	b3AlignedObjectArray<unsigned int> offsetSplitBodies;
	offsetSplitBodies.resize(numBodies);
	unsigned int totalNumSplitBodies;
	m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodies,&totalNumSplitBodies);
	int numlastBody = bodyCount[numBodies-1];
	totalNumSplitBodies += numlastBody;
	printf("totalNumSplitBodies = %d\n",totalNumSplitBodies);

	



	b3AlignedObjectArray<b3GpuConstraint4> contactConstraints;
	contactConstraints.resize(numManifolds);

	for (int i=0;i<numManifolds;i++)
	{
		ContactToConstraintKernel(&manifoldPtr[0],bodies,inertias,&contactConstraints[0],numManifolds,
			solverInfo.m_deltaTime,
			solverInfo.m_positionDrift,
			solverInfo.m_positionConstraintCoeff,
			i, bodyCount);
	}
	int maxIter = solverInfo.m_numIterations;


	b3AlignedObjectArray<b3Vector3> deltaLinearVelocities;
	b3AlignedObjectArray<b3Vector3> deltaAngularVelocities;
	deltaLinearVelocities.resize(totalNumSplitBodies);
	deltaAngularVelocities.resize(totalNumSplitBodies);
	for (int i=0;i<totalNumSplitBodies;i++)
	{
		deltaLinearVelocities[i].setZero();
		deltaAngularVelocities[i].setZero();
	}



	for (int iter = 0;iter<maxIter;iter++)
	{
		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;
			b3RigidBodyData& bodyA = bodies[aIdx];
			b3RigidBodyData& bodyB = bodies[bIdx];

			b3Vector3 zero = b3MakeVector3(0,0,0);
			
			b3Vector3* dlvAPtr=&zero;
			b3Vector3* davAPtr=&zero;
			b3Vector3* dlvBPtr=&zero;
			b3Vector3* davBPtr=&zero;
			
			if (bodyA.m_invMass)
			{
				int bodyOffsetA = offsetSplitBodies[aIdx];
				int constraintOffsetA = contactConstraintOffsets[i].x;
				int splitIndexA = bodyOffsetA+constraintOffsetA;
				dlvAPtr = &deltaLinearVelocities[splitIndexA];
				davAPtr = &deltaAngularVelocities[splitIndexA];
			}

			if (bodyB.m_invMass)
			{
				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], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, 
					(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)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].m_invMass)
			{
				int bodyOffset = offsetSplitBodies[i];
				int count = bodyCount[i];
				float factor = 1.f/float(count);
				b3Vector3 averageLinVel;
				averageLinVel.setZero();
				b3Vector3 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;
				}
			}
		}
	}
	for (int iter = 0;iter<maxIter;iter++)
	{
		int i=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;
			b3RigidBodyData& bodyA = bodies[aIdx];
			b3RigidBodyData& bodyB = bodies[bIdx];

			b3Vector3 zero = b3MakeVector3(0,0,0);
			
			b3Vector3* dlvAPtr=&zero;
			b3Vector3* davAPtr=&zero;
			b3Vector3* dlvBPtr=&zero;
			b3Vector3* davBPtr=&zero;
			
			if (bodyA.m_invMass)
			{
				int bodyOffsetA = offsetSplitBodies[aIdx];
				int constraintOffsetA = contactConstraintOffsets[i].x;
				int splitIndexA = bodyOffsetA+constraintOffsetA;
				dlvAPtr = &deltaLinearVelocities[splitIndexA];
				davAPtr = &deltaAngularVelocities[splitIndexA];
			}

			if (bodyB.m_invMass)
			{
				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], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,inertias[aIdx].m_invInertiaWorld, 
				(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)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].m_invMass)
			{
				int bodyOffset = offsetSplitBodies[i];
				int count = bodyCount[i];
				float factor = 1.f/float(count);
				b3Vector3 averageLinVel;
				averageLinVel.setZero();
				b3Vector3 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;
				}
			}
		}



	}


	//easy
	for (int i=0;i<numBodies;i++)
	{
		if (bodies[i].m_invMass)
		{
			int bodyOffset = offsetSplitBodies[i];
			int count = bodyCount[i];
			if (count)
			{
				bodies[i].m_linVel += deltaLinearVelocities[bodyOffset];
				bodies[i].m_angVel += deltaAngularVelocities[bodyOffset];
			}
		}
	}
}
Пример #3
0
void  b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyData>* bodiesGPU,b3OpenCLArray<b3InertiaData>* inertiasGPU,b3OpenCLArray<b3Contact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo)
{

	b3AlignedObjectArray<b3RigidBodyData> bodiesCPU;
	bodiesGPU->copyToHost(bodiesCPU);
	b3AlignedObjectArray<b3InertiaData> inertiasCPU;
	inertiasGPU->copyToHost(inertiasCPU);
	b3AlignedObjectArray<b3Contact4> manifoldPtrCPU;
	manifoldPtrGPU->copyToHost(manifoldPtrCPU);
	
	int numBodiesCPU = bodiesGPU->size();
	int numManifoldsCPU = manifoldPtrGPU->size();
	B3_PROFILE("b3GpuJacobiContactSolver::solveGroupMixed");

	b3AlignedObjectArray<unsigned int> bodyCount;
	bodyCount.resize(numBodiesCPU);
	for (int i=0;i<numBodiesCPU;i++)
		bodyCount[i] = 0;

	b3AlignedObjectArray<b3Int2> 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]++;
		} 
	}

	b3AlignedObjectArray<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;
	b3Int2 val2;
	val2.x=0;
	val2.y=0;

	 {
		B3_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);
	}

	{
		B3_PROFILE("m_countBodiesKernel");
		b3LauncherCL 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);

	
	{
		B3_PROFILE("contactToConstraintSplitKernel");
		b3LauncherCL 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);
	}



	b3AlignedObjectArray<b3GpuConstraint4> 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;


	b3AlignedObjectArray<b3Vector3> deltaLinearVelocities;
	b3AlignedObjectArray<b3Vector3> 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);


	
	{
		B3_PROFILE("m_clearVelocitiesKernel");
		b3LauncherCL 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++)
	{

				{
			B3_PROFILE("m_solveContactKernel");
			b3LauncherCL 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;
			b3RigidBodyData& bodyA = bodiesCPU[aIdx];
			b3RigidBodyData& bodyB = bodiesCPU[bIdx];

			b3Vector3 zero(0,0,0);
			
			b3Vector3* dlvAPtr=&zero;
			b3Vector3* davAPtr=&zero;
			b3Vector3* dlvBPtr=&zero;
			b3Vector3* davBPtr=&zero;
			
			if (bodyA.m_invMass)
			{
				int bodyOffsetA = offsetSplitBodies[aIdx];
				int constraintOffsetA = contactConstraintOffsets[i].x;
				int splitIndexA = bodyOffsetA+constraintOffsetA;
				dlvAPtr = &deltaLinearVelocities[splitIndexA];
				davAPtr = &deltaAngularVelocities[splitIndexA];
			}

			if (bodyB.m_invMass)
			{
				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], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertiasCPU[aIdx].m_invInertiaWorld, 
					(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
					maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr		);


			}
		}

		
		{
			B3_PROFILE("average velocities");
			b3LauncherCL 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].m_invMass)
			{
				int bodyOffset = offsetSplitBodies[i];
				int count = bodyCount[i];
				float factor = 1.f/float(count);
				b3Vector3 averageLinVel;
				averageLinVel.setZero();
				b3Vector3 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

		{
			B3_PROFILE("m_solveFrictionKernel");
			b3LauncherCL 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;
			b3RigidBodyData& bodyA = bodiesCPU[aIdx];
			b3RigidBodyData& bodyB = bodiesCPU[bIdx];

			b3Vector3 zero(0,0,0);
			
			b3Vector3* dlvAPtr=&zero;
			b3Vector3* davAPtr=&zero;
			b3Vector3* dlvBPtr=&zero;
			b3Vector3* davBPtr=&zero;
			
			if (bodyA.m_invMass)
			{
				int bodyOffsetA = offsetSplitBodies[aIdx];
				int constraintOffsetA = contactConstraintOffsets[i].x;
				int splitIndexA = bodyOffsetA+constraintOffsetA;
				dlvAPtr = &deltaLinearVelocities[splitIndexA];
				davAPtr = &deltaAngularVelocities[splitIndexA];
			}

			if (bodyB.m_invMass)
			{
				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], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,inertiasCPU[aIdx].m_invInertiaWorld, 
				(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
				maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr);

		}

		{
			B3_PROFILE("average velocities");
			b3LauncherCL 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].m_invMass)
			{
				int bodyOffset = offsetSplitBodies[i];
				int count = bodyCount[i];
				float factor = 1.f/float(count);
				b3Vector3 averageLinVel;
				averageLinVel.setZero();
				b3Vector3 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

	}

	{
		B3_PROFILE("update body velocities");
		b3LauncherCL 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].m_invMass)
		{
			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);


}
Пример #4
0
	void run(int tIdx)
	{
		int offset = 0;
		for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++)
		{
			int numInBatch = m_batchSizes->at(m_cellIndex*B3_MAX_NUM_BATCHES+ii);
			if (!numInBatch)
				break;

			for (int jj=0;jj<numInBatch;jj++)
			{
				int i = m_start + offset+jj;
				int batchId = m_constraints[i].m_batchIdx;
				b3Assert(batchId==ii);
				float frictionCoeff = m_constraints[i].getFrictionCoeff();
				int aIdx = (int)m_constraints[i].m_bodyA;
				int bIdx = (int)m_constraints[i].m_bodyB;
				int localBatch = m_constraints[i].m_batchIdx;
				b3RigidBodyCL& bodyA = m_bodies[aIdx];
				b3RigidBodyCL& bodyB = m_bodies[bIdx];

				if( !m_solveFriction )
				{
					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

					solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
				}
				else
				{
					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 +=m_constraints[i].m_appliedRambdaDt[j];
					}
					frictionCoeff = 0.7f;
					for(int j=0; j<4; j++)
					{
						maxRambdaDt[j] = frictionCoeff*sum;
						minRambdaDt[j] = -maxRambdaDt[j];
					}
					solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
			
				}
			}
			offset+=numInBatch;


		}
/*		for (int bb=0;bb<m_maxNumBatches;bb++)
		{
			//for(int ic=m_nConstraints-1; ic>=0; ic--)
			for(int ic=0; ic<m_nConstraints; ic++)
			{
				
				int i = m_start + ic;
				if (m_constraints[i].m_batchIdx != bb)
					continue;

				float frictionCoeff = m_constraints[i].getFrictionCoeff();
				int aIdx = (int)m_constraints[i].m_bodyA;
				int bIdx = (int)m_constraints[i].m_bodyB;
				int localBatch = m_constraints[i].m_batchIdx;
				b3RigidBodyCL& bodyA = m_bodies[aIdx];
				b3RigidBodyCL& bodyB = m_bodies[bIdx];

				if( !m_solveFriction )
				{
					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

					solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
				}
				else
				{
					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 +=m_constraints[i].m_appliedRambdaDt[j];
					}
					frictionCoeff = 0.7f;
					for(int j=0; j<4; j++)
					{
						maxRambdaDt[j] = frictionCoeff*sum;
						minRambdaDt[j] = -maxRambdaDt[j];
					}
					solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
			
				}
			}
		}
		*/


		
	}