src/physics/sim.cpp

   1 #include "Contact.hpp"
   2 #include "Derivative.hpp"
   3 #include "Object.hpp"
   4 #include "Simulation.hpp"
   5 #include "State.hpp"
   6
   7 #include "../geometry/primitive.hpp"
   8
   9
  10 namespace gong {
  11 namespace physics {
  12
  13 Derivative::Derivative() noexcept
  14 : dpos(0.0f)
  15 , dlin(0.0f)
  16 , dorient(1.0f, 0.0f, 0.0f, 0.0f)
  17 , dang(0.0f) {
  18
  19 }
  20
  21 Derivative::Derivative(
  22         const glm::vec3 &p,
  23         const glm::vec3 &l,
  24         const glm::quat &o,
  25         const glm::vec3 &a
  26 ) noexcept
  27 : dpos(p)
  28 , dlin(l)
  29 , dorient(o)
  30 , dang(a) {
  31
  32 }
  33
  34
  35 void Object::TestCollision(Object &other, std::vector<Contact> &contacts) {
  36         if (!Intersection(bounds, other.bounds)) {
  37                 return;
  38         }
  39         // TODO: actual object shape intersection test
  40         ReverseCollisionTest(other, contacts);
  41 }
  42
  43
  44 void Simulation::Update(float dt) {
  45         // integrate state
  46         for (Object *o : objects) {
  47                 Integrate(o->state, dt);
  48         }
  49
  50         // detect collisions
  51         contacts.clear();
  52         if (objects.size() < 2) {
  53                 // we don't handle self-collision, so nothing to do
  54                 return;
  55         }
  56         for (std::size_t i = 0; i < objects.size() - 1; ++i) {
  57                 for (std::size_t j = i + 1; j < objects.size(); ++j) {
  58                         objects[i]->TestCollision(*objects[j], contacts);
  59                 }
  60         }
  61
  62         // TODO: resolve collisions
  63 }
  64
  65 void Simulation::Integrate(State &s, float dt) {
  66         Derivative a(Evaluate(s, 0.0f, Derivative()));
  67         Derivative b(Evaluate(s, dt * 0.5f, a));
  68         Derivative c(Evaluate(s, dt * 0.5f, b));
  69         Derivative d(Evaluate(s, dt, c));
  70         constexpr float by_six = 1.0f / 6.0f;
  71         Derivative f((a + (2.0f * (b + c)) + d) * by_six);
  72         s += f * dt;
  73 }
  74
  75 Derivative Simulation::Evaluate(const State &s, float dt, const Derivative &d) {
  76         // TODO: calculate linear and torque forces from list of raw forces
  77         glm::vec3 force(gravity);
  78         glm::vec3 torque(0.0f);
  79         return Derivative(
  80                 s.lin + d.dpos * dt * s.inverse_mass,
  81                 force,
  82                 glm::normalize(s.Spin() + d.dorient * dt * s.inverse_inertia),
  83                 torque
  84         );
  85 }
  86
  87
  88 State &operator +=(State &s, const Derivative &d) noexcept {
  89         s.pos += d.dpos;
  90         s.lin += d.dlin;
  91         s.orient *= d.dorient;
  92         s.ang += d.dang;
  93         return s;
  94 }
  95
  96 }
  97 }