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;
}
void renderTriangleModels()
{
// 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.getTriangleModels().size(); i++)
{
// mesh
TriangleModel *triModel = model.getTriangleModels()[i];
const IndexedFaceMesh &mesh = triModel->getParticleMesh();
Visualization::drawTexturedMesh(pd, mesh, triModel->getIndexOffset(), surfaceColor);
}
if (shader)
shader->end();
}
//////////////////////////////////////////////////////////////////////////
// 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;
}
bool ShapeMatchingConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
for (unsigned int i = 0; i < m_numberOfBodies; i++)
{
m_x[i] = pd.getPosition(m_bodies[i]);
}
const bool res = PositionBasedDynamics::solve_ShapeMatchingConstraint(
m_x0, m_x, m_w, m_numberOfBodies,
m_restCm, m_invRestMat,
model.getSolidStiffness(), false,
m_corr);
if (res)
{
for (unsigned int i = 0; i < m_numberOfBodies; i++)
{
// Important: Divide position correction by the number of clusters
// which contain the vertex.
if (m_w[i] != 0.0f)
pd.getPosition(m_bodies[i]) += (1.0f / m_numClusters[i]) * m_corr[i];
}
}
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;
}
void reset()
{
model.reset();
sim.reset();
TimeManager::getCurrent()->setTime(0.0);
model.cleanup();
buildModel();
}
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 timeStep ()
{
if (doPause)
return;
// Simulation code
for (unsigned int i = 0; i < 8; i++)
sim.step(model);
for (unsigned int i = 0; i < model.getTriangleModels().size(); i++)
model.getTriangleModels()[i]->updateMeshNormals(model.getParticles());
}
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++;
}
}
//////////////////////////////////////////////////////////////////////////
// RigidBodyParticleBallJoint
//////////////////////////////////////////////////////////////////////////
bool RigidBodyParticleBallJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex, const unsigned int particleIndex)
{
m_bodies[0] = rbIndex;
m_bodies[1] = particleIndex;
SimulationModel::RigidBodyVector &rbs = model.getRigidBodies();
ParticleData &pd = model.getParticles();
RigidBody &rb = *rbs[m_bodies[0]];
return PositionBasedRigidBodyDynamics::init_RigidBodyParticleBallJoint(
rb.getPosition(),
rb.getRotation(),
pd.getPosition(particleIndex),
m_jointInfo);
}
void buildModel()
{
TimeManager::getCurrent()->setTimeStepSize(0.002f);
sim.setDamping(0.001f);
model.setElasticRodBendAndTwistStiffness(0.5f);
model.setElasticRodStretchStiffness(1.0f);
restDarboux = Vector3r(0.0f, 0.0f, 0.0f);
createRod();
}
bool FEMTetConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
const unsigned i3 = m_bodies[2];
const unsigned i4 = m_bodies[3];
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
Eigen::Vector3f &x3 = pd.getPosition(i3);
Eigen::Vector3f &x4 = pd.getPosition(i4);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass3 = pd.getInvMass(i3);
const float invMass4 = pd.getInvMass(i4);
float currentVolume = -(1.0f / 6.0f) * (x4 - x1).dot((x3 - x1).cross(x2 - x1));
bool handleInversion = false;
if (currentVolume / m_volume < 0.2) // Only 20% of initial volume left
handleInversion = true;
Eigen::Vector3f corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_FEMTetraConstraint(
x1, invMass1,
x2, invMass2,
x3, invMass3,
x4, invMass4,
m_volume,
m_invRestMat,
model.getSolidStiffness(),
model.getSolidPoissonRatio(), handleInversion,
corr1, corr2, corr3, corr4);
if (res)
{
if (invMass1 != 0.0f)
x1 += corr1;
if (invMass2 != 0.0f)
x2 += corr2;
if (invMass3 != 0.0f)
x3 += corr3;
if (invMass4 != 0.0f)
x4 += corr4;
}
return res;
}
//////////////////////////////////////////////////////////////////////////
// RigidBodyContactConstraint
//////////////////////////////////////////////////////////////////////////
bool RigidBodyContactConstraint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2,
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] = rbIndex1;
m_bodies[1] = rbIndex2;
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
RigidBody &rb1 = *rb[m_bodies[0]];
RigidBody &rb2 = *rb[m_bodies[1]];
m_sum_impulses = 0.0;
return PositionBasedRigidBodyDynamics::init_RigidBodyContactConstraint(
rb1.getInvMass(),
rb1.getPosition(),
rb1.getVelocity(),
rb1.getInertiaTensorInverseW(),
rb1.getRotation(),
rb1.getAngularVelocity(),
rb2.getInvMass(),
rb2.getPosition(),
rb2.getVelocity(),
rb2.getInertiaTensorInverseW(),
rb2.getRotation(),
rb2.getAngularVelocity(),
cp1, cp2, normal, restitutionCoeff,
m_constraintInfo);
}
bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
const unsigned int particle3, const unsigned int particle4)
{
m_bodies[0] = particle1;
m_bodies[1] = particle2;
m_bodies[2] = particle3;
m_bodies[3] = particle4;
ParticleData &pd = model.getParticles();
const Eigen::Vector3f &p0 = pd.getPosition0(particle1);
const Eigen::Vector3f &p1 = pd.getPosition0(particle2);
const Eigen::Vector3f &p2 = pd.getPosition0(particle3);
const Eigen::Vector3f &p3 = pd.getPosition0(particle4);
Eigen::Vector3f e = p3 - p2;
float elen = e.norm();
if (elen < 1e-6f)
return false;
float invElen = 1.0f / elen;
Eigen::Vector3f n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm();
Eigen::Vector3f n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm();
n1.normalize();
n2.normalize();
float dot = n1.dot(n2);
if (dot < -1.0f) dot = -1.0f;
if (dot > 1.0f) dot = 1.0f;
m_restAngle = acosf(dot);
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();
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();
}
}
}
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;
}
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();
}
bool GenericIsometricBendingConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i0 = m_bodies[0];
const unsigned i1 = m_bodies[1];
const unsigned i2 = m_bodies[2];
const unsigned i3 = m_bodies[3];
Eigen::Vector3f &x0 = pd.getPosition(i0);
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
Eigen::Vector3f &x3 = pd.getPosition(i3);
const float invMass0 = pd.getInvMass(i0);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass3 = pd.getInvMass(i3);
float invMass[4] = { invMass0, invMass1, invMass2, invMass3 };
const Eigen::Vector3f x[4] = { x0, x1, x2, x3 };
Eigen::Vector3f corr[4];
const bool res = PositionBasedGenericConstraints::solve_GenericConstraint<4, 1>(
invMass, x, &m_Q,
GenericIsometricBendingConstraint::constraintFct,
//GenericIsometricBendingConstraint::gradientFct,
corr);
if (res)
{
const float stiffness = model.getClothBendingStiffness();
if (invMass0 != 0.0f)
x0 += stiffness*corr[0];
if (invMass1 != 0.0f)
x1 += stiffness*corr[1];
if (invMass2 != 0.0f)
x2 += stiffness*corr[2];
if (invMass3 != 0.0f)
x3 += stiffness*corr[3];
}
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);
}
}
}
// 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;
}
bool VolumeConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
const unsigned i3 = m_bodies[2];
const unsigned i4 = m_bodies[3];
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
Eigen::Vector3f &x3 = pd.getPosition(i3);
Eigen::Vector3f &x4 = pd.getPosition(i4);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass3 = pd.getInvMass(i3);
const float invMass4 = pd.getInvMass(i4);
Eigen::Vector3f corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_VolumeConstraint(x1, invMass1,
x2, invMass2,
x3, invMass3,
x4, invMass4,
m_restVolume,
model.getSolidStiffness(),
model.getSolidStiffness(),
corr1, corr2, corr3, corr4);
if (res)
{
if (invMass1 != 0.0f)
x1 += corr1;
if (invMass2 != 0.0f)
x2 += corr2;
if (invMass3 != 0.0f)
x3 += corr3;
if (invMass4 != 0.0f)
x4 += corr4;
}
return res;
}
// main
int main( int argc, char **argv )
{
REPORT_MEMORY_LEAKS
base = new DemoBase();
base->init(argc, argv, "Rigid body collision 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.0, Vector3r (5.0, 30.0, 70.0), Vector3r (5.0, 0.0, 0.0));
TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance' min=0.0 step=0.001 precision=3 group=Simulation ");
TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, &model, " label='Contact stiffness RB' min=0.0 step=0.1 precision=2 group=Simulation ");
TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, &model, " label='Contact stiffness Particle-RB' min=0.0 step=0.1 precision=2 group=Simulation ");
glutMainLoop ();
base->cleanup();
Utilities::Timing::printAverageTimes();
Utilities::Timing::printTimeSums();
delete Simulation::getCurrent();
delete base;
delete model;
return 0;
}
bool TargetVelocityMotorHingeJoint::updateConstraint(SimulationModel &model)
{
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
RigidBody &rb1 = *rb[m_bodies[0]];
RigidBody &rb2 = *rb[m_bodies[1]];
return PositionBasedRigidBodyDynamics::update_TargetVelocityMotorHingeJoint(
rb1.getPosition(),
rb1.getRotation(),
rb2.getPosition(),
rb2.getRotation(),
m_jointInfo);
}
