////////////////////////////////////////////////////////////////////////// // ElasticRodBendAndTwistConstraint ////////////////////////////////////////////////////////////////////////// bool ElasticRodBendAndTwistConstraint::initConstraint(SimulationModel &model, const unsigned int pA, const unsigned int pB, const unsigned int pC, const unsigned int pD, const unsigned int pE) { m_bodies[0] = pA; m_bodies[1] = pB; m_bodies[2] = pC; m_bodies[3] = pD; //ghost point id m_bodies[4] = pE; //ghost point id ParticleData &pd = model.getParticles(); ParticleData &pg = model.getGhostParticles(); const Eigen::Vector3f &xA = pd.getPosition0(m_bodies[0]); const Eigen::Vector3f &xB = pd.getPosition0(m_bodies[1]); const Eigen::Vector3f &xC = pd.getPosition0(m_bodies[2]); const Eigen::Vector3f &xD = pg.getPosition0(m_bodies[3]); const Eigen::Vector3f &xE = pg.getPosition0(m_bodies[4]); PositionBasedElasticRod::ComputeMaterialFrame(xA, xB, xD, m_dA); PositionBasedElasticRod::ComputeMaterialFrame(xB, xC, xE, m_dB); PositionBasedElasticRod::ComputeDarbouxVector(m_dA, m_dB, 1.0f, m_restDarbouxVector); m_bendAndTwistKs.setOnes(); return true; }
////////////////////////////////////////////////////////////////////////// // ElasticRodEdgeConstraint ////////////////////////////////////////////////////////////////////////// bool ElasticRodEdgeConstraint::initConstraint(SimulationModel &model, const unsigned int pA, const unsigned int pB, const unsigned int pG) { m_bodies[0] = pA; m_bodies[1] = pB; m_bodies[2] = pG; ParticleData &pd = model.getParticles(); ParticleData &pg = model.getGhostParticles(); const Eigen::Vector3f &xA_0 = pd.getPosition0(pA); const Eigen::Vector3f &xB_0 = pd.getPosition0(pB); const Eigen::Vector3f &xG_0 = pg.getPosition0(pG); m_restLength = 1.0f; return true; }
void TimeStepController::clearAccelerations(SimulationModel &model) { ////////////////////////////////////////////////////////////////////////// // rigid body model ////////////////////////////////////////////////////////////////////////// SimulationModel::RigidBodyVector &rb = model.getRigidBodies(); const Eigen::Vector3f grav(0.0f, -9.81f, 0.0f); for (size_t i=0; i < rb.size(); i++) { // Clear accelerations of dynamic particles if (rb[i]->getMass() != 0.0) { Eigen::Vector3f &a = rb[i]->getAcceleration(); a = grav; } } ////////////////////////////////////////////////////////////////////////// // particle model ////////////////////////////////////////////////////////////////////////// ParticleData &pd = model.getParticles(); const unsigned int count = pd.size(); for (unsigned int i = 0; i < count; i++) { // Clear accelerations of dynamic particles if (pd.getMass(i) != 0.0) { Eigen::Vector3f &a = pd.getAcceleration(i); a = grav; } } ParticleData &pg = model.getGhostParticles(); for (unsigned int i = 0; i < pg.size(); i++) { // Clear accelerations of dynamic particles if (pg.getMass(i) != 0.0) { Eigen::Vector3f &a = pg.getAcceleration(i); //a = grav; a.setZero(); } } }
bool ElasticRodBendAndTwistConstraint::solvePositionConstraint(SimulationModel &model) { ParticleData &pd = model.getParticles(); ParticleData &pg = model.getGhostParticles(); Eigen::Vector3f &xA = pd.getPosition(m_bodies[0]); Eigen::Vector3f &xB = pd.getPosition(m_bodies[1]); Eigen::Vector3f &xC = pd.getPosition(m_bodies[2]); Eigen::Vector3f &xD = pg.getPosition(m_bodies[3]); Eigen::Vector3f &xE = pg.getPosition(m_bodies[4]); const float wA = pd.getInvMass(m_bodies[0]); const float wB = pd.getInvMass(m_bodies[1]); const float wC = pd.getInvMass(m_bodies[2]); const float wD = pg.getInvMass(m_bodies[3]); const float wE = pg.getInvMass(m_bodies[4]); Eigen::Vector3f corr[5]; bool res = PositionBasedElasticRod::ProjectBendingAndTwistingConstraint( xA, wA, xB, wB, xC, wC, xD, wD, xE, wE, m_bendAndTwistKs, 1.0f, m_restDarbouxVector, corr[0], corr[1], corr[2], corr[3], corr[4], true); if (res) { const float stiffness = model.getElasticRodBendAndTwistStiffness(); if (wA != 0.0f) xA += stiffness*corr[0]; if (wB != 0.0f) xB += stiffness*corr[1]; if (wC != 0.0f) xC += stiffness*corr[2]; if (wD != 0.0f) xD += stiffness*corr[3]; if (wE != 0.0f) xE += stiffness*corr[4]; } return res; }
/** Create the elastic rod model */ void createRod() { ParticleData &particles = model.getParticles(); ParticleData &ghostParticles = model.getGhostParticles(); SimulationModel::ConstraintVector &constraints = model.getConstraints(); //centreline points for (unsigned int i = 0; i < numberOfPoints; i++) { particles.addVertex(Vector3r((float)i*1.0f, 0.0f, 0.0f)); } //edge ghost points for (unsigned int i = 0; i < numberOfPoints-1; i++) { ghostParticles.addVertex(Vector3r((float)i*1.0f + 0.5f, 1.0f, 0.0f)); } //lock two first particles and first ghost point particles.setMass(0, 0.0f); particles.setMass(1, 0.0f); ghostParticles.setMass(0, 0.0f); for (unsigned int i = 0; i < numberOfPoints - 1; i++) { model.addElasticRodEdgeConstraint(i, i + 1, i); if (i < numberOfPoints - 2) { // Single rod element: // D E //ghost points // | | // --A---B---C-- // rod points int pA = i; int pB = i + 1; int pC = i + 2; int pD = i; int pE = i + 1; model.addElasticRodBendAndTwistConstraint(pA, pB, pC, pD, pE); } } }
bool ElasticRodEdgeConstraint::solvePositionConstraint(SimulationModel &model) { //ElasticRodSimulationModel simModel = dynamic_cast<ElasticRodSimulationModel&>(model); ParticleData &pd = model.getParticles(); ParticleData &pg = model.getGhostParticles(); const unsigned iA = m_bodies[0]; const unsigned iB = m_bodies[1]; const unsigned iG = m_bodies[2]; Eigen::Vector3f &xA = pd.getPosition(iA); Eigen::Vector3f &xB = pd.getPosition(iB); Eigen::Vector3f &xG = pg.getPosition(iG); float wA = pd.getInvMass(iA); float wB = pd.getInvMass(iB); float wG = pg.getInvMass(iG); Eigen::Vector3f corr[3]; //const bool res = PositionBasedDynamics::solve_DistanceConstraint(xA, wA, xB, wB, m_restLength, 1.0f, 1.0f, corr[0], corr[1]); const bool res = PositionBasedElasticRod::ProjectEdgeConstraints(xA, wA, xB, wB, xG, wG, 1.0f, m_restLength, m_restLength, corr[0], corr[1], corr[2]); if (res) { if (wA != 0.0f) xA += corr[0]; if (wB != 0.0f) xB += corr[1]; if (wG != 0.0f) xG += corr[2]; } return res; }
void render() { MiniGL::coordinateSystem(); // Draw sim model ParticleData &particles = model.getParticles(); ParticleData &ghostParticles = model.getGhostParticles(); SimulationModel::ConstraintVector &constraints = model.getConstraints(); float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 }; float pointColor[4] = { 0.1f, 0.1f, 0.5f, 1 }; float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f }; float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 }; for (size_t i = 0; i < numberOfPoints; i++) { MiniGL::drawSphere(particles.getPosition(i), 0.2f, pointColor); } for (size_t i = 0; i < numberOfPoints-1; i++) { MiniGL::drawSphere(ghostParticles.getPosition(i), 0.1f, ghostPointColor); MiniGL::drawVector(particles.getPosition(i), particles.getPosition(i + 1), 0.2f, edgeColor); } for (size_t i = 0; i < constraints.size(); i++) { if (constraints[i]->getTypeId() == ElasticRodBendAndTwistConstraint::TYPE_ID) { ((ElasticRodBendAndTwistConstraint*)constraints[i])->m_restDarbouxVector = restDarboux; } } MiniGL::drawTime( TimeManager::getCurrent ()->getTime ()); }
void TimeStepController::step(SimulationModel &model) { TimeManager *tm = TimeManager::getCurrent (); const float h = tm->getTimeStepSize(); ////////////////////////////////////////////////////////////////////////// // rigid body model ////////////////////////////////////////////////////////////////////////// clearAccelerations(model); SimulationModel::RigidBodyVector &rb = model.getRigidBodies(); ParticleData &pd = model.getParticles(); ParticleData &pg = model.getGhostParticles(); #pragma omp parallel default(shared) { #pragma omp for schedule(static) nowait for (int i = 0; i < (int) rb.size(); i++) { rb[i]->getLastPosition() = rb[i]->getOldPosition(); rb[i]->getOldPosition() = rb[i]->getPosition(); TimeIntegration::semiImplicitEuler(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getVelocity(), rb[i]->getAcceleration()); rb[i]->getLastRotation() = rb[i]->getOldRotation(); rb[i]->getOldRotation() = rb[i]->getRotation(); TimeIntegration::semiImplicitEulerRotation(h, rb[i]->getMass(), rb[i]->getInertiaTensorInverseW(), rb[i]->getRotation(), rb[i]->getAngularVelocity(), rb[i]->getTorque()); rb[i]->rotationUpdated(); } ////////////////////////////////////////////////////////////////////////// // particle model ////////////////////////////////////////////////////////////////////////// #pragma omp for schedule(static) for (int i = 0; i < (int) pd.size(); i++) { pd.getLastPosition(i) = pd.getOldPosition(i); pd.getOldPosition(i) = pd.getPosition(i); pd.getVelocity(i) *= (1.0f - m_damping); TimeIntegration::semiImplicitEuler(h, pd.getMass(i), pd.getPosition(i), pd.getVelocity(i), pd.getAcceleration(i)); } for (int i = 0; i < (int)pg.size(); i++) { pg.getLastPosition(i) = pg.getOldPosition(i); pg.getOldPosition(i) = pg.getPosition(i); pg.getVelocity(i) *= (1.0f - m_damping); TimeIntegration::semiImplicitEuler(h, pg.getMass(i), pg.getPosition(i), pg.getVelocity(i), pg.getAcceleration(i)); } } positionConstraintProjection(model); #pragma omp parallel default(shared) { // Update velocities #pragma omp for schedule(static) nowait for (int i = 0; i < (int) rb.size(); i++) { if (m_velocityUpdateMethod == 0) { TimeIntegration::velocityUpdateFirstOrder(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getOldPosition(), rb[i]->getVelocity()); TimeIntegration::angularVelocityUpdateFirstOrder(h, rb[i]->getMass(), rb[i]->getRotation(), rb[i]->getOldRotation(), rb[i]->getAngularVelocity()); } else { TimeIntegration::velocityUpdateSecondOrder(h, rb[i]->getMass(), rb[i]->getPosition(), rb[i]->getOldPosition(), rb[i]->getLastPosition(), rb[i]->getVelocity()); TimeIntegration::angularVelocityUpdateSecondOrder(h, rb[i]->getMass(), rb[i]->getRotation(), rb[i]->getOldRotation(), rb[i]->getLastRotation(), rb[i]->getAngularVelocity()); } } // Update velocities #pragma omp for schedule(static) for (int i = 0; i < (int) pd.size(); i++) { if (m_velocityUpdateMethod == 0) TimeIntegration::velocityUpdateFirstOrder(h, pd.getMass(i), pd.getPosition(i), pd.getOldPosition(i), pd.getVelocity(i)); else TimeIntegration::velocityUpdateSecondOrder(h, pd.getMass(i), pd.getPosition(i), pd.getOldPosition(i), pd.getLastPosition(i), pd.getVelocity(i)); } #pragma omp for schedule(static) for (int i = 0; i < (int)pg.size(); i++) { if (m_velocityUpdateMethod == 0) TimeIntegration::velocityUpdateFirstOrder(h, pg.getMass(i), pg.getPosition(i), pg.getOldPosition(i), pg.getVelocity(i)); else TimeIntegration::velocityUpdateSecondOrder(h, pg.getMass(i), pg.getPosition(i), pg.getOldPosition(i), pg.getLastPosition(i), pg.getVelocity(i)); } } velocityConstraintProjection(model); // compute new time tm->setTime (tm->getTime () + h); }