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 :
20 : #include <algorithm>
21 : #include <cmath>
22 :
23 : namespace physics
24 : {
25 : namespace
26 : {
27 28604 : void SyncDynamicAabbsToTransforms(ecs::World& world)
28 : {
29 28604 : auto* aabbStorage = world.GetStorage<AABBComponent>();
30 28604 : auto* tfStorage = world.GetStorage<TransformComponent>();
31 28604 : auto* rbStorage = world.GetStorage<RigidBodyComponent>();
32 28604 : if (!aabbStorage || !tfStorage || !rbStorage)
33 : {
34 4497 : return;
35 : }
36 :
37 24107 : auto& aabbs = aabbStorage->GetData();
38 24107 : const auto& entities = aabbStorage->GetEntities();
39 24107 : const size_t count = aabbs.size();
40 735040 : for (size_t i = 0; i < count; ++i)
41 : {
42 710933 : const ecs::EntityId id = entities[i];
43 710933 : auto* rb = rbStorage->Get(id);
44 710933 : auto* tf = tfStorage->Get(id);
45 710933 : if (!rb || rb->invMass == 0.0f || !tf)
46 : {
47 70161 : continue;
48 : }
49 :
50 640772 : auto& aabb = aabbs[i];
51 640772 : const float halfW = std::max(0.0f, (aabb.maxX - aabb.minX) * 0.5f);
52 640772 : const float halfH = std::max(0.0f, (aabb.maxY - aabb.minY) * 0.5f);
53 640772 : aabb.minX = tf->x - halfW;
54 640772 : aabb.maxX = tf->x + halfW;
55 640772 : aabb.minY = tf->y - halfH;
56 640772 : aabb.maxY = tf->y + halfH;
57 : }
58 : }
59 : }
60 :
61 41 : PhysicsSystem::PhysicsSystem()
62 : {
63 41 : ApplySettings();
64 41 : }
65 :
66 2492 : void PhysicsSystem::Update(ecs::World& world, float dt)
67 : {
68 2492 : if (!std::isfinite(dt) || dt < 0.0f)
69 : {
70 0 : return;
71 : }
72 :
73 2492 : const int substeps = std::max(1, m_settings.substeps);
74 2492 : const float subDt = dt / static_cast<float>(substeps);
75 :
76 31096 : for (int i = 0; i < substeps; ++i)
77 : {
78 28604 : m_integration.Update(world, subDt);
79 28604 : SyncDynamicAabbsToTransforms(world);
80 :
81 28604 : m_events.clear();
82 28604 : m_broadphaseAABBs.clear();
83 28604 : m_broadphaseIds.clear();
84 :
85 28604 : auto* aabbStorage = world.GetStorage<AABBComponent>();
86 28604 : if (aabbStorage)
87 : {
88 24123 : const auto& aabbs = aabbStorage->GetData();
89 24123 : const auto& entities = aabbStorage->GetEntities();
90 :
91 24123 : size_t count = aabbs.size();
92 24123 : m_broadphaseAABBs.reserve(count);
93 24123 : m_broadphaseIds.reserve(count);
94 :
95 24123 : m_broadphaseAABBs.insert(m_broadphaseAABBs.end(), aabbs.begin(), aabbs.end());
96 24123 : m_broadphaseIds.insert(m_broadphaseIds.end(), entities.begin(), entities.end());
97 : }
98 :
99 28604 : auto* circleStorage = world.GetStorage<CircleColliderComponent>();
100 28604 : auto* tfStorage = world.GetStorage<TransformComponent>();
101 28604 : if (circleStorage && tfStorage)
102 : {
103 28538 : const auto& circles = circleStorage->GetData();
104 28538 : const auto& entities = circleStorage->GetEntities();
105 28538 : const size_t count = circles.size();
106 :
107 28538 : m_broadphaseAABBs.reserve(m_broadphaseAABBs.size() + count);
108 28538 : m_broadphaseIds.reserve(m_broadphaseIds.size() + count);
109 :
110 1414461 : for (size_t j = 0; j < count; ++j)
111 : {
112 1385923 : const ecs::EntityId id = entities[j];
113 1385923 : if (aabbStorage && aabbStorage->Get(id))
114 : {
115 120000 : continue;
116 : }
117 :
118 1265923 : auto* tf = tfStorage->Get(id);
119 1265923 : if (!tf)
120 : {
121 0 : continue;
122 : }
123 :
124 1265923 : const auto& circle = circles[j];
125 1265923 : const float radius = std::max(0.0f, circle.radius);
126 1265923 : const float cx = tf->x + circle.offsetX;
127 1265923 : const float cy = tf->y + circle.offsetY;
128 1265923 : m_broadphaseAABBs.push_back({cx - radius, cy - radius, cx + radius, cy + radius});
129 1265923 : m_broadphaseIds.push_back(id);
130 : }
131 : }
132 :
133 28604 : if (!m_broadphaseAABBs.empty())
134 : {
135 28604 : m_collision.Detect(m_broadphaseAABBs, m_broadphaseIds, m_events, m_jobSystem);
136 : }
137 :
138 28604 : if (!m_events.empty()) {
139 28472 : m_resolution.ResolvePosition(m_events, world, m_jobSystem);
140 : }
141 :
142 28604 : m_constraints.Resolve(world, subDt);
143 28604 : m_integration.UpdateVelocities(world, subDt);
144 :
145 28604 : if (!m_events.empty()) {
146 28472 : m_resolution.ResolveVelocity(m_events, world, m_jobSystem);
147 : }
148 : }
149 : }
150 :
151 38 : void PhysicsSystem::SetSettings(const PhysicsSettings& settings)
152 : {
153 38 : m_settings = settings;
154 38 : ApplySettings();
155 38 : }
156 :
157 79 : void PhysicsSystem::ApplySettings()
158 : {
159 79 : CollisionResolutionSystem::SolverSettings solver{};
160 79 : solver.positionIterations = m_settings.positionIterations;
161 79 : solver.velocityIterations = m_settings.velocityIterations;
162 79 : solver.penetrationSlop = m_settings.penetrationSlop;
163 79 : solver.correctionPercent = m_settings.correctionPercent;
164 79 : solver.maxCorrection = m_settings.maxPositionCorrection;
165 79 : m_resolution.SetSolverSettings(solver);
166 79 : m_constraints.SetIterationCount(m_settings.constraintIterations);
167 79 : }
168 : }
|
