Merge pull request GeomScale#299 from vfisikop/trigonometric_intersect

Refactor trigonometric_positive_intersect function for hpolytopes
vfisikop · Apr 9, 2024 · 6a5a17e · 6a5a17e
2 parents 9114f37 + 742eb01
commit 6a5a17e
Showing 1 changed file with 22 additions and 19 deletions.
diff --git a/include/convex_bodies/hpolytope.h b/include/convex_bodies/hpolytope.h
@@ -750,33 +750,36 @@ class HPolytope {
     }
 
 
-    //------------oracle for exact hmc spherical gaussian sampling---------------//
-
+    // Boundary oracle for exact hmc spherical gaussian sampling
     // compute intersection point of ray starting from r and pointing to v
-    // with polytope discribed by A and b
+    // with polytope discribed by A and b (the ray describes a curve)
     std::pair<NT, int> trigonometric_positive_intersect(Point const& r, Point const& v,
                                                         NT const& omega, int &facet_prev) const
     {
+        constexpr NT pi_2 = NT(2.0) * M_PI;
+        NT t = std::numeric_limits<NT>::max();
 
-        NT lamda = 0, C, Phi, t1, t2, tmin;
-        NT min_plus = std::numeric_limits<NT>::max(), t = std::numeric_limits<NT>::max();
-        NT max_minus = std::numeric_limits<NT>::lowest();
-        VT sum_nom, sum_denom;
-        unsigned int j;
-        int m = num_of_hyperplanes(), facet = -1;
-
+        int m = num_of_hyperplanes();
+        int facet = -1;
 
+        VT sum_nom;
+        VT sum_denom;
         sum_nom.noalias() = A * r.getCoefficients();
         sum_denom.noalias() = A * v.getCoefficients();
 
         NT* sum_nom_data = sum_nom.data();
         NT* sum_denom_data = sum_denom.data();
         const NT* b_data = b.data();
 
+        const NT omega_sqr = omega * omega;
+        const NT pi_2_omega = pi_2 / omega;
+
         for (int i = 0; i < m; i++) {
 
-            C = std::sqrt((*sum_nom_data) * (*sum_nom_data) + ((*sum_denom_data) * (*sum_denom_data)) / (omega * omega));
-            Phi = std::atan((-(*sum_denom_data)) / ((*sum_nom_data) * omega));
+            NT C = std::sqrt((*sum_nom_data) * (*sum_nom_data) + ((*sum_denom_data) * (*sum_denom_data))
+              / omega_sqr);
+            NT Phi = std::atan((-(*sum_denom_data)) / ((*sum_nom_data) * omega));
+
             if ((*sum_denom_data) < 0.0 && Phi < 0.0) {
                 Phi += M_PI;
             } else if ((*sum_denom_data) > 0.0 && Phi > 0.0) {
@@ -785,20 +788,20 @@ class HPolytope {
 
             if (C > (*b_data)) {
                 NT acos_b = std::acos((*b_data) / C);
-                t1 = (acos_b - Phi) / omega;
+                NT t1 = (acos_b - Phi) / omega;
                 if (facet_prev == i && std::abs(t1) < 1e-10){
-                    t1 = (2.0 * M_PI) / omega;
+                    t1 = pi_2_omega;
                 }
 
-                t2 = (-acos_b - Phi) / omega;
+                NT t2 = (-acos_b - Phi) / omega;
                 if (facet_prev == i && std::abs(t2) < 1e-10){
-                    t2 = (2.0 * M_PI) / omega;
+                    t2 = pi_2_omega;
                 }
 
-                t1 += (t1 < NT(0)) ? (2.0 * M_PI) / omega : NT(0);
-                t2 += (t2 < NT(0)) ? (2.0 * M_PI) / omega : NT(0);
+                t1 += (t1 < NT(0)) ? pi_2_omega : NT(0);
+                t2 += (t2 < NT(0)) ? pi_2_omega : NT(0);
 
-                tmin = std::min(t1, t2);
+                NT tmin = std::min(t1, t2);
 
                 if (tmin < t && tmin > NT(0)) {
                     facet = i;