コード例 #1
0
ファイル: physics_func.cpp プロジェクト: madsbh/experiments
void BulletConstraintSolver()
{
	btPgsSolver pgs;
	btContactSolverInfo info;
	rbs.resize(0);

	for (int i=0;i<numRigidBodies;i++)
	{
		btRigidBody& rb = rbs.expandNonInitializing();

		rb.m_companionId=-1;
		rb.m_angularFactor.setValue(1,1,1);
		rb.m_anisotropicFriction.setValue(1,1,1);
		
		rb.m_invMass = bodies[i].getMassInv();
		rb.m_linearFactor.setValue(1,1,1);
		btVector3 pos(states[i].getPosition().getX(),states[i].getPosition().getY(),states[i].getPosition().getZ());
		rb.m_worldTransform.setIdentity();
		btQuaternion orn(states[i].getOrientation().getX(),states[i].getOrientation().getY(),states[i].getOrientation().getZ(),states[i].getOrientation().getW());
		rb.m_worldTransform.setRotation(orn);
		rb.m_worldTransform.setOrigin(pos);
		PfxMatrix3 ori(states[i].getOrientation());
		rb.m_worldTransform.setRotation(orn);

		PfxMatrix3 inertiaInvWorld = ori * bodies[i].getInertiaInv() * transpose(ori);
		rb.m_invInertiaWorld.setIdentity();

		if (rb.m_invMass)
		{
			for (int row=0;row<3;row++)
			{
				for (int col=0;col<3;col++)
				{
					rb.m_invInertiaWorld[col][row] = inertiaInvWorld.getElem(col,row);
				}
			}
		} else
		{
			rb.m_invInertiaWorld = btMatrix3x3(0,0,0,0,0,0,0,0,0);
		}
		rb.m_linearVelocity.setValue(states[i].getLinearVelocity().getX(),states[i].getLinearVelocity().getY(),states[i].getLinearVelocity().getZ());
		rb.m_angularVelocity.setValue(states[i].getAngularVelocity().getX(),states[i].getAngularVelocity().getY(),states[i].getAngularVelocity().getZ());
//		printf("body added\n");
	}

	btAlignedObjectArray<btCollisionObject*> bodyPtrs;
	bodyPtrs.resize(rbs.size());
	for (int i=0;i<rbs.size();i++)
	{
		bodyPtrs[i] = &rbs[i];
	}


	unsigned int numCurrentPairs = numPairs[pairSwap];
	PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];

	PfxSetupContactConstraintsParam param;
	param.contactPairs = currentPairs;
	param.numContactPairs = numCurrentPairs;
	param.offsetContactManifolds = contacts;
	param.offsetRigidStates = states;
	param.offsetRigidBodies = bodies;
	param.offsetSolverBodies = solverBodies;
	param.numRigidBodies = numRigidBodies;
	param.timeStep = timeStep;
	param.separateBias = separateBias;

	BulletSetupContactConstraints(param);

	btAlignedObjectArray<btPersistentManifold*> manifoldPtrs;
	manifoldPtrs.resize(manifolds.size());

	for (int i=0;i<manifolds.size();i++)
	{
		manifoldPtrs[i] = &manifolds[i];
	}

	if (bodyPtrs.size() && manifoldPtrs.size())
	{
		pgs.solveGroup(&bodyPtrs[0],bodyPtrs.size(),&manifoldPtrs[0],manifoldPtrs.size(),0,0,info,0,0,0);

		for (int i=0;i<numRigidBodies;i++)
		{
			btVector3 linvel = rbs[i].getLinearVelocity();
			btVector3 angvel = rbs[i].getAngularVelocity();
			states[i].setLinearVelocity(PfxVector3(linvel.getX(),linvel.getY(),linvel.getZ()));
			states[i].setAngularVelocity(PfxVector3(angvel.getX(),angvel.getY(),angvel.getZ()));
		}
	}

	BulletWriteWarmstartContactConstraints(param);



}