(dynamics, x, u, dt)
| 20 | |
| 21 | |
| 22 | def rk4(dynamics, x, u, dt): |
| 23 | k1 = dynamics(x, u) |
| 24 | k2 = dynamics(x + dt / 2 * k1, u) |
| 25 | k3 = dynamics(x + dt / 2 * k2, u) |
| 26 | k4 = dynamics(x + dt * k3, u) |
| 27 | return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) |
| 28 | |
| 29 | |
| 30 | def check_collision(x, obs_center, obs_radius): |