120 lines
3.0 KiB
Python
120 lines
3.0 KiB
Python
import math
|
|
import numpy as np
|
|
|
|
try:
|
|
from lib import RTLFWriter
|
|
except ImportError:
|
|
class RTLFWriter:
|
|
def __init__(self, *args, **kwargs): pass
|
|
def __enter__(self): return self
|
|
def __exit__(self, *args): pass
|
|
def write_record(self, *args): pass
|
|
|
|
|
|
def simulate_closed_circuit(filename, car_id=7):
|
|
track_name = "FIA constraint GP"
|
|
|
|
# === Reference GPS location (realistic) ===
|
|
base_lat = 52.0733
|
|
base_lon = -1.0147
|
|
|
|
# === Simulation settings ===
|
|
dt = 0.05
|
|
lap_time = 92.0
|
|
samples = int(lap_time / dt)
|
|
|
|
# === Track geometry (meters) ===
|
|
a = 1100.0
|
|
b = 750.0
|
|
|
|
theta = np.linspace(0, 2 * math.pi, samples, endpoint=False)
|
|
|
|
x_m = (
|
|
a * np.cos(theta)
|
|
+ 200 * np.cos(3 * theta)
|
|
+ 70 * np.sin(5 * theta)
|
|
)
|
|
y_m = (
|
|
b * np.sin(theta)
|
|
+ 160 * np.sin(2 * theta)
|
|
- 60 * np.cos(4 * theta)
|
|
)
|
|
|
|
# === Elevation profile (meters) ===
|
|
z_m = (
|
|
12 * np.sin(theta)
|
|
+ 6 * np.sin(3 * theta)
|
|
)
|
|
|
|
# === Derivatives (meters) ===
|
|
dx = np.gradient(x_m, dt)
|
|
dy = np.gradient(y_m, dt)
|
|
ddx = np.gradient(dx, dt)
|
|
ddy = np.gradient(dy, dt)
|
|
|
|
# === Curvature ===
|
|
curvature = np.abs(dx * ddy - dy * ddx) / np.power(dx*dx + dy*dy, 1.5)
|
|
curvature = np.nan_to_num(curvature)
|
|
|
|
# === Banking angle (radians) ===
|
|
banking = 0.08 * np.tanh(800 * curvature)
|
|
|
|
# === Grip-limited speed model ===
|
|
g = 9.81
|
|
mu = 1.6 # racing slicks
|
|
v_max_curve = np.sqrt((mu * g * (1 + np.sin(banking))) / (curvature + 1e-6))
|
|
|
|
speed = np.clip(v_max_curve, 30.0, 83.0)
|
|
|
|
# === Acceleration ===
|
|
norm = np.hypot(dx, dy)
|
|
vx = speed * dx / norm
|
|
vy = speed * dy / norm
|
|
|
|
ax = np.gradient(vx, dt)
|
|
ay = np.gradient(vy, dt)
|
|
az = np.gradient(np.gradient(z_m, dt), dt)
|
|
|
|
# === Forces ===
|
|
mass = 1250.0
|
|
fx = mass * ax
|
|
fy = mass * ay
|
|
fz = mass * (g + az)
|
|
|
|
# === Convert meters → lat/long ===
|
|
meters_per_deg_lat = 111_320.0
|
|
meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(base_lat))
|
|
|
|
lat = base_lat + (y_m / meters_per_deg_lat)
|
|
lon = base_lon + (x_m / meters_per_deg_lon)
|
|
|
|
# === OPTIONAL GPS noise (centimeter scale) ===
|
|
gps_noise_m = 0.25
|
|
lat += np.random.normal(0, gps_noise_m / meters_per_deg_lat, samples)
|
|
lon += np.random.normal(0, gps_noise_m / meters_per_deg_lon, samples)
|
|
|
|
# === Force exact loop closure ===
|
|
lat[-1] = lat[0]
|
|
lon[-1] = lon[0]
|
|
|
|
with RTLFWriter(filename, car_id, track_name) as writer:
|
|
t = 0.0
|
|
for i in range(samples):
|
|
writer.write_record(
|
|
t,
|
|
float(lon[i]),
|
|
float(lat[i]),
|
|
float(fx[i]),
|
|
float(fy[i]),
|
|
float(fz[i]),
|
|
float(ax[i]),
|
|
float(ay[i]),
|
|
float(az[i]),
|
|
float(speed[i])
|
|
)
|
|
t += dt
|
|
|
|
|
|
if __name__ == "__main__":
|
|
simulate_closed_circuit("realistic.rtl")
|
|
print("Realistic simulation complete.") |