def _clamp(x: float, lo: float, hi: float) -> float:
return max(lo, min(hi, x))
def rhs(t, y):
rx, ry, rz = y[0], y[1], y[2]
vx_i, vy_i, vz_i = y[3], y[4], y[5]
t_go = y[6]
N_eff = y[7]
d = np.empty(8)
R = np.sqrt(rx * rx + ry * ry + rz * rz)
if R < _RANGE_GUARD:
d[:] = 0.0
return d
vx_rel = vx_i - vtx
vy_rel = vy_i - vty
vz_rel = vz_i - vtz
R_dot = (rx * vx_rel + ry * vy_rel + rz * vz_rel) / R
V_c = -R_dot
t_go_safe = max(t_go, t_go_min)
t_go_sq = t_go_safe * t_go_safe
zem_x = rx + vx_rel * t_go_safe
zem_y = ry + vy_rel * t_go_safe
zem_z = rz + vz_rel * t_go_safe
a_x = N_eff * zem_x / t_go_sq
a_y = N_eff * zem_y / t_go_sq
a_z = N_eff * zem_z / t_go_sq
a_mag = np.sqrt(a_x * a_x + a_y * a_y + a_z * a_z)
if a_mag > a_max:
scale = a_max / a_mag
a_x *= scale
a_y *= scale
a_z *= scale
wx = np.interp(t, _noise_ts, wind_noise[0])
wy = np.interp(t, _noise_ts, wind_noise[1])
wz = np.interp(t, _noise_ts, wind_noise[2])
d[0] = vx_rel
d[1] = vy_rel
d[2] = vz_rel
d[3] = a_x + wx
d[4] = a_y + wy
d[5] = a_z + wz
d[6] = -V_c / max(R, _RANGE_GUARD)
zem_mag = np.sqrt(zem_x**2 + zem_y**2 + zem_z**2)
maneuver_ratio = zem_mag / (a_tgt_est * t_go_sq + 1.0)
N_target = N_eff_0 + 2.0 * maneuver_ratio
d[7] = _clamp((N_target - N_eff) / tau_n, -10.0, 10.0)
return d