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 "jobs/JobSystem.hpp"
21 :
22 : #include <algorithm>
23 : #include <cmath>
24 :
25 : namespace physics
26 : {
27 : namespace
28 : {
29 341 : inline void EnsureDerivedMass(RigidBodyComponent& body)
30 : {
31 341 : if (body.mass <= 0.0f)
32 : {
33 0 : body.invMass = 0.0f;
34 0 : body.inertia = 0.0f;
35 0 : body.invInertia = 0.0f;
36 0 : return;
37 : }
38 :
39 341 : if (body.invMass <= 0.0f)
40 : {
41 0 : body.invMass = 1.0f / body.mass;
42 : }
43 :
44 341 : if (body.inertia <= 0.0f)
45 : {
46 0 : body.inertia = 0.5f * body.mass;
47 : }
48 :
49 341 : if (body.invInertia <= 0.0f && body.inertia > 0.0f)
50 : {
51 0 : body.invInertia = 1.0f / body.inertia;
52 : }
53 : }
54 : }
55 :
56 28605 : void PhysicsIntegrationSystem::Update(ecs::World& world, float dt)
57 : {
58 28605 : auto* rbStorage = world.GetStorage<RigidBodyComponent>();
59 28605 : auto* tfStorage = world.GetStorage<TransformComponent>();
60 28605 : if (!rbStorage || !tfStorage) return;
61 :
62 28589 : auto& bodies = rbStorage->GetData();
63 28589 : const auto& entities = rbStorage->GetEntities();
64 28589 : size_t count = bodies.size();
65 :
66 28604 : auto integrateRange = [&](size_t start, size_t end) {
67 2023788 : for (size_t i = start; i < end; ++i) {
68 1995184 : ecs::EntityId id = entities[i];
69 1995184 : TransformComponent* tf = tfStorage->Get(id);
70 1995184 : if (tf) {
71 1995184 : auto& b = bodies[i];
72 1995184 : if (b.invMass == 0.0f && b.mass > 0.0f) {
73 0 : EnsureDerivedMass(b);
74 : }
75 1995184 : if (b.invMass == 0.0f) {
76 91969 : b.angularVelocity = 0.0f;
77 91969 : b.torque = 0.0f;
78 91969 : continue;
79 : }
80 :
81 1903215 : b.lastX = tf->x;
82 1903215 : b.lastY = tf->y;
83 1903215 : b.lastAngle = tf->rotation;
84 :
85 1903215 : const float ax = m_env.windX - m_env.drag * b.vx;
86 1903215 : const float ay = m_env.gravityY + m_env.windY - m_env.drag * b.vy;
87 :
88 1903215 : b.vx += ax * dt;
89 1903215 : b.vy += ay * dt;
90 :
91 1903215 : const float maxVel = 50.0f;
92 1903215 : float vSq = b.vx * b.vx + b.vy * b.vy;
93 1903215 : if (vSq > maxVel * maxVel) {
94 102862 : float v = std::sqrt(vSq);
95 102862 : b.vx = (b.vx / v) * maxVel;
96 102862 : b.vy = (b.vy / v) * maxVel;
97 : }
98 :
99 1903215 : tf->x += b.vx * dt;
100 1903215 : tf->y += b.vy * dt;
101 :
102 1903215 : if (b.invInertia == 0.0f && b.inertia > 0.0f)
103 : {
104 0 : b.invInertia = 1.0f / b.inertia;
105 : }
106 1903215 : if (b.invInertia > 0.0f)
107 : {
108 1903215 : float angularAccel = b.torque * b.invInertia - b.angularDrag * b.angularVelocity;
109 1903215 : b.angularVelocity += angularAccel * dt;
110 1903215 : const float damping = std::max(0.0f, 1.0f - b.angularFriction * dt);
111 1903215 : b.angularVelocity *= damping;
112 1903215 : tf->rotation += b.angularVelocity * dt;
113 : }
114 : else
115 : {
116 0 : b.angularVelocity = 0.0f;
117 : }
118 1903215 : b.torque = 0.0f;
119 :
120 : }
121 : }
122 57193 : };
123 :
124 28589 : if (m_jobSystem && count > 256) {
125 1 : const std::size_t batchSize = std::max(std::size_t(64), count / (m_jobSystem->WorkerCount() * 4));
126 1 : auto handles = m_jobSystem->Dispatch(count, batchSize, integrateRange);
127 1 : m_jobSystem->Wait(handles);
128 1 : } else {
129 28588 : integrateRange(0, count);
130 : }
131 : }
132 :
133 22 : void PhysicsIntegrationSystem::Integrate(std::vector<TransformComponent>& transforms,
134 : std::vector<RigidBodyComponent>& bodies,
135 : float dt) const
136 : {
137 22 : const std::size_t count = std::min(transforms.size(), bodies.size());
138 :
139 26 : auto integrateRange = [&](std::size_t start, std::size_t end)
140 : {
141 367 : for (std::size_t i = start; i < end; ++i)
142 : {
143 341 : auto& t = transforms[i];
144 341 : auto& b = bodies[i];
145 341 : EnsureDerivedMass(b);
146 :
147 341 : const float ax = m_env.windX - m_env.drag * b.vx;
148 341 : const float ay = m_env.gravityY + m_env.windY - m_env.drag * b.vy;
149 :
150 341 : b.vx += ax * dt;
151 341 : b.vy += ay * dt;
152 :
153 341 : const float maxVel = 50.0f;
154 341 : float vSq = b.vx * b.vx + b.vy * b.vy;
155 341 : if (vSq > maxVel * maxVel) {
156 0 : float v = std::sqrt(vSq);
157 0 : b.vx = (b.vx / v) * maxVel;
158 0 : b.vy = (b.vy / v) * maxVel;
159 : }
160 :
161 341 : t.x += b.vx * dt;
162 341 : t.y += b.vy * dt;
163 :
164 341 : if (b.invInertia == 0.0f && b.inertia > 0.0f)
165 : {
166 0 : b.invInertia = 1.0f / b.inertia;
167 : }
168 341 : if (b.invInertia > 0.0f)
169 : {
170 341 : float angularAccel = b.torque * b.invInertia - b.angularDrag * b.angularVelocity;
171 341 : b.angularVelocity += angularAccel * dt;
172 341 : const float damping = std::max(0.0f, 1.0f - b.angularFriction * dt);
173 341 : b.angularVelocity *= damping;
174 341 : t.rotation += b.angularVelocity * dt;
175 : }
176 : else
177 : {
178 0 : b.angularVelocity = 0.0f;
179 : }
180 341 : b.torque = 0.0f;
181 : }
182 48 : };
183 :
184 22 : if (m_jobSystem && count > 256)
185 : {
186 1 : const std::size_t batchSize = std::max(std::size_t(64), count / (m_jobSystem->WorkerCount() * 4));
187 1 : auto handles = m_jobSystem->Dispatch(count, batchSize, integrateRange);
188 1 : m_jobSystem->Wait(handles);
189 1 : }
190 : else
191 : {
192 21 : integrateRange(0, count);
193 : }
194 22 : }
195 :
196 28604 : void PhysicsIntegrationSystem::UpdateVelocities(ecs::World& world, float dt)
197 : {
198 28604 : if (!std::isfinite(dt) || dt <= 0.0f)
199 : {
200 32 : return;
201 : }
202 :
203 28572 : auto* rbStorage = world.GetStorage<RigidBodyComponent>();
204 28572 : auto* tfStorage = world.GetStorage<TransformComponent>();
205 28572 : if (!rbStorage || !tfStorage) return;
206 :
207 28572 : auto& bodies = rbStorage->GetData();
208 28572 : const auto& entities = rbStorage->GetEntities();
209 28572 : size_t count = bodies.size();
210 :
211 2022740 : for (size_t i = 0; i < count; ++i) {
212 1994168 : ecs::EntityId id = entities[i];
213 1994168 : TransformComponent* tf = tfStorage->Get(id);
214 1994168 : if (tf) {
215 1994168 : auto& b = bodies[i];
216 1994168 : if (b.invMass == 0.0f) continue;
217 :
218 1902199 : b.vx = (tf->x - b.lastX) / dt;
219 1902199 : b.vy = (tf->y - b.lastY) / dt;
220 1902199 : b.angularVelocity = (tf->rotation - b.lastAngle) / dt;
221 :
222 1902199 : const float maxVel = 50.0f;
223 1902199 : float vSq = b.vx * b.vx + b.vy * b.vy;
224 1902199 : if (vSq > maxVel * maxVel) {
225 184435 : float v = std::sqrt(vSq);
226 184435 : b.vx = (b.vx / v) * maxVel;
227 184435 : b.vy = (b.vy / v) * maxVel;
228 : }
229 : }
230 : }
231 : }
232 : }
|
