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);
}
}
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;
}
}
}
//////////////////////////////////////////////////////////////////////////
// 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);
}
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++;
}
}
//////////////////////////////////////////////////////////////////////////
// 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 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);
}
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);
}
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);
}
//////////////////////////////////////////////////////////////////////////
// TargetVelocityMotorHingeJoint
//////////////////////////////////////////////////////////////////////////
bool TargetVelocityMotorHingeJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
{
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]];
return PositionBasedRigidBodyDynamics::init_TargetVelocityMotorHingeJoint(
rb1.getPosition(),
rb1.getRotation(),
rb2.getPosition(),
rb2.getRotation(),
pos, axis,
m_jointInfo);
}
//////////////////////////////////////////////////////////////////////////
// BallOnLineJoint
//////////////////////////////////////////////////////////////////////////
bool BallOnLineJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &dir)
{
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]];
return PositionBasedRigidBodyDynamics::init_BallOnLineJoint(
rb1.getPosition(),
rb1.getRotation(),
rb2.getPosition(),
rb2.getRotation(),
pos, dir,
m_jointInfo);
}
void mouseMove(int x, int y)
{
Vector3r mousePos;
MiniGL::unproject(x, y, mousePos);
const Vector3r diff = mousePos - oldMousePos;
TimeManager *tm = TimeManager::getCurrent();
const float h = tm->getTimeStepSize();
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
for (size_t j = 0; j < selectedBodies.size(); j++)
{
rb[selectedBodies[j]]->getVelocity() += 1.0f / h * diff;
}
oldMousePos = mousePos;
}
//////////////////////////////////////////////////////////////////////////
// UniversalJoint
//////////////////////////////////////////////////////////////////////////
bool UniversalJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Vector3r &pos, const Vector3r &axis1, const Vector3r &axis2)
{
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]];
return PositionBasedRigidBodyDynamics::init_UniversalJoint(
rb1.getPosition(),
rb1.getRotation(),
rb2.getPosition(),
rb2.getRotation(),
pos,
axis1,
axis2,
m_jointInfo);
}
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 TargetAngleMotorHingeJoint::solvePositionConstraint(SimulationModel &model)
{
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
RigidBody &rb1 = *rb[m_bodies[0]];
RigidBody &rb2 = *rb[m_bodies[1]];
Eigen::Vector3f corr_x1, corr_x2;
Eigen::Quaternionf corr_q1, corr_q2;
const bool res = PositionBasedRigidBodyDynamics::solve_TargetAngleMotorHingeJoint(
rb1.getInvMass(),
rb1.getPosition(),
rb1.getInertiaTensorInverseW(),
rb1.getRotation(),
rb2.getInvMass(),
rb2.getPosition(),
rb2.getInertiaTensorInverseW(),
rb2.getRotation(),
m_targetAngle,
m_jointInfo,
corr_x1,
corr_q1,
corr_x2,
corr_q2);
if (res)
{
if (rb1.getMass() != 0.0f)
{
rb1.getPosition() += corr_x1;
rb1.getRotation().coeffs() += corr_q1.coeffs();
rb1.getRotation().normalize();
rb1.rotationUpdated();
}
if (rb2.getMass() != 0.0f)
{
rb2.getPosition() += corr_x2;
rb2.getRotation().coeffs() += corr_q2.coeffs();
rb2.getRotation().normalize();
rb2.rotationUpdated();
}
}
return res;
}
bool RigidBodyContactConstraint::solveVelocityConstraint(SimulationModel &model)
{
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
RigidBody &rb1 = *rb[m_bodies[0]];
RigidBody &rb2 = *rb[m_bodies[1]];
Vector3r corr_v1, corr_v2;
Vector3r corr_omega1, corr_omega2;
const bool res = PositionBasedRigidBodyDynamics::velocitySolve_RigidBodyContactConstraint(
rb1.getInvMass(),
rb1.getPosition(),
rb1.getVelocity(),
rb1.getInertiaTensorInverseW(),
rb1.getAngularVelocity(),
rb2.getInvMass(),
rb2.getPosition(),
rb2.getVelocity(),
rb2.getInertiaTensorInverseW(),
rb2.getAngularVelocity(),
m_stiffness,
m_frictionCoeff,
m_sum_impulses,
m_constraintInfo,
corr_v1,
corr_omega1,
corr_v2,
corr_omega2);
if (res)
{
if (rb1.getMass() != 0.0)
{
rb1.getVelocity() += corr_v1;
rb1.getAngularVelocity() += corr_omega1;
}
if (rb2.getMass() != 0.0)
{
rb2.getVelocity() += corr_v2;
rb2.getAngularVelocity() += corr_omega2;
}
}
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 TargetVelocityMotorHingeJoint::solveVelocityConstraint(SimulationModel &model)
{
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
RigidBody &rb1 = *rb[m_bodies[0]];
RigidBody &rb2 = *rb[m_bodies[1]];
Eigen::Vector3f corr_v1, corr_v2;
Eigen::Vector3f corr_omega1, corr_omega2;
const bool res = PositionBasedRigidBodyDynamics::velocitySolve_TargetVelocityMotorHingeJoint(
rb1.getInvMass(),
rb1.getPosition(),
rb1.getVelocity(),
rb1.getInertiaTensorInverseW(),
rb1.getAngularVelocity(),
rb2.getInvMass(),
rb2.getPosition(),
rb2.getVelocity(),
rb2.getInertiaTensorInverseW(),
rb2.getAngularVelocity(),
m_targetAngularVelocity,
m_jointInfo,
corr_v1,
corr_omega1,
corr_v2,
corr_omega2);
if (res)
{
if (rb1.getMass() != 0.0f)
{
rb1.getVelocity() += corr_v1;
rb1.getAngularVelocity() += corr_omega1;
}
if (rb2.getMass() != 0.0f)
{
rb2.getVelocity() += corr_v2;
rb2.getAngularVelocity() += corr_omega2;
}
}
return res;
}
void mouseMove(int x, int y)
{
Eigen::Vector3f mousePos;
MiniGL::unproject(x, y, mousePos);
const Eigen::Vector3f diff = mousePos - oldMousePos;
TimeManager *tm = TimeManager::getCurrent();
const float h = tm->getTimeStepSize();
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
for (size_t j = 0; j < selectedBodies.size(); j++)
{
rb[selectedBodies[j]]->getVelocity() += 1.0f / h * diff;
}
ParticleData &pd = model.getParticles();
for (unsigned int j = 0; j < selectedParticles.size(); j++)
{
pd.getVelocity(selectedParticles[j]) += 5.0*diff / h;
}
oldMousePos = mousePos;
}
bool RigidBodyParticleBallJoint::solvePositionConstraint(SimulationModel &model)
{
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
ParticleData &pd = model.getParticles();
RigidBody &rb1 = *rb[m_bodies[0]];
Eigen::Vector3f corr_x1, corr_x2;
Eigen::Quaternionf corr_q1;
const bool res = PositionBasedRigidBodyDynamics::solve_RigidBodyParticleBallJoint(
rb1.getInvMass(),
rb1.getPosition(),
rb1.getInertiaTensorInverseW(),
rb1.getRotation(),
pd.getInvMass(m_bodies[1]),
pd.getPosition(m_bodies[1]),
m_jointInfo,
corr_x1,
corr_q1,
corr_x2);
if (res)
{
if (rb1.getMass() != 0.0f)
{
rb1.getPosition() += corr_x1;
rb1.getRotation().coeffs() += corr_q1.coeffs();
rb1.getRotation().normalize();
rb1.rotationUpdated();
}
if (pd.getMass(m_bodies[1]) != 0.0f)
{
pd.getPosition(m_bodies[1]) += corr_x2;
}
}
return res;
}
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]);
}
}
}
/** 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));
//////////////////////////////////////////////////////////////////////////
// -5, 5
//////////////////////////////////////////////////////////////////////////
rb[9] = new RigidBody();
rb[9]->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[10] = new RigidBody();
rb[10]->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[11] = new RigidBody();
rb[11]->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(9, 10, Eigen::Vector3f(-5.0f, 0.0f, 5.0f));
model.addBallJoint(10, 11, Eigen::Vector3f(-5.0f, 2.0f, 5.0f));
model.addRigidBodyParticleBallJoint(2, 0);
model.addRigidBodyParticleBallJoint(5, (nRows - 1)*nCols);
model.addRigidBodyParticleBallJoint(8, nRows*nCols - 1);
model.addRigidBodyParticleBallJoint(11, nCols-1);
}
/** 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++)
{
Real current_z = startz_bodies;
for (unsigned int k = 0; k < num_bodies_z; k++)
{
rb[rbIndex] = new RigidBody();
Real ax = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
Real ay = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
Real az = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
Real w = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
Quaternionr q(w, ax, ay, az);
q.normalize();
currentType = rand() % 4;
if (currentType == 0)
{
rb[rbIndex]->initBody(100.0,
Vector3r(current_x, current_y, current_z),
q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
vdTorus, meshTorus,
Vector3r(2.0, 2.0, 2.0));
const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
const unsigned int nVert = static_cast<unsigned int>(vertices->size());
cd.addCollisionTorus(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector2r(2.0, 1.0));
}
else if (currentType == 1)
{
rb[rbIndex]->initBody(100.0,
Vector3r(current_x, current_y, current_z),
q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
vdCube, meshCube,
Vector3r(4.0, 1.0, 1.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(4.0, 1.0, 1.0));
}
else if (currentType == 2)
{
rb[rbIndex]->initBody(100.0,
Vector3r(current_x, current_y, current_z),
q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
vdSphere, meshSphere);
const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
const unsigned int nVert = static_cast<unsigned int>(vertices->size());
cd.addCollisionSphere(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, 1.0);
}
else if (currentType == 3)
{
rb[rbIndex]->initBody(100.0,
Vector3r(current_x, current_y, current_z),
q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
vdCylinder, meshCylinder,
Vector3r(0.75, 5.0, 0.75));
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.75, 5.0));
}
currentType = (currentType + 1) % 4;
current_z += dz_bodies;
rbIndex++;
}
current_x += dx_bodies;
}
current_y += dy_bodies;
}
}
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);
}
