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

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

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

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

bool ElasticRodBendAndTwistConstraint::solvePositionConstraint(SimulationModel &model)
{
	ParticleData &pd = model.getParticles();
	ParticleData &pg = model.getGhostParticles();

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

	const float wA = pd.getInvMass(m_bodies[0]);
	const float wB = pd.getInvMass(m_bodies[1]);
	const float wC = pd.getInvMass(m_bodies[2]);
	const float wD = pg.getInvMass(m_bodies[3]);
	const float wE = pg.getInvMass(m_bodies[4]);

	Eigen::Vector3f corr[5];

	bool res = PositionBasedElasticRod::ProjectBendingAndTwistingConstraint(
		xA, wA, xB, wB, xC, wC, xD, wD, xE, wE, 
		m_bendAndTwistKs, 1.0f, m_restDarbouxVector, 
		corr[0], corr[1], corr[2], corr[3], corr[4], true);

	if (res)
	{
		const float stiffness = model.getElasticRodBendAndTwistStiffness();
		if (wA != 0.0f)
			xA += stiffness*corr[0];
		
		if (wB != 0.0f)
			xB += stiffness*corr[1];
		
		if (wC != 0.0f)
			xC += stiffness*corr[2];
		
		if (wD != 0.0f)
			xD += stiffness*corr[3];

		if (wE != 0.0f)
			xE += stiffness*corr[4];
	}
	return res;
}

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

void TimeStepController::step(SimulationModel &model)
{
 	TimeManager *tm = TimeManager::getCurrent ();
 	const float h = tm->getTimeStepSize();
 
	//////////////////////////////////////////////////////////////////////////
	// rigid body model
	//////////////////////////////////////////////////////////////////////////
 	clearAccelerations(model);
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	ParticleData &pd = model.getParticles();
	ParticleData &pg = model.getGhostParticles();

	#pragma omp parallel default(shared)
	{
		#pragma omp for schedule(static) nowait
		for (int i = 0; i < (int) rb.size(); i++)
 		{ 
			rb[i]->getLastPosition() = rb[i]->getOldPosition();
			rb[i]->getOldPosition() = rb[i]->getPosition();
			TimeIntegration::semiImplicitEuler(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getVelocity(), rb[i]->getAcceleration());
			rb[i]->getLastRotation() = rb[i]->getOldRotation();
			rb[i]->getOldRotation() = rb[i]->getRotation();
			TimeIntegration::semiImplicitEulerRotation(h, rb[i]->getMass(), rb[i]->getInertiaTensorInverseW(), rb[i]->getRotation(), rb[i]->getAngularVelocity(), rb[i]->getTorque());
			rb[i]->rotationUpdated();
 		}

		//////////////////////////////////////////////////////////////////////////
		// particle model
		//////////////////////////////////////////////////////////////////////////
		#pragma omp for schedule(static) 
		for (int i = 0; i < (int) pd.size(); i++)
		{
			pd.getLastPosition(i) = pd.getOldPosition(i);
			pd.getOldPosition(i) = pd.getPosition(i);

			pd.getVelocity(i) *= (1.0f - m_damping);

			TimeIntegration::semiImplicitEuler(h, pd.getMass(i), pd.getPosition(i), pd.getVelocity(i), pd.getAcceleration(i));
		}

		for (int i = 0; i < (int)pg.size(); i++)
		{
			pg.getLastPosition(i) = pg.getOldPosition(i);
			pg.getOldPosition(i) = pg.getPosition(i);

			pg.getVelocity(i) *= (1.0f - m_damping);

			TimeIntegration::semiImplicitEuler(h, pg.getMass(i), pg.getPosition(i), pg.getVelocity(i), pg.getAcceleration(i));
		}
	}
 
	positionConstraintProjection(model);
 
	#pragma omp parallel default(shared)
	{
 		// Update velocities	
		#pragma omp for schedule(static) nowait
		for (int i = 0; i < (int) rb.size(); i++)
 		{
			if (m_velocityUpdateMethod == 0)
			{
				TimeIntegration::velocityUpdateFirstOrder(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getOldPosition(), rb[i]->getVelocity());
				TimeIntegration::angularVelocityUpdateFirstOrder(h, rb[i]->getMass(), rb[i]->getRotation(), rb[i]->getOldRotation(), rb[i]->getAngularVelocity());
			}
			else
			{
				TimeIntegration::velocityUpdateSecondOrder(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getOldPosition(), rb[i]->getLastPosition(), rb[i]->getVelocity());
				TimeIntegration::angularVelocityUpdateSecondOrder(h, rb[i]->getMass(), rb[i]->getRotation(), rb[i]->getOldRotation(), rb[i]->getLastRotation(), rb[i]->getAngularVelocity());
			}
 		}

		// Update velocities	
		#pragma omp for schedule(static) 
		for (int i = 0; i < (int) pd.size(); i++)
		{
			if (m_velocityUpdateMethod == 0)
				TimeIntegration::velocityUpdateFirstOrder(h, pd.getMass(i), pd.getPosition(i), pd.getOldPosition(i), pd.getVelocity(i));
			else
				TimeIntegration::velocityUpdateSecondOrder(h, pd.getMass(i), pd.getPosition(i), pd.getOldPosition(i), pd.getLastPosition(i), pd.getVelocity(i));
		}

		#pragma omp for schedule(static) 
		for (int i = 0; i < (int)pg.size(); i++)
		{
			if (m_velocityUpdateMethod == 0)
				TimeIntegration::velocityUpdateFirstOrder(h, pg.getMass(i), pg.getPosition(i), pg.getOldPosition(i), pg.getVelocity(i));
			else
				TimeIntegration::velocityUpdateSecondOrder(h, pg.getMass(i), pg.getPosition(i), pg.getOldPosition(i), pg.getLastPosition(i), pg.getVelocity(i));
		}
	}

	velocityConstraintProjection(model);

	// compute new time	
	tm->setTime (tm->getTime () + h);
}