Line data Source code
1 : /*
2 : * Copyright (C) 2025 aeml
3 : *
4 : * This program is free software: you can redistribute it and/or modify
5 : * it under the terms of the GNU General Public License as published by
6 : * the Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This program is distributed in the hope that it will be useful,
10 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : * GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License
15 : * along with this program. If not, see <https://www.gnu.org/licenses/>.
16 : */
17 :
18 : #include "physics/Systems.hpp"
19 : #include "jobs/JobSystem.hpp"
20 : #include <algorithm>
21 : #include <cmath>
22 :
23 : namespace physics
24 : {
25 : namespace
26 : {
27 : struct Contact
28 : {
29 : RigidBodyComponent* bA;
30 : RigidBodyComponent* bB;
31 : TransformComponent* tA;
32 : TransformComponent* tB;
33 : ecs::EntityId entityA;
34 : ecs::EntityId entityB;
35 : float leverA{0.0f};
36 : float leverB{0.0f};
37 : float nx;
38 : float ny;
39 : float pen;
40 : float restitution;
41 : float friction;
42 : float invMassSum;
43 : };
44 :
45 : constexpr float kContactEpsilon = 1e-6f;
46 :
47 2736940 : float EstimateLever(const CircleColliderComponent* circle, const AABBComponent* aabb)
48 : {
49 2736940 : if (circle)
50 : {
51 302556 : return std::max(circle->radius, 0.0f);
52 : }
53 2434384 : if (aabb)
54 : {
55 2434384 : const float w = aabb->maxX - aabb->minX;
56 2434384 : const float h = aabb->maxY - aabb->minY;
57 2434384 : return 0.5f * std::sqrt(std::max(0.0f, w * w + h * h));
58 : }
59 0 : return 0.0f;
60 : }
61 :
62 400896 : bool CircleCircleManifold(const TransformComponent& tA,
63 : const CircleColliderComponent& cA,
64 : const TransformComponent& tB,
65 : const CircleColliderComponent& cB,
66 : float& nx,
67 : float& ny,
68 : float& penetration)
69 : {
70 400896 : const float ax = tA.x + cA.offsetX;
71 400896 : const float ay = tA.y + cA.offsetY;
72 400896 : const float bx = tB.x + cB.offsetX;
73 400896 : const float by = tB.y + cB.offsetY;
74 :
75 : // Vector from A to B (Normal direction for solver)
76 400896 : const float dx = bx - ax;
77 400896 : const float dy = by - ay;
78 400896 : const float radiusA = std::max(cA.radius, 0.0f);
79 400896 : const float radiusB = std::max(cB.radius, 0.0f);
80 400896 : const float radii = radiusA + radiusB;
81 400896 : if (radii <= 0.0f) return false;
82 :
83 400896 : const float distSq = dx * dx + dy * dy;
84 400896 : if (distSq <= kContactEpsilon)
85 : {
86 0 : penetration = radii;
87 : // Arbitrary normal if centers coincide
88 0 : nx = 0.0f;
89 0 : ny = 1.0f;
90 0 : return true;
91 : }
92 :
93 400896 : const float dist = std::sqrt(distSq);
94 400896 : if (dist >= radii) return false;
95 :
96 127220 : nx = dx / dist;
97 127220 : ny = dy / dist;
98 127220 : penetration = radii - dist;
99 127220 : return penetration > 0.0f;
100 : }
101 :
102 50212 : bool CircleAabbManifold(const TransformComponent& tCircle,
103 : const CircleColliderComponent& circle,
104 : const AABBComponent& box,
105 : float& nx,
106 : float& ny,
107 : float& penetration)
108 : {
109 50212 : const float cx = tCircle.x + circle.offsetX;
110 50212 : const float cy = tCircle.y + circle.offsetY;
111 50212 : const float radius = std::max(circle.radius, 0.0f);
112 50212 : if (radius <= 0.0f) return false;
113 :
114 50212 : const float closestX = std::clamp(cx, box.minX, box.maxX);
115 50212 : const float closestY = std::clamp(cy, box.minY, box.maxY);
116 :
117 : // Vector from Circle to Box (Normal direction for solver)
118 50212 : const float dx = closestX - cx;
119 50212 : const float dy = closestY - cy;
120 50212 : const float distSq = dx * dx + dy * dy;
121 :
122 50212 : if (distSq > radius * radius + kContactEpsilon)
123 : {
124 2096 : return false;
125 : }
126 :
127 48116 : if (distSq > kContactEpsilon)
128 : {
129 47994 : const float dist = std::sqrt(distSq);
130 47994 : nx = dx / dist;
131 47994 : ny = dy / dist;
132 47994 : penetration = radius - dist;
133 47994 : return penetration > 0.0f;
134 : }
135 :
136 : // Circle center inside the box. Push out along the closest face.
137 122 : const float left = cx - box.minX;
138 122 : const float right = box.maxX - cx;
139 122 : const float bottom = cy - box.minY;
140 122 : const float top = box.maxY - cy;
141 122 : float minDist = left;
142 :
143 : // Default to pushing Left (towards minX). Solver applies -Normal to A.
144 : // To push Left (-1,0), we need Normal = (1,0).
145 122 : nx = 1.0f;
146 122 : ny = 0.0f;
147 :
148 122 : if (right < minDist) { minDist = right; nx = -1.0f; ny = 0.0f; }
149 122 : if (bottom < minDist) { minDist = bottom; nx = 0.0f; ny = 1.0f; }
150 122 : if (top < minDist) { minDist = top; nx = 0.0f; ny = -1.0f; }
151 :
152 : // Penetration is radius + distance to edge (to clear the edge)
153 122 : penetration = radius + minDist;
154 122 : return true;
155 : }
156 :
157 56944 : std::vector<Contact> GatherContacts(const std::vector<CollisionEvent>& events, ecs::World& world)
158 : {
159 56944 : auto* tfStorage = world.GetStorage<TransformComponent>();
160 56944 : auto* rbStorage = world.GetStorage<RigidBodyComponent>();
161 56944 : auto* aabbStorage = world.GetStorage<AABBComponent>();
162 56944 : auto* circleStorage = world.GetStorage<CircleColliderComponent>();
163 :
164 56944 : std::vector<Contact> contacts;
165 56944 : if (!tfStorage || !rbStorage) return contacts;
166 :
167 56912 : contacts.reserve(events.size());
168 :
169 3616274 : for (const auto& event : events)
170 : {
171 3559362 : ecs::EntityId idA = event.entityA;
172 3559362 : ecs::EntityId idB = event.entityB;
173 :
174 3559362 : auto* bA = rbStorage->Get(idA);
175 3559362 : auto* bB = rbStorage->Get(idB);
176 3559362 : auto* tA = tfStorage->Get(idA);
177 3559362 : auto* tB = tfStorage->Get(idB);
178 3559362 : auto* aabbA = aabbStorage ? aabbStorage->Get(idA) : nullptr;
179 3559362 : auto* aabbB = aabbStorage ? aabbStorage->Get(idB) : nullptr;
180 :
181 5838638 : if (!bA || !bB || !tA || !tB) continue;
182 :
183 3559362 : float nx = event.normalX;
184 3559362 : float ny = event.normalY;
185 3559362 : float pen = event.penetration;
186 :
187 3559362 : auto* cA = circleStorage ? circleStorage->Get(idA) : nullptr;
188 3559362 : auto* cB = circleStorage ? circleStorage->Get(idB) : nullptr;
189 :
190 3559362 : if (cA && cB)
191 : {
192 400896 : if (!CircleCircleManifold(*tA, *cA, *tB, *cB, nx, ny, pen))
193 : {
194 273676 : continue;
195 : }
196 : }
197 3158466 : else if (cA && !cB)
198 : {
199 0 : if (aabbB)
200 : {
201 0 : if (!CircleAabbManifold(*tA, *cA, *aabbB, nx, ny, pen))
202 : {
203 0 : continue;
204 : }
205 : }
206 : }
207 3158466 : else if (!cA && cB)
208 : {
209 50212 : if (aabbA)
210 : {
211 50212 : if (!CircleAabbManifold(*tB, *cB, *aabbA, nx, ny, pen))
212 : {
213 2096 : continue;
214 : }
215 48116 : nx = -nx;
216 48116 : ny = -ny;
217 : }
218 : }
219 :
220 3283590 : if (pen <= 0.0f) continue;
221 :
222 1368470 : Contact c;
223 1368470 : c.bA = bA;
224 1368470 : c.bB = bB;
225 1368470 : c.tA = tA;
226 1368470 : c.tB = tB;
227 1368470 : c.entityA = idA;
228 1368470 : c.entityB = idB;
229 1368470 : c.leverA = EstimateLever(cA, aabbA);
230 1368470 : c.leverB = EstimateLever(cB, aabbB);
231 1368470 : c.nx = nx;
232 1368470 : c.ny = ny;
233 1368470 : c.pen = pen;
234 1368470 : c.restitution = std::min(bA->restitution, bB->restitution);
235 1368470 : c.friction = std::sqrt(bA->friction * bA->friction + bB->friction * bB->friction);
236 1368470 : c.invMassSum = bA->invMass + bB->invMass;
237 1368470 : if (c.invMassSum == 0.0f) continue;
238 :
239 1280086 : contacts.push_back(c);
240 : }
241 56912 : return contacts;
242 0 : }
243 :
244 : struct IslandBuilder
245 : {
246 2560172 : int Add(ecs::EntityId id)
247 : {
248 2560172 : auto it = indices.find(id);
249 2560172 : if (it != indices.end())
250 : {
251 1273992 : return it->second;
252 : }
253 1286180 : const int idx = static_cast<int>(parent.size());
254 1286180 : indices.emplace(id, idx);
255 1286180 : parent.push_back(idx);
256 1286180 : rank.push_back(0);
257 1286180 : return idx;
258 : }
259 :
260 1280086 : void Unite(ecs::EntityId a, ecs::EntityId b)
261 : {
262 1280086 : const int ia = Add(a);
263 1280086 : const int ib = Add(b);
264 1280086 : int ra = Find(ia);
265 1280086 : int rb = Find(ib);
266 1280086 : if (ra == rb) return;
267 1146218 : if (rank[ra] < rank[rb]) std::swap(ra, rb);
268 1146218 : parent[rb] = ra;
269 1146218 : if (rank[ra] == rank[rb]) ++rank[ra];
270 : }
271 :
272 1280086 : int Root(ecs::EntityId id)
273 : {
274 1280086 : auto it = indices.find(id);
275 1280086 : if (it == indices.end()) return -1;
276 1280086 : return Find(it->second);
277 : }
278 :
279 : private:
280 5940046 : int Find(int idx)
281 : {
282 5940046 : if (parent[idx] != idx)
283 : {
284 2099788 : parent[idx] = Find(parent[idx]);
285 : }
286 5940046 : return parent[idx];
287 : }
288 :
289 : std::unordered_map<ecs::EntityId, int> indices;
290 : std::vector<int> parent;
291 : std::vector<int> rank;
292 : };
293 :
294 48402 : std::vector<std::vector<const Contact*>> BuildIslands(const std::vector<Contact>& contacts)
295 : {
296 48402 : std::vector<std::vector<const Contact*>> islands;
297 48402 : if (contacts.empty())
298 : {
299 0 : return islands;
300 : }
301 :
302 48402 : IslandBuilder builder;
303 1328488 : for (const auto& c : contacts)
304 : {
305 1280086 : builder.Unite(c.entityA, c.entityB);
306 : }
307 :
308 48402 : std::unordered_map<int, std::size_t> rootToIsland;
309 48402 : islands.reserve(contacts.size());
310 1328488 : for (const auto& c : contacts)
311 : {
312 1280086 : const int root = builder.Root(c.entityA);
313 1280086 : if (root < 0) continue;
314 1280086 : auto [it, inserted] = rootToIsland.emplace(root, islands.size());
315 : std::size_t islandIndex;
316 1280086 : if (inserted)
317 : {
318 139962 : islands.emplace_back();
319 139962 : islandIndex = islands.size() - 1;
320 : }
321 : else
322 : {
323 1140124 : islandIndex = it->second;
324 : }
325 1280086 : islands[islandIndex].push_back(&c);
326 : }
327 48402 : return islands;
328 48402 : }
329 :
330 : template <typename Fn>
331 48402 : void ExecuteIslands(const std::vector<std::vector<const Contact*>>& islands,
332 : jobs::JobSystem* jobSystem,
333 : Fn&& fn)
334 : {
335 48402 : if (islands.empty())
336 : {
337 9897 : return;
338 : }
339 :
340 48402 : if (!jobSystem || islands.size() < 2)
341 : {
342 19794 : for (const auto& island : islands)
343 : {
344 9897 : fn(island);
345 : }
346 9897 : return;
347 : }
348 :
349 38505 : const std::size_t workerCount = std::max<std::size_t>(1, jobSystem->WorkerCount());
350 38505 : const std::size_t batch = std::max<std::size_t>(1, islands.size() / (workerCount * 2));
351 291447 : auto handles = jobSystem->Dispatch(islands.size(), batch, [&](std::size_t start, std::size_t end)
352 : {
353 256536 : for (std::size_t i = start; i < end; ++i)
354 : {
355 130065 : fn(islands[i]);
356 : }
357 : });
358 38505 : jobSystem->Wait(handles);
359 38505 : }
360 : }
361 :
362 28472 : void CollisionResolutionSystem::ResolvePosition(const std::vector<CollisionEvent>& events, ecs::World& world, jobs::JobSystem* jobSystem) const
363 : {
364 28472 : auto contacts = GatherContacts(events, world);
365 28472 : if (contacts.empty())
366 : {
367 3808 : return;
368 : }
369 :
370 24664 : const int positionIterations = std::max(1, m_settings.positionIterations);
371 24664 : const float percent = m_settings.correctionPercent;
372 24664 : const float slop = m_settings.penetrationSlop;
373 24664 : const float maxCorrection = m_settings.maxCorrection;
374 24664 : auto islands = BuildIslands(contacts);
375 :
376 83044 : auto solveIsland = [&](const std::vector<const Contact*>& islandContacts)
377 : {
378 1743886 : for (int i = 0; i < positionIterations; ++i)
379 : {
380 14959224 : for (const Contact* contactPtr : islandContacts)
381 : {
382 13298382 : const auto& c = *contactPtr;
383 13298382 : float correction = std::max(c.pen - slop, 0.0f) / c.invMassSum * percent;
384 13298382 : if (correction > maxCorrection) correction = maxCorrection;
385 :
386 13298382 : float cx = correction * c.nx;
387 13298382 : float cy = correction * c.ny;
388 :
389 13298382 : c.tA->x -= cx * c.bA->invMass;
390 13298382 : c.tA->y -= cy * c.bA->invMass;
391 13298382 : c.tB->x += cx * c.bB->invMass;
392 13298382 : c.tB->y += cy * c.bB->invMass;
393 : }
394 : }
395 107708 : };
396 :
397 24664 : ExecuteIslands(islands, jobSystem, solveIsland);
398 28472 : }
399 :
400 28472 : void CollisionResolutionSystem::ResolveVelocity(const std::vector<CollisionEvent>& events, ecs::World& world, jobs::JobSystem* jobSystem) const
401 : {
402 28472 : auto contacts = GatherContacts(events, world);
403 28472 : if (contacts.empty())
404 : {
405 4734 : return;
406 : }
407 :
408 23738 : const int velocityIterations = std::max(1, m_settings.velocityIterations);
409 23738 : auto islands = BuildIslands(contacts);
410 :
411 56918 : auto solveIsland = [&](const std::vector<const Contact*>& islandContacts)
412 : {
413 626080 : for (int i = 0; i < velocityIterations; ++i)
414 : {
415 6720794 : for (const Contact* contactPtr : islandContacts)
416 : {
417 6151632 : const auto& c = *contactPtr;
418 6151632 : float rvx = c.bB->vx - c.bA->vx;
419 6151632 : float rvy = c.bB->vy - c.bA->vy;
420 :
421 6151632 : float velAlongNormal = rvx * c.nx + rvy * c.ny;
422 :
423 6151632 : float j = 0.0f;
424 6151632 : if (velAlongNormal < 0)
425 : {
426 4592976 : j = -(1 + c.restitution) * velAlongNormal;
427 4592976 : j /= c.invMassSum;
428 :
429 4592976 : float impulseX = j * c.nx;
430 4592976 : float impulseY = j * c.ny;
431 :
432 4592976 : c.bA->vx -= impulseX * c.bA->invMass;
433 4592976 : c.bA->vy -= impulseY * c.bA->invMass;
434 4592976 : c.bB->vx += impulseX * c.bB->invMass;
435 4592976 : c.bB->vy += impulseY * c.bB->invMass;
436 : }
437 :
438 6151632 : if (c.pen > -0.05f)
439 : {
440 6151632 : rvx = c.bB->vx - c.bA->vx;
441 6151632 : rvy = c.bB->vy - c.bA->vy;
442 :
443 6151632 : float tx = -c.ny;
444 6151632 : float ty = c.nx;
445 6151632 : float velAlongTangent = rvx * tx + rvy * ty;
446 :
447 6151632 : float jt = -velAlongTangent;
448 6151632 : jt /= c.invMassSum;
449 :
450 6151632 : float maxJt = 0.0f;
451 6151632 : if (j > 0) {
452 4592976 : maxJt = c.friction * j;
453 : } else {
454 1558656 : maxJt = c.friction * 0.1f;
455 : }
456 :
457 6151632 : if (std::abs(jt) > maxJt)
458 : {
459 233408 : jt = (jt > 0) ? maxJt : -maxJt;
460 : }
461 :
462 6151632 : float frictionImpulseX = jt * tx;
463 6151632 : float frictionImpulseY = jt * ty;
464 :
465 6151632 : c.bA->vx -= frictionImpulseX * c.bA->invMass;
466 6151632 : c.bA->vy -= frictionImpulseY * c.bA->invMass;
467 6151632 : c.bB->vx += frictionImpulseX * c.bB->invMass;
468 6151632 : c.bB->vy += frictionImpulseY * c.bB->invMass;
469 :
470 6151632 : if (c.leverA > 0.0f && c.bA->invInertia > 0.0f)
471 : {
472 6151632 : c.bA->angularVelocity -= jt * c.leverA * c.bA->invInertia;
473 : }
474 6151632 : if (c.leverB > 0.0f && c.bB->invInertia > 0.0f)
475 : {
476 6151632 : c.bB->angularVelocity += jt * c.leverB * c.bB->invInertia;
477 : }
478 : }
479 : }
480 : }
481 80656 : };
482 :
483 23738 : ExecuteIslands(islands, jobSystem, solveIsland);
484 28472 : }
485 :
486 0 : void CollisionResolutionSystem::Resolve(const std::vector<CollisionEvent>& events, ecs::World& world) const
487 : {
488 0 : ResolvePosition(events, world, nullptr);
489 0 : ResolveVelocity(events, world, nullptr);
490 0 : }
491 :
492 1 : void CollisionResolutionSystem::Resolve(const std::vector<CollisionEvent>& events,
493 : std::vector<TransformComponent>& transforms,
494 : std::vector<RigidBodyComponent>& bodies) const
495 : {
496 2 : for (const auto& event : events)
497 : {
498 : // Legacy/Test support: assume entityId corresponds to index in the provided vectors
499 1 : size_t idxA = static_cast<size_t>(event.entityA);
500 1 : size_t idxB = static_cast<size_t>(event.entityB);
501 :
502 1 : if (idxA >= transforms.size() || idxB >= transforms.size()) continue;
503 :
504 1 : auto& tA = transforms[idxA];
505 1 : auto& bA = bodies[idxA];
506 1 : auto& tB = transforms[idxB];
507 1 : auto& bB = bodies[idxB];
508 :
509 : // Relative velocity
510 1 : float rvx = bB.vx - bA.vx;
511 1 : float rvy = bB.vy - bA.vy;
512 :
513 : // Velocity along normal
514 1 : float velAlongNormal = rvx * event.normalX + rvy * event.normalY;
515 :
516 : // Do not resolve if velocities are separating
517 1 : if (velAlongNormal > 0)
518 0 : continue;
519 :
520 : // Restitution (min of both)
521 1 : float e = std::min(bA.restitution, bB.restitution);
522 :
523 : // Impulse scalar
524 1 : float j = -(1 + e) * velAlongNormal;
525 1 : j /= (bA.invMass + bB.invMass);
526 :
527 : // Apply impulse
528 1 : float impulseX = j * event.normalX;
529 1 : float impulseY = j * event.normalY;
530 :
531 1 : bA.vx -= impulseX * bA.invMass;
532 1 : bA.vy -= impulseY * bA.invMass;
533 1 : bB.vx += impulseX * bB.invMass;
534 1 : bB.vy += impulseY * bB.invMass;
535 :
536 : // Positional correction (Linear Projection)
537 1 : const float percent = 0.2f;
538 1 : const float slop = 0.01f;
539 1 : float correction = std::max(event.penetration - slop, 0.0f) / (bA.invMass + bB.invMass) * percent;
540 :
541 1 : float cx = correction * event.normalX;
542 1 : float cy = correction * event.normalY;
543 :
544 1 : tA.x -= cx * bA.invMass;
545 1 : tA.y -= cy * bA.invMass;
546 1 : tB.x += cx * bB.invMass;
547 1 : tB.y += cy * bB.invMass;
548 : }
549 1 : }
550 :
551 28604 : void ConstraintResolutionSystem::Resolve(ecs::World& world, float dt) const
552 : {
553 28604 : auto* jointStorage = world.GetStorage<DistanceJointComponent>();
554 28604 : auto* tfStorage = world.GetStorage<TransformComponent>();
555 28604 : auto* rbStorage = world.GetStorage<RigidBodyComponent>();
556 :
557 28604 : if (!jointStorage || !tfStorage || !rbStorage) return;
558 :
559 17328 : const auto& joints = jointStorage->GetData();
560 :
561 : // Pre-resolve pointers
562 : struct Constraint
563 : {
564 : TransformComponent* tA;
565 : TransformComponent* tB;
566 : RigidBodyComponent* bA;
567 : RigidBodyComponent* bB;
568 : float targetDistance;
569 : float invMassSum;
570 : float compliance;
571 : };
572 :
573 17328 : std::vector<Constraint> constraints;
574 17328 : constraints.reserve(joints.size());
575 :
576 138768 : for (const auto& joint : joints)
577 : {
578 121440 : auto* tA = tfStorage->Get(joint.entityA);
579 121440 : auto* tB = tfStorage->Get(joint.entityB);
580 121440 : auto* bA = rbStorage->Get(joint.entityA);
581 121440 : auto* bB = rbStorage->Get(joint.entityB);
582 :
583 121440 : if (!tA || !tB || !bA || !bB) continue;
584 121440 : float invMassSum = bA->invMass + bB->invMass;
585 121440 : if (invMassSum == 0.0f) continue;
586 :
587 121440 : constraints.push_back({tA, tB, bA, bB, joint.targetDistance, invMassSum, joint.compliance});
588 : }
589 :
590 17328 : const int iterations = std::max(1, m_iterations);
591 17328 : const float dtSafe = std::max(dt, 1e-4f);
592 294576 : for (int iter = 0; iter < iterations; ++iter)
593 : {
594 2220288 : for (const auto& c : constraints)
595 : {
596 1943040 : float dx = c.tB->x - c.tA->x;
597 1943040 : float dy = c.tB->y - c.tA->y;
598 1943040 : float dist = std::sqrt(dx*dx + dy*dy);
599 1943040 : if (dist < 0.0001f) continue;
600 :
601 1943040 : float diff = dist - c.targetDistance;
602 1943040 : float complianceTerm = (c.compliance > 0.0f) ? (c.compliance / (dtSafe * dtSafe)) : 0.0f;
603 1943040 : float denom = c.invMassSum + complianceTerm;
604 1943040 : if (denom <= 0.0f) continue;
605 1943040 : float correction = diff / denom;
606 :
607 1943040 : float px = (dx / dist) * correction;
608 1943040 : float py = (dy / dist) * correction;
609 :
610 1943040 : c.tA->x += px * c.bA->invMass;
611 1943040 : c.tA->y += py * c.bA->invMass;
612 1943040 : c.tB->x -= px * c.bB->invMass;
613 1943040 : c.tB->y -= py * c.bB->invMass;
614 : }
615 : }
616 17328 : }
617 :
618 : }
|
