ic-ctbu-backend/plugin/Lepao/generateGyrFromPath.js

"use strict";

/**
 * 传入 path_test.json 同类路径点数组，返回模拟真机上传的 IMU 采样数组（与小程序一致：仅最后一段；间隔约 60–70ms；各轴 float32 量化）。
 * 单参数、无文件读写。
 */
module.exports = function generateGyrFromPath(pathRows) {
  const R0 = 6371000;
  const D2R = Math.PI / 180;

  function parseRow(row) {
    if (!row || typeof row.d !== "string") return null;
    const [ts] = row.d.trim().split(/\s+/);
    const t = parseInt(ts, 10);
    if (!Number.isFinite(t)) return null;
    const lat = row.a;
    const lng = row.o;
    if (typeof lat !== "number" || typeof lng !== "number") return null;
    const speed = typeof row.s === "number" ? row.s : 0;
    return { t, lat, lng, speed };
  }

  function sortPath(points) {
    const list = points.map(parseRow).filter(Boolean);
    list.sort((a, b) => a.t - b.t);
    if (list.length < 2) return list;
    const d = [list[0]];
    for (let i = 1; i < list.length; i++) {
      if (list[i].t !== list[i - 1].t) d.push(list[i]);
    }
    return d;
  }

  function hav(lat1, lng1, lat2, lng2) {
    const φ1 = lat1 * D2R;
    const φ2 = lat2 * D2R;
    const dφ = (lat2 - lat1) * D2R;
    const dλ = (lng2 - lng1) * D2R;
    const s =
      Math.sin(dφ / 2) ** 2 +
      Math.cos(φ1) * Math.cos(φ2) * Math.sin(dλ / 2) ** 2;
    return 2 * R0 * Math.asin(Math.min(1, Math.sqrt(s)));
  }

  function bear(lat1, lng1, lat2, lng2) {
    const φ1 = lat1 * D2R;
    const φ2 = lat2 * D2R;
    const dλ = (lng2 - lng1) * D2R;
    const y = Math.sin(dλ) * Math.cos(φ2);
    const x =
      Math.cos(φ1) * Math.sin(φ2) -
      Math.sin(φ1) * Math.cos(φ2) * Math.cos(dλ);
    return Math.atan2(y, x);
  }

  function unwrap(prev, next) {
    let d = next - prev;
    while (d > Math.PI) d -= 2 * Math.PI;
    while (d < -Math.PI) d += 2 * Math.PI;
    return prev + d;
  }

  function lerpPath(path, t, key) {
    if (t <= path[0].t) return path[0][key];
    const L = path[path.length - 1];
    if (t >= L.t) return L[key];
    let i = 0;
    while (i < path.length - 1 && path[i + 1].t < t) i++;
    const p0 = path[i];
    const p1 = path[i + 1];
    const f = (t - p0.t) / (p1.t - p0.t);
    return p0[key] + f * (p1[key] - p0[key]);
  }

  function ll(path, t) {
    return { lat: lerpPath(path, t, "lat"), lng: lerpPath(path, t, "lng") };
  }

  function segmentStartMs(t0, tEnd) {
    const sec = Math.max(0, tEnd - t0) / 1000;
    return t0 + Math.floor(sec / 60) * 60 * 1000;
  }

  /** 与真机常见的 60–72ms 间隔起伏一致（含 70→60 这类相邻差） */
  function gapMs(i) {
    const g = [70, 60, 65, 70, 60, 62, 68, 60, 72, 61];
    return g[i % g.length];
  }

  function f32(x) {
    return Math.fround(x);
  }

  function rngFactory(path) {
    let h = 2166136261 >>> 0;
    for (let i = 0; i < path.length; i++) {
      const p = path[i];
      h ^= p.t >>> 0;
      h = Math.imul(h, 16777619);
      const x = ((p.lat * 1e6) ^ (p.lng * 1e6)) | 0;
      h ^= x;
      h = Math.imul(h, 16777619);
    }
    let s = h || 0x9e3779b9;
    return function () {
      s = (s + 0x6d2b79f5) >>> 0;
      let t = Math.imul(s ^ (s >>> 15), 1 | s);
      t ^= t + Math.imul(t ^ (t >>> 7), 61 | t);
      return ((t ^ (t >>> 14)) >>> 0) / 4294967296;
    };
  }

  if (!Array.isArray(pathRows) || pathRows.length < 2) return [];

  const path = sortPath(pathRows);
  if (path.length < 2) return [];

  const rand = rngFactory(path);
  const t0 = path[0].t;
  const t1 = path[path.length - 1].t;
  const tSeg = segmentStartMs(t0, t1);
  const rngHalf = 35;

  const pA0 = ll(path, tSeg);
  const pA1 = ll(path, Math.min(t1, tSeg + Math.max(rngHalf, 1)));
  let prevU = bear(pA0.lat, pA0.lng, pA1.lat, pA1.lng);
  let prevV = lerpPath(path, tSeg, "speed");

  const out = [];
  let tCur = tSeg;
  let idx = 0;
  let lastT = tCur - gapMs(0);

  /** 贴近真机样例的加计零偏量级（持机姿态下重力分摊 + 跑动微扰，非 9.8 单列） */
  const baseAx = -1.08;
  const baseAy = 0.9;
  const baseAz = -0.06;
  const amp = 0.36;

  // 一阶低通，避免数值跳变过于“脚本化”
  const lp = 0.82; // 越大越平滑
  let lpAx = baseAx;
  let lpAy = baseAy;
  let lpAz = baseAz;
  let lpGx = 0.43;
  let lpGy = 0.42;
  let lpGz = -1.78;

  function clamp(x, lo, hi) {
    return x < lo ? lo : x > hi ? hi : x;
  }

  while (tCur <= t1) {
    const dtMs = tCur - lastT;
    const dtSec = Math.max(dtMs, 1) / 1000;

    const pb = ll(path, Math.max(t0, tCur - rngHalf));
    const pf = ll(path, Math.min(t1, tCur + rngHalf));
    const vPath = lerpPath(path, tCur, "speed");
    const dist = hav(pb.lat, pb.lng, pf.lat, pf.lng);
    const vTrack =
      (2 * rngHalf) > 0 ? dist / ((2 * rngHalf) / 1000) : vPath;
    const v = 0.62 * vPath + 0.38 * Math.max(0, vTrack);

    const br = bear(pb.lat, pb.lng, pf.lat, pf.lng);
    const uW = unwrap(prevU, br);
    const yaw = (uW - prevU) / dtSec;
    const aT = (v - prevV) / dtSec;
    const aL = v * yaw;

    prevU = uW;
    prevV = v;
    lastT = tCur;

    const co = Math.cos(0.31);
    const si = Math.sin(0.31);
    const jx = aL * co - aT * si;
    const jy = aL * si + aT * co;
    const runSec = (tCur - t0) / 1000;
    const ph = 2 * Math.PI * 1.82 * runSec;

    let ax =
      baseAx +
      amp * jx +
      0.2 * Math.sin(ph) +
      0.1 * (rand() - 0.5);
    let ay =
      baseAy +
      amp * jy +
      0.17 * Math.cos(ph * 0.97) +
      0.1 * (rand() - 0.5);
    let az =
      baseAz +
      0.28 * Math.sin(2 * ph) +
      0.18 * Math.cos(ph) +
      0.08 * (rand() - 0.5);

    let gx = 0.43 + 0.12 * Math.cos(ph) + 0.07 * (rand() - 0.5);
    let gy = 0.42 + 0.26 * Math.sin(ph * 1.07) + 0.1 * (rand() - 0.5);
    let gz = -1.78 + 0.8 * yaw + 0.14 * (rand() - 0.5);

    // 去除极端尖峰注入，改为限幅 + 平滑，更贴近正常跑步统计特征
    gx = clamp(gx, -2.2, 2.2);
    gy = clamp(gy, -2.2, 2.2);
    gz = clamp(gz, -3.2, 1.2);

    lpAx = lp * lpAx + (1 - lp) * ax;
    lpAy = lp * lpAy + (1 - lp) * ay;
    lpAz = lp * lpAz + (1 - lp) * az;
    lpGx = lp * lpGx + (1 - lp) * gx;
    lpGy = lp * lpGy + (1 - lp) * gy;
    lpGz = lp * lpGz + (1 - lp) * gz;

    out.push({
      t: tCur,
      ax: f32(lpAx),
      ay: f32(lpAy),
      az: f32(lpAz),
      gx: f32(lpGx),
      gy: f32(lpGy),
      gz: f32(lpGz),
    });

    tCur += gapMs(idx++);
  }

  return out;
};