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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
use crate::{
utils::{AABB, Circle, CollisionParameters, generate_circles, random_biased},
vector::Vector,
};
pub struct CollisionResult<const N: usize, V: Vector<N>> {
pub point: V,
pub gradient: V,
}
pub trait SDF<const N: usize, V: Vector<N>> {
fn dist(&self, p: V) -> f32;
fn aabb(&self) -> AABB<N, V>;
fn gradient(&self, p: V, params: &CollisionParameters) -> V {
// (self.dist(p)
// - Vec2::new(
// self.dist(p + Vec2::new(params.normal_epsilon, 0.0)),
// self.dist(p + Vec2::new(0.0, params.normal_epsilon)),
// ))
// * -1.0
let mut res = [0.0; N];
let k = self.dist(p);
for i in 0..N {
let mut a = [0.0; N];
a[i] = params.normal_epsilon;
res[i] = k - self.dist(p + V::from_arr(a));
}
(V::from_arr(res) * -1.0).normalized()
}
/// returns sum of the squares of the distance to each object, and the gradient
/// of the combined distance. used for gradient descent
fn combined<T: SDF<N, V>>(&self, other: &T, p: V, params: &CollisionParameters) -> (f32, V) {
let d1 = self.dist(p);
let d2 = other.dist(p);
let g1 = self.gradient(p, params);
let g2 = other.gradient(p, params);
let grad = g1 * d1 * 2.0 + g2 * d2 * 2.0;
(d1.powi(2) + d2.powi(2), grad)
}
fn gradient_descent<T: SDF<N, V>>(
&self,
other: &T,
ip: V,
params: &CollisionParameters,
) -> Option<V> {
let mut x = ip.clone();
for _ in 0..params.max_gds_iter {
let dg = self.combined(other, x, params);
x = x - dg.1 * params.learning_rate;
if dg.0 <= params.collision_epsilon {
return Some(x);
}
}
None
}
fn get_coll_point<T: SDF<N, V>>(
&self,
other: &T,
params: &CollisionParameters,
) -> Option<CollisionResult<N, V>> {
if let Some(inter) = self.aabb().intersect(&other.aabb()) {
if inter.get_dim().min_elem() <= 0.0 {
return None;
}
let circles = generate_circles(
&inter,
params.area_percentage,
params.max_packing_iter,
params.minimum_radius,
25,
);
let dim = inter.get_dim();
let top_left = inter.get_center() - dim * 0.5;
let circles_filtered = circles
.iter()
.map(|x| Circle {
c: x.c + top_left,
r: x.r,
})
.filter(|x| other.dist(x.c) - x.r < 0.0)
.filter(|x| self.dist(x.c) - x.r < 0.0);
//println!("{}", circles_filtered.clone().count());
let mut average_grad = V::from_arr([0.0; N]);
let mut average_point = V::from_arr([0.0; N]);
let mut num: f32 = 0.0;
for i in circles_filtered {
let conv_p = self.gradient_descent(other, i.c, params);
if let Some(coll_point) = conv_p {
let dg = other.gradient(coll_point, params);
let grad = dg;
average_grad = average_grad + grad;
average_point = average_point + coll_point;
num += 1.0;
break;
}
}
if num > 0.0 {
average_grad = average_grad * num.recip();
average_point = average_point * num.recip();
return Some(CollisionResult {
point: average_point,
gradient: average_grad,
});
}
}
None
}
/// approximate collision depth. unlike iterative refinement, which is used by the fractal
/// [demo](https://0x177.codeberg.page/coll_demo_pub.html), this aims to
/// calculate the actual depth, not just a scale for the normal vector.
/// iterative refinement is not implemented by thimni itself because it would require
/// changes to the SDF trait that i consider to be intrusive. however, it is very
/// trivial to implement
fn monte_carlo_depth<T: SDF<N, V>>(
&self,
other: &T,
params: &CollisionParameters,
res: &CollisionResult<N, V>,
) -> f32 {
let inter = self.aabb().intersect(&other.aabb()).unwrap();
(-(0..params.monte_carlo_samples)
.map(|_| {
let mut random_values = V::from_arr([0.0; N]);
for i in 0..N {
random_values.set(
i,
random_biased(
inter.min.get(i),
inter.max.get(i),
params.monte_carlo_bias_factor,
res.point.get(i),
),
);
}
self.dist(random_values).max(other.dist(random_values))
})
.min_by(|a, b| a.total_cmp(b))
.unwrap())
.max(0.0)
}
}