BFGS solver protype, projection matrices for fixed points, fix spelli…

…ng of implicit, fix implicit solver energy

CMakeLists.txt

@@ -156,7 +156,9 @@ add_subdirectory(${PROJECT_SOURCE_DIR}/src/FEM)
 add_subdirectory(${PROJECT_SOURCE_DIR}/src/Collisions)
 add_subdirectory(${PROJECT_SOURCE_DIR}/src/UI)
 
+set(GAUSS_INCLUDE_DIR ${GAUSS_INCLUDE_DIR} ${PROJECT_SOURCE_DIR}/ThirdParty/LBFGS++/include)
 include_directories(${GAUSS_INCLUDE_DIR})
+
 set(Gauss_EXT_LIBS ${GAUSS_LIBS} CACHE STRING "" FORCE)
 set(Gauss_EXT_INCLUDE_DIRS ${GAUSS_INCLUDE_DIR} CACHE STRING " " FORCE)
 set(Gauss_EXT_LIBDIR ${GAUSS_LINK_DIR} CACHE STRING "" FORCE)

src/Base/include/ConstraintFixedPoint.h

@@ -12,6 +12,7 @@
 #include <Constraint.h>
 #include <DOFParticle.h>
 #include <UtilitiesEigen.h>
+#include <iterator>
 
 namespace Gauss {
     template<typename DataType>
@@ -90,6 +91,81 @@ namespace Gauss {
             world.addConstraint(new ConstraintFixedPoint<decltype(minX)>(&system->getQ()[iV], Eigen::Vector3d(0,0,0)));
         }
     }
+
+    //Utility functions to fix a bunch of points
+    template<typename FEMSystem>
+    Eigen::VectorXi minVertices(FEMSystem *system, unsigned int dim = 0) {
+
+
+        //find all vertices with minimum x coordinate and fix DOF associated with them
+        auto minX = system->getImpl().getV()(0,dim);
+        std::vector<unsigned int> minV;
+
+        for(unsigned int ii=0; ii<system->getImpl().getV().rows(); ++ii) {
+
+            if(system->getImpl().getV()(ii,dim) < minX) {
+                minX = system->getImpl().getV()(ii,dim);
+                minV.clear();
+                minV.push_back(ii);
+            } else if(fabs(system->getImpl().getV()(ii,dim) - minX) < 1e-5) {
+                minV.push_back(ii);
+            }
+        }
+
+        Eigen::VectorXi indices(minV.size());
+
+        //add a bunch of constraints
+        for(unsigned int iV = 0; iV < minV.size(); ++iV) {
+            indices(iV) = minV[iV];
+        }
+
+        return indices;
+    }
+
+
+    template<typename World, typename FEMSystem, typename DataType = double>
+    Eigen::SparseMatrix<DataType> fixedPointProjectionMatrix(Eigen::VectorXi &indices, FEMSystem &system,World &world) {
+
+        std::vector<Eigen::Triplet<DataType> > triplets;
+        Eigen::SparseMatrix<DataType> P;
+        Eigen::VectorXi sortedIndices = indices;
+        std::sort(sortedIndices.data(), sortedIndices.data()+indices.rows());
+
+        //build a projection matrix P which projects fixed points out of a physical syste
+        int fIndex = 0;
+
+        //total number of DOFS in system
+
+        unsigned int n = mapStateEigen<0>(world).rows();
+        unsigned int m = mapStateEigen<0>(world).rows() - 3*indices.rows();
+
+        P.resize(m,n);
+
+        //number of unconstrained DOFs
+        unsigned int rowIndex =0;
+        for(unsigned int vIndex = 0; vIndex < system.getImpl().getV().rows(); vIndex++) {
+
+            while((vIndex < system.getImpl().getV().rows()) && (fIndex < sortedIndices.rows()) &&(vIndex == sortedIndices[fIndex])) {
+                fIndex++;
+                vIndex++;
+            }
+
+            if(vIndex == system.getImpl().getV().rows())
+                break;
+
+            //add triplet into matrix
+            triplets.push_back(Eigen::Triplet<DataType>(system.getQ().getGlobalId() +rowIndex, system.getQ().getGlobalId() + 3*vIndex,1));
+            triplets.push_back(Eigen::Triplet<DataType>(system.getQ().getGlobalId() +rowIndex+1, system.getQ().getGlobalId() + 3*vIndex+1, 1));
+            triplets.push_back(Eigen::Triplet<DataType>(system.getQ().getGlobalId() +rowIndex+2, system.getQ().getGlobalId() + 3*vIndex+2, 1));
+
+            rowIndex+=3;
+        }
+
+        P.setFromTriplets(triplets.begin(), triplets.end());
+
+        //build the matrix and  return
+        return P;
+    }
 
 
 }

src/Base/include/TimeStepperEulerImplicit.h

@@ -99,7 +99,8 @@ void TimeStepperImplEulerImplicit<DataType, MatrixAssembler, VectorAssembler>::s
     //Eigen::SparseMatrix<DataType, Eigen::RowMajor> systemMatrix = (*m_massMatrix)- dt*dt*(*m_stiffnessMatrix);
 
     //we're going to build equality constraints into our gradient and hessian calcuations
-    auto E = [&world](auto &a) { return getEnergy(world); };
+    auto E = [&world, &massMatrix, &qDot](auto &a) { return (getEnergy(world) -
+                                                             mapStateEigen<1>(world).transpose()*(*massMatrix).block(0,0,world.getNumQDotDOFs(),world.getNumQDotDOFs())*qDot); };
 
     auto H = [&world, &massMatrix, &stiffnessMatrix, &dt, &qDot](auto &a)->auto & {
         //get stiffness matrix

src/Base/include/TimeStepperEulerImplicitBFGS.h

@@ -0,0 +1,143 @@
+//
+//  TimeStepperEulerImplicit.h
+//  Gauss
+//
+//  Created by David Levin on 11/23/17.
+//
+//
+
+#ifndef TimeStepperEulerImplicitBFGS_h
+#define TimeStepperEulerImplicitBFGS_h
+
+
+#include <World.h>
+#include <Assembler.h>
+#include <TimeStepper.h>
+#include <Eigen/Dense>
+
+#include <Eigen/Sparse>
+#include <UtilitiesEigen.h>
+#include <UtilitiesMATLAB.h>
+#include <LBFGS.h>
+
+//TODO Solver Interface
+namespace Gauss {
+
+    //Given Initial state, step forward in time using linearly implicit Euler Integrator
+    template<typename DataType, typename MatrixAssembler, typename VectorAssembler>
+    class TimeStepperImplEulerImplicitBFGS
+    {
+    public:
+
+        template<typename Matrix>
+        TimeStepperImplEulerImplicitBFGS(Matrix &P)  {
+
+           //P is constraint projection matrix
+            m_P = P;
+
+        }
+
+        TimeStepperImplEulerImplicitBFGS(const TimeStepperImplEulerImplicitBFGS &toCopy) {
+
+        }
+
+        ~TimeStepperImplEulerImplicitBFGS() { }
+
+        //Methods
+        //init() //initial conditions will be set at the begining
+        template<typename World>
+        void step(World &world, double dt, double t);
+
+        inline typename VectorAssembler::MatrixType & getLagrangeMultipliers() { return m_lagrangeMultipliers; }
+
+    protected:
+
+        MatrixAssembler m_massMatrix;
+        VectorAssembler m_forceVector;
+        Eigen::SparseMatrix<DataType> m_P;
+
+
+
+        typename VectorAssembler::MatrixType m_lagrangeMultipliers;
+
+    private:
+    };
+}
+
+template<typename DataType, typename MatrixAssembler, typename VectorAssembler>
+template<typename World>
+void TimeStepperImplEulerImplicitBFGS<DataType, MatrixAssembler, VectorAssembler>::step(World &world, double dt, double t) {
+
+    //First two lines work around the fact that C++11 lambda can't directly capture a member variable.
+    MatrixAssembler &massMatrix = m_massMatrix;
+    VectorAssembler &forceVector = m_forceVector;
+    Eigen::SparseMatrix<DataType> &P = m_P;
+
+    //Grab the state
+    Eigen::VectorXd q = mapStateEigen<0>(world);
+    Eigen::VectorXd qDot = mapStateEigen<1>(world);
+
+
+    ///get mass matrix
+    ASSEMBLEMATINIT(massMatrix, world.getNumQDotDOFs(), world.getNumQDotDOFs());
+    ASSEMBLELIST(massMatrix, world.getSystemList(), getMassMatrix);
+    ASSEMBLEEND(massMatrix);
+
+    //std::cout<<"F: "<<(*forceVector)<<"\n";
+    //setup RHS
+
+
+    //toMatlab(*massMatrix, "./testMHex.txt");
+    //toMatlab(*stiffnessMatrix, "./testKHex.txt");
+    //toMatlab(*forceVector, "./testfHex.txt");
+
+    //std::cout<<"F: \n"<<(*forceVector)<<"\n";
+
+
+    //Eigen::SparseMatrix<DataType, Eigen::RowMajor> systemMatrix = (*m_massMatrix)- dt*dt*(*m_stiffnessMatrix);
+
+    //we're going to build equality constraints into our gradient and hessian calcuations
+    auto objective = [&world, &q, &qDot, &dt, &forceVector, &massMatrix, &P](Eigen::VectorXd &x, Eigen::VectorXd &grad) -> double {
+
+        //std::cout<<"X\n"<<x<<"\n\n";
+        mapStateEigen<1>(world) = P.transpose()*x;
+        mapStateEigen<0>(world) = q+dt*P.transpose()*x;
+
+        ASSEMBLEVECINIT(forceVector, world.getNumQDotDOFs());
+        ASSEMBLELIST(forceVector, world.getForceList(), getForce);
+        ASSEMBLELIST(forceVector, world.getSystemList(), getForce);
+        ASSEMBLEEND(forceVector);
+
+        (*forceVector) *= -dt;
+        (*forceVector) += (*massMatrix)*(P.transpose()*x - qDot);
+
+        grad = P*(*forceVector);
+        double E = (getEnergy(world) - x.transpose()*P*(*massMatrix)*qDot);
+
+        return E;
+
+    };
+
+    LBFGSpp::LBFGSParam<DataType> param;
+    param.epsilon = 1e-3;
+    param.max_iterations = 1000;
+    param.linesearch = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;
+
+    // Create solver and function object
+    LBFGSpp::LBFGSSolver<DataType> m_solver(param);
+
+    double fx = 0.0;
+    Eigen::VectorXd qNew = P*qDot;
+
+    m_solver.minimize(objective, qNew, fx);
+    mapStateEigen<1>(world) = P.transpose()*qNew;
+    mapStateEigen<0>(world) = q + dt*P.transpose()*qNew;
+
+
+}
+
+template<typename DataType, typename MatrixAssembler, typename VectorAssembler>
+using TimeStepperEulerImplicitBFGS = TimeStepper<DataType, TimeStepperImplEulerImplicitBFGS<DataType, MatrixAssembler, VectorAssembler> >;
+
+
+#endif /* TimeStepperEulerImplicit_h */

src/Base/include/TimeStepperEulerImplicitLinear.h

@@ -161,7 +161,7 @@ void TimeStepperImplEulerImplicitLinear<DataType, MatrixAssembler, VectorAssembl
 }
 
 template<typename DataType, typename MatrixAssembler, typename VectorAssembler>
-using TimeStepperEulerImplictLinear = TimeStepper<DataType, TimeStepperImplEulerImplicitLinear<DataType, MatrixAssembler, VectorAssembler> >;
+using TimeStepperEulerImplicitLinear = TimeStepper<DataType, TimeStepperImplEulerImplicitLinear<DataType, MatrixAssembler, VectorAssembler> >;
 
 
 

src/Collisions/include/TimeStepperEulerImplicitLinearCollisions.h

@@ -137,7 +137,7 @@ namespace Gauss {
     }
 
     template<typename DataType, typename MatrixAssembler, typename VectorAssembler>
-    using TimeStepperEulerImplictLinearCollisions = TimeStepper<DataType, TimeStepperImplEulerImplicitLinearCollisions<DataType, MatrixAssembler, VectorAssembler> >;
+    using TimeStepperEulerImplicitLinearCollisions = TimeStepper<DataType, TimeStepperImplEulerImplicitLinearCollisions<DataType, MatrixAssembler, VectorAssembler> >;
     }
 }
 #endif /* TimeStepperEulerImplicitLinearCollisions_h */

src/Examples/CMakeLists.txt

@@ -22,6 +22,8 @@ if(USE_PARDISO)
 	target_link_libraries(Example11 Core Base FEM Optimization UI ${GAUSS_LIBS})
 endif(USE_PARDISO)
 
+add_executable(Example13 example13.cpp)
+
 target_link_libraries(Example1 Core Base UI ${GAUSS_LIBS})
 target_link_libraries(Example2 Core Base FEM UI ${GAUSS_LIBS})
 target_link_libraries(Example3 Core Base FEM UI ${GAUSS_LIBS})
@@ -38,6 +40,6 @@ target_link_libraries(Example5 Core Base FEM UI ${GAUSS_LIBS})
 target_link_libraries(Example7 Core Base FEM UI ${GAUSS_LIBS})
 target_link_libraries(Example9 Core Base FEM UI ${GAUSS_LIBS})
 target_link_libraries(Example10 Core Base FEM UI ${GAUSS_LIBS})
-
+target_link_libraries(Example13 Core Base FEM UI ${GAUSS_LIBS})
 
 

src/Examples/example1.cpp

@@ -16,7 +16,7 @@ typedef World<double,
               std::tuple<ForceSpringParticles<double> *>,
               std::tuple<ConstraintFixedPoint<double> *> >  MyWorld;
 
-typedef TimeStepperEulerImplictLinear<double,
+typedef TimeStepperEulerImplicitLinear<double,
                                       Assembler<double, AssemblerImplEigenSparseMatrix>,
                                       Assembler<double, AssemblerImplEigenVector> > MyTimeStepper;
 

src/Examples/example10.cpp

@@ -24,7 +24,7 @@ typedef PhysicalSystemFEM<double, NeohookeanTet> FEMLinearTets;
 typedef World<double, std::tuple<FEMLinearTets *,PhysicalSystemParticleSingle<double> *>,
 std::tuple<ForceSpringFEMParticle<double> *, ForceParticlesGravity<double> *>,
 std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
-typedef TimeStepperEulerImplictLinear<double, AssemblerEigenSparseMatrix<double>,
+typedef TimeStepperEulerImplicitLinear<double, AssemblerEigenSparseMatrix<double>,
 AssemblerEigenVector<double> > MyTimeStepper;
 
 typedef Scene<MyWorld, MyTimeStepper> MyScene;

src/Examples/example11.cpp

@@ -59,53 +59,6 @@ int main(int argc, char **argv) {
 
     MyTimeStepper stepper(0.1);
 
-    //static solve minimizing the potential energy of an object
-
-    //Newtons method requires energy, gradient, hessian, solver and initial position
-    //Need some assemblers
-    AssemblerEigenSparseMatrix<double> K;
-    AssemblerEigenVector<double> f;
-    SolverPardiso<Eigen::SparseMatrix<double, Eigen::RowMajor>> solver;
-
-    //we're going to build equality constraints into our gradient and hessiang calcuations
-    auto E = [&world](auto &a) { return getEnergy(world); }; //Not used by Newtons solver currently
-
-    auto H = [&world, &K](auto &a)->auto & {
-        //get mass matrix
-        ASSEMBLEMATINIT(K, world.getNumQDotDOFs()+world.getNumConstraints(), world.getNumQDotDOFs()+world.getNumConstraints());
-        ASSEMBLELIST(K, world.getSystemList(), getStiffnessMatrix);
-        ASSEMBLELIST(K, world.getForceList(), getStiffnessMatrix);
-        ASSEMBLELISTOFFSET(K, world.getConstraintList(), getGradient, world.getNumQDotDOFs(), 0);
-        ASSEMBLELISTOFFSETTRANSPOSE(K, world.getConstraintList(), getGradient, 0, world.getNumQDotDOFs());
-        ASSEMBLEEND(K);
-
-        (*K) *= -1;
-        return (*K);
-    };
-
-    auto g = [&world, &f](auto &a) -> auto & {
-        ASSEMBLEVECINIT(f, world.getNumQDotDOFs()+world.getNumConstraints());
-        ASSEMBLELIST(f, world.getForceList(), getForce);
-        ASSEMBLELIST(f, world.getSystemList(), getForce);
-        ASSEMBLEEND(f);
-
-        (*f) *= -1;
-        return (*f);
-    };
-
-     auto solve = [&solver](auto &A, auto &b)->auto & {
-        return solver.solve(A,b);
-    };
-
-    auto update = [&world](auto &update) {
-        mapStateEigen<0>(world) = update.head(world.getNumQDOFs());
-    };
-
-    //solve this using newton's method
-    //auto x0 = Eigen::VectorXd(world.getNumQDotDOFs()+world.getNumConstraints());
-    //x0.head(world.getNumQDotDOFs()) = mapStateEigen<0>(world);
-    //Optimization::newton(E, g, H, solve, x0, update, 1e-5, 1000);
-
     //Display
     QGuiApplication app(argc, argv);
 

src/Examples/example12.cpp

@@ -23,7 +23,7 @@ typedef World<double, std::tuple<FEMLinearTets *,PhysicalSystemParticleSingle<do
 std::tuple<ForceSpringFEMParticle<double> *, ForceParticlesGravity<double> *>,
 std::tuple<ConstraintFixedPoint<double> *, MyCollisionDetector *> > MyWorld;
 
-typedef TimeStepperEulerImplictLinearCollisions<double, AssemblerEigenSparseMatrix<double>,
+typedef TimeStepperEulerImplicitLinearCollisions<double, AssemblerEigenSparseMatrix<double>,
 AssemblerEigenVector<double> > MyTimeStepper;
 
 typedef Scene<MyWorld, MyTimeStepper> MyScene;

src/Examples/example2.cpp

@@ -18,7 +18,7 @@ using namespace ParticleSystem; //For Force Spring
 typedef PhysicalSystemFEM<double, LinearTet> FEMLinearTets;
 
 typedef World<double, std::tuple<FEMLinearTets *>, std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
-typedef TimeStepperEulerImplictLinear<double, AssemblerEigenSparseMatrix<double>,
+typedef TimeStepperEulerImplicitLinear<double, AssemblerEigenSparseMatrix<double>,
 AssemblerEigenVector<double> > MyTimeStepper;
 
 typedef Scene<MyWorld, MyTimeStepper> MyScene;

src/Examples/example3.cpp

@@ -18,7 +18,7 @@ using namespace ParticleSystem; //For Force Spring
 typedef PhysicalSystemFEM<double, LinearHex> FEMLinearHexes;
 
 typedef World<double, std::tuple<FEMLinearHexes *>, std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
-typedef TimeStepperEulerImplictLinear<double, AssemblerEigenSparseMatrix<double>, AssemblerEigenVector<double> > MyTimeStepper;
+typedef TimeStepperEulerImplicitLinear<double, AssemblerEigenSparseMatrix<double>, AssemblerEigenVector<double> > MyTimeStepper;
 
 typedef Scene<MyWorld, MyTimeStepper> MyScene;
 

src/Examples/example4.cpp

@@ -16,7 +16,7 @@ using namespace ParticleSystem; //For Force Spring
 typedef PhysicalSystemFEM<double, LinearHex> FEMLinearHexes;
 
 typedef World<double, std::tuple<FEMLinearHexes *>, std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
-typedef TimeStepperEulerImplictLinear<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double> >,
+typedef TimeStepperEulerImplicitLinear<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double> >,
 AssemblerParallel<double, AssemblerEigenVector<double> > > MyTimeStepper;
 
 typedef Scene<MyWorld, MyTimeStepper> MyScene;