Position Nudging after Position Update (#308)

* Position Nudging after Position Update

* Complexity improvements and Polytope Normalization

* HPolytope Normalization Flag

* Polytope normalization within Walk Constructor

* Alias HPolytope Normalization for Nudging inside Gaussian HMC

* Polytope Normalization in ComputeInner Ball Fixed

* Polytope Normalization Style change

* Nudge in function within Gaussian HMC, and restore Hpoly file

* More efficient Nudge in Process

include/convex_bodies/hpolytope.h

@@ -927,4 +927,4 @@ class HPolytope {
     }
 };
 
-#endif
+#endif

include/random_walks/gaussian_hamiltonian_monte_carlo_exact_walk.hpp

@@ -53,6 +53,7 @@ struct Walk
     typedef typename Polytope::PointType Point;
     typedef typename Point::FT NT;
     typedef typename Polytope::VT VT;
+    typedef typename Polytope::MT MT;
 
     template <typename GenericPolytope>
     Walk(GenericPolytope &P, Point const& p, NT const& a_i, RandomNumberGenerator &rng)
@@ -80,7 +81,7 @@ struct Walk
     <
         typename GenericPolytope
     >
-    inline void apply(GenericPolytope& P,
+    inline void apply(GenericPolytope const& P,
                       Point& p,
                       NT const& a_i,
                       unsigned int const& walk_length,
@@ -89,6 +90,9 @@ struct Walk
         unsigned int n = P.dimension();
         NT T;
 
+        GenericPolytope P_normalized = P;
+        P_normalized.normalize();
+
         for (auto j=0u; j<walk_length; ++j)
         {
             T = rng.sample_urdist() * _Len;
@@ -105,6 +109,7 @@ struct Walk
                 _lambda_prev = pbpair.first;
                 T -= _lambda_prev;
                 update_position(_p, _v, _lambda_prev, _omega);
+                nudge_in(P_normalized, _p);
                 P.compute_reflection(_v, _p, pbpair.second);
                 it++;
             }
@@ -120,7 +125,7 @@ struct Walk
     <
         typename GenericPolytope
     >
-    inline void get_starting_point(GenericPolytope& P,
+    inline void get_starting_point(GenericPolytope const& P,
                                    Point const& center,
                                    Point &q,
                                    unsigned int const& walk_length,
@@ -141,7 +146,7 @@ struct Walk
     <
         typename GenericPolytope
     >
-    inline void parameters_burnin(GenericPolytope& P,
+    inline void parameters_burnin(GenericPolytope const& P,
                                   Point const& center,
                                   unsigned int const& num_points,
                                   unsigned int const& walk_length,
@@ -191,7 +196,7 @@ private :
     <
         typename GenericPolytope
     >
-    inline void initialize(GenericPolytope& P,
+    inline void initialize(GenericPolytope const& P,
                            Point const& p,
                            NT const& a_i,
                            RandomNumberGenerator &rng)
@@ -204,6 +209,9 @@ private :
         NT T = rng.sample_urdist() * _Len;
         int it = 0;
 
+        GenericPolytope P_normalized = P;
+        P_normalized.normalize();
+
         while (it <= _rho)
         {
             auto pbpair
@@ -218,12 +226,50 @@ private :
             }
             _lambda_prev = pbpair.first;
             update_position(_p, _v, _lambda_prev, _omega);
+            nudge_in(P_normalized, _p);
             T -= _lambda_prev;
             P.compute_reflection(_v, _p, pbpair.second);
             it++;
         }
     }
 
+    template
+    <
+        typename GenericPolytope
+    >
+    inline void nudge_in(GenericPolytope& P, Point& p, NT tol=NT(0))
+    {
+        MT A = P.get_mat();
+        VT b = P.get_vec();
+        int m = A.rows();
+
+        VT b_Ax = b - A * p.getCoefficients();
+        const NT* b_Ax_data = b_Ax.data();
+
+        NT dist;
+
+        for (int i = 0; i < m; i++) {
+
+            dist = *b_Ax_data;
+
+            if (dist < NT(-tol)){
+                //Nudging correction
+                NT eps = -1e-7;
+
+                NT eps_1 = -dist;
+                //A.row is already normalized, no need to do it again
+                VT A_i = A.row(i);
+                NT eps_2 = eps_1 + eps;
+
+                //Nudge the point inside with respect to the normal its vector
+                Point shift(A_i);
+                shift.operator*=(eps_2);
+                p.operator+=(shift);
+            }
+            b_Ax_data++;
+        }
+    }
+
     inline void update_position(Point &p, Point &v, NT const& T, NT const& omega)
     {
         NT next_p, next_v;
@@ -274,4 +320,3 @@ private :
 
 
 #endif // RANDOM_WALKS_GAUSSIAN_HMC_WALK_HPP
-