C++ SimulationModel类代码示例

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (0.005);

	createMesh();

	// create static rigid body
	string fileName = dataPath + "/models/cube.obj";
	IndexedFaceMesh mesh;
	VertexData vd;
	OBJLoader::loadObj(fileName, vd, mesh);	

	string fileNameTorus = dataPath + "/models/torus.obj";
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	OBJLoader::loadObj(fileNameTorus, vdTorus, meshTorus);

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	rb.resize(2);

	// floor
	rb[0] = new RigidBody();
	rb[0]->initBody(1.0,
		Vector3r(0.0, -5.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh,
		Vector3r(100.0, 1.0, 100.0));
	rb[0]->setMass(0.0);

	// torus
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(5.0, -1.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdTorus, meshTorus,
		Vector3r(3.0, 3.0, 3.0));
	rb[1]->setMass(0.0);
	rb[1]->setFrictionCoeff(0.1);

	sim.setCollisionDetection(model, &cd);
	cd.setTolerance(0.05);
	
	const std::vector<Vector3r> *vertices1 = rb[0]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert1 = static_cast<unsigned int>(vertices1->size());
	cd.addCollisionBox(0, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices1)[0], nVert1, Vector3r(100.0, 1.0, 100.0));

	const std::vector<Vector3r> *vertices2 = rb[1]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert2 = static_cast<unsigned int>(vertices2->size());
	cd.addCollisionTorus(1, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices2)[0], nVert2, Vector2r(3.0, 1.5));

	SimulationModel::TetModelVector &tm = model.getTetModels();
	ParticleData &pd = model.getParticles();
	for (unsigned int i = 0; i < tm.size(); i++)
	{
		const unsigned int nVert = tm[i]->getParticleMesh().numVertices();
		unsigned int offset = tm[i]->getIndexOffset();
		tm[i]->setFrictionCoeff(0.1);
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert);
	}
}

bool GenericDistanceConstraint::solvePositionConstraint(SimulationModel &model)
{
    ParticleData &pd = model.getParticles();

    const unsigned i1 = m_bodies[0];
    const unsigned i2 = m_bodies[1];

    Eigen::Vector3f &x1 = pd.getPosition(i1);
    Eigen::Vector3f &x2 = pd.getPosition(i2);
    const float invMass1 = pd.getInvMass(i1);
    const float invMass2 = pd.getInvMass(i2);

    const float invMass[2] = { invMass1, invMass2 };
    const Eigen::Vector3f x[2] = { x1, x2 };

    Eigen::Vector3f corr[2];
    const bool res = PositionBasedGenericConstraints::solve_GenericConstraint<2, 1>(
                         invMass, x, &m_restLength,
                         GenericDistanceConstraint::constraintFct,
                         GenericDistanceConstraint::gradientFct,
                         corr);

    if (res)
    {
        const float stiffness = model.getClothStiffness();
        if (invMass1 != 0.0f)
            x1 += stiffness * corr[0];
        if (invMass2 != 0.0f)
            x2 += stiffness * corr[1];
    }
    return res;
}

//////////////////////////////////////////////////////////////////////////
// ParticleRigidBodyContactConstraint
//////////////////////////////////////////////////////////////////////////
bool ParticleRigidBodyContactConstraint::initConstraint(SimulationModel &model, 
	const unsigned int particleIndex, const unsigned int rbIndex,
	const Vector3r &cp1, const Vector3r &cp2,
	const Vector3r &normal, const Real dist,
	const Real restitutionCoeff, const Real stiffness, const Real frictionCoeff)
{
	m_stiffness = stiffness;
	m_frictionCoeff = frictionCoeff;

	m_bodies[0] = particleIndex;
	m_bodies[1] = rbIndex;
	SimulationModel::RigidBodyVector &rbs = model.getRigidBodies();
	ParticleData &pd = model.getParticles();

	RigidBody &rb = *rbs[m_bodies[1]];

	m_sum_impulses = 0.0;

	return PositionBasedRigidBodyDynamics::init_ParticleRigidBodyContactConstraint(
		pd.getInvMass(particleIndex),
		pd.getPosition(particleIndex),
		pd.getVelocity(particleIndex),
		rb.getInvMass(),
		rb.getPosition(),
		rb.getVelocity(),
		rb.getInertiaTensorInverseW(),
		rb.getRotation(),
		rb.getAngularVelocity(),		
		cp1, cp2, normal, restitutionCoeff,
		m_constraintInfo);
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
 	std::vector<unsigned int> hits;
 	
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	
	selectedBodies.clear(); 
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > x;
	x.resize(rb.size());
 	for (unsigned int i = 0; i < rb.size(); i++)
 	{
 		x[i] = rb[i]->getPosition();
 	}
 
 	Selection::selectRect(start, end, &x[0], &x[rb.size() - 1], selectedBodies);
	if ((selectedBodies.size() > 0) || (selectedParticles.size() > 0))
 		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
 	else
 		MiniGL::setMouseMoveFunc(-1, NULL);
 
 	MiniGL::unproject(end[0], end[1], oldMousePos);
}

bool DistanceConstraint::solvePositionConstraint(SimulationModel &model)
{
	ParticleData &pd = model.getParticles();

	const unsigned i1 = m_bodies[0];
	const unsigned i2 = m_bodies[1];

	Eigen::Vector3f &x1 = pd.getPosition(i1);
	Eigen::Vector3f &x2 = pd.getPosition(i2);
	const float invMass1 = pd.getInvMass(i1);
	const float invMass2 = pd.getInvMass(i2);

	Eigen::Vector3f corr1, corr2;
	const bool res = PositionBasedDynamics::solve_DistanceConstraint(
		x1, invMass1, x2, invMass2,
		m_restLength, model.getClothStiffness(), model.getClothStiffness(), corr1, corr2);

	if (res)
	{
		if (invMass1 != 0.0f)
			x1 += corr1;
		if (invMass2 != 0.0f)
			x2 += corr2;
	}
	return res;
}

void TimeStepController::clearAccelerations(SimulationModel &model)
{
	//////////////////////////////////////////////////////////////////////////
	// rigid body model
	//////////////////////////////////////////////////////////////////////////

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
 	const Eigen::Vector3f grav(0.0f, -9.81f, 0.0f);
 	for (size_t i=0; i < rb.size(); i++)
 	{
 		// Clear accelerations of dynamic particles
 		if (rb[i]->getMass() != 0.0)
 		{
			Eigen::Vector3f &a = rb[i]->getAcceleration();
 			a = grav;
 		}
 	}

	//////////////////////////////////////////////////////////////////////////
	// particle model
	//////////////////////////////////////////////////////////////////////////

	ParticleData &pd = model.getParticles();
	const unsigned int count = pd.size();
	for (unsigned int i = 0; i < count; i++)
	{
		// Clear accelerations of dynamic particles
		if (pd.getMass(i) != 0.0)
		{
			Eigen::Vector3f &a = pd.getAcceleration(i);
			a = grav;
		}
	}
}

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++;
 	}
}

//////////////////////////////////////////////////////////////////////////
// ElasticRodBendAndTwistConstraint
//////////////////////////////////////////////////////////////////////////
bool ElasticRodBendAndTwistConstraint::initConstraint(SimulationModel &model, const unsigned int pA, const unsigned int pB,
	const unsigned int pC, const unsigned int pD, const unsigned int pE)
{
	m_bodies[0] = pA;
	m_bodies[1] = pB;
	m_bodies[2] = pC;
	m_bodies[3] = pD; //ghost point id
	m_bodies[4] = pE; //ghost point id

	ParticleData &pd = model.getParticles();
	ParticleData &pg = model.getGhostParticles();

	const Eigen::Vector3f &xA = pd.getPosition0(m_bodies[0]);
	const Eigen::Vector3f &xB = pd.getPosition0(m_bodies[1]);
	const Eigen::Vector3f &xC = pd.getPosition0(m_bodies[2]);
	const Eigen::Vector3f &xD = pg.getPosition0(m_bodies[3]);
	const Eigen::Vector3f &xE = pg.getPosition0(m_bodies[4]);

	PositionBasedElasticRod::ComputeMaterialFrame(xA, xB, xD, m_dA);
	PositionBasedElasticRod::ComputeMaterialFrame(xB, xC, xE, m_dB);

	PositionBasedElasticRod::ComputeDarbouxVector(m_dA, m_dB, 1.0f, m_restDarbouxVector);
	m_bendAndTwistKs.setOnes();

	return true;
}

bool RigidBodyParticleBallJoint::updateConstraint(SimulationModel &model)
{
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	ParticleData &pd = model.getParticles();
	RigidBody &rb1 = *rb[m_bodies[0]];
	return PositionBasedRigidBodyDynamics::update_RigidBodyParticleBallJoint(
		rb1.getPosition(),
		rb1.getRotation(),
		pd.getPosition(m_bodies[1]),
		m_jointInfo);
}

void TimeStepController::contactCallbackFunction(const unsigned int contactType, const unsigned int bodyIndex1, const unsigned int bodyIndex2,
		const Vector3r &cp1, const Vector3r &cp2, 
		const Vector3r &normal, const Real dist,
		const Real restitutionCoeff, const Real frictionCoeff, void *userData)
{
	SimulationModel *model = (SimulationModel*)userData;
	if (contactType == CollisionDetection::RigidBodyContactType)
		model->addRigidBodyContactConstraint(bodyIndex1, bodyIndex2, cp1, cp2, normal, dist, restitutionCoeff,frictionCoeff);
	else if (contactType == CollisionDetection::ParticleRigidBodyContactType)
		model->addParticleRigidBodyContactConstraint(bodyIndex1, bodyIndex2, cp1, cp2, normal, dist, restitutionCoeff, frictionCoeff);
}

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++;
	}
}

void TimeStepController::clearAccelerations(SimulationModel &model)
{
	//////////////////////////////////////////////////////////////////////////
	// rigid body model
	//////////////////////////////////////////////////////////////////////////

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
 	for (size_t i=0; i < rb.size(); i++)
 	{
 		// Clear accelerations of dynamic particles
 		if (rb[i]->getMass() != 0.0)
 		{
			Vector3r &a = rb[i]->getAcceleration();
 			a = m_gravity;
 		}
 	}

	//////////////////////////////////////////////////////////////////////////
	// particle model
	//////////////////////////////////////////////////////////////////////////

	ParticleData &pd = model.getParticles();
	const unsigned int count = pd.size();
	for (unsigned int i = 0; i < count; i++)
	{
		// Clear accelerations of dynamic particles
		if (pd.getMass(i) != 0.0)
		{
			Vector3r &a = pd.getAcceleration(i);
			a = m_gravity;
		}
	}

ParticleData &pg = model.getGhostParticles();
	for (unsigned int i = 0; i <  pg.size(); i++)
	{
		// Clear accelerations of dynamic particles
		if (pg.getMass(i) != 0.0)
		{
			Vector3r &a = pg.getAcceleration(i);
			//a = grav;
			a.setZero();
		}
	}
}

void renderModels()
{
	// Draw simulation model

	const ParticleData &pd = model.getParticles();
	float surfaceColor[4] = { 0.2f, 0.5f, 1.0f, 1 };

	if (shader)
	{
		shader->begin();
		glUniform3fv(shader->getUniform("surface_color"), 1, surfaceColor);
		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 (unsigned int i = 0; i < model.getTetModels().size(); i++)
	{
		TetModel *tetModel = model.getTetModels()[i];
		const IndexedFaceMesh &surfaceMesh = tetModel->getSurfaceMesh();
		Visualization::drawMesh(pd, surfaceMesh, tetModel->getIndexOffset(), surfaceColor);
	}

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	float rbColor[4] = { 0.4f, 0.4f, 0.4f, 1 };
	for (size_t i = 0; i < rb.size(); i++)
	{
		const VertexData &vd = rb[i]->getGeometry().getVertexData();
		const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
		if (shader)
			glUniform3fv(shader->getUniform("surface_color"), 1, rbColor);
		Visualization::drawTexturedMesh(vd, mesh, 0, rbColor);
	}

	if (shader)
		shader->end();
}

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

// main 
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	base = new DemoBase();
	base->init(argc, argv, "Bar demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	initParameters();

	Simulation::getCurrent()->setSimulationMethodChangedCallback([&]() { reset(); initParameters(); base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep()); });

	// OpenGL
	MiniGL::setClientIdleFunc (50, timeStep);		
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);			
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
			" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "PoissonRatio", TW_TYPE_REAL, setPoissonRatio, getPoissonRatio, model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Strain based dynamics' ");

	glutMainLoop ();	

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}