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/CollisionSystem.hpp"
19 : #include "jobs/JobSystem.hpp"
20 : #include <algorithm>
21 : #include <mutex>
22 : #include <cmath>
23 : #include <unordered_map>
24 :
25 : namespace physics {
26 :
27 : namespace {
28 : constexpr float CELL_SIZE = 2.0f;
29 :
30 2136322 : inline std::pair<int, int> GetCellCoords(float x, float y) {
31 2136322 : return { static_cast<int>(std::floor(x / CELL_SIZE)), static_cast<int>(std::floor(y / CELL_SIZE)) };
32 : }
33 :
34 20660899 : inline uint64_t PackKey(int x, int y) {
35 20660899 : return (static_cast<uint64_t>(x) << 32) | (static_cast<uint32_t>(y));
36 : }
37 :
38 : struct CellEntry {
39 : uint64_t key;
40 : uint32_t index;
41 : // Sort by key, then index for determinism
42 318379408 : bool operator<(const CellEntry& rhs) const {
43 318379408 : if (key != rhs.key) return key < rhs.key;
44 1589011 : return index < rhs.index;
45 : }
46 : };
47 :
48 29223974 : bool GetManifold(const AABBComponent& a, const AABBComponent& b, CollisionEvent& event) {
49 : // Check for overlap
50 29223974 : if (a.maxX < b.minX || a.minX > b.maxX || a.maxY < b.minY || a.minY > b.maxY)
51 27424195 : return false;
52 :
53 1799779 : float x_overlap = std::min(a.maxX, b.maxX) - std::max(a.minX, b.minX);
54 1799779 : float y_overlap = std::min(a.maxY, b.maxY) - std::max(a.minY, b.minY);
55 :
56 1799779 : if (x_overlap < y_overlap) {
57 : // Point from A to B
58 1119123 : event.normalX = (a.minX + a.maxX) < (b.minX + b.maxX) ? 1.0f : -1.0f;
59 1119123 : event.normalY = 0.0f;
60 1119123 : event.penetration = x_overlap;
61 : } else {
62 680656 : event.normalX = 0.0f;
63 680656 : event.normalY = (a.minY + a.maxY) < (b.minY + b.maxY) ? 1.0f : -1.0f;
64 680656 : event.penetration = y_overlap;
65 : }
66 1799779 : return true;
67 : }
68 : }
69 :
70 28613 : void CollisionSystem::Detect(const std::vector<AABBComponent>& aabbs,
71 : const std::vector<std::uint32_t>& entityIds,
72 : std::vector<CollisionEvent>& outEvents,
73 : jobs::JobSystem* jobSystem) const {
74 28613 : outEvents.clear();
75 28613 : const std::size_t n = aabbs.size();
76 28613 : if (n < 2 || n != entityIds.size()) return;
77 :
78 : // Use Spatial Hash for large N
79 28581 : if (jobSystem && n > 100) {
80 : // 1. Build Grid Entries (Deterministic & Contiguous)
81 8810 : std::vector<CellEntry> entries;
82 8810 : entries.reserve(n * 4); // Heuristic
83 :
84 912102 : for (std::size_t i = 0; i < n; ++i) {
85 903292 : const auto& box = aabbs[i];
86 903292 : auto [minX, minY] = GetCellCoords(box.minX, box.minY);
87 903292 : auto [maxX, maxY] = GetCellCoords(box.maxX, box.maxY);
88 :
89 2786463 : for (int x = minX; x <= maxX; ++x) {
90 22214332 : for (int y = minY; y <= maxY; ++y) {
91 20331161 : entries.push_back({PackKey(x, y), static_cast<uint32_t>(i)});
92 : }
93 : }
94 : }
95 :
96 : // 2. Sort entries (Deterministic)
97 8810 : std::sort(entries.begin(), entries.end());
98 :
99 : // 3. Identify Tasks (Cells)
100 : struct GridTask {
101 : uint64_t key;
102 : std::size_t start;
103 : std::size_t count;
104 : };
105 8810 : std::vector<GridTask> tasks;
106 8810 : tasks.reserve(entries.size() / 2); // Rough estimate
107 :
108 8810 : if (!entries.empty()) {
109 8810 : std::size_t currentStart = 0;
110 8810 : uint64_t currentKey = entries[0].key;
111 :
112 20331161 : for (std::size_t i = 1; i < entries.size(); ++i) {
113 20322351 : if (entries[i].key != currentKey) {
114 19471697 : if (i - currentStart > 1) { // Only cells with > 1 entity
115 465662 : tasks.push_back({currentKey, currentStart, i - currentStart});
116 : }
117 19471697 : currentKey = entries[i].key;
118 19471697 : currentStart = i;
119 : }
120 : }
121 : // Last one
122 8810 : if (entries.size() - currentStart > 1) {
123 4671 : tasks.push_back({currentKey, currentStart, entries.size() - currentStart});
124 : }
125 : }
126 :
127 8810 : if (tasks.empty()) return;
128 :
129 : // 4. Dispatch
130 : // Per-task results to avoid mutex and ensure deterministic merge
131 8810 : std::vector<std::vector<CollisionEvent>> taskResults(tasks.size());
132 :
133 8810 : const std::size_t batchSize = std::max(std::size_t(16), tasks.size() / (jobSystem->WorkerCount() * 4));
134 :
135 0 : auto handles = jobSystem->Dispatch(tasks.size(), batchSize, [&](std::size_t start, std::size_t end) {
136 33600 : CollisionEvent event;
137 503933 : for (std::size_t t = start; t < end; ++t) {
138 470333 : const auto& task = tasks[t];
139 470333 : auto& results = taskResults[t];
140 :
141 : // Check all pairs in this cell
142 : // entries[task.start ... task.start + task.count] contains indices
143 1791320 : for (std::size_t i = 0; i < task.count; ++i) {
144 2876342 : for (std::size_t j = i + 1; j < task.count; ++j) {
145 1555355 : uint32_t idxA = entries[task.start + i].index;
146 1555355 : uint32_t idxB = entries[task.start + j].index;
147 :
148 1555355 : const auto& boxA = aabbs[idxA];
149 1555355 : const auto& boxB = aabbs[idxB];
150 :
151 : // Fast overlap check
152 1555355 : if (boxA.maxX < boxB.minX || boxA.minX > boxB.maxX ||
153 804349 : boxA.maxY < boxB.minY || boxA.minY > boxB.maxY)
154 1225617 : continue;
155 :
156 : // Primary cell check
157 329738 : float interMinX = std::max(boxA.minX, boxB.minX);
158 329738 : float interMinY = std::max(boxA.minY, boxB.minY);
159 :
160 329738 : auto [cx, cy] = GetCellCoords(interMinX, interMinY);
161 329738 : if (PackKey(cx, cy) == task.key) {
162 215608 : if (GetManifold(boxA, boxB, event)) {
163 215608 : event.entityA = entityIds[idxA];
164 215608 : event.entityB = entityIds[idxB];
165 215608 : results.push_back(event);
166 : }
167 : }
168 : }
169 : }
170 : }
171 42410 : });
172 :
173 8810 : jobSystem->Wait(handles);
174 :
175 : // 5. Merge Results (Deterministic Order)
176 479143 : for (const auto& res : taskResults) {
177 470333 : outEvents.insert(outEvents.end(), res.begin(), res.end());
178 : }
179 :
180 17620 : } else {
181 : // Serial O(N^2) execution
182 19771 : CollisionEvent event;
183 1094550 : for (std::size_t i = 0; i < n; ++i) {
184 30083145 : for (std::size_t j = i + 1; j < n; ++j) {
185 29008366 : if (GetManifold(aabbs[i], aabbs[j], event)) {
186 1584171 : event.entityA = entityIds[i];
187 1584171 : event.entityB = entityIds[j];
188 1584171 : outEvents.push_back(event);
189 : }
190 : }
191 : }
192 : }
193 : }
194 :
195 : }
|
