//////////////////////////////////////////////////////////////////////////
// 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;
}
//////////////////////////////////////////////////////////////////////////
// ElasticRodEdgeConstraint
//////////////////////////////////////////////////////////////////////////
bool ElasticRodEdgeConstraint::initConstraint(SimulationModel &model, const unsigned int pA, const unsigned int pB, const unsigned int pG)
{
m_bodies[0] = pA;
m_bodies[1] = pB;
m_bodies[2] = pG;
ParticleData &pd = model.getParticles();
ParticleData &pg = model.getGhostParticles();
const Eigen::Vector3f &xA_0 = pd.getPosition0(pA);
const Eigen::Vector3f &xB_0 = pd.getPosition0(pB);
const Eigen::Vector3f &xG_0 = pg.getPosition0(pG);
m_restLength = 1.0f;
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);
}
}
}
bool ElasticRodEdgeConstraint::solvePositionConstraint(SimulationModel &model)
{
//ElasticRodSimulationModel simModel = dynamic_cast<ElasticRodSimulationModel&>(model);
ParticleData &pd = model.getParticles();
ParticleData &pg = model.getGhostParticles();
const unsigned iA = m_bodies[0];
const unsigned iB = m_bodies[1];
const unsigned iG = m_bodies[2];
Eigen::Vector3f &xA = pd.getPosition(iA);
Eigen::Vector3f &xB = pd.getPosition(iB);
Eigen::Vector3f &xG = pg.getPosition(iG);
float wA = pd.getInvMass(iA);
float wB = pd.getInvMass(iB);
float wG = pg.getInvMass(iG);
Eigen::Vector3f corr[3];
//const bool res = PositionBasedDynamics::solve_DistanceConstraint(xA, wA, xB, wB, m_restLength, 1.0f, 1.0f, corr[0], corr[1]);
const bool res = PositionBasedElasticRod::ProjectEdgeConstraints(xA, wA, xB, wB, xG, wG, 1.0f, m_restLength, m_restLength, corr[0], corr[1], corr[2]);
if (res)
{
if (wA != 0.0f)
xA += corr[0];
if (wB != 0.0f)
xB += corr[1];
if (wG != 0.0f)
xG += corr[2];
}
return res;
}
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);
}
