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