Source file Solver3d.ml

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
(* Claude Code
 *
 * Copyright (C) 2026 Yoann Padioleau
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public License
 * (LGPL) as published by the Free Software Foundation; either version
 * 2 of the License, or (at your option) any later version.
 *)

(* See Solver3d.mli *)

type options = {
  iterations : int;
  warm_starting : bool;
  baumgarte : float;
  slop : float;
  bounce_threshold : float;
  matching : float;
}

(* in metres and seconds, where the 2D engine's are in pixels: 5 mm of
 * tolerated overlap, a metre a second under which nothing bounces, and
 * a contact point within 3 cm of last step's is the same one *)
let default =
  { iterations = 10; warm_starting = true; baumgarte = 0.2; slop = 0.005; bounce_threshold = 1.; matching = 0.03 }

type pair = { a : int; b : int; contacts : Contact3d.t list; restitution : float; friction : float }

module Pairs = Map.Make (struct
  type t = int * int

  let compare = compare
end)

(* A contact point's impulses: the normal one as a number, friction as
 * a *vector*. Friction is solved along two tangents chosen from the
 * normal (Resolve3d.tangents), and that choice jumps by a quarter turn
 * when the normal wobbles across a tie -- so remembering two numbers
 * means replaying last step's friction along this step's axes, which
 * kicks a resting pile every few dozen steps. A vector has no such
 * opinion, and is taken apart again in whatever basis the new step
 * picked. *)
type remembered = { at : Vec3.t; normal_impulse : float; friction_impulse : Vec3.t }

type memory = remembered list Pairs.t

let nothing = Pairs.empty

(* one contact point being solved: everything that does not change
 * during the iterations, worked out once, and the impulses so far *)
type point = {
  a : int;
  b : int;
  p : Vec3.t;
  n : Vec3.t;
  t1 : Vec3.t;
  t2 : Vec3.t;
  (* 1 / resistance along each of the three *)
  mass_n : float;
  mass_t1 : float;
  mass_t2 : float;
  (* the separating speed asked for: Baumgarte's, or the bounce's *)
  bias : float;
  friction : float;
  mutable pn : float;
  mutable p1 : float;
  mutable p2 : float;
}

let solve (o : options) ~(dt : float) ?(joints = []) (bodies : Body3d.t array) (pairs : pair list) (memory : memory) :
    Body3d.t array * memory =
  let bodies = Array.copy bodies in
  let apply (pt : point) (impulse : Vec3.t) =
    let a, b = Resolve3d.apply 1. impulse pt.p (bodies.(pt.a), bodies.(pt.b)) in
    bodies.(pt.a) <- a;
    bodies.(pt.b) <- b
  in
  let prepare (pr : pair) (c : Contact3d.t) : point option =
    let a = bodies.(pr.a) and b = bodies.(pr.b) in
    let n = c.Contact3d.normal in
    let t1, t2 = Resolve3d.tangents n in
    let k_n = Resolve3d.resistance a b c.Contact3d.point n in
    if k_n <= 1e-12 then None (* two immovable bodies: nothing to solve *)
    else
      let k1 = Resolve3d.resistance a b c.Contact3d.point t1 and k2 = Resolve3d.resistance a b c.Contact3d.point t2 in
      let vn = Vec3.dot (Resolve3d.relative_velocity a b c.Contact3d.point) n in
      let bounce = if vn < -.o.bounce_threshold then -.pr.restitution *. vn else 0. in
      let bias = Float.max (o.baumgarte /. dt *. Float.max 0. (c.Contact3d.depth -. o.slop)) bounce in
      (* warm starting: this point's impulses at the previous step,
       * found again by where it is *)
      let before =
        if not o.warm_starting then None
        else
          Option.bind
            (Pairs.find_opt (pr.a, pr.b) memory)
            (List.find_opt (fun r -> Vec3.length (Vec3.sub r.at c.Contact3d.point) < o.matching))
      in
      let pn, p1, p2 =
        match before with
        | Some r -> (r.normal_impulse, Vec3.dot r.friction_impulse t1, Vec3.dot r.friction_impulse t2)
        | None -> (0., 0., 0.)
      in
      Some
        { a = pr.a; b = pr.b; p = c.Contact3d.point; n; t1; t2; mass_n = 1. /. k_n;
          mass_t1 = (if k1 > 1e-12 then 1. /. k1 else 0.);
          mass_t2 = (if k2 > 1e-12 then 1. /. k2 else 0.);
          bias; friction = pr.friction; pn; p1; p2 }
  in
  let points = List.concat_map (fun pr -> List.filter_map (prepare pr) pr.contacts) pairs in
  (* the remembered impulses, put back before the first iteration *)
  List.iter
    (fun pt -> apply pt (Vec3.add (Vec3.scale pt.pn pt.n) (Vec3.add (Vec3.scale pt.p1 pt.t1) (Vec3.scale pt.p2 pt.t2))))
    points;
  (* the joints' rows (Joint3d), solved in the same iterations, before
   * the contacts, and no differently: a row is a direction and a speed
   * to reach along it, as a contact point's normal is *)
  let rows = List.concat_map (Joint3d.rows ~beta:o.baumgarte ~dt bodies) joints in
  for _ = 1 to o.iterations do
    List.iter (Joint3d.solve_row bodies) rows;
    points
    |> List.iter (fun pt ->
           let rel () = Resolve3d.relative_velocity bodies.(pt.a) bodies.(pt.b) pt.p in
           (* along the normal, towards the separating speed asked for;
            * what is clamped is the *sum* of this contact's impulses,
            * which may never pull *)
           let was = pt.pn in
           pt.pn <- Float.max 0. (was +. (pt.mass_n *. (pt.bias -. Vec3.dot (rel ()) pt.n)));
           apply pt (Vec3.scale (pt.pn -. was) pt.n);
           (* friction along each tangent, each within mu times the
            * normal impulse: the pyramid of Resolve3d.mli *)
           let limit = pt.friction *. pt.pn in
           let slide t mass current =
             let wanted = current -. (mass *. Vec3.dot (rel ()) t) in
             Float.max (-.limit) (Float.min limit wanted)
           in
           let was = pt.p1 in
           pt.p1 <- slide pt.t1 pt.mass_t1 was;
           apply pt (Vec3.scale (pt.p1 -. was) pt.t1);
           let was = pt.p2 in
           pt.p2 <- slide pt.t2 pt.mass_t2 was;
           apply pt (Vec3.scale (pt.p2 -. was) pt.t2))
  done;
  let remember m pt =
    let r =
      { at = pt.p; normal_impulse = pt.pn;
        friction_impulse = Vec3.add (Vec3.scale pt.p1 pt.t1) (Vec3.scale pt.p2 pt.t2) }
    in
    Pairs.update (pt.a, pt.b) (fun l -> Some (r :: Option.value ~default:[] l)) m
  in
  (bodies, List.fold_left remember Pairs.empty points)

let impulses (memory : memory) (ab : int * int) : float list =
  List.rev_map (fun r -> r.normal_impulse) (Option.value ~default:[] (Pairs.find_opt ab memory))