radar_system/data_acq_and_processing/gpr_cpp/src/geometry.cpp

#include "gpr/geometry.hpp"

#include <algorithm>
#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) + sar.ground_depth_m;
    double zs = y * std::sin(p) + z * std::cos(p);
    return {x, ys, zs};
}

double ground_plane_y(double depth, double ground_depth_m, double pitch_rad) {
    return (-ground_depth_m + depth * std::sin(pitch_rad)) / std::cos(pitch_rad);
}

Grid build_full_region_grid(const SARConfig& sar) {
    // numpy: linspace(lo, hi, max(round(span/step)+1, 2)); nearbyint = банковское
    // округление python round()
    auto axis = [](double lo, double hi, double step, std::vector<double>& out) {
        int n = std::max((int)std::nearbyint((hi - lo) / step) + 1, 2);
        out.resize(n);
        for (int i = 0; i < n; ++i) out[i] = lo + (hi - lo) * i / (double)(n - 1);
    };
    Grid g;
    axis(sar.x_min_m, sar.x_max_m, sar.voxel_m[0], g.x);
    axis(sar.y_min_m, sar.y_max_m, sar.voxel_m[1], g.y);
    axis(sar.z_min_m, sar.z_max_m, sar.voxel_m[2], g.z);
    return g;
}

void notebook_grid_bounds(const GeometryConfig& geom, const BPConfig& bp, const SARConfig& sar,
                          double pitch_deg, double x_margin_m,
                          double& x_min, double& x_max, double& z_min, double& z_max) {
    double x_lo = geom.x_tx[0], x_hi = geom.x_tx[0];
    for (double x : geom.x_tx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
    for (double x : geom.x_rx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
    x_min = x_lo - x_margin_m;
    x_max = x_hi + x_margin_m;

    // Радарная z линейна по Z сцены при фиксированной высоте:
    // z_rad(Y, Z=0) = -(Y - ground_depth)*sin(p), z = (depth - z_rad)/cos(p).
    double p = pitch_deg * M_PI / 180.0;
    double cos_p = std::cos(p), sin_p = std::sin(p);
    double z_lo = 0.0, z_hi = 0.0;
    bool first = true;
    for (double y : {sar.y_min_m, sar.y_max_m}) {
        for (double depth : {bp.min_depth, bp.max_depth}) {
            double z_rad0 = -(y - sar.ground_depth_m) * sin_p;
            double z = (depth - z_rad0) / cos_p;
            if (first) { z_lo = z_hi = z; first = false; }
            z_lo = std::min(z_lo, z);
            z_hi = std::max(z_hi, z);
        }
    }
    z_min = z_lo;
    z_max = z_hi;
}

}  // namespace gpr