-
Notifications
You must be signed in to change notification settings - Fork 28
/
narrowphase.cpp
374 lines (309 loc) · 14.3 KB
/
narrowphase.cpp
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
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
#include "edyn/collision/narrowphase.hpp"
#include "edyn/comp/constraint.hpp"
#include "edyn/comp/material.hpp"
#include "edyn/comp/position.hpp"
#include "edyn/comp/orientation.hpp"
#include "edyn/comp/shape.hpp"
#include "edyn/comp/constraint_row.hpp"
#include "edyn/comp/tag.hpp"
#include "edyn/comp/aabb.hpp"
#include "edyn/comp/dirty.hpp"
#include "edyn/collision/contact_manifold.hpp"
#include "edyn/collision/contact_point.hpp"
#include "edyn/collision/collide.hpp"
#include "edyn/math/geom.hpp"
#include "edyn/util/constraint_util.hpp"
#include "edyn/parallel/parallel_for_async.hpp"
#include <entt/entt.hpp>
#include <variant>
namespace edyn {
// Update distance of persisted contact points.
static
void update_contact_distances(entt::registry ®istry) {
auto cp_view = registry.view<contact_point>();
auto tr_view = registry.view<position, orientation>();
cp_view.each([&] (auto, contact_point &cp) {
auto [posA, ornA] = tr_view.get<position, orientation>(cp.body[0]);
auto [posB, ornB] = tr_view.get<position, orientation>(cp.body[1]);
auto pivotA_world = posA + rotate(ornA, cp.pivotA);
auto pivotB_world = posB + rotate(ornB, cp.pivotB);
auto normal_world = rotate(ornB, cp.normalB);
cp.distance = dot(normal_world, pivotA_world - pivotB_world);
});
}
// Merges a `collision_point` onto a `contact_point`.
static
void merge_point(const collision_result::collision_point &rp, contact_point &cp) {
cp.pivotA = rp.pivotA;
cp.pivotB = rp.pivotB;
cp.normalB = rp.normalB;
cp.distance = rp.distance;
}
static
void create_contact_constraint(entt::registry ®istry, entt::entity manifold_entity,
entt::entity contact_entity, contact_point &cp) {
auto &materialA = registry.get<material>(cp.body[0]);
auto &materialB = registry.get<material>(cp.body[1]);
cp.restitution = materialA.restitution * materialB.restitution;
cp.friction = materialA.friction * materialB.friction;
auto stiffness = large_scalar;
auto damping = large_scalar;
if (materialA.stiffness < large_scalar || materialB.stiffness < large_scalar) {
stiffness = 1 / (1 / materialA.stiffness + 1 / materialB.stiffness);
damping = 1 / (1 / materialA.damping + 1 / materialB.damping);
}
auto contact = contact_constraint();
contact.stiffness = stiffness;
contact.damping = damping;
// Contact constraints are never graph edges since they're effectively
// a child of a manifold and the manifold is the graph edge.
constexpr auto is_graph_edge = false;
make_constraint(contact_entity, registry, std::move(contact), cp.body[0], cp.body[1], is_graph_edge);
}
template<typename ContactPointViewType> static
size_t find_nearest_contact(const contact_manifold &manifold,
const collision_result::collision_point &coll_pt,
const ContactPointViewType &cp_view) {
auto shortest_dist = contact_caching_threshold * contact_caching_threshold;
auto num_points = manifold.num_points();
auto nearest_idx = num_points;
for (size_t i = 0; i < num_points; ++i) {
auto &cp = cp_view.template get<contact_point>(manifold.point[i]);
auto dA = length_sqr(coll_pt.pivotA - cp.pivotA);
auto dB = length_sqr(coll_pt.pivotB - cp.pivotB);
if (dA < shortest_dist) {
shortest_dist = dA;
nearest_idx = i;
}
if (dB < shortest_dist) {
shortest_dist = dB;
nearest_idx = i;
}
}
return nearest_idx;
}
static
void create_contact_point(entt::registry ®istry, entt::entity manifold_entity,
contact_manifold &manifold, const collision_result::collision_point &rp) {
auto idx = manifold.num_points();
if (idx >= max_contacts) return;
auto contact_entity = registry.create();
manifold.point[idx] = contact_entity;
auto &cp = registry.emplace<contact_point>(
contact_entity,
manifold.body,
rp.pivotA, // pivotA
rp.pivotB, // pivotB
rp.normalB, // normalB
0, // friction
0, // restitution
0, // lifetime
rp.distance // distance
);
if (registry.has<material>(manifold.body[0]) && registry.has<material>(manifold.body[1])) {
create_contact_constraint(registry, manifold_entity, contact_entity, cp);
}
registry.get_or_emplace<dirty>(contact_entity)
.set_new()
.created<contact_point>();
registry.get_or_emplace<dirty>(manifold_entity).updated<contact_manifold>();
}
static
void destroy_contact_point(entt::registry ®istry, entt::entity manifold_entity, entt::entity contact_entity) {
registry.destroy(contact_entity);
registry.get_or_emplace<dirty>(manifold_entity).updated<contact_manifold>();
}
using contact_point_view_t = entt::basic_view<entt::entity, entt::exclude_t<>, contact_point, constraint>;
using constraint_row_view_t = entt::basic_view<entt::entity, entt::exclude_t<>, constraint_row_data>;
template<typename Function> static
void process_collision(entt::entity manifold_entity, contact_manifold &manifold,
const collision_result &result,
const contact_point_view_t &cp_view,
const constraint_row_view_t &cr_view, Function new_point_func) {
// Merge new with existing contact points.
for (size_t i = 0; i < result.num_points; ++i) {
auto &rp = result.point[i];
// Find closest existing point.
auto nearest_idx = find_nearest_contact(manifold, rp, cp_view);
if (nearest_idx < manifold.num_points()) {
auto &cp = cp_view.get<contact_point>(manifold.point[nearest_idx]);
++cp.lifetime;
merge_point(rp, cp);
} else {
// Assign it to array of points and set it up.
// Find best insertion index. Try pivotA first.
std::array<vector3, max_contacts> pivots;
std::array<scalar, max_contacts> distances;
auto num_points = manifold.num_points();
for (size_t i = 0; i < num_points; ++i) {
auto &cp = cp_view.get<contact_point>(manifold.point[i]);
pivots[i] = cp.pivotB;
distances[i] = cp.distance;
}
auto idx = insert_index(pivots, distances, num_points, rp.pivotB, rp.distance);
// No closest point found for pivotA, try pivotB.
if (idx >= num_points) {
for (size_t i = 0; i < num_points; ++i) {
auto &cp = cp_view.get<contact_point>(manifold.point[i]);
pivots[i] = cp.pivotB;
}
idx = insert_index(pivots, distances, manifold.num_points(), rp.pivotB, rp.distance);
}
if (idx < max_contacts) {
auto is_new_contact = idx == manifold.num_points();
if (is_new_contact) {
new_point_func(rp);
} else {
// Replace existing contact point.
auto contact_entity = manifold.point[idx];
auto &cp = cp_view.get<contact_point>(contact_entity);
cp.lifetime = 0;
merge_point(rp, cp);
// Zero out warm-starting impulses.
auto &con = cp_view.get<constraint>(contact_entity);
auto num_rows = con.num_rows();
for (size_t i = 0; i < num_rows; ++i) {
cr_view.get(con.row[i]).impulse = 0;
}
}
}
}
}
}
template<typename ContactPointViewType, typename Function> static
void prune(contact_manifold &manifold,
const vector3 &posA, const quaternion &ornA,
const vector3 &posB, const quaternion &ornB,
const ContactPointViewType &cp_view, Function destroy_point_func) {
constexpr auto threshold_sqr = contact_breaking_threshold * contact_breaking_threshold;
// Remove separating contact points.
for (size_t i = manifold.num_points(); i > 0; --i) {
size_t k = i - 1;
auto point_entity = manifold.point[k];
auto &cp = cp_view.template get<contact_point>(point_entity);
auto pA = posA + rotate(ornA, cp.pivotA);
auto pB = posB + rotate(ornB, cp.pivotB);
auto n = rotate(ornB, cp.normalB);
auto d = pA - pB;
auto dn = dot(d, n); // separation along normal
auto dp = d - dn * n; // tangential separation on contact plane
if (dn > contact_breaking_threshold || length_sqr(dp) > threshold_sqr) {
// Swap with last element.
size_t last_idx = manifold.num_points() - 1;
if (last_idx != k) {
manifold.point[k] = manifold.point[last_idx];
}
manifold.point[last_idx] = entt::null;
destroy_point_func(point_entity);
}
}
}
void detect_collision(const contact_manifold &manifold, collision_result &result,
const body_view_t &body_view, const rotated_mesh_view_t &rmesh_view) {
auto [aabbA, posA, ornA] = body_view.get<AABB, position, orientation>(manifold.body[0]);
auto [aabbB, posB, ornB] = body_view.get<AABB, position, orientation>(manifold.body[1]);
const auto offset = vector3_one * -contact_breaking_threshold;
// Only proceed to closest points calculation if AABBs intersect, since
// a manifold is allowed to exist whilst the AABB separation is smaller
// than `manifold.separation_threshold` which is greater than the
// contact breaking threshold.
if (intersect(aabbA.inset(offset), aabbB)) {
auto &shapeA = body_view.get<shape>(manifold.body[0]);
auto &shapeB = body_view.get<shape>(manifold.body[1]);
auto ctx = collision_context{posA, ornA, posB, ornB, contact_breaking_threshold};
if (std::holds_alternative<polyhedron_shape>(shapeA.var)) {
ctx.rmeshA = rmesh_view.get(manifold.body[0]);
}
if (std::holds_alternative<polyhedron_shape>(shapeB.var)) {
ctx.rmeshB = rmesh_view.get(manifold.body[1]);
}
std::visit([&result, &ctx] (auto &&sA, auto &&sB) {
result = collide(sA, sB, ctx);
}, shapeA.var, shapeB.var);
} else {
result.num_points = 0;
}
}
void process_result(entt::registry ®istry, entt::entity manifold_entity,
contact_manifold &manifold, const collision_result &result,
const transform_view_t &tr_view) {
auto cp_view = registry.view<contact_point, constraint>();
auto cr_view = registry.view<constraint_row_data>();
process_collision(manifold_entity, manifold, result, cp_view, cr_view,
[&] (const collision_result::collision_point &rp) {
create_contact_point(registry, manifold_entity, manifold, rp);
});
auto [posA, ornA] = tr_view.get<position, orientation>(manifold.body[0]);
auto [posB, ornB] = tr_view.get<position, orientation>(manifold.body[1]);
prune(manifold, posA, ornA, posB, ornB, cp_view,
[&] (entt::entity contact_entity) {
destroy_contact_point(registry, manifold_entity, contact_entity);
});
}
narrowphase::narrowphase(entt::registry ®)
: m_registry(®)
{}
bool narrowphase::parallelizable() const {
return m_registry->size<contact_manifold>() > 1;
}
void narrowphase::update() {
update_contact_distances(*m_registry);
auto manifold_view = m_registry->view<contact_manifold>();
update_contact_manifolds(manifold_view.begin(), manifold_view.end(), manifold_view);
}
void narrowphase::update_async(job &completion_job) {
update_contact_distances(*m_registry);
EDYN_ASSERT(parallelizable());
auto manifold_view = m_registry->view<contact_manifold>();
auto body_view = m_registry->view<AABB, shape, position, orientation>();
auto rmesh_view = m_registry->view<rotated_mesh>();
auto cp_view = m_registry->view<contact_point, constraint>();
auto cr_view = m_registry->view<constraint_row_data>();
// Resize result collection vectors to allocate one slot for each iteration
// of the parallel_for.
m_cp_construction_infos.resize(manifold_view.size());
m_cp_destruction_infos.resize(manifold_view.size());
auto &dispatcher = job_dispatcher::global();
parallel_for_async(dispatcher, size_t{0}, manifold_view.size(), size_t{1}, completion_job,
[this, body_view, manifold_view, cp_view, cr_view, rmesh_view] (size_t index) {
auto entity = manifold_view[index];
auto &manifold = manifold_view.get(entity);
collision_result result;
auto &construction_info = m_cp_construction_infos[index];
detect_collision(manifold, result, body_view, rmesh_view);
process_collision(entity, manifold, result, cp_view, cr_view,
[&construction_info] (const collision_result::collision_point &rp) {
construction_info.point[construction_info.count++] = rp;
});
auto &destruction_info = m_cp_destruction_infos[index];
auto [posA, ornA] = body_view.get<position, orientation>(manifold.body[0]);
auto [posB, ornB] = body_view.get<position, orientation>(manifold.body[1]);
prune(manifold, posA, ornA, posB, ornB, cp_view,
[&destruction_info] (entt::entity contact_entity) {
destruction_info.contact_entity[destruction_info.count++] = contact_entity;
});
});
}
void narrowphase::finish_async_update() {
auto manifold_view = m_registry->view<contact_manifold>();
// Create contact points.
for (size_t i = 0; i < manifold_view.size(); ++i) {
auto entity = manifold_view[i];
auto &manifold = manifold_view.get(entity);
auto &info_result = m_cp_construction_infos[i];
for (size_t j = 0; j < info_result.count; ++j) {
create_contact_point(*m_registry, entity, manifold, info_result.point[j]);
}
}
m_cp_construction_infos.clear();
// Destroy contact points.
for (size_t i = 0; i < manifold_view.size(); ++i) {
auto entity = manifold_view[i];
auto &info_result = m_cp_destruction_infos[i];
for (size_t j = 0; j < info_result.count; ++j) {
destroy_contact_point(*m_registry, entity, info_result.contact_entity[j]);
}
}
m_cp_destruction_infos.clear();
}
}