#include "World.h" #include using namespace FEM; #define BIG_EPSILON 1E-4 #define EPSILON 1E-6 World:: World( IntegrationMethod im, double time_step, int max_iteration, const Eigen::Vector3d& gravity, double damping_coeff) :mIntegrationMethod(im), mTimeStep(time_step), mMaxIteration(max_iteration), mGravity(gravity), mDampingCoefficient(damping_coeff), mNumVertices(0), mConstraintDofs(0), mIsInitialized(false) { } std::shared_ptr World:: Clone() { auto new_world = Create(); new_world->mIsInitialized = mIsInitialized; new_world->mNumVertices = mNumVertices; new_world->mConstraintDofs = mConstraintDofs; new_world->mMaxIteration = mMaxIteration; new_world->mTimeStep = mTimeStep; new_world->mTime = mTime; new_world->mDampingCoefficient = mDampingCoefficient; new_world->mGravity = mGravity; new_world->mIntegrationMethod = mIntegrationMethod; new_world->mUnitMass = mUnitMass; new_world->mConstraints = mConstraints; for(int i=0;imConstraints[i] = mConstraints[i]->Clone(); new_world->mPositions = mPositions; new_world->mVelocities = mVelocities; new_world->mExternalForces = mExternalForces; new_world->mMassMatrix = mMassMatrix; new_world->mInvMassMatrix = mInvMassMatrix; new_world->mIdentityMatrix = mIdentityMatrix; new_world->mq = mq; new_world->mJ = mJ; new_world->mL = mL; if(mIntegrationMethod == PROJECTIVE_DYNAMICS) { Eigen::SparseMatrix H2ML = (1.0/(mTimeStep*mTimeStep))*mMassMatrix+mL; FactorizeLDLT(H2ML,new_world->mSolver); } else if(mIntegrationMethod == PROJECTIVE_QUASI_STATIC) FactorizeLDLT(mL,new_world->mSolver); return new_world; } std::shared_ptr World:: Create( IntegrationMethod im, double time_step, int max_iteration, const Eigen::Vector3d& gravity, double damping_coeff) { auto w = new World(im,time_step,max_iteration,gravity,damping_coeff); return std::shared_ptr(w); } void World:: Initialize() { mTime = 0.0; mVelocities.resize(mNumVertices*3); mVelocities.setZero(); mExternalForces.resize(mNumVertices*3); mExternalForces.setZero(); mMassMatrix.resize(mNumVertices*3,mNumVertices*3); mInvMassMatrix.resize(mNumVertices*3,mNumVertices*3); mIdentityMatrix.resize(mNumVertices*3,mNumVertices*3); std::vector> i_triplets; std::vector> m_triplets; std::vector> inv_m_triplets; for(int i =0;i(i*3+0,i*3+0,mUnitMass[i])); m_triplets.push_back(Eigen::Triplet(i*3+1,i*3+1,mUnitMass[i])); m_triplets.push_back(Eigen::Triplet(i*3+2,i*3+2,mUnitMass[i])); inv_m_triplets.push_back(Eigen::Triplet(i*3+0,i*3+0,1.0/mUnitMass[i])); inv_m_triplets.push_back(Eigen::Triplet(i*3+1,i*3+1,1.0/mUnitMass[i])); inv_m_triplets.push_back(Eigen::Triplet(i*3+2,i*3+2,1.0/mUnitMass[i])); i_triplets.push_back(Eigen::Triplet(i*3+0,i*3+0,1.0)); i_triplets.push_back(Eigen::Triplet(i*3+1,i*3+1,1.0)); i_triplets.push_back(Eigen::Triplet(i*3+2,i*3+2,1.0)); } mMassMatrix.setFromTriplets(m_triplets.cbegin(), m_triplets.cend()); mInvMassMatrix.setFromTriplets(inv_m_triplets.cbegin(), inv_m_triplets.cend()); mIdentityMatrix.setFromTriplets(i_triplets.cbegin(), i_triplets.cend()); mq.resize(mNumVertices*3); mq.setZero(); if( mIntegrationMethod == IntegrationMethod::PROJECTIVE_DYNAMICS || mIntegrationMethod == IntegrationMethod::PROJECTIVE_QUASI_STATIC) Precompute(); mIsInitialized = true; std::cout<<"Total degree of freedom : "<(i*3,0) = mGravity; mq = mPositions + mTimeStep*mVelocities + (mTimeStep*mTimeStep)*(mInvMassMatrix*mExternalForces); switch(mIntegrationMethod) { case NEWTON_METHOD: x_next = IntegrateNewtonMethod(); break; case QUASI_STATIC: x_next = IntegrateQuasiStatic(); break; case PROJECTIVE_DYNAMICS: x_next = IntegrateProjectiveDynamics(); break; case PROJECTIVE_QUASI_STATIC: x_next = IntegrateProjectiveQuasiStatic(); break; default: return; }; if(integrate) { IntegratePositionsAndVelocities(x_next); mTime += mTimeStep; } else mPositions = x_next; } void World:: AddBody(const Eigen::VectorXd& x0, const std::vector>& constraints,double m) { if(mIsInitialized) { std::cout<<"Add Body before initializing engine."<& c) { mConstraints.push_back(c); if((mIntegrationMethod == IntegrationMethod::PROJECTIVE_DYNAMICS|| mIntegrationMethod == IntegrationMethod::PROJECTIVE_QUASI_STATIC)&& mIsInitialized) { mConstraintDofs = 0; Precompute(); } } void World:: RemoveConstraint(const std::shared_ptr& c) { bool isRemoved = false; for(int i =0;i H(mNumVertices*3,mNumVertices*3); EvaluateConstraintsHessian(mPositions,H); FactorizeLDLT(H,mSolver); dx_da = -mSolver.solve(dg_da); } } Eigen::VectorXd World:: IntegrateNewtonMethod() { Eigen::VectorXd x_next(mNumVertices*3); x_next = mq; Eigen::VectorXd g_k(mNumVertices*3); Eigen::SparseMatrix H_k(mNumVertices*3,mNumVertices*3); for(int k=0;k H_k(mNumVertices*3,mNumVertices*3); for(int k=0;k& A, Eigen::SimplicialLLT>& lltSolver) { Eigen::SparseMatrix A_prime = A; lltSolver.analyzePattern(A_prime); lltSolver.factorize(A_prime); double damping = 1E-6; bool success = true; while (lltSolver.info() != Eigen::Success) { damping *= 10; A_prime = A + damping*mIdentityMatrix; lltSolver.factorize(A_prime); success = false; } if (!success) std::cout << "factorize failure (damping : " << damping<<" )"<& A, Eigen::SimplicialLDLT>& ldltSolver) { Eigen::SparseMatrix A_prime = A; ldltSolver.analyzePattern(A_prime); ldltSolver.factorize(A_prime); double damping = 1E-6; bool success = true; while (ldltSolver.info() != Eigen::Success) { damping *= 10; A_prime = A + damping*mIdentityMatrix; ldltSolver.factorize(A_prime); success = false; } if (!success) std::cout << "factorize failure (damping : " << damping<<" )"<& H) { EvaluateConstraintsHessian(x,H); if(mIntegrationMethod == NEWTON_METHOD) H = H + (1.0/(mTimeStep*mTimeStep))*mMassMatrix; } double World:: EvaluateConstraintsEnergy(const Eigen::VectorXd& x) { double energy=0; #pragma omp parallel for for(int i=0;iEvaluatePotentialEnergy(x); } for(auto& c : mConstraints) { c->GetPotentialEnergy(energy); } return energy; } void World:: EvaluateConstraintsGradient(const Eigen::VectorXd& x,Eigen::VectorXd& g) { g.resize(mNumVertices*3); g.setZero(); #pragma omp parallel for for(int i=0;iEvaluateGradient(x); } for(auto& c : mConstraints) { c->GetGradient(g); } } void World:: EvaluateConstraintsHessian(const Eigen::VectorXd& x,Eigen::SparseMatrix& H) { H.resize(mNumVertices*3,mNumVertices*3); std::vector> h_triplets; #pragma omp parallel for for(int i=0;iEvaluateHessian(x); } for(auto& c : mConstraints) { c->GetHessian(h_triplets); } H.setFromTriplets(h_triplets.cbegin(),h_triplets.cend()); } double World:: ComputeStepSize(const Eigen::VectorXd& x, const Eigen::VectorXd& g,const Eigen::VectorXd& d) { double alpha = 1.0; double c1 = 0.00; double c2 = 0.1; double f_x,f_x_next; Eigen::VectorXd x_next; f_x = EvaluateEnergy(x); double g_dot_d = g.dot(d); for(int i=0;i<16;i++) { x_next = x + alpha*d; f_x_next = EvaluateEnergy(x_next); if(f_x + alpha*c1*g_dot_d>=f_x_next) break; alpha *= c2; } return alpha; } void World:: EvaluateDVector(const Eigen::VectorXd& x,Eigen::VectorXd& d) { d.resize(mConstraintDofs*3); int n = mConstraints.size(); #pragma omp parallel for for(int i=0;iEvaluateDVector(x); } int index = 0; for(auto& c : mConstraints) { c->GetDVector(index,d); } } void World:: EvaluateJMatrix(Eigen::SparseMatrix& J) { J.resize(mNumVertices*3,mConstraintDofs*3); std::vector> J_triplets; int index = 0; for(auto& c : mConstraints) { c->EvaluateJMatrix(index,J_triplets); } J.setFromTriplets(J_triplets.cbegin(),J_triplets.cend()); } void World:: EvaluateLMatrix(Eigen::SparseMatrix& L) { L.resize(mNumVertices*3,mNumVertices*3); std::vector> L_triplets; for(auto& c: mConstraints) { c->EvaluateLMatrix(L_triplets); } L.setFromTriplets(L_triplets.cbegin(),L_triplets.cend()); } void World:: Precompute() { mConstraintDofs = 0; //For computing constraint's dofs Eigen::VectorXd d_temp(3*mConstraints.size()*6); int index = 0; for(auto& c : mConstraints) { c->GetDVector(index,d_temp); } mConstraintDofs = index; EvaluateLMatrix(mL); EvaluateJMatrix(mJ); Eigen::SparseMatrix H2ML = (1.0/(mTimeStep*mTimeStep))*mMassMatrix+mL; if(mIntegrationMethod == PROJECTIVE_DYNAMICS) FactorizeLDLT(H2ML,mSolver); else if(mIntegrationMethod == PROJECTIVE_QUASI_STATIC) FactorizeLDLT(mL,mSolver); } void World:: InversionFree(Eigen::VectorXd& x) { //TODO } void World:: IntegratePositionsAndVelocities(const Eigen::VectorXd& x_next) { mVelocities = (1.0/mTimeStep)*(x_next - mPositions); mPositions = x_next; mVelocities = mDampingCoefficient*mVelocities; }