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
type t = Sphere of float | Box of Vec3.t | Capsule of float * float | Plane of Vec3.t * float
type placed = { shape : t; pos : Vec3.t; orientation : Quat.t }
let place ?(orientation = Quat.identity) pos shape = { shape; pos; orientation }
let axes (p : placed) : Vec3.t * Vec3.t * Vec3.t =
( Quat.rotate p.orientation (1., 0., 0.),
Quat.rotate p.orientation (0., 1., 0.),
Quat.rotate p.orientation (0., 0., 1.) )
let corners (p : placed) : Vec3.t list =
match p.shape with
| Box (hx, hy, hz) ->
let ax, ay, az = axes p in
List.concat_map
(fun sx ->
List.concat_map
(fun sy ->
List.map
(fun sz ->
Vec3.add p.pos
(Vec3.add (Vec3.scale (sx *. hx) ax) (Vec3.add (Vec3.scale (sy *. hy) ay) (Vec3.scale (sz *. hz) az))))
[ -1.; 1. ])
[ -1.; 1. ])
[ -1.; 1. ]
| _ -> []
let face_axes (p : placed) : Vec3.t list =
match p.shape with
| Box _ ->
let ax, ay, az = axes p in
[ ax; ay; az ]
| Plane (n, _) -> [ n ]
| _ -> []
let segment (p : placed) : Vec3.t * Vec3.t =
match p.shape with
| Capsule (half, _) ->
let _, ay, _ = axes p in
(Vec3.add p.pos (Vec3.scale (-.half) ay), Vec3.add p.pos (Vec3.scale half ay))
| _ -> (p.pos, p.pos)
let support (p : placed) (d : Vec3.t) : Vec3.t =
let u = Vec3.normalize d in
match p.shape with
| Sphere r -> Vec3.add p.pos (Vec3.scale r u)
| Box _ -> (
match corners p with
| [] -> p.pos
| c :: cs -> List.fold_left (fun best q -> if Vec3.dot q u > Vec3.dot best u then q else best) c cs)
| Capsule (_, r) ->
let a, b = segment p in
let tip = if Vec3.dot a u > Vec3.dot b u then a else b in
Vec3.add tip (Vec3.scale r u)
| Plane _ -> p.pos
let extent (p : placed) (axis : Vec3.t) : float * float =
match p.shape with
| Sphere r ->
let c = Vec3.dot p.pos axis in
(c -. r, c +. r)
| Box _ ->
let ds = List.map (fun c -> Vec3.dot c axis) (corners p) in
(List.fold_left Float.min infinity ds, List.fold_left Float.max neg_infinity ds)
| Capsule (_, r) ->
let a, b = segment p in
let da = Vec3.dot a axis and db = Vec3.dot b axis in
(Float.min da db -. r, Float.max da db +. r)
| Plane (n, d) ->
if Vec3.dot n axis > 0.999999 then (neg_infinity, d) else (neg_infinity, infinity)
let bounds (p : placed) : Vec3.t * Vec3.t =
match p.shape with
| Plane _ -> ((neg_infinity, neg_infinity, neg_infinity), (infinity, infinity, infinity))
| _ ->
let along a =
let lo, hi = extent p a in
(lo, hi)
in
let lx, hx = along (1., 0., 0.) and ly, hy = along (0., 1., 0.) and lz, hz = along (0., 0., 1.) in
((lx, ly, lz), (hx, hy, hz))
let volume : t -> float = function
| Sphere r -> 4. /. 3. *. Float.pi *. r *. r *. r
| Box (hx, hy, hz) -> 8. *. hx *. hy *. hz
| Capsule (half, r) -> (Float.pi *. r *. r *. (2. *. half)) +. (4. /. 3. *. Float.pi *. r *. r *. r)
| Plane _ -> 0.
let inertia ~mass : t -> Mat3.t = function
| Sphere r -> Body3d.solid_sphere ~mass ~radius:r
| Box (hx, hy, hz) -> Body3d.box ~mass (2. *. hx, 2. *. hy, 2. *. hz)
| Plane _ -> Body3d.never_turns
| Capsule (half, r) ->
let l = 2. *. half in
let v_cyl = Float.pi *. r *. r *. l and v_hemi = 2. /. 3. *. Float.pi *. r *. r *. r in
let total = v_cyl +. (2. *. v_hemi) in
let mc = mass *. v_cyl /. total and mh = mass *. v_hemi /. total in
let along = (mc *. r *. r /. 2.) +. (2. *. mh *. 2. /. 5. *. r *. r) in
let across =
(mc *. ((l *. l /. 12.) +. (r *. r /. 4.)))
+. (2. *. mh *. ((2. /. 5. *. r *. r) +. (l *. l /. 4.) +. (3. *. l *. r /. 8.)))
in
Mat3.diagonal across along across