Switch to eigen operations for cross and dot products.

Update comment around for loop.

Change to double slash.

Change cross product to eigen.

src/pessimisticPatternMatcherUtils.cc

@@ -1,11 +1,11 @@
-/// -*- LSST-C++ -*-
+// -*- LSST-C++ -*-
 
 /*
  * This file is part of meas_astrom.
  *
  * Developed for the LSST Data Management System.
  * This product includes software developed by the LSST Project
- * (https:///www.lsst.org).
+ * (https://www.lsst.org).
  * See the COPYRIGHT file at the top-level directory of this distribution
  * for details of code ownership.
  *
@@ -20,11 +20,12 @@
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https:///www.gnu.org/licenses/>.
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
 #include <cmath>
-#include <iostream>
+#include <Eigen/Dense>
+#include "ndarray/eigen.h"
 #include "lsst/meas/astrom/pessimisticPatternMatcherUtils.h"
 
 namespace lsst {
@@ -36,33 +37,26 @@ int check_spoke(double cos_theta_src, double sin_theta_src, ndarray::Array<doubl
                 double proj_ref_ctr_dist_sq, ndarray::Array<long int, 1, 1> const& ref_dist_idx_array,
                 ndarray::Array<uint16_t, 1, 1> const& ref_id_array,
                 ndarray::Array<double, 2, 1> const& reference_array, double src_sin_tol) {
-    /// Loop over our candidate reference objects.
+    // Loop over our candidate reference objects. ref_dist_idx_array is a view into ref_id_array
+    // and is not the same length as ref_id_array.
     for (auto idx = ref_dist_idx_array.begin(); idx != ref_dist_idx_array.end(); idx++) {
-        /// Compute the delta vector from the pattern center.
+        // Compute the delta vector from the pattern center.
         unsigned int ref_id = ref_id_array[*idx];
-        double ref_delta[3];
-        double proj_ref_delta[3];
-        ref_delta[0] = reference_array[ref_id][0] - ref_ctr[0];
-        ref_delta[1] = reference_array[ref_id][1] - ref_ctr[1];
-        ref_delta[2] = reference_array[ref_id][2] - ref_ctr[2];
+        ndarray::Array<double, 1, 1> ref_delta = copy(reference_array[ref_id] - ref_ctr);
 
-        double ref_dot = ref_delta[0] * ref_ctr[0] + ref_delta[1] * ref_ctr[1] + ref_delta[2] * ref_ctr[2];
-        proj_ref_delta[0] = ref_delta[0] - ref_dot * ref_ctr[0];
-        proj_ref_delta[1] = ref_delta[1] - ref_dot * ref_ctr[1];
-        proj_ref_delta[2] = ref_delta[2] - ref_dot * ref_ctr[2];
-        /// Compute the cos between our "center" reference vector and the
-        /// current reference candidate.
+        double ref_dot = ndarray::asEigenMatrix(ref_delta).dot(ndarray::asEigenMatrix(ref_ctr));
+        ndarray::Array<double, 1, 1> proj_ref_delta = copy(ref_delta - ref_dot * ref_ctr);
+        // Compute the cos between our "center" reference vector and the
+        // current reference candidate.
         double proj_delta_dist_sq =
-                (proj_ref_delta[0] * proj_ref_delta[0] + proj_ref_delta[1] * proj_ref_delta[1] +
-                 proj_ref_delta[2] * proj_ref_delta[2]);
+                ndarray::asEigenMatrix(proj_ref_delta).dot(ndarray::asEigenMatrix(proj_ref_delta));
         double geom_dist_ref = sqrt(proj_ref_ctr_dist_sq * proj_delta_dist_sq);
         double cos_theta_ref =
-                (proj_ref_delta[0] * proj_ref_ctr_delta[0] + proj_ref_delta[1] * proj_ref_ctr_delta[1] +
-                 proj_ref_delta[2] * proj_ref_ctr_delta[2]) /
+                ndarray::asEigenMatrix(proj_ref_delta).dot(ndarray::asEigenMatrix(proj_ref_ctr_delta)) /
                 geom_dist_ref;
 
-        /// Make sure we can safely make the comparison in case
-        /// our "center" and candidate vectors are mostly aligned.
+        // Make sure we can safely make the comparison in case
+        // our "center" and candidate vectors are mostly aligned.
         double cos_sq_comparison;
         if (cos_theta_ref * cos_theta_ref < (1 - src_sin_tol * src_sin_tol)) {
             cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
@@ -71,41 +65,33 @@ int check_spoke(double cos_theta_src, double sin_theta_src, ndarray::Array<doubl
             cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
                                 (src_sin_tol * src_sin_tol);
         }
-        /// Test the difference of the cosine of the reference angle against
-        /// the source angle. Assumes that the delta between the two is
-        /// small.
+        // Test the difference of the cosine of the reference angle against
+        // the source angle. Assumes that the delta between the two is
+        // small.
         if (cos_sq_comparison > src_sin_tol * src_sin_tol) {
             continue;
         }
-        /// The cosine tests the magnitude of the angle but not
-        /// its direction. To do that we need to know the sine as well.
-        /// This cross product calculation does that.
-        double cross_ref[3];
-        cross_ref[0] =
-                (proj_ref_delta[1] * proj_ref_ctr_delta[2] - proj_ref_delta[2] * proj_ref_ctr_delta[1]) /
-                geom_dist_ref;
-        cross_ref[1] =
-                (proj_ref_delta[2] * proj_ref_ctr_delta[0] - proj_ref_delta[0] * proj_ref_ctr_delta[2]) /
-                geom_dist_ref;
-        cross_ref[2] =
-                (proj_ref_delta[0] * proj_ref_ctr_delta[1] - proj_ref_delta[1] * proj_ref_ctr_delta[0]) /
-                geom_dist_ref;
-        double sin_theta_ref =
-                cross_ref[0] * ref_ctr[0] + cross_ref[1] * ref_ctr[1] + cross_ref[2] * ref_ctr[2];
-        /// Check the value of the cos again to make sure that it is not
-        /// near zero.
+        // The cosine tests the magnitude of the angle but not
+        // its direction. To do that we need to know the sine as well.
+        // This cross product calculation does that.
+        Eigen::Vector3d cross_ref = ndarray::asEigenMatrix(proj_ref_delta)
+                                            .head<3>()
+                                            .cross(ndarray::asEigenMatrix(proj_ref_ctr_delta).head<3>()) /
+                                    geom_dist_ref;
+        double sin_theta_ref = cross_ref.dot(ndarray::asEigenMatrix(ref_ctr));
+        // Check the value of the cos again to make sure that it is not
+        // near zero.
         double sin_comparison;
         if (abs(cos_theta_src) < src_sin_tol) {
             sin_comparison = (sin_theta_src - sin_theta_ref) / src_sin_tol;
         } else {
             sin_comparison = (sin_theta_src - sin_theta_ref) / cos_theta_ref;
         }
 
-        if (abs(sin_comparison) > src_sin_tol) {
-            continue;
+        // Return the correct id of the candidate we found.
+        if (abs(sin_comparison) < src_sin_tol) {
+            return ref_id;
         }
-        /// Return the correct id of the candidate we found.
-        return ref_id;
     }
     return -1;
 }