src/physics/sim.cpp

   1 #include "Derivative.hpp"
   2 #include "Object.hpp"
   3 #include "Simulation.hpp"
   4 #include "State.hpp"
   5
   6
   7 namespace gong {
   8 namespace physics {
   9
  10 Derivative::Derivative() noexcept
  11 : dpos(0.0f)
  12 , dlin(0.0f)
  13 , dorient(1.0f, 0.0f, 0.0f, 0.0f)
  14 , dang(0.0f) {
  15
  16 }
  17
  18 Derivative::Derivative(
  19         const glm::vec3 &p,
  20         const glm::vec3 &l,
  21         const glm::quat &o,
  22         const glm::vec3 &a
  23 ) noexcept
  24 : dpos(p)
  25 , dlin(l)
  26 , dorient(o)
  27 , dang(a) {
  28
  29 }
  30
  31
  32 void Simulation::Update(float dt) {
  33         // integrate state
  34         for (Object *o : objects) {
  35                 Integrate(o->state, dt);
  36         }
  37         // TODO: detect collisions
  38         // TODO: resolve collisions
  39 }
  40
  41 void Simulation::Integrate(State &s, float dt) {
  42         Derivative a(Evaluate(s, 0.0f, Derivative()));
  43         Derivative b(Evaluate(s, dt * 0.5f, a));
  44         Derivative c(Evaluate(s, dt * 0.5f, b));
  45         Derivative d(Evaluate(s, dt, c));
  46         constexpr float by_six = 1.0f / 6.0f;
  47         Derivative f((a + (2.0f * (b + c)) + d) * by_six);
  48         s += f * dt;
  49 }
  50
  51 Derivative Simulation::Evaluate(const State &s, float dt, const Derivative &d) {
  52         // TODO: calculate linear and torque forces from list of raw forces
  53         glm::vec3 force(gravity);
  54         glm::vec3 torque(0.0f);
  55         return Derivative(
  56                 s.lin + d.dpos * dt * s.inverse_mass,
  57                 force,
  58                 glm::normalize(s.Spin() + d.dorient * dt * s.inverse_inertia),
  59                 torque
  60         );
  61 }
  62
  63
  64 State &operator +=(State &s, const Derivative &d) noexcept {
  65         s.pos += d.dpos;
  66         s.lin += d.dlin;
  67         s.orient *= d.dorient;
  68         s.ang += d.dang;
  69         return s;
  70 }
  71
  72 }
  73 }