ドローンは4つの回転の差だけで飛び、制御がなければ倒れる

const G = 9.81;                  // 重力加速度 m/s²const RHO = 1.2;                 // 空気の密度 kg/m³const DEG = Math.PI / 180;const H_STEP = 1 / 1000;         // 運動を進める刻み 秒// 機体: 離陸重量 895 g・水平の最高速度 21 m/s・最大傾斜角 35° の市販の空撮機を手本にした大きさconst M0 = 0.9;                  // 基準の質量 kgconst L = 0.19;                  // 重心からプロペラの中心まで mconst A = L / Math.SQRT2;        // 前後と左右に測った腕の長さ mconst R_P = 0.12;                // プロペラの半径 mconst H_ROTOR = 0.03;            // 重心から見たプロペラの面の高さ mconst FOOT = 0.06;               // 重心から脚の先まで mconst T_MAX = 5;                 // プロペラ 1 つの最大の推力 Nconst K_T = 1.1e-5;              // 推力の係数 N/(rad/s)²。推力 = K_T Ω²(ホバリングで約 4,270 rpm)const KAPPA = 0.016;             // 反トルク ÷ 推力 mconst TAU_M = 0.03;              // モーターの遅れ(回転数が目標へ近づく時定数)秒const J_R = 2e-5;                // プロペラ 1 つの慣性モーメント kg·m²const I_T0 = 0.0065;             // 基準の質量のときの慣性モーメント(前後・左右に傾く軸)kg·m²const I_S0 = 0.0115;             // 基準の質量のときの慣性モーメント(向きを変える軸)kg·m²const CDA_SIDE = 0.023;          // 抵抗の係数 × 面積(前後と左右)m²。傾き 35° で 21 m/s になる値const CDA_TOP = 0.046;           // 抵抗の係数 × 面積(上下)m²const CP_Y = 0.015;              // 空気の力がかかる点の、重心からの高さ mconst GAMMA = 0.005;             // 回転を止めようとする空気の力の係数 N·m·s/radconst TILT_MAX = 35 * DEG;       // 傾きの上限 rad/** 4 つのプロペラ: 前(x)・右(z)の位置 m と、回る向き(+1 = 上から見て左回り) */const ROTORS = [  { name: "右前", x: A, z: A, s: 1 },  { name: "左前", x: A, z: -A, s: -1 },  { name: "左後", x: -A, z: -A, s: 1 },  { name: "右後", x: -A, z: A, s: -1 },];const EPS = [0.015, -0.005, -0.01, 0.008]; // プロペラごとの推力の差(割合)/** 地面に触れるか調べる点(機体に固定した座標): 4 本の脚の先と、プロペラの上と、機体の上面 */const TOUCH: [number, number, number][] = [];for (const r of ROTORS) TOUCH.push([r.x, -FOOT, r.z], [r.x, H_ROTOR + 0.01, r.z]);TOUCH.push([0, 0.05, 0]);

type Mode = "pos" | "angle" | "rate" | "off";interface Drone {  m: number; It: number; Is: number; t: number;  p: THREE.Vector3; v: THREE.Vector3; q: THREE.Quaternion; w: THREE.Vector3; // 位置・速度・向き・角速度(機体の座標。x 前・y 上・z 右)  fwd: THREE.Vector3; up: THREE.Vector3; right: THREE.Vector3;               // 機体の前・上・右の向き(世界の座標)  om: Float64Array; cmd: Float64Array; next: Float64Array; thr: Float64Array; ge: Float64Array; // 回転数 rad/s・推力の指令 N・次の指令・実際の推力 N・地面効果の倍率  stopped: boolean[]; dead: boolean; touching: boolean; hitV: number; tau: THREE.Vector3;  wind: THREE.Vector3; windMean: number; windYaw: number; gust: number; gustT: number; gustLen: number;  mode: Mode; hz: number; kv: number; kth: number; kr: number; acc: number; bias: number;  stick: { rx: number; ry: number; lx: number; ly: number };  target: THREE.Vector3; yawT: number; faceTarget: boolean; manual: boolean;  wf: THREE.Vector3; vf: THREE.Vector3; iv: THREE.Vector3; ir: THREE.Vector3;  refV: THREE.Vector3; refUp: THREE.Vector3; refW: THREE.Vector3; hasRef: [boolean, boolean, boolean];  spinning: boolean;}const V3 = () => new THREE.Vector3();function newDrone(m = M0): Drone {  const d: Drone = {    m, It: 0, Is: 0, t: 0, p: V3(), v: V3(), q: new THREE.Quaternion(), w: V3(), fwd: new THREE.Vector3(1, 0, 0), up: new THREE.Vector3(0, 1, 0), right: new THREE.Vector3(0, 0, 1),    om: new Float64Array(4), cmd: new Float64Array(4), next: new Float64Array(4), thr: new Float64Array(4), ge: new Float64Array(4).fill(1),    stopped: [false, false, false, false], dead: false, touching: false, hitV: 0, tau: V3(),    wind: V3(), windMean: 0, windYaw: 0, gust: 0, gustT: -99, gustLen: 2,    mode: "pos", hz: 500, kv: 2.5, kth: 7, kr: 25, acc: 0, bias: 0,    stick: { rx: 0, ry: 0, lx: 0, ly: 0 }, target: V3(), yawT: 0, faceTarget: false, manual: false,    wf: V3(), vf: V3(), iv: V3(), ir: V3(), refV: V3(), refUp: new THREE.Vector3(0, 1, 0), refW: V3(), hasRef: [true, true, true], spinning: false,  };  setMass(d, m);  return d;}/** 重さを変える: 形はそのままなので、慣性モーメントは重さに比例するとした */function setMass(d: Drone, m: number) { d.m = m; d.It = (I_T0 * m) / M0; d.Is = (I_S0 * m) / M0; }/** 空中に水平に置き、回転数と指令を、釣り合う値から始める */function placeDrone(d: Drone, x: number, y: number, z: number, yaw: number) {  d.p.set(x, y, z); d.v.set(0, 0, 0); d.w.set(0, 0, 0); d.wf.set(0, 0, 0); d.vf.set(0, 0, 0); d.iv.set(0, 0, 0); d.ir.set(0, 0, 0);  d.q.setFromAxisAngle(new THREE.Vector3(0, 1, 0), yaw);  d.target.set(x, y, z); d.yawT = yaw; d.dead = false; d.touching = false; d.stopped.fill(false); d.spinning = false; d.manual = false;  const T = (d.m * G) / 4;  for (let i = 0; i < 4; i++) { d.om[i] = Math.sqrt(Math.min(T, T_MAX) / K_T); d.cmd[i] = d.next[i] = T; d.thr[i] = T; d.ge[i] = 1; }  frameAxes(d); d.refUp.set(0, 1, 0); d.refV.set(0, 0, 0); d.refW.set(0, 0, 0);}function frameAxes(d: Drone) {  d.fwd.set(1, 0, 0).applyQuaternion(d.q); d.up.set(0, 1, 0).applyQuaternion(d.q); d.right.set(0, 0, 1).applyQuaternion(d.q);}/** 風 = 一定の風 + ゆっくり揺れる成分 + 突風(1 − cos の形で強まって弱まる) */function updateWind(d: Drone) {  const t = d.t, u = (t - d.gustT) / d.gustLen;  const g = u >= 0 && u <= 1 ? 0.5 * d.gust * (1 - Math.cos(2 * Math.PI * u)) : 0;  const m = d.windMean, sway = m * (0.1 * Math.sin(1.1 * t) + 0.07 * Math.sin(2.7 * t + 1) + 0.04 * Math.sin(6.3 * t + 2));  const sp = m + sway + g, cy = Math.cos(d.windYaw), sy = Math.sin(d.windYaw);  d.wind.set(sp * cy, 0.08 * m * Math.sin(1.7 * t), -sp * sy);}const _air = V3(), _f = V3(), _tb = V3(), _c = V3(), _vc = V3(), _ft = V3(), _wc = V3(), _qt = new THREE.Quaternion();/** 1 刻み: 制御 → プロペラの回転数と推力 → 空気の力 → 地面 → 速度と位置、角速度と向きを進める */function stepDrone(d: Drone, h: number) {  frameAxes(d);  updateWind(d);  // 制御は決まった回数でだけ動き、次に動くまで同じ指令を出し続ける  d.acc += h;  const tick = 1 / d.hz;  if (d.acc >= tick - 1e-9) { d.acc -= tick; control(d, tick); }  const { fwd, up, right, w, p, v } = d;  _air.subVectors(v, d.wind);  const ax = _air.dot(fwd), ay = _air.dot(up), az = _air.dot(right), sp = _air.length();  let sum = 0, tx = 0, ty = 0, tz = 0, hr = 0;  for (let i = 0; i < 4; i++) {    const r = ROTORS[i];    // モーターの遅れ: 回転数は指令へ時定数 TAU_M で近づく    const goal = d.stopped[i] || d.dead ? 0 : Math.sqrt(Math.max(d.cmd[i], 0) / K_T);    const dOm = (goal - d.om[i]) / TAU_M;    d.om[i] = Math.max(0, d.om[i] + dOm * h);    const T0 = K_T * d.om[i] * d.om[i];    // プロペラが軸の向きに動くと、入ってくる空気の角度が変わって推力が増減する(上昇で減り、下降で増える)    const vax = ay + (w.z * r.x - w.x * r.z), inflow = 1 - 0.1 * Math.tanh(vax / 1.5);    // 地面効果: Cheeseman と Bennett の式。横に動くと吹き下ろしが流れ、効きが弱まる    const z = Math.max(0.6 * R_P, p.y + fwd.y * r.x + up.y * H_ROTOR + right.y * r.z);    const vi2 = T0 / (2 * RHO * Math.PI * R_P * R_P), vh2 = ax * ax + az * az;    const ge = 1 / (1 - ((R_P / (4 * z)) ** 2) / (1 + vh2 / Math.max(vi2, 0.25)));    // EPS: プロペラごとの作りの差で、同じ回転数でも推力が少し違う    const T = T0 * inflow * ge * (1 + EPS[i]);    d.thr[i] = T; d.ge[i] = ge; sum += T;    tx += -r.z * T; tz += r.x * T;    // 反トルク: プロペラが空気を押す反動と、回転数を変えるときの反動。プロペラと逆の向きに機体を回す    ty += -r.s * (KAPPA * T + J_R * dOm);    hr += r.s * J_R * d.om[i];  }  // 機体の空気抵抗(速さの 2 乗に比例。機体の座標で向きごとに違う)  const fx = -0.5 * RHO * CDA_SIDE * ax * sp, fy = -0.5 * RHO * CDA_TOP * ay * sp, fz = -0.5 * RHO * CDA_SIDE * az * sp;  // 合わせた力(世界の座標)  _f.set(0, -d.m * G, 0).addScaledVector(up, sum + fy).addScaledVector(fwd, fx).addScaledVector(right, fz);  // トルク(機体の座標): 推力の差 + 反トルク + 空気の力の点のずれ + 回転を止める空気の力  _tb.set(tx + CP_Y * fz - GAMMA * w.x, ty - GAMMA * w.y, tz - CP_Y * fx - GAMMA * w.z);  ground(d);  d.tau.copy(_tb);  // 速度と位置を進める(半陰的オイラー法: 先に速度を、新しい速度で位置を)  v.addScaledVector(_f, h / d.m);  p.addScaledVector(v, h);  // 角速度を進める: オイラーの運動方程式 I dω/dt = τ − ω × (Iω + プロペラの角運動量)  const Ix = d.It, Iy = d.Is, Iz = d.It;  const Lx = Ix * w.x, Ly = Iy * w.y + hr, Lz = Iz * w.z;  w.x += (h * (_tb.x - (w.y * Lz - w.z * Ly))) / Ix;  w.y += (h * (_tb.y - (w.z * Lx - w.x * Lz))) / Iy;  w.z += (h * (_tb.z - (w.x * Ly - w.y * Lx))) / Iz;  if (w.length() > 60) w.setLength(60);  // 向き(四元数)を進める: dq/dt = ½ q (ω, 0)。ω は機体の座標  const q = d.q, k = 0.5 * h, qx = q.x, qy = q.y, qz = q.z, qw = q.w;  q.set(qx + k * (qw * w.x + qy * w.z - qz * w.y), qy + k * (qw * w.y + qz * w.x - qx * w.z), qz + k * (qw * w.z + qx * w.y - qy * w.x), qw - k * (qx * w.x + qy * w.y + qz * w.z)).normalize();  d.t += h;}/** 地面: 脚の先などが地面に入ったら、ばねと減衰で押し返し、こする向きと逆に摩擦をかける。強くぶつかるか、大きく傾いて触れたら墜落 */function ground(d: Drone) {  const { p, v, w, fwd, up, right } = d;  d.touching = false;  if (p.y > 0.5) return;  for (const c of TOUCH) {    _c.copy(fwd).multiplyScalar(c[0]).addScaledVector(up, c[1]).addScaledVector(right, c[2]);    const depth = -(p.y + _c.y);    if (depth <= 0) continue;    if (!d.touching && v.y < -2.2) d.hitV = Math.max(d.hitV, -v.y);    d.touching = true;    // 接点の速さ = 重心の速さ + 回転の分(機体の座標で ω × r を求めて世界へ戻す)    const wcx = w.y * c[2] - w.z * c[1], wcy = w.z * c[0] - w.x * c[2], wcz = w.x * c[1] - w.y * c[0];    _vc.copy(v).addScaledVector(fwd, wcx).addScaledVector(up, wcy).addScaledVector(right, wcz);    const Fn = Math.max(0, 2500 * depth - 40 * _vc.y);    const vt = Math.hypot(_vc.x, _vc.z), mu = (0.6 * Fn) / Math.max(vt, 0.1);    _ft.set(-_vc.x * mu, Fn, -_vc.z * mu);    _f.add(_ft);    const bx = _ft.dot(fwd), by = _ft.dot(up), bz = _ft.dot(right);    _tb.x += c[1] * bz - c[2] * by; _tb.y += c[2] * bx - c[0] * bz; _tb.z += c[0] * by - c[1] * bx;  }  if (d.touching && !d.dead && (d.hitV > 2.2 || up.y < 0.5)) d.dead = true;  if (!d.touching) d.hitV = 0;}

const K_P = 1;                   // 位置のゲイン 1/sconst K_P_Z = 1.2;               // 高さのゲイン 1/sconst K_V_Z = 4;                 // 上下の速度のゲイン 1/sconst A_BRAKE = 4.5;             // 目標の手前で減速する加速度 m/s²const V_MAX = 25;                // 指令する速さの上限 m/s(21 m/s で頭打ちになるのは、傾きの上限と空気抵抗のため)const V_STICK = 10;              // スティックをいっぱいに倒したときの速さ m/sconst W_TILT = 6;                // 傾きを変える角速度の上限 rad/sconst W_YAW = 2.5;               // 向きを変える角速度の上限 rad/sconst TAU_GYRO = 0.004;          // 角速度の計測をならす時定数 秒const _qd = new THREE.Quaternion(), _qe = new THREE.Quaternion(), _m4 = new THREE.Matrix4(), _x = V3(), _y = V3(), _z = V3();const hasStick = (d: Drone) => Math.abs(d.stick.rx) + Math.abs(d.stick.ry) > 0.02;/** 位置 → 速度 → 欲しい加速度。傾きの上限の中で、機体の上の向きの目標(refUp)と推力の合計を返す */function positionLoop(d: Drone, dt: number) {  const { p, v, target, refV, up, iv, stick } = d;  const cy = Math.cos(d.yawT), sy = Math.sin(d.yawT);  if (hasStick(d)) {    const a = stick.ry * V_STICK, b = stick.rx * V_STICK;    refV.x = cy * a + sy * b; refV.z = -sy * a + cy * b;    target.x = p.x + v.x * 0.6; target.z = p.z + v.z * 0.6;  } else {    // 遠くでは一定の加速度で止まれる速さ(距離の平方根)、近くでは距離に比例する速さを目標にする    const ex = target.x - p.x, ez = target.z - p.z, dist = Math.hypot(ex, ez);    const sp = Math.min(K_P * dist, Math.sqrt(2 * A_BRAKE * dist), V_MAX), k = dist > 1e-6 ? sp / dist : 0;    refV.x = ex * k; refV.z = ez * k;    if (d.faceTarget && dist > 3) d.yawT = Math.atan2(-ez, ex);  }  if (Math.abs(stick.ly) > 0.02) { refV.y = stick.ly * 3; target.y = Math.max(0.1, p.y + v.y * 0.3); }  else refV.y = clamp(K_P_Z * (target.y - p.y), -3, 4);  const ex = refV.x - v.x, ey = refV.y - v.y, ez = refV.z - v.z;  let ax = d.kv * ex + iv.x, az = d.kv * ez + iv.z;  const ay = clamp(K_V_Z * ey + iv.y, -6, 8), gy = G + ay;  // 傾きの上限: 横の加速度は (g + 上の加速度) × tan(35°) まで。頭打ちの間は積分を止める(ワインドアップを防ぐ)  const ah = Math.hypot(ax, az), lim = gy * Math.tan(TILT_MAX), sat = ah > lim;  if (sat) { ax *= lim / ah; az *= lim / ah; }  else { iv.x = clamp(iv.x + 0.5 * d.kv * ex * dt, -7, 7); iv.z = clamp(iv.z + 0.5 * d.kv * ez * dt, -7, 7); }  iv.y = clamp(iv.y + 2 * ey * dt, -5, 5);  d.refUp.set(ax, gy, az).normalize();  // 推力の合計: 欲しい力のうち、機体の今の上の向きに沿う分  return Math.max(0.1 * d.m * G, d.m * (ax * up.x + gy * up.y + az * up.z));}/** 傾き → 角速度: 目標の向きと今の向きの差(四元数)に、傾きのゲインを掛ける。向きを変える分は弱くする */function attitudeLoop(d: Drone) {  _y.copy(d.refUp);  _x.set(Math.cos(d.yawT), 0, -Math.sin(d.yawT));  _z.crossVectors(_x, _y).normalize();  _x.crossVectors(_y, _z);  _qd.setFromRotationMatrix(_m4.makeBasis(_x, _y, _z));  _qe.copy(d.q).invert().multiply(_qd);  const s = _qe.w < 0 ? -2 : 2;  d.refW.set(clamp(d.kth * s * _qe.x, -W_TILT, W_TILT), clamp(0.4 * d.kth * s * _qe.y, -W_YAW, W_YAW), clamp(d.kth * s * _qe.z, -W_TILT, W_TILT));}/** 角速度 → トルク → プロペラごとの推力。PI 制御に、回転の項(ω × Iω)を足す */function rateLoop(d: Drone, dt: number, Tc: number) {  const { wf, refW, ir } = d;  const ex = refW.x - wf.x, ey = refW.y - wf.y, ez = refW.z - wf.z;  ir.x = clamp(ir.x + d.kr * ex * dt, -30, 30); ir.y = clamp(ir.y + 0.1 * d.kr * ey * dt, -5, 5); ir.z = clamp(ir.z + d.kr * ez * dt, -30, 30);  let hr = 0;  for (let i = 0; i < 4; i++) hr += ROTORS[i].s * J_R * d.om[i];  const Lx = d.It * wf.x, Ly = d.Is * wf.y + hr, Lz = d.It * wf.z;  mix(d, Tc,    d.It * (d.kr * ex + ir.x) + (wf.y * Lz - wf.z * Ly),    d.Is * (0.25 * d.kr * ey + ir.y) + (wf.z * Lx - wf.x * Lz),    d.It * (d.kr * ez + ir.z) + (wf.x * Ly - wf.y * Lx));}/** プロペラの配分: 欲しい推力の合計とトルクを、4 つの推力に分ける。限界に当たるときは、傾きを先に守り、推力の合計、向きの順に譲る */function mix(d: Drone, Tc: number, tx: number, ty: number, tz: number) {  const rp = _rp;  let hi = -1e9, lo = 1e9;  for (let i = 0; i < 4; i++) {    const r = ROTORS[i];    rp[i] = (-r.z * tx + r.x * tz) / (4 * A * A);    hi = Math.max(hi, rp[i]); lo = Math.min(lo, rp[i]);  }  const span = hi - lo;  if (span > T_MAX) { const k = T_MAX / span; for (let i = 0; i < 4; i++) rp[i] *= k; hi *= k; lo *= k; }  const c = clamp(Tc / 4, -lo, Math.max(-lo, T_MAX - hi));  let yLo = -1e9, yHi = 1e9;  for (let i = 0; i < 4; i++) {    const b = c + rp[i];    if (ROTORS[i].s > 0) { yHi = Math.min(yHi, b); yLo = Math.max(yLo, b - T_MAX); }    else { yHi = Math.min(yHi, T_MAX - b); yLo = Math.max(yLo, -b); }  }  const y = clamp(ty / (4 * KAPPA), Math.min(yLo, yHi), Math.max(yLo, yHi));  for (let i = 0; i < 4; i++) d.next[i] = c + rp[i] - ROTORS[i].s * y;}const _rp = new Float64Array(4);/** 制御の 1 回: 前の回に計算した指令をプロペラへ出し(計算にかかる時間の分だけ遅れる)、新しい測定から次の指令を求める */function control(d: Drone, dt: number) {  d.cmd.set(d.next);  d.wf.lerp(d.w, 1 - Math.exp(-dt / TAU_GYRO));  d.vf.lerp(d.v, 1 - Math.exp(-dt / 0.25));  const st = d.stick;  d.spinning = false;  if (d.dead) { d.next.fill(0); return; }  const nStop = d.stopped.filter(Boolean).length, hover = d.m * G;  if (d.mode === "pos" && nStop === 1) { spinControl(d, dt); return; }  if (d.mode === "off") {    // 制御なし: スティックの量がそのまま推力の差になる    d.hasRef[0] = d.hasRef[1] = d.hasRef[2] = false;    const base = (hover * (1 + 0.4 * st.ly + d.bias)) / 4, dd = base * 0.03;    for (let i = 0; i < 4; i++) {      const r = ROTORS[i];      d.next[i] = clamp(base - Math.sign(r.x) * st.ry * dd - Math.sign(r.z) * st.rx * dd - r.s * st.lx * base * 0.1, 0, T_MAX);    }    d.ir.set(0, 0, 0); d.iv.set(0, 0, 0);    return;  }  d.yawT = wrapAngle(d.yawT + st.lx * 1.5 * dt);  let Tc = hover;  if (d.mode === "pos") {    d.hasRef[0] = d.hasRef[1] = d.hasRef[2] = true;    Tc = positionLoop(d, dt);    attitudeLoop(d);  } else if (d.mode === "angle") {    d.hasRef[0] = false; d.hasRef[1] = d.hasRef[2] = true;    // 傾きまで: スティックが傾きの目標になる。推力の合計は、傾いても重さを支える分にする    const cy = Math.cos(d.yawT), sy = Math.sin(d.yawT), a = Math.tan(st.ry * 25 * DEG), b = Math.tan(st.rx * 25 * DEG);    d.refUp.set(cy * a + sy * b, 1, -sy * a + cy * b).normalize();    Tc = (hover * (1 + 0.4 * st.ly + d.bias)) / Math.max(0.5, d.up.y);    attitudeLoop(d);    d.iv.set(0, 0, 0);  } else {    d.hasRef[0] = d.hasRef[1] = false; d.hasRef[2] = true;    // 角速度まで: スティックが角速度の目標になる。傾きを水平に戻すのは操縦する人    d.refW.set(st.rx * 2.5, st.lx * 1.5, -st.ry * 2.5);    Tc = hover * (1 + 0.4 * st.ly + d.bias);    d.iv.set(0, 0, 0);  }  rateLoop(d, dt, Tc);}

// プロペラが 1 つ止まると、向きの釣り合いは取れない。そこで向きを保つのをあきらめ、機体を回しながら、// 機体に固定した 1 本の軸 n を真上に保つ。残る 3 つの推力は、この軸のまわりに回り続けても n の向きがぶれない値に決めるconst N_Y = 0.28;                // 軸 n を、機体の真上から、止まったプロペラの対角へ傾ける量(sin)const W_N = 7, ZETA_N = 0.9;     // 軸 n の向きを戻す速さ rad/s と減衰比const K_ARM = 0.0156;            // 空気が回転を止める力の見込み N·m·s/radfunction spinControl(d: Drone, dt: number) {  d.spinning = true;  d.hasRef[0] = true; d.hasRef[1] = true; d.hasRef[2] = false;  const i = d.stopped.indexOf(true), o = (i + 2) % 4;  // 計算に使う座標: e3 = 機体の上、e2 = 対角のプロペラ o への向き、e1 = e2 × e3(a はその先のプロペラ、b はその反対)  const ox = ROTORS[o].x / L, oz = ROTORS[o].z / L, e1x = -oz, e1z = ox;  let a = 0, b = 0;  for (let j = 0; j < 4; j++) if (j !== i && j !== o) { if (ROTORS[j].x * e1x + ROTORS[j].z * e1z > 0) a = j; else b = j; }  const wf = d.wf, P = wf.x * e1x + wf.z * e1z, Q = wf.x * ox + wf.z * oz, R = wf.y;  // 位置と高さ: 機体が回って重心が小さな円を描く分をならした速度(vf)で、弱いゲインで戻す  const { p, target, vf, refV, iv } = d;  const sx = clamp(0.8 * (target.x - p.x), -1.5, 1.5), sz = clamp(0.8 * (target.z - p.z), -1.5, 1.5), sy = clamp(1.2 * (target.y - p.y), -1.5, 2);  refV.set(sx, sy, sz);  let ax = 1.2 * (sx - vf.x), az = 1.2 * (sz - vf.z);  const ah = Math.hypot(ax, az); if (ah > 2.5) { ax *= 2.5 / ah; az *= 2.5 / ah; }  iv.y = clamp(iv.y + 1.5 * (sy - vf.y) * dt, -5, 5);  const ay = clamp(3 * (sy - vf.y) + iv.y, -5, 6);  d.refUp.set(ax, G + ay, az).normalize();  // 欲しい軸の向きを、計算の座標で表す(nx = 0・ny = N_Y がつり合いの形)  const u = d.refUp, bx = u.dot(d.fwd), bz = u.dot(d.right), nz = Math.max(0.2, u.dot(d.up));  const nx = bx * e1x + bz * e1z, ny = bx * ox + bz * oz;  // n の変わり方(機体から見た dn/dt = −ω × n)  const dnx = R * ny - Q * nz, dny = P * nz - R * nx, dnz = Q * nx - P * ny;  const nyT = N_Y * Math.min(1, (R / 16) ** 2);  // 目標の 2 階微分(減衰つきのばね)と釣り合うように、角加速度を決める  const ddx = -2 * ZETA_N * W_N * dnx - W_N * W_N * nx, ddy = -2 * ZETA_N * W_N * dny - W_N * W_N * (ny - nyT);  let hr = 0, tr = -GAMMA * R;  for (let j = 0; j < 4; j++) { hr += ROTORS[j].s * J_R * d.om[j]; tr += -ROTORS[j].s * KAPPA * d.thr[j]; }  const dR = tr / d.Is;  const dQ = (dR * ny + R * dny - Q * dnz - ddx) / nz, dP = (ddy - P * dnz + dR * nx + R * dnx) / nz;  // 欲しいトルク = 慣性モーメント × 角加速度 + 回転の項 + 空気が回転を止める力(GAMMA と、プロペラの腕が上下に動いて推力が変わる分 K_ARM)  const t1 = d.It * dP + (d.Is - d.It) * Q * R + Q * hr + K_ARM * P, t2 = d.It * dQ - (d.Is - d.It) * P * R - P * hr + K_ARM * Q;  // 配分: o が e1 まわりのトルク、a と b の差が e2 まわりのトルクを受け持つ  const F = (d.m * (G + ay)) / Math.max(0.3, d.up.y);  const fo = clamp(t1 / L, 0, T_MAX), sum = Math.max(0, F - fo), diff = t2 / L;  d.next[i] = 0; d.next[o] = fo;  d.next[a] = clamp((sum - diff) / 2, 0, T_MAX); d.next[b] = clamp((sum + diff) / 2, 0, T_MAX);}

type Side = "l" | "r";interface StickState { id: number | null; x: number; y: number }const stickGeom = (api: Api) => {  const r = api.u(40), m = api.u(14), y = api.h - m - r;  return { r, l: { x: m + r, y }, r_: { x: api.w - m - r, y } };};const btnRect = (api: Api, i: number) => {  const w = api.u(58), h = api.u(30), g = api.u(8), y = api.h - api.u(14) - h, x0 = api.w / 2 - w - g / 2;  return [x0 + i * (w + g), y, w, h] as const;};const panelTop = (api: Api) => (api.full ? Math.max(api.u(10), 58) : api.u(10));function rotorPanel(api: Api) {  const u = api.u, W = Math.min(api.w * 0.36, u(150)), H = W + u(44), x0 = api.w - W - u(10), y0 = panelTop(api);  const cx = x0 + W / 2, cy = y0 + u(18) + W / 2, k = (W * 0.25) / A, rd = k * R_P;  return { x0, y0, W, H, cx, cy, k, rd };}function chartPanel(api: Api, s: S) {  const u = api.u, narrow = api.w < 560, rows = AXES[s.axis].rows.slice(narrow && AXES[s.axis].rows.length > 4 ? 1 : 0);  const W = clamp(api.w * 0.42, u(190), u(250)), rh = u(narrow ? 33 : 32), H = u(36) + rows.length * rh;  return { x0: u(10), y0: panelTop(api), W, H, rh, rows };}/** スマホで操作パネルを開いて描画面が低くなったときは、機体が隠れないようにグラフを出さない */const chartsFit = (api: Api) => api.w >= 560 || api.h >= 360;function setStick(api: Api, s: S, side: Side, x: number, y: number) {  const g = stickGeom(api), c = side === "l" ? g.l : g.r_, k = g.r * 0.62;  let dx = (x - c.x) / k, dy = (c.y - y) / k;  const n = Math.hypot(dx, dy); if (n > 1) { dx /= n; dy /= n; }  s.pads[side].x = dx; s.pads[side].y = dy;}/** スティック・ボタン・図のプロペラは描画面に直接付け、視点を回す操作より先に受ける(2 本の指で同時に使える) */function bindInput(api: Api, s: S) {  const el = api.ctx.canvas;  const at = (e: PointerEvent) => { const b = el.getBoundingClientRect(); return [e.clientX - b.left, e.clientY - b.top]; };  const take = (e: PointerEvent) => { e.stopImmediatePropagation(); e.preventDefault(); };  const inRect = (x: number, y: number, r: readonly [number, number, number, number]) => x >= r[0] && x <= r[0] + r[2] && y >= r[1] && y <= r[1] + r[3];  const down = (e: PointerEvent) => {    const [x, y] = at(e), g = stickGeom(api);    for (const side of ["l", "r"] as Side[]) {      const c = side === "l" ? g.l : g.r_, P = s.pads[side];      if (Math.hypot(x - c.x, y - c.y) < g.r * 1.2 && P.id === null) {        take(e); try { el.setPointerCapture(e.pointerId); } catch { /* 取れなくても押した状態は保つ */ }        P.id = e.pointerId; setStick(api, s, side, x, y);        return;      }    }    if (inRect(x, y, btnRect(api, 0))) { take(e); startGust(api, s); return; }    if (inRect(x, y, btnRect(api, 1))) { take(e); s.follow = !s.follow; return; }    const rp = rotorPanel(api);    if (x >= rp.x0 && x <= rp.x0 + rp.W && y >= rp.y0 && y <= rp.y0 + rp.H) {      take(e);      for (let i = 0; i < 4; i++) {        const r = ROTORS[i], cx = rp.cx + r.z * rp.k, cy = rp.cy - r.x * rp.k;        if (Math.hypot(x - cx, y - cy) < rp.rd * 1.05) { toggleRotor(api, s, i); return; }      }      return;    }    const cp = chartPanel(api, s);    if (chartsFit(api) && x >= cp.x0 && x <= cp.x0 + cp.W && y >= cp.y0 && y <= cp.y0 + cp.H) { take(e); s.axis = (s.axis + 1) % AXES.length; }  };  const move = (e: PointerEvent) => {    for (const side of ["l", "r"] as Side[]) if (s.pads[side].id === e.pointerId) { take(e); const [x, y] = at(e); setStick(api, s, side, x, y); return; }  };  const up = (e: PointerEvent) => {    for (const side of ["l", "r"] as Side[]) if (s.pads[side].id === e.pointerId) { take(e); s.pads[side].id = null; s.pads[side].x = 0; s.pads[side].y = 0; return; }  };  el.addEventListener("pointerdown", down, true); el.addEventListener("pointermove", move, true);  el.addEventListener("pointerup", up, true); el.addEventListener("pointercancel", up, true);  return () => {    el.removeEventListener("pointerdown", down, true); el.removeEventListener("pointermove", move, true);    el.removeEventListener("pointerup", up, true); el.removeEventListener("pointercancel", up, true);  };}function startGust(api: Api, s: S) {  const d = s.d;  d.gust = 8; d.gustLen = 2; d.gustT = d.t;  api.note("8 m/s の突風が、2 秒かけて強まって弱まります");}function toggleRotor(api: Api, s: S, i: number) {  const d = s.d;  if (d.dead) return;  d.stopped[i] = !d.stopped[i];  // 動かし直したら、今いる所で止まり直す  if (!d.stopped[i]) { d.iv.set(0, 0, 0); d.ir.set(0, 0, 0); d.target.copy(d.p); }  const n = d.stopped.filter(Boolean).length;  api.note(!d.stopped[i] ? `${ROTORS[i].name}のプロペラを動かしました` : n >= 2 ? "プロペラが 2 つ止まると、このシミュレーションの制御では飛び続けられません" : d.mode === "pos" ? `${ROTORS[i].name}のプロペラを止めました。向きを保つのをあきらめ、機体を回しながら高さを保ちます` : `${ROTORS[i].name}のプロペラを止めました。向きも保とうとする制御では釣り合いが取れません`, 3000);}/** キーとスティックを合わせて、制御へ渡す入力にする。キーは 0.12 秒かけて倒す */function readSticks(s: S, dt: number) {  const K = s.keys, st = s.d.stick, k = 1 - Math.exp(-dt / 0.12);  const blend = (now: number, pad: number, a: boolean, b: boolean, active: boolean) => (active ? pad : now + ((a ? 1 : 0) - (b ? 1 : 0) - now) * k);  st.ry = blend(st.ry, s.pads.r.y, K.ArrowUp, K.ArrowDown, s.pads.r.id !== null);  st.rx = blend(st.rx, s.pads.r.x, K.ArrowRight, K.ArrowLeft, s.pads.r.id !== null);  st.ly = blend(st.ly, s.pads.l.y, K.KeyW, K.KeyS, s.pads.l.id !== null);  // 左へ倒すと左へ回る(+ が上から見て左回り)  st.lx = blend(st.lx, -s.pads.l.x, K.KeyA, K.KeyD, s.pads.l.id !== null);  if (Math.abs(st.ry) + Math.abs(st.rx) + Math.abs(st.ly) + Math.abs(st.lx) > 0.05 && !s.d.manual) {    s.d.manual = true; s.route = []; s.d.faceTarget = false;  }}/** 地面の点をタップすると、そこへ飛ぶ */function tapGround(api: Api, s: S) {  const d = s.d;  if (d.dead) { revive(api, s); return; }  const hit = s.v.onPlane(api.pointer.x, api.pointer.y, s.plane);  if (!hit || Math.hypot(hit.x - d.p.x, hit.z - d.p.z) > 400) return;  if (d.mode !== "pos") { api.note("目標の点へ飛ぶのは、制御の段が「位置まで」のときです"); return; }  s.route = []; d.manual = false;  d.target.set(hit.x, Math.max(d.target.y, 1.5), hit.z); d.faceTarget = true;  api.note(`目標の点を置きました(${fmt(Math.hypot(hit.x - d.p.x, hit.z - d.p.z), 0)} m 先)`);}function revive(api: Api, s: S) {  const d = s.d, y = Math.max(s.homeY, 1), yaw = Math.atan2(-d.fwd.z, d.fwd.x);  placeDrone(d, d.p.x, y, d.p.z, Number.isFinite(yaw) ? yaw : 0);  s.ev.crash = false; s.ev.t30 = false; s.ev.cutT = d.mode === "off" ? d.t : -1; s.route = [];}/** 場面ごとの進み方(制御を切る・目標の点を順に飛ぶ・向きを変える・突風・プロペラを止める・降りる) */function scenario(api: Api, s: S) {  const d = s.d, t = d.t - s.t0, key = s.scene, ev = s.ev;  if (key === "cut" && !ev.cut && t > 2.5) { ev.cut = true; ev.cutT = d.t; api.set("mode", "off"); d.mode = "off"; api.note("制御を切りました。4 つのプロペラは同じ回転数になり、作りの差の分だけ推力がずれます", 2600); }  if (s.route.length && Math.hypot(d.target.x - d.p.x, d.target.z - d.p.z) < 2.5) {    s.routeI = (s.routeI + 1) % s.route.length;    const q = s.route[s.routeI]; d.target.set(q[0], q[1], q[2]);  }  if (key === "yaw" && !d.manual && d.mode === "pos") {    const n = Math.floor((t - 2) / 5);    if (n >= 0 && n !== ev.yawN) { ev.yawN = n; d.yawT = wrapAngle(d.yawT + (n % 2 ? -1 : 1) * Math.PI * 0.999); api.note(n % 2 ? "今度は右へ半周。左回りの組を速く、右回りの組を遅くします" : "左へ半周。右回りの組を速く、左回りの組を遅くします", 2200); }  }  if (key === "gust" && t > 3) { const n = Math.floor((t - 3) / 7); if (n !== ev.gustN) { ev.gustN = n; startGust(api, s); } }  if (key === "out" && !ev.out && t > 3) { ev.out = true; toggleRotor(api, s, 0); }  if (key === "ground") {    d.bias = d.mode === "pos" ? 0 : -0.03;    if (d.mode === "pos" && t > 2 && !d.manual) d.target.y = Math.max(0.13, s.homeY - 0.15 * (t - 2));  }}/** 出来事の知らせ(同じ知らせを毎コマ出さない) */function events(api: Api, s: S) {  const d = s.d, ev = s.ev;  if (ev.cutT >= 0 && !ev.t30 && d.up.y < Math.cos(30 * DEG)) { ev.t30 = true; api.note(`切ってから${fmt(d.t - ev.cutT, 1)}秒で、傾きが30°を超えました`, 2600); }  if (d.dead && !ev.crash) { ev.crash = true; api.note("墜落しました。描画面をタップすると飛び直します", 3600); }  if (!ev.heavy && d.m * G > 4 * T_MAX * 0.97) { ev.heavy = true; api.note("重さが、4 つのプロペラの最大の推力に届きました。傾ける余力が残りません", 3200); }  if (d.m * G < 4 * T_MAX * 0.9) ev.heavy = false;  if (!ev.alt && d.p.y >= 150) { ev.alt = true; api.note("高さ 150 m を超えました。日本では、この高さの飛行には許可が要ります", 3200); }  if (d.p.y < 140) ev.alt = false;}

interface Mats { land: THREE.Material; grid: THREE.Material; fine: THREE.Material; body: THREE.Material; dark: THREE.Material; arm: THREE.Material; prop: THREE.Material; bar: THREE.Material; post: THREE.Material; mark: THREE.Material; line: THREE.Material; streak: THREE.Material }/** 機体: 胴体・カメラ・4 本の腕・モーター・脚・プロペラと、回る向きの矢印、推力の棒 */function makeDrone(s: S) {  const m = s.mats, g = new THREE.Group(), add = (geo: THREE.BufferGeometry, mat: THREE.Material, x = 0, y = 0, z = 0, parent: THREE.Object3D = g) => {    const o = new THREE.Mesh(geo, mat); o.position.set(x, y, z); o.castShadow = true; o.receiveShadow = true; parent.add(o); return o;  };  add(new THREE.BoxGeometry(0.2, 0.055, 0.09), m.body);  add(new THREE.BoxGeometry(0.11, 0.03, 0.07), m.body, -0.015, 0.04, 0);  add(new THREE.SphereGeometry(0.024, 14, 10), m.dark, 0.115, -0.012, 0);  const armG = new THREE.BoxGeometry(L, 0.012, 0.018), motorG = new THREE.CylinderGeometry(0.018, 0.018, 0.03, 16), legG = new THREE.BoxGeometry(0.007, FOOT, 0.007);  const bladeG = new THREE.BoxGeometry(2 * R_P * 0.98, 0.003, 0.016), ringG = new THREE.TorusGeometry(R_P, 0.0028, 6, 48); ringG.rotateX(Math.PI / 2);  const barG = new THREE.CylinderGeometry(0.005, 0.005, 1, 10); barG.translate(0, 0.5, 0);  const tipG = new THREE.ConeGeometry(0.013, 0.032, 12); tipG.translate(0, 0.02, 0);  const arcR = 0.078, arcG = new THREE.TorusGeometry(arcR, 0.0035, 6, 28, 1.5 * Math.PI); arcG.rotateX(-Math.PI / 2);  const headG = new THREE.ConeGeometry(0.012, 0.03, 10), yAxis = new THREE.Vector3(0, 1, 0);  ROTORS.forEach((r, i) => {    const arm = add(armG, r.x > 0 ? m.dark : m.arm, r.x / 2, 0.008, r.z / 2); arm.rotation.y = -Math.atan2(r.z, r.x);    add(motorG, m.dark, r.x, H_ROTOR - 0.014, r.z);    add(legG, m.arm, r.x, -FOOT / 2, r.z);    add(ringG, m.prop, r.x, H_ROTOR + 0.008, r.z);    const hub = new THREE.Group(); hub.position.set(r.x, H_ROTOR + 0.006, r.z); g.add(hub);    add(bladeG, m.prop, 0, 0, 0, hub);    s.blades[i] = hub;    // 回る向きの矢印: 左回りは弧の終わり、右回りは弧の始まりに矢じりを付ける    add(arcG, m.bar, r.x, H_ROTOR + 0.012, r.z);    const a = r.s > 0 ? 1.5 * Math.PI : 0, hx = arcR * Math.cos(a), hz = -arcR * Math.sin(a), tx = -Math.sin(a) * r.s, tz = -Math.cos(a) * r.s;    const head = add(headG, m.bar, r.x + hx, H_ROTOR + 0.012, r.z + hz);    head.quaternion.setFromUnitVectors(yAxis, new THREE.Vector3(tx, 0, tz));    const bar = new THREE.Mesh(barG, m.bar); bar.position.set(r.x, H_ROTOR + 0.03, r.z); g.add(bar); s.bars[i] = bar;    const tip = new THREE.Mesh(tipG, m.bar); tip.position.copy(bar.position); g.add(tip); s.tips[i] = tip;  });  s.v.scene.add(g);  return g;}/** 地面: 面と 5 m ごとの線、近くの 1 m ごとの線。機体を追って区切りのよい所へ動かすので、どこまで飛んでも続く */function makeWorld(s: S) {  const v = s.v, m = s.mats, grid = (half: number, step: number) => {    const P: number[] = [];    for (let a = -half; a <= half + 1e-6; a += step) P.push(a, 0, -half, a, 0, half, -half, 0, a, half, 0, a);    const g = new THREE.BufferGeometry(); g.setAttribute("position", new THREE.BufferAttribute(new Float32Array(P), 3));    return g;  };  s.land = new THREE.Mesh(new THREE.PlaneGeometry(1600, 1600).rotateX(-Math.PI / 2), m.land); s.land.receiveShadow = true; v.scene.add(s.land);  s.grid5 = new THREE.LineSegments(grid(200, 5), m.grid); s.grid5.position.y = 0.03; s.grid5.frustumCulled = false; v.scene.add(s.grid5);  s.grid1 = new THREE.LineSegments(grid(10, 1), m.fine); s.grid1.position.y = 0.03; s.grid1.frustumCulled = false; v.scene.add(s.grid1);  // 目印の柱(高さ 2.5 m・15 m おき): 速さと高さの見当になる  const pg = new THREE.BoxGeometry(0.12, 2.5, 0.12); pg.translate(0, 1.25, 0);  s.posts = new THREE.InstancedMesh(pg, m.post, 121); s.posts.castShadow = true; s.posts.frustumCulled = false;  const M = new THREE.Matrix4();  for (let i = 0; i < 11; i++) for (let j = 0; j < 11; j++) s.posts.setMatrixAt(i * 11 + j, M.makeTranslation((i - 5.5) * 15, 0, (j - 5.5) * 15));  s.posts.instanceMatrix.needsUpdate = true; v.scene.add(s.posts);  // 目標の点: 目標の高さの輪と、地面までの柱と、地面の輪  s.mark = new THREE.Group();  const ring = new THREE.TorusGeometry(0.35, 0.012, 8, 40); ring.rotateX(Math.PI / 2);  s.markRing = new THREE.Mesh(ring, m.mark); s.mark.add(s.markRing);  const pole = new THREE.BoxGeometry(0.03, 1, 0.03); pole.translate(0, 0.5, 0);  s.markPole = new THREE.Mesh(pole, m.mark); s.mark.add(s.markPole);  const groundRing = new THREE.TorusGeometry(0.15, 0.005, 6, 32); groundRing.rotateX(Math.PI / 2);  const gr = new THREE.Mesh(groundRing, m.mark); gr.position.y = 0.04; s.mark.add(gr);  v.scene.add(s.mark);  // 機体から地面へ下ろす線と、風で流れる短い線  const dg = new THREE.BufferGeometry(); dg.setAttribute("position", new THREE.BufferAttribute(new Float32Array(6), 3));  s.drop = new THREE.LineSegments(dg, m.line); s.drop.frustumCulled = false; v.scene.add(s.drop);  const wg = new THREE.BufferGeometry(); s.streakP = new Float32Array(N_STREAK * 6); wg.setAttribute("position", new THREE.BufferAttribute(s.streakP, 3));  s.streaks = new THREE.LineSegments(wg, m.streak); s.streaks.frustumCulled = false; v.scene.add(s.streaks);  const rnd = s.rng;  s.streakQ = new Float32Array(N_STREAK * 3);  for (let i = 0; i < N_STREAK; i++) { s.streakQ[i * 3] = (rnd() - 0.5) * 2 * STREAK_BOX; s.streakQ[i * 3 + 1] = rnd() * 2 * STREAK_BOX; s.streakQ[i * 3 + 2] = (rnd() - 0.5) * 2 * STREAK_BOX; }}const N_STREAK = 160, STREAK_BOX = 14;const THRUST_LEN = 0.06; // 推力の棒の長さ m/N(5 N で 0.3 m)/** 計算の結果を絵の部品に写す。プロペラは見やすいように、本当の回転数の 3% の速さで回して描く */function syncView(api: Api, s: S) {  const d = s.d;  s.drone.position.copy(d.p); s.drone.quaternion.copy(d.q);  for (let i = 0; i < 4; i++) {    s.blades[i].rotation.y += ROTORS[i].s * d.om[i] * 0.03 * api.dt;    const len = Math.max(0.001, d.thr[i] * THRUST_LEN);    s.bars[i].scale.y = len; s.tips[i].position.y = H_ROTOR + 0.03 + len;    s.bars[i].visible = s.tips[i].visible = d.thr[i] > 0.02;  }  const sx = Math.round(d.p.x / 5) * 5, sz = Math.round(d.p.z / 5) * 5;  s.grid5.position.x = sx; s.grid5.position.z = sz;  s.grid1.position.x = Math.round(d.p.x); s.grid1.position.z = Math.round(d.p.z);  s.grid1.visible = api.quality > 0;  s.land.position.x = d.p.x; s.land.position.z = d.p.z;  s.posts.position.x = Math.round(d.p.x / 15) * 15; s.posts.position.z = Math.round(d.p.z / 15) * 15;  // 目標の点: 機体の近くでは輪を小さくし、遠いと柱を見せる  const tDist = Math.hypot(d.target.x - d.p.x, d.target.z - d.p.z);  s.mark.visible = d.mode === "pos" && !d.dead;  s.mark.position.set(d.target.x, 0, d.target.z);  s.markRing.position.y = d.target.y; s.markRing.scale.setScalar(clamp(tDist * 0.05, 0.5, 2)); s.markRing.visible = tDist > 1 || Math.abs(d.target.y - d.p.y) > 0.5;  s.markPole.scale.set(clamp(tDist * 0.03, 0.3, 3), Math.max(0.01, d.target.y), clamp(tDist * 0.03, 0.3, 3)); s.markPole.visible = tDist > 1.5;  const dp = s.drop.geometry.attributes.position as THREE.BufferAttribute, da = dp.array as Float32Array;  da[0] = d.p.x; da[1] = d.p.y; da[2] = d.p.z; da[3] = d.p.x; da[4] = 0.04; da[5] = d.p.z; dp.needsUpdate = true;  // 風の線: 風の速さで動かし、箱の外へ出たら反対側へ戻す  const wl = d.wind.length();  s.streaks.visible = wl > 0.3;  if (s.streaks.visible) {    const c = s.v.orbit.target, Q = s.streakQ, P = s.streakP, B = 2 * STREAK_BOX, st = 0.14;    for (let i = 0; i < N_STREAK; i++) {      const a = i * 3, b = i * 6;      Q[a] += d.wind.x * api.dt; Q[a + 1] += d.wind.y * api.dt; Q[a + 2] += d.wind.z * api.dt;      const x = c.x + ((((Q[a] - c.x + STREAK_BOX) % B) + B) % B) - STREAK_BOX, y = ((((Q[a + 1]) % B) + B) % B) + 0.2, z = c.z + ((((Q[a + 2] - c.z + STREAK_BOX) % B) + B) % B) - STREAK_BOX;      P[b] = x; P[b + 1] = y; P[b + 2] = z; P[b + 3] = x - d.wind.x * st; P[b + 4] = y - d.wind.y * st; P[b + 5] = z - d.wind.z * st;    }    (s.streaks.geometry.attributes.position as THREE.BufferAttribute).needsUpdate = true;  }}const _e = V3();/** 視点: 機体を追いかける。前へ飛ぶ場面では、読者が動かしていなければ、機体の後ろへ回る */function updateCamera(api: Api, s: S) {  const o = s.v.orbit, d = s.d;  const k = s.camSnap ? 1 : 1 - Math.exp(-api.dt * 5);  // 目標の位置を速度の分だけ先へ置くと、速く飛んでも機体が画面の真ん中に残る  if (s.follow) o.target.lerp(_e.set(d.p.x + d.v.x * 0.2, Math.max(0.1, d.p.y + d.v.y * 0.2), d.p.z + d.v.z * 0.2), k);  if (s.chase && o.idle > 4 && s.follow) {    // 真後ろからでは傾きが見えないので、斜め後ろへ回る    const want = Math.atan2(-d.fwd.x, -d.fwd.z) + 1;    o.yaw += wrapAngle(want - o.yaw) * (s.camSnap ? 1 : 1 - Math.exp(-api.dt * 1.2));  }  s.camSnap = false;}

const TN = 240;                  // グラフに残す点の数const T_SAMPLE = 1 / 40;         // グラフの点の間隔 秒(240 点で 6 秒)interface Row { name: string; unit: string; min: number; a: Float32Array; b: Float32Array }const row = (name: string, unit: string, min: number): Row => ({ name, unit, min, a: new Float32Array(TN).fill(NaN), b: new Float32Array(TN).fill(NaN) });const AXES: { name: string; rows: Row[] }[] = [];function makeAxes() {  AXES.length = 0;  AXES.push(    { name: "前後", rows: [row("目標までの距離", "m", 1), row("速度", "m/s", 1), row("傾き", "°", 2), row("角速度", "°/s", 10), row("推力の差(後ろ−前)", "N", 0.2)] },    { name: "左右", rows: [row("目標までの距離", "m", 1), row("速度", "m/s", 1), row("傾き", "°", 2), row("角速度", "°/s", 10), row("推力の差(左−右)", "N", 0.2)] },    { name: "向き", rows: [row("向き", "°", 5), row("角速度", "°/s", 10), row("推力の差(右回り−左回り)", "N", 0.2)] },    { name: "上下", rows: [row("高さ", "m", 0.2), row("上下の速さ", "m/s", 0.5), row("推力の合計", "N", 0.5), row("回転数の平均", "rpm", 50)] },  );}/** 制御の各段の目標と実際を、グラフの配列に記録する */function sample(s: S) {  const d = s.d, th = d.thr, nx = d.next, i = s.head;  let fx = d.fwd.x, fz = d.fwd.z, fl = Math.hypot(fx, fz);  if (fl < 1e-3) { fx = 1; fz = 0; fl = 1; }  fx /= fl; fz /= fl;  const rx = -fz, rz = fx, [ok0, ok1, ok2] = d.hasRef, ex = d.target.x - d.p.x, ez = d.target.z - d.p.z, N = NaN, u = d.up, ru = d.refUp, RD = 1 / DEG;  const put = (ax: number, r: number, a: number, b: number) => { const R = AXES[ax].rows[r]; R.a[i] = a; R.b[i] = b; };  put(0, 0, ex * fx + ez * fz, ok0 ? 0 : N); put(1, 0, ex * rx + ez * rz, ok0 ? 0 : N);  put(0, 1, d.v.x * fx + d.v.z * fz, ok0 ? d.refV.x * fx + d.refV.z * fz : N); put(1, 1, d.v.x * rx + d.v.z * rz, ok0 ? d.refV.x * rx + d.refV.z * rz : N);  put(0, 2, Math.atan2(u.x * fx + u.z * fz, u.y) * RD, ok1 ? Math.atan2(ru.x * fx + ru.z * fz, ru.y) * RD : N);  put(1, 2, Math.atan2(u.x * rx + u.z * rz, u.y) * RD, ok1 ? Math.atan2(ru.x * rx + ru.z * rz, ru.y) * RD : N);  put(0, 3, -d.w.z * RD, ok2 ? -d.refW.z * RD : N); put(1, 3, d.w.x * RD, ok2 ? d.refW.x * RD : N);  put(0, 4, th[2] + th[3] - th[0] - th[1], nx[2] + nx[3] - nx[0] - nx[1]); put(1, 4, th[1] + th[2] - th[0] - th[3], nx[1] + nx[2] - nx[0] - nx[3]);  const yaw = Math.atan2(-d.fwd.z, d.fwd.x);  s.yawU += wrapAngle(yaw - s.yawP); s.yawP = yaw;  put(2, 0, s.yawU * RD, ok2 && !d.spinning ? (s.yawU + wrapAngle(d.yawT - yaw)) * RD : N);  put(2, 1, d.w.y * RD, ok2 ? d.refW.y * RD : N);  put(2, 2, th[1] + th[3] - th[0] - th[2], nx[1] + nx[3] - nx[0] - nx[2]);  put(3, 0, d.p.y, ok0 ? d.target.y : N); put(3, 1, d.v.y, ok0 ? d.refV.y : N);  put(3, 2, th[0] + th[1] + th[2] + th[3], nx[0] + nx[1] + nx[2] + nx[3]);  put(3, 3, ((d.om[0] + d.om[1] + d.om[2] + d.om[3]) / 4) * 60 / (2 * Math.PI), N);  s.head = (i + 1) % TN;}const _pts = new Float32Array(TN * 2);/** 1 本の折れ線。NaN の所で切る。配列はつなぎ目から古い順にたどる */function drawSeries(api: Api, s: S, v: Float32Array, x0: number, y0: number, W: number, H: number, lo: number, hi: number, dash: number[] | undefined, w: number) {  let n = 0;  const flush = () => { if (n >= 4) api.poly(_pts.subarray(0, n), { w, dash }); n = 0; };  for (let k = 0; k < TN; k++) {    const y = v[(s.head + k) % TN];    if (y !== y) { flush(); continue; }    _pts[n++] = x0 + (k / (TN - 1)) * W; _pts[n++] = y0 + H - ((clamp(y, lo, hi) - lo) / (hi - lo)) * H;  }  flush();}function drawCharts(api: Api, s: S) {  const u = api.u, { x0, y0, W, H, rh, rows } = chartPanel(api, s), fs = Math.max(u(10.5), 9.5), pad = u(7);  api.rect(x0, y0, W, H, { fill: "paper", stroke: "ink", w: u(1) });  api.text(`${AXES[s.axis].name}の制御(タップで切り替え)`, x0 + pad, y0 + u(15), { size: fs });  api.text("点線が目標、実線が実際", x0 + pad, y0 + H - u(5), { size: fs * 0.86 });  rows.forEach((R, ir) => {    const top = y0 + u(22) + ir * rh, py = top + fs + u(2), ph = rh - fs - u(5);    let lo = Infinity, hi = -Infinity;    for (let k = 0; k < TN; k++) { const a = R.a[k], b = R.b[k]; if (a === a) { if (a < lo) lo = a; if (a > hi) hi = a; } if (b === b) { if (b < lo) lo = b; if (b > hi) hi = b; } }    if (!(hi >= lo)) { lo = 0; hi = 1; }    if (hi - lo < R.min) { const c = (hi + lo) / 2; lo = c - R.min / 2; hi = c + R.min / 2; }    const last = (s.head + TN - 1) % TN, cur = R.a[last], d = R.unit === "rpm" ? 0 : Math.abs(cur) < 100 && R.min < 5 ? (R.min < 0.3 ? 2 : 1) : 0;    api.text(`${R.name} ${R.unit}`, x0 + pad, top + fs * 0.95, { size: fs * 0.92 });    api.text(fmt(cur, d), x0 + W - pad, top + fs * 0.95, { size: fs, font: "mono", align: "right" });    if (lo < 0 && hi > 0) { const zy = py + ph - ((0 - lo) / (hi - lo)) * ph; api.line(x0 + pad, zy, x0 + W - pad, zy, { w: u(0.5) }); }    drawSeries(api, s, R.b, x0 + pad, py, W - 2 * pad, ph, lo, hi, [u(2.5), u(2.5)], u(1));    drawSeries(api, s, R.a, x0 + pad, py, W - 2 * pad, ph, lo, hi, undefined, u(1.5));  });}/** 上から見た 4 つのプロペラ: 円の周りの矢印が回る向き、棒が推力(印はホバリングの推力)、真ん中の矢印が反トルクの合計 */function drawRotors(api: Api, s: S) {  const u = api.u, d = s.d, P = rotorPanel(api), fs = Math.max(u(10.5), 9.5), { cx, cy, k, rd } = P;  api.rect(P.x0, P.y0, P.W, P.H, { fill: "paper", stroke: "ink", w: u(1) });  api.text("上から見たプロペラ", P.x0 + u(7), P.y0 + u(15), { size: fs });  // 胴体と前の印  api.poly([[cx - u(8), cy + u(16)], [cx + u(8), cy + u(16)], [cx + u(8), cy - u(16)], [cx, cy - u(24)], [cx - u(8), cy - u(16)]], { fill: "paper", stroke: "ink", w: u(1.2), close: true });  api.text("前", cx, cy - u(8), { size: fs * 0.9, align: "center" });  ROTORS.forEach((r, i) => {    const x = cx + r.z * k, y = cy - r.x * k, stop = d.stopped[i];    api.line(cx, cy, x, y, { w: u(2.4) });    api.circle(x, y, rd, { fill: "paper", stroke: "ink", w: u(1) });    // 回る向きの矢印(左回り = 画面の上で角度が減る向き)    if (!stop) {      const R = rd * 0.86, arc: Pt[] = [], n = 22, a0 = -2.2, span = 1.5 * Math.PI, dir = -r.s;      for (let j = 0; j <= n; j++) { const a = a0 + dir * (j / n) * span; arc.push([x + R * Math.cos(a), y + R * Math.sin(a)]); }      api.poly(arc, { w: u(1.4) });      const ae = a0 + dir * span, hx = x + R * Math.cos(ae), hy = y + R * Math.sin(ae), tx = -Math.sin(ae) * dir, ty = Math.cos(ae) * dir, H = u(6);      api.poly([[hx + tx * H, hy + ty * H], [hx - ty * H * 0.55, hy + tx * H * 0.55], [hx + ty * H * 0.55, hy - tx * H * 0.55]], { fill: "ink", stroke: false, close: true });    }    // 推力の棒    const bw = u(9), bh = rd * 1.1, bx = x - bw / 2, by = y - bh / 2 + u(1), fill = clamp(d.thr[i] / T_MAX, 0, 1) * bh, hover = ((d.m * G) / 4 / T_MAX) * bh;    api.rect(bx, by, bw, bh, { fill: "paper", stroke: "ink", w: u(1) });    if (fill > 0.3) api.rect(bx, by + bh - fill, bw, fill, { fill: "ink", stroke: false });    if (hover < bh) api.line(bx - u(3), by + bh - hover, bx + bw + u(3), by + bh - hover, { w: u(1.2) });    // 数字は機体の外側へ置く    api.label(stop ? "停止" : fmt(d.thr[i], 1), x + Math.sign(r.z) * rd * 0.62, y - Math.sign(r.x) * rd * 0.88 + (r.x > 0 ? fs * 0.35 : fs * 0.5), { size: fs, font: stop ? "sans" : "mono", align: "center", pad: u(1.5) });    if (stop) { const q = rd * 0.7; api.line(x - q, y - q, x + q, y + q, { w: u(2) }); api.line(x - q, y + q, x + q, y - q, { w: u(2) }); }  });  // 反トルクの合計(+ が左回り)  let ty = 0;  for (let i = 0; i < 4; i++) ty += -ROTORS[i].s * KAPPA * d.thr[i];  const m = Math.abs(ty);  if (m > 0.002) {    const R = u(21), n = 16, dir = ty > 0 ? -1 : 1, span = clamp(m / 0.06, 0.25, 1) * Math.PI * 1.1, a0 = Math.PI / 2 - (dir * span) / 2, arc: Pt[] = [];    for (let j = 0; j <= n; j++) { const a = a0 + dir * (j / n) * span; arc.push([cx + R * Math.cos(a), cy + R * Math.sin(a)]); }    api.poly(arc, { w: u(2.2) });    const ae = a0 + dir * span, hx = cx + R * Math.cos(ae), hy = cy + R * Math.sin(ae), tx = -Math.sin(ae) * dir, ty2 = Math.cos(ae) * dir, H = u(7);    api.poly([[hx + tx * H, hy + ty2 * H], [hx - ty2 * H * 0.55, hy + tx * H * 0.55], [hx + ty2 * H * 0.55, hy - tx * H * 0.55]], { fill: "ink", stroke: false, close: true });  }  api.text(`反トルクの合計 ${fmt(m, 2)} N·m`, cx, P.y0 + P.H - u(14), { size: fs * 0.92, align: "center" });  api.text("タップで止める", cx, P.y0 + P.H - u(3), { size: fs * 0.86, align: "center" });}function drawSticks(api: Api, s: S) {  const u = api.u, g = stickGeom(api), fs = Math.max(u(10.5), 9.5), st = s.d.stick;  const one = (c: { x: number; y: number }, dx: number, dy: number, label: string) => {    api.circle(c.x, c.y, g.r, { fill: "paper", stroke: "ink", w: u(1.4) });    api.line(c.x - g.r * 0.7, c.y, c.x + g.r * 0.7, c.y, { w: u(0.7) }); api.line(c.x, c.y - g.r * 0.7, c.x, c.y + g.r * 0.7, { w: u(0.7) });    api.circle(c.x + dx * g.r * 0.62, c.y - dy * g.r * 0.62, g.r * 0.34, { fill: "ink", stroke: false });    api.label(label, c.x, c.y - g.r - u(6), { size: fs, align: "center", pad: u(2) });  };  one(g.l, -st.lx, st.ly, "上下と向き");  one(g.r_, st.rx, st.ry, "前後と左右");  const btn = (i: number, t: string, on: boolean) => {    const [x, y, w, h] = btnRect(api, i);    api.rect(x, y, w, h, { fill: on ? "ink" : "paper", stroke: "ink", w: u(1.2) });    api.text(t, x + w / 2, y + h / 2 + fs * 0.36, { size: fs, align: "center", color: on ? "paper" : "ink" });  };  btn(0, "突風", s.d.t - s.d.gustT < s.d.gustLen);  btn(1, "追う", s.follow);}const _vp = new THREE.Matrix4();/** 飛んだ跡: 3D の点を自分で 2D に写して、太さのある線で描く */function drawTrail(api: Api, s: S) {  const cam = s.v.camera;  _vp.multiplyMatrices(cam.projectionMatrix, cam.matrixWorldInverse);  const e = _vp.elements, T = s.trail, n = Math.min(s.trailN, N_TRAIL), start = s.trailN >= N_TRAIL ? s.trailN % N_TRAIL : 0, P = s.trailP;  let c = 0;  const flush = () => { if (c >= 4) api.poly(P.subarray(0, c), { w: api.u(1.6) }); c = 0; };  for (let k = 0; k < n; k++) {    const a = ((start + k) % N_TRAIL) * 3, x = T[a], y = T[a + 1], z = T[a + 2], w = e[3] * x + e[7] * y + e[11] * z + e[15];    if (w < 0.2) { flush(); continue; }    P[c++] = (((e[0] * x + e[4] * y + e[8] * z + e[12]) / w + 1) / 2) * api.w; P[c++] = ((1 - (e[1] * x + e[5] * y + e[9] * z + e[13]) / w) / 2) * api.h;  }  flush();}const N_TRAIL = 800;function overlay(api: Api, s: S) {  const d = s.d, u = api.u, fs = Math.max(u(11), 10);  drawTrail(api, s);  if (d.mode === "pos" && !d.dead) {    const dist = Math.hypot(d.target.x - d.p.x, d.target.y - d.p.y, d.target.z - d.p.z);    if (dist > 0.6) { const q = s.v.project(d.target); if (q.on) api.label(`目標 ${fmt(dist, dist < 10 ? 1 : 0)} m`, q.x + u(8), q.y - u(8), { size: fs, pad: u(2) }); }  }  if (d.dead) { const q = s.v.project(d.p); if (q.on) api.label("墜落 タップで飛び直す", q.x, q.y - u(26), { size: fs, align: "center" }); }  if (chartsFit(api)) drawCharts(api, s);  drawRotors(api, s);  drawSticks(api, s);  const tilt = Math.acos(clamp(d.up.y, -1, 1)) / DEG, sp = d.v.length(), sum = d.thr[0] + d.thr[1] + d.thr[2] + d.thr[3], rpm = ((d.om[0] + d.om[1] + d.om[2] + d.om[3]) / (d.stopped.some(Boolean) ? 3 : 4)) * 60 / (2 * Math.PI);  let geSum = 0, geN = 0;  for (let i = 0; i < 4; i++) if (!d.stopped[i]) { geSum += d.ge[i]; geN++; }  const ge = geN ? geSum / geN : 1, zr = (d.p.y + d.up.y * H_ROTOR) / R_P;  api.readout([    `高さ  ${fmt(d.p.y, 2)} m`,    `速さ  ${fmt(sp, 1)} m/s(${fmt(sp * 3.6, 0)} km/h)`,    `傾き  ${fmt(tilt, 1)}°`,    `推力の合計  ${fmt(sum, 1)} N(重さ ${fmt(d.m * G, 1)} N)`,    d.spinning ? `機体の回転  ${fmt(Math.abs(d.w.y) / (2 * Math.PI), 1)} 回/秒` : ge > 1.002 && rpm > 100 ? `地面効果  +${fmt((ge - 1) * 100, ge < 1.01 ? 2 : 1)}%(半径の ${fmt(zr, 2)} 倍)・${fmt(rpm, 0)} rpm` : `回転数の平均  ${fmt(rpm, 0)} rpm`,  ]);}