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