C++ SimulationModel::getConstraints方法代码示例

void TimeStepController::constraintProjection(SimulationModel &model)
{
	const unsigned int maxIter = 5;
	unsigned int iter = 0;

	// init constraint groups if necessary
	model.initConstraintGroups();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();
	SimulationModel::ConstraintGroupVector &groups = model.getConstraintGroups();

 	while (iter < maxIter)
 	{
		for (unsigned int group = 0; group < groups.size(); group++)
		{
			#pragma omp parallel default(shared)
			{
				#pragma omp for schedule(static) 
				for (int i = 0; i < (int)groups[group].size(); i++)
				{
					const unsigned int constraintIndex = groups[group][i];

					constraints[constraintIndex]->updateConstraint(model);
					constraints[constraintIndex]->solveConstraint(model);
				}
			}
		}

 		iter++;
 	}
}

void TimeStepController::velocityConstraintProjection(SimulationModel &model)
{
	unsigned int iter = 0;

	// init constraint groups if necessary
	model.initConstraintGroups();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();
	SimulationModel::ConstraintGroupVector &groups = model.getConstraintGroups();
	SimulationModel::RigidBodyContactConstraintVector &rigidBodyContacts = model.getRigidBodyContactConstraints();
	SimulationModel::ParticleRigidBodyContactConstraintVector &particleRigidBodyContacts = model.getParticleRigidBodyContactConstraints();

	for (unsigned int group = 0; group < groups.size(); group++)
	{
		const int groupSize = (int)groups[group].size();
		#pragma omp parallel if(groupSize > MIN_PARALLEL_SIZE) default(shared)
		{
			#pragma omp for schedule(static) 
			for (int i = 0; i < groupSize; i++)
			{
				const unsigned int constraintIndex = groups[group][i];
				constraints[constraintIndex]->updateConstraint(model);
			}
		}
	}

	while (iter < m_maxIterVel)
	{
		for (unsigned int group = 0; group < groups.size(); group++)
		{
			const int groupSize = (int)groups[group].size();
			#pragma omp parallel if(groupSize > MIN_PARALLEL_SIZE) default(shared)
			{
				#pragma omp for schedule(static) 
				for (int i = 0; i < groupSize; i++)
				{
					const unsigned int constraintIndex = groups[group][i];
					constraints[constraintIndex]->solveVelocityConstraint(model);
				}
			}
		}

		// solve contacts
		for (unsigned int i = 0; i < rigidBodyContacts.size(); i++)
			rigidBodyContacts[i].solveVelocityConstraint(model);
		for (unsigned int i = 0; i < particleRigidBodyContacts.size(); i++)
			particleRigidBodyContacts[i].solveVelocityConstraint(model);

		iter++;
	}
}

/** Create the elastic rod model
*/
void createRod()
{
	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	//centreline points
	for (unsigned int i = 0; i < numberOfPoints; i++)
	{	
		particles.addVertex(Vector3r((float)i*1.0f, 0.0f, 0.0f));
	}

	//edge ghost points
	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		ghostParticles.addVertex(Vector3r((float)i*1.0f + 0.5f, 1.0f, 0.0f));
	}

	//lock two first particles and first ghost point
	particles.setMass(0, 0.0f);
	particles.setMass(1, 0.0f);
	ghostParticles.setMass(0, 0.0f);

	for (unsigned int i = 0; i < numberOfPoints - 1; i++)
	{
		model.addElasticRodEdgeConstraint(i, i + 1, i);
		
		if (i < numberOfPoints - 2)
		{	
			//  Single rod element:
			//      D   E		//ghost points
			//		|	|
			//  --A---B---C--	// rod points
			int pA = i;
			int pB = i + 1;
			int pC = i + 2;
			int pD = i;
			int pE = i + 1;
			model.addElasticRodBendAndTwistConstraint(pA, pB, pC, pD, pE);
		}
	}
}

void render()
{
	MiniGL::coordinateSystem();

	// Draw sim model

	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float pointColor[4] = { 0.1f, 0.1f, 0.5f, 1 };
	float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f };
	float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 };

	for (size_t i = 0; i < numberOfPoints; i++)
	{
		MiniGL::drawSphere(particles.getPosition(i), 0.2f, pointColor);
	}
	
	for (size_t i = 0; i < numberOfPoints-1; i++)
	{
		MiniGL::drawSphere(ghostParticles.getPosition(i), 0.1f, ghostPointColor);
		MiniGL::drawVector(particles.getPosition(i), particles.getPosition(i + 1), 0.2f, edgeColor);
	}

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == ElasticRodBendAndTwistConstraint::TYPE_ID)
		{
			((ElasticRodBendAndTwistConstraint*)constraints[i])->m_restDarbouxVector = restDarboux;
		}
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the rigid body model
*/
void createBodyModel()
{
	SimulationModel *model = Simulation::getCurrent()->getModel();
	SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();

	string fileNameBox = FileSystem::normalizePath(base->getDataPath() + "/models/cube.obj");
	IndexedFaceMesh meshBox;
	VertexData vdBox;
	loadObj(fileNameBox, vdBox, meshBox, Vector3r::Ones());

	string fileNameCylinder = FileSystem::normalizePath(base->getDataPath() + "/models/cylinder.obj");
	IndexedFaceMesh meshCylinder;
	VertexData vdCylinder;
	loadObj(fileNameCylinder, vdCylinder, meshCylinder, Vector3r::Ones());

	string fileNameTorus = FileSystem::normalizePath(base->getDataPath() + "/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	loadObj(fileNameTorus, vdTorus, meshTorus, Vector3r::Ones());

	string fileNameCube = FileSystem::normalizePath(base->getDataPath() + "/models/cube_5.obj");
	IndexedFaceMesh meshCube;
	VertexData vdCube;
	loadObj(fileNameCube, vdCube, meshCube, Vector3r::Ones());

	string fileNameSphere = FileSystem::normalizePath(base->getDataPath() + "/models/sphere.obj");
	IndexedFaceMesh meshSphere;
	VertexData vdSphere;
	loadObj(fileNameSphere, vdSphere, meshSphere, 2.0*Vector3r::Ones());


	const unsigned int num_piles_x = 5;
	const unsigned int num_piles_z = 5;
	const Real dx_piles = 4.0;
	const Real dz_piles = 4.0;
	const Real startx_piles = -0.5 * (Real)(num_piles_x - 1)*dx_piles;
	const Real startz_piles = -0.5 * (Real)(num_piles_z - 1)*dz_piles;
	const unsigned int num_piles = num_piles_x * num_piles_z;
	const unsigned int num_bodies_x = 3;
	const unsigned int num_bodies_y = 5;
	const unsigned int num_bodies_z = 3;
	const Real dx_bodies = 6.0;
	const Real dy_bodies = 6.0;
	const Real dz_bodies = 6.0;
	const Real startx_bodies = -0.5 * (Real)(num_bodies_x - 1)*dx_bodies;
	const Real starty_bodies = 14.0;
	const Real startz_bodies = -0.5 * (Real)(num_bodies_z - 1)*dz_bodies;
	const unsigned int num_bodies = num_bodies_x * num_bodies_y * num_bodies_z;
	rb.resize(num_piles + num_bodies + 1);
	unsigned int rbIndex = 0;

	// floor
	rb[rbIndex] = new RigidBody();
	rb[rbIndex]->initBody(1.0,
		Vector3r(0.0, -0.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdBox, meshBox, Vector3r(100.0, 1.0, 100.0));
	rb[rbIndex]->setMass(0.0);

	const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert = static_cast<unsigned int>(vertices->size());

	cd.addCollisionBox(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector3r(100.0, 1.0, 100.0));
	rbIndex++;

	Real current_z = startz_piles;
	for (unsigned int i = 0; i < num_piles_z; i++)
	{ 
		Real current_x = startx_piles;
		for (unsigned int j = 0; j < num_piles_x; j++)
		{
			rb[rbIndex] = new RigidBody();
			rb[rbIndex]->initBody(100.0,
				Vector3r(current_x, 5.0, current_z),
				Quaternionr(1.0, 0.0, 0.0, 0.0),
				vdCylinder, meshCylinder, 
				Vector3r(0.5, 10.0, 0.5));
			rb[rbIndex]->setMass(0.0);

			const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
			const unsigned int nVert = static_cast<unsigned int>(vertices->size());
			cd.addCollisionCylinder(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector2r(0.5, 10.0));
			current_x += dx_piles;
			rbIndex++;
		}
		current_z += dz_piles;
	}

	srand((unsigned int) time(NULL));

	Real current_y = starty_bodies;
	unsigned int currentType = 0;
	for (unsigned int i = 0; i < num_bodies_y; i++)
	{
		Real current_x = startx_bodies;
		for (unsigned int j = 0; j < num_bodies_x; j++)
		{
//.........这里部分代码省略.........

/** Create the model
*/
void createRigidBodyModel()
{
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	string fileName = dataPath + "/models/cube.obj";
	IndexedFaceMesh mesh;
	VertexData vd;
	OBJLoader::loadObj(fileName, vd, mesh, Eigen::Vector3f(width, height, depth));

	rb.resize(12);

	//////////////////////////////////////////////////////////////////////////
	// -5, -5
	//////////////////////////////////////////////////////////////////////////
	rb[0] = new RigidBody();
	rb[0]->initBody(0.0f,
		Eigen::Vector3f(-5.0, 0.0f, -5.0),
		computeInertiaTensorBox(1.0f, 0.5f, 0.5f, 0.5f),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f), 
		vd, mesh);

	// dynamic body
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0f,
		Eigen::Vector3f(-5.0f, 1.0f, -5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	// dynamic body
	rb[2] = new RigidBody();
	rb[2]->initBody(1.0f,
		Eigen::Vector3f(-5.0f, 3.0f, -5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	model.addBallJoint(0, 1, Eigen::Vector3f(-5.0f, 0.0f, -5.0f));
	model.addBallJoint(1, 2, Eigen::Vector3f(-5.0f, 2.0f, -5.0f));

	//////////////////////////////////////////////////////////////////////////
	// 5, -5
	//////////////////////////////////////////////////////////////////////////
	rb[3] = new RigidBody();
	rb[3]->initBody(0.0f,
		Eigen::Vector3f(5.0, 0.0f, -5.0),
		computeInertiaTensorBox(1.0f, 0.5f, 0.5f, 0.5f),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	// dynamic body
	rb[4] = new RigidBody();
	rb[4]->initBody(1.0f,
		Eigen::Vector3f(5.0f, 1.0f, -5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	// dynamic body
	rb[5] = new RigidBody();
	rb[5]->initBody(1.0f,
		Eigen::Vector3f(5.0f, 3.0f, -5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	model.addBallJoint(3, 4, Eigen::Vector3f(5.0f, 0.0f, -5.0f));
	model.addBallJoint(4, 5, Eigen::Vector3f(5.0f, 2.0f, -5.0f));

	//////////////////////////////////////////////////////////////////////////
	// 5, 5
	//////////////////////////////////////////////////////////////////////////
	rb[6] = new RigidBody();
	rb[6]->initBody(0.0f,
		Eigen::Vector3f(5.0, 0.0f, 5.0),
		computeInertiaTensorBox(1.0f, 0.5f, 0.5f, 0.5f),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	// dynamic body
	rb[7] = new RigidBody();
	rb[7]->initBody(1.0f,
		Eigen::Vector3f(5.0f, 1.0f, 5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	// dynamic body
	rb[8] = new RigidBody();
	rb[8]->initBody(1.0f,
		Eigen::Vector3f(5.0f, 3.0f, 5.0f),
		computeInertiaTensorBox(1.0f, width, height, depth),
		Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f),
		vd, mesh);

	model.addBallJoint(6, 7, Eigen::Vector3f(5.0f, 0.0f, 5.0f));
	model.addBallJoint(7, 8, Eigen::Vector3f(5.0f, 2.0f, 5.0f));
//.........这里部分代码省略.........

void renderRigidBodyModel()
{
	// Draw simulation model

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float surfaceColor[4] = { 0.1f, 0.4f, 0.8f, 1 };

	if (shader)
	{
		shader->begin();
		glUniform1f(shader->getUniform("shininess"), 5.0f);
		glUniform1f(shader->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shader->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shader->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (size_t i = 0; i < rb.size(); i++)
	{
		bool selected = false;
		for (unsigned int j = 0; j < selectedBodies.size(); j++)
		{
			if (selectedBodies[j] == i)
				selected = true;
		}

		if (rb[i]->getMass() != 0.0f)
		{

			const VertexData &vd = rb[i]->getGeometry().getVertexData();
			const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
			if (!selected)
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, surfaceColor);
				Visualization::drawMesh(vd, mesh, surfaceColor);
			}
			else
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, selectionColor);
				Visualization::drawMesh(vd, mesh, selectionColor);
			}
		}
	}
	if (shader)
		shader->end();

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == BallJoint::TYPE_ID)
		{
			renderBallJoint(*(BallJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == RigidBodyParticleBallJoint::TYPE_ID)
		{
			renderRigidBodyParticleBallJoint(*(RigidBodyParticleBallJoint*)constraints[i]);
		}
	}
}