More stable energy + newton bug fixes

src/Base/include/TimeStepperEulerImplicit.h

@@ -142,7 +142,6 @@ void TimeStepperImplEulerImplicit<DataType, MatrixAssembler, VectorAssembler>::s
     //solve this using newton's method
     auto x0 = Eigen::VectorXd(world.getNumQDotDOFs()+world.getNumConstraints());
     x0.setZero();
-    mapStateEigen<1>(world).setZero();
     //x0.head(world.getNumQDotDOFs()) = mapStateEigen<1>(world);
     Optimization::newton(E, g, H, solve, x0, update, 1e-4, 10000);
     //std::cout<<mapStateEigen<1>(world)<<"\n";

src/Base/include/UtilitiesBase.h

@@ -62,6 +62,7 @@ void getForceVector(Matrix &forceVector, System &system, World &world) {
     ASSEMBLEVECINIT(forceVector, system.getQ().getNumScalarDOF());
     forceVector.setOffset(-system.getQ().getGlobalId(), 0);
     system.getForce(forceVector, world.getState());
+
     //ASSEMBLELIST(forceVector, world.getForceList(), getForce);
     //ASSEMBLELIST(forceVector, world.getSystemList(), getForce);
     ASSEMBLEEND(forceVector);

src/Examples/example10.cpp

@@ -52,7 +52,7 @@ int main(int argc, char **argv) {
     test1->getImpl().setMass(1000);
     ForceSpringFEMParticle<double> *forceSpring = new ForceSpringFEMParticle<double>(PosFEM<double>(&test->getQ()[0],0, &test->getImpl().getV()),
                                                                                      PosParticle<double>(&test1->getQ()),
-                                                                                     4.0, 400000.0);
+                                                                                     5, 20000.0);
     world.addSystem(test);
     world.addSystem(test1);
     world.addForce(forceSpring);
@@ -67,10 +67,7 @@ int main(int argc, char **argv) {
     q1[0] = 5.0;
     q1[1] = 7.0;
 
-    AssemblerEigenVector<double> force;
-    getInternalForceVector(force, *test, world);
-    std::cout<<*force<<"\n";
-    MyTimeStepper stepper(0.01);
+    MyTimeStepper stepper(0.1);
 
     //Display
     QGuiApplication app(argc, argv);

src/Examples/example11.cpp

@@ -57,6 +57,8 @@ int main(int argc, char **argv) {
     auto q = mapStateEigen(world);
     q.setZero();
 
+    MyTimeStepper stepper(0.1);
+
     //Display
     QGuiApplication app(argc, argv);
 

src/Optimization/include/Newton.h

@@ -18,14 +18,16 @@ namespace Gauss {
 
              //std::cout<<"NORM2: "<<x0.norm()<<"\n";
             //first version is a lazy Newton step with no line search.
+
+            pscallback(x0);
             double E0 = f(x0);
             auto p = -solver(H(x0), g(x0));
             double gStep = g(x0).transpose()*p;
 
             //back tracking line search
             double alpha =1;
             double c = 1e-4; //from Nocedal and Wright pg 31
-            double rho = 0.5;
+            double rho = 0.7;
 
             while(true) {
 
@@ -38,9 +40,9 @@ namespace Gauss {
                 //std::cout<<f(x0+alpha*p)<<" "<<E0 + c*alpha*gStep<<" "<<E0<<" "<<alpha<<"\n";
                 alpha = alpha*rho;
 
-                if(alpha < 1e-8) {
-                    alpha = 0;
-                }
+                //if(alpha < 1e-8) {
+                  //  alpha = 0;
+                //}
             }
 
             x0 = x0+alpha*p;

src/ParticleSystem/include/ForceSpring.h

@@ -33,7 +33,12 @@ namespace Gauss {
             inline DataType getEnergy(State<DataType> &state) {
 
                 //spring energy = k*(1.0 - l/l0).^2]
-                DataType l = (1.0-(m_q1(state)-m_q0(state)).norm());
+                double mag = (m_q1(state)-m_q0(state)).norm();
+                //if(fabs(mag) < 1e-3) {
+                  //  mag = 1e-3;
+                //}
+
+                DataType l = (1.0- mag/m_l0);
                 return 0.5*m_k*l*l;
             }
 
@@ -77,15 +82,15 @@ namespace Gauss {
                 auto q0 = m_q0(state);
                 auto q1 = m_q1(state);
 
-                double l = (q1-q0).norm();
+                double mag = (q1-q0).norm();
 
-                if(fabs(l) < 1e-8) {
-                    l = 1e-8;
-                }
+
+                if(fabs(mag) < 1e-8) {
+                    mag = 1e-8;
+               }
 
 
-                double strain = 1.0 - l/m_l0;
-                Eigen::Vector3d fSpring = (m_k/(m_l0*m_l0))*strain*(q1-q0)/l;
+                Eigen::Vector3d fSpring = (m_k/m_l0)*(1.0/mag -1.0/m_l0)*(q1-q0);
                 assign(f, fSpring, std::array<DOFBase<DataType,0> , 1>{{*m_q1.getDOF()}});
                 fSpring = -fSpring;
                 assign(f, fSpring, std::array<DOFBase<DataType,0> , 1>{{*m_q0.getDOF()}});
@@ -98,17 +103,19 @@ namespace Gauss {
                 auto q0 = m_q0(state);
                 auto q1 = m_q1(state);
 
-                double l = (q1-q0).norm();
-
-                if(fabs(l) < 1e-8) {
-                    l = 1e-8;
-                }
+                Eigen::Vector3x<DataType> dq = q1-q0;
+
+                double mag = (q1-q0).norm();
+                if(fabs(mag) < 1e-8) {
+                    mag = 1e-8;
+               }
 
+                Eigen::Vector3x<DataType> dqN = dq/mag;
 
-                double strain = 1.0 - l/m_l0;
-                double b = (m_k/(m_l0*m_l0*l));
-                double a = b*strain;
-                double c = b/l;
+                //double strain = 1.0 - l/m_l0;
+                double b = m_k/m_l0;
+                double a = b*(1.0/mag - 1.0/m_l0);
+                double c = b/(mag);
 
                 Eigen::Matrix<double, 6,3> B;
                 B << -1,  0,  0,
@@ -119,7 +126,7 @@ namespace Gauss {
                       0,  0,  1;
 
                 Eigen::Matrix<double, 6,6> Hspring;
-                Hspring = -a*B*B.transpose() + c*B*(q1-q0)*((B*(q1-q0)).transpose());
+                Hspring = a*B*B.transpose() - c*B*dqN*dqN.transpose()*B.transpose();
                 assign(H, Hspring, std::array<DOFBase<DataType,0> , 1>{{*m_q0.getDOF()}}, std::array<DOFBase<DataType,0> , 1>{{*m_q1.getDOF()}});
 
             }