Collision probability computation for conjunction assessment
collision: refactor tca into helpers (merlint E005)
Extract tca_at_index and d2_between from the 72-line tca function.
Now 40 lines.
+25
-42
+25
-42
lib/collision.ml
+25
-42
lib/collision.ml
···
395
395
396
396
type tca = { time : float; miss_distance : float; relative_velocity : float }
397
397
398
+
let tca_at_index times pos1 pos2 vel1 vel2 i d2 =
399
+
let d = sqrt d2 in
400
+
let dv = Vec3.length (Vec3.sub vel1.(i) vel2.(i)) in
401
+
{ time = times.(i); miss_distance = d; relative_velocity = dv }
402
+
403
+
let d2_between (pos1 : Vec3.t array) (pos2 : Vec3.t array) j =
404
+
let dx = pos1.(j).x -. pos2.(j).x in
405
+
let dy = pos1.(j).y -. pos2.(j).y in
406
+
let dz = pos1.(j).z -. pos2.(j).z in
407
+
(dx *. dx) +. (dy *. dy) +. (dz *. dz)
408
+
398
409
let tca ~times ~pos1 ~pos2 ~vel1 ~vel2 =
399
410
let n = Array.length times in
400
411
if n < 3 then None
401
412
else
402
413
(* Step 1: find the minimum-distance index *)
414
+
(* Step 1: find the minimum-distance index *)
403
415
let min_i = ref 0 in
404
416
let min_d2 = ref Float.infinity in
405
417
for i = 0 to n - 1 do
406
-
let dx = pos1.(i).x -. pos2.(i).x in
407
-
let dy = pos1.(i).y -. pos2.(i).y in
408
-
let dz = pos1.(i).z -. pos2.(i).z in
409
-
let d2 = (dx *. dx) +. (dy *. dy) +. (dz *. dz) in
418
+
let d2 = d2_between pos1 pos2 i in
410
419
if d2 < !min_d2 then (
411
420
min_d2 := d2;
412
421
min_i := i)
413
422
done;
414
423
let i = !min_i in
415
424
if i = 0 || i = n - 1 then
416
-
(* Minimum at edge — cannot refine *)
417
-
let d = sqrt !min_d2 in
418
-
let dv = Vec3.length (Vec3.sub vel1.(i) vel2.(i)) in
419
-
Some { time = times.(i); miss_distance = d; relative_velocity = dv }
425
+
Some (tca_at_index times pos1 pos2 vel1 vel2 i !min_d2)
420
426
else
421
-
(* Step 2: quadratic fit on d^2(t) using 3 points around minimum.
422
-
d^2(t) = a*t^2 + b*t + c. Minimum at t* = -b/(2a). *)
427
+
(* Step 2: quadratic refinement around minimum *)
423
428
let t0 = times.(i - 1) and t1 = times.(i) and t2 = times.(i + 1) in
424
-
let d2_at j =
425
-
let dx = pos1.(j).x -. pos2.(j).x in
426
-
let dy = pos1.(j).y -. pos2.(j).y in
427
-
let dz = pos1.(j).z -. pos2.(j).z in
428
-
(dx *. dx) +. (dy *. dy) +. (dz *. dz)
429
-
in
430
-
let y0 = d2_at (i - 1) and y1 = d2_at i and y2 = d2_at (i + 1) in
431
-
(* Quadratic fit: p(t) = a*(t-t1)^2 + b*(t-t1) + c
432
-
where c = y1, and a,b from the two other points.
433
-
Minimum at t1 - b/(2a). *)
429
+
let y0 = d2_between pos1 pos2 (i - 1) in
430
+
let y1 = d2_between pos1 pos2 i in
431
+
let y2 = d2_between pos1 pos2 (i + 1) in
434
432
let h0 = t0 -. t1 and h2 = t2 -. t1 in
435
433
let denom = h0 *. h2 *. (h2 -. h0) in
436
-
if Float.abs denom < 1e-30 then
437
-
let d = sqrt !min_d2 in
438
-
let dv = Vec3.length (Vec3.sub vel1.(i) vel2.(i)) in
439
-
Some { time = t1; miss_distance = d; relative_velocity = dv }
434
+
if Float.abs denom < 1e-30 || denom = 0.0 then
435
+
Some (tca_at_index times pos1 pos2 vel1 vel2 i !min_d2)
440
436
else
441
-
(* a = (h2*(y0-y1) - h0*(y2-y1)) / (h0*h2*(h2-h0)) *)
442
437
let a = ((h2 *. (y0 -. y1)) -. (h0 *. (y2 -. y1))) /. denom in
443
-
(* b = (h0*h0*(y2-y1) - h2*h2*(y0-y1)) / (h0*h2*(h2-h0)) *)
444
438
let b =
445
439
((h0 *. h0 *. (y2 -. y1)) -. (h2 *. h2 *. (y0 -. y1))) /. denom
446
440
in
447
441
if a <= 0.0 then
448
-
(* Concave — no minimum, use step-based *)
449
-
let d = sqrt !min_d2 in
450
-
let dv = Vec3.length (Vec3.sub vel1.(i) vel2.(i)) in
451
-
Some { time = t1; miss_distance = d; relative_velocity = dv }
442
+
Some (tca_at_index times pos1 pos2 vel1 vel2 i !min_d2)
452
443
else
453
444
let dt = -.b /. (2.0 *. a) in
454
-
let t_star = t1 +. dt in
455
-
(* Clamp to [t0, t2] *)
456
-
let t_star = Float.max t0 (Float.min t2 t_star) in
457
-
let dt_clamped = t_star -. t1 in
458
-
let d2_star =
459
-
Float.max 0.0
460
-
((a *. dt_clamped *. dt_clamped) +. (b *. dt_clamped) +. y1)
461
-
in
462
-
let d_star = sqrt d2_star in
463
-
(* Relative velocity: interpolate linearly *)
464
-
let frac = (t_star -. t0) /. (t2 -. t0) in
465
-
let frac = Float.max 0.0 (Float.min 1.0 frac) in
445
+
let t_star = Float.max t0 (Float.min t2 (t1 +. dt)) in
446
+
let dt_c = t_star -. t1 in
447
+
let d_star = sqrt (Float.max 0.0 ((a *. dt_c *. dt_c) +. (b *. dt_c) +. y1)) in
448
+
let frac = Float.max 0.0 (Float.min 1.0 ((t_star -. t0) /. (t2 -. t0))) in
466
449
let dv_lo = Vec3.length (Vec3.sub vel1.(i - 1) vel2.(i - 1)) in
467
450
let dv_hi = Vec3.length (Vec3.sub vel1.(i + 1) vel2.(i + 1)) in
468
451
let dv = dv_lo +. (frac *. (dv_hi -. dv_lo)) in