make gpr update

data_acq_and_processing/gpr_cpp/src/geometry.cpp

@@ -0,0 +1,68 @@
+#include "gpr/geometry.hpp"
+
+#include <cmath>
+
+namespace gpr {
+
+double pair_geo_ref(const GeometryConfig& g, const BPConfig& bp, int i_tx, int i_rx, double yy) {
+    double xc = (g.x_tx[i_tx] + g.x_rx[i_rx]) / 2.0;
+    double Rtx = std::sqrt(std::pow(xc - g.x_tx[i_tx], 2) + std::pow(yy - g.y_tx[i_tx], 2) +
+                           std::pow(bp.comp_ref_depth - g.z_tx[i_tx], 2));
+    double Rrx = std::sqrt(std::pow(xc - g.x_rx[i_rx], 2) + std::pow(yy - g.y_rx[i_rx], 2) +
+                           std::pow(bp.comp_ref_depth - g.z_rx[i_rx], 2));
+    return 1.0 / (Rtx * Rrx + 1e-12) + 1e-30;
+}
+
+std::vector<FrameOffset> frame_offsets(const std::vector<double>& frame_times, double t_ref,
+                                       const MotionConfig& cfg) {
+    std::vector<FrameOffset> out(frame_times.size());
+    double pitch = cfg.pitch_deg * M_PI / 180.0;
+    for (size_t k = 0; k < frame_times.size(); ++k) {
+        double t = frame_times[k] - t_ref;
+        double h = cfg.direction_sign * cfg.speed_m_s * t;
+        out[k].dx_m = 0.0;
+        out[k].dy_m = h * std::sin(pitch);
+        out[k].dz_m = h * std::cos(pitch);
+        out[k].dt_s = t;
+        out[k].horizontal_m = h;
+    }
+    return out;
+}
+
+std::vector<double> frame_weights(const std::vector<FrameOffset>& offs, double edge_weight) {
+    size_t n = offs.size();
+    std::vector<double> w(n, 1.0);
+    double edge = std::min(std::max(edge_weight, 0.0), 1.0);
+    if (n == 0 || edge >= 1.0) return w;
+    double dmax = 0.0;
+    for (auto& o : offs) dmax = std::max(dmax, std::abs(o.dt_s));
+    if (dmax <= 1e-12) return w;
+    for (size_t i = 0; i < n; ++i) {
+        double r = std::min(std::abs(offs[i].dt_s) / dmax, 1.0);
+        w[i] = edge + (1.0 - edge) * 0.5 * (1.0 + std::cos(M_PI * r));
+    }
+    return w;
+}
+
+std::array<double, 3> point_to_scene(double x, double y, double z, const MotionConfig& mot,
+                                     const SARConfig& sar) {
+    if (!sar.coord_mode_ground) return {x, y, z};
+    double p = mot.pitch_deg * M_PI / 180.0;
+    double ys = y * std::cos(p) - z * std::sin(p);
+    double zs = y * std::sin(p) + z * std::cos(p);
+    return {x, ys, zs};
+}
+
+Grid build_grid_scene(const std::array<double, 3>& c, const SARConfig& sar) {
+    Grid g;
+    auto lin = [](double a, double b, int n, std::vector<double>& out) {
+        out.resize(n);
+        for (int i = 0; i < n; ++i) out[i] = a + (b - a) * i / (double)(n - 1);
+    };
+    lin(c[0] - sar.half_width_m[0], c[0] + sar.half_width_m[0], sar.grid[0], g.x);
+    lin(c[1] - sar.half_width_m[1], c[1] + sar.half_width_m[1], sar.grid[1], g.y);
+    lin(c[2] - sar.half_width_m[2], c[2] + sar.half_width_m[2], sar.grid[2], g.z);
+    return g;
+}
+
+}  // namespace gpr