simple physics simulation

[gong.git] / src / physics / sim.cpp

diff --git a/src/physics/sim.cpp b/src/physics/sim.cpp
new file mode 100644 (file)
index 0000000..568d28c
--- /dev/null
+++ b/src/physics/sim.cpp
@@ -0,0 +1,73 @@
+#include "Derivative.hpp"
+#include "Object.hpp"
+#include "Simulation.hpp"
+#include "State.hpp"
+
+
+namespace gong {
+namespace physics {
+
+Derivative::Derivative() noexcept
+: dpos(0.0f)
+, dlin(0.0f)
+, dorient(1.0f, 0.0f, 0.0f, 0.0f)
+, dang(0.0f) {
+
+}
+
+Derivative::Derivative(
+       const glm::vec3 &p,
+       const glm::vec3 &l,
+       const glm::quat &o,
+       const glm::vec3 &a
+) noexcept
+: dpos(p)
+, dlin(l)
+, dorient(o)
+, dang(a) {
+
+}
+
+
+void Simulation::Update(float dt) {
+       // integrate state
+       for (Object *o : objects) {
+               Integrate(o->state, dt);
+       }
+       // TODO: detect collisions
+       // TODO: resolve collisions
+}
+
+void Simulation::Integrate(State &s, float dt) {
+       Derivative a(Evaluate(s, 0.0f, Derivative()));
+       Derivative b(Evaluate(s, dt * 0.5f, a));
+       Derivative c(Evaluate(s, dt * 0.5f, b));
+       Derivative d(Evaluate(s, dt, c));
+       constexpr float by_six = 1.0f / 6.0f;
+       Derivative f((a + (2.0f * (b + c)) + d) * by_six);
+       s += f * dt;
+}
+
+Derivative Simulation::Evaluate(const State &s, float dt, const Derivative &d) {
+       // TODO: calculate linear and torque forces from list of raw forces
+       glm::vec3 force(gravity);
+       glm::vec3 torque(0.0f);
+       return Derivative(
+               s.lin + d.dpos * dt * s.inverse_mass,
+               force,
+               glm::normalize(s.Spin() + d.dorient * dt * s.inverse_inertia),
+               torque
+       );
+}
+
+
+State &operator +=(State &s, const Derivative &d) noexcept {
+       s.pos += d.dpos;
+       s.lin += d.dlin;
+       s.orient *= d.dorient;
+       s.ang += d.dang;
+       return s;
+}
+
+}
+}