27 #include <Eigen/Dense>
28 #include "ndarray/eigen.h"
35 int check_spoke(
double cos_theta_src,
double sin_theta_src, ndarray::Array<double, 1, 1>
const& ref_ctr,
36 ndarray::Array<double, 1, 1>
const& proj_ref_ctr_delta,
double proj_ref_ctr_dist_sq,
37 ndarray::Array<long int, 1, 1>
const& ref_dist_idx_array,
38 ndarray::Array<uint16_t, 1, 1>
const& ref_id_array,
39 ndarray::Array<double, 2, 1>
const& reference_array,
double src_sin_tol) {
42 for (
auto idx = ref_dist_idx_array.begin(); idx != ref_dist_idx_array.end(); idx++) {
44 unsigned int ref_id = ref_id_array[*idx];
45 ndarray::Array<double, 1, 1> ref_delta = copy(reference_array[ref_id] - ref_ctr);
47 double ref_dot = ndarray::asEigenMatrix(ref_delta).dot(ndarray::asEigenMatrix(ref_ctr));
48 ndarray::Array<double, 1, 1> proj_ref_delta = copy(ref_delta - ref_dot * ref_ctr);
51 double proj_delta_dist_sq =
52 ndarray::asEigenMatrix(proj_ref_delta).dot(ndarray::asEigenMatrix(proj_ref_delta));
53 double geom_dist_ref = sqrt(proj_ref_ctr_dist_sq * proj_delta_dist_sq);
54 double cos_theta_ref =
55 ndarray::asEigenMatrix(proj_ref_delta).dot(ndarray::asEigenMatrix(proj_ref_ctr_delta)) /
60 double cos_sq_comparison;
61 if (cos_theta_ref * cos_theta_ref < (1 - src_sin_tol * src_sin_tol)) {
62 cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
63 (1 - cos_theta_ref * cos_theta_ref);
65 cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
66 (src_sin_tol * src_sin_tol);
71 if (cos_sq_comparison > src_sin_tol * src_sin_tol) {
77 Eigen::Vector3d cross_ref = ndarray::asEigenMatrix(proj_ref_delta)
79 .cross(ndarray::asEigenMatrix(proj_ref_ctr_delta).head<3>()) /
81 double sin_theta_ref = cross_ref.dot(ndarray::asEigenMatrix(ref_ctr));
84 double sin_comparison;
85 if (abs(cos_theta_src) < src_sin_tol) {
86 sin_comparison = (sin_theta_src - sin_theta_ref) / src_sin_tol;
88 sin_comparison = (sin_theta_src - sin_theta_ref) / cos_theta_ref;
92 if (abs(sin_comparison) < src_sin_tol) {
int check_spoke(double cos_theta_src, double sin_theta_src, ndarray::Array< double, 1, 1 > const &ref_ctr, ndarray::Array< double, 1, 1 > const &proj_ref_ctr_delta, 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)
Check the opening angle between the first spoke of our pattern for the source object against the refe...