Physics.hpp

//
// Created by jibbo on 4/3/21.
//
 
#ifndef ENGINE_PROJ_PHYSICS_HPP
#define ENGINE_PROJ_PHYSICS_HPP
 
#include "Components.hpp"
#include "Debug.hpp"
#include <pch.hpp>
#include "ECS.hpp"
#include "Time.hpp"
#include "PhysicsParam.hpp"
 
namespace Engine {
  struct Physics {
 
    enum CollisionType {
      D2, D3
    };
 
 
    //<editor-fold desc="Raycasting">
    static std::vector<CollisionPoint> Raycast(const Scene& scene, glm::vec3 pos, glm::vec3 dir, float maxDistance = FLT_MAX) {
 
      std::vector<CollisionPoint> points;
//      ECS ecs = scene.GetECS();
//      Shape shape = {glm::mat4{1}, CreateRef<VertexCollider>(std::vector<glm::vec3>{pos, pos + dir * maxDistance})};
//
//      for (auto id : ecs.GetEntitiesWith(ecs.GetArchetype<Transform, Collider>())) {
//        Ref<Transform> tf = ecs.GetComponent<Transform>(id);
//        Ref<Collider> c = ecs.GetComponent<Collider>(id);
//        CollisionPoint cp = GJK_EPA(shape, Shape{tf->GetMatrix(), c});
//
//        if (cp.HasCollision) {
//          points.emplace_back(cp);
//        }
//      }
 
      return points;
    }
 
    static CollisionPoint RaycastClosest(const Scene& scene, glm::vec3 pos, glm::vec3 dir, float maxDistance = FLT_MAX) {
 
      std::vector<CollisionPoint> points = Raycast(scene, pos, dir, maxDistance);
      CollisionPoint collisionPoint = {{}, {}, 0, false};
      float penetration = 0;
      for (const auto& point : points) {
        if (point.PenetrationDepth > penetration) {
          collisionPoint = point;
          penetration = point.PenetrationDepth;
        }
      }
      return collisionPoint;
    }
    //</editor-fold>
 
    //<editor-fold desc="GJK & EPA Methods">
 
    static CollisionPoint GJK_EPA(const Shape &colliderA, const Shape &colliderB, CollisionType type = D3) {
      Simplex simplex;
      if (GJK(colliderA, colliderB, simplex, type)) {
        if (type == D2) {
          return CollisionPoint{{}, {}, 0, true};
        }
        return EPA2(simplex, colliderA, colliderB);
      }
      return CollisionPoint{{}, {}, 0, false};
    }
 
 
    static CollisionPoint EPA2(Simplex &simplex, const Shape &shapeA, const Shape &shapeB) {
      Support current{}; // storage for new simplex points
      Support temp{}; // storage for swapping simplex points
      Support current_edge[2]; // storage for the current edge used to remove other edges
      Support l_edges[EPA_MAX_NUM_LOOSE_EDGES][2]; // keep track of edges we need to fix after removing faces
      FacePoint faces[EPA_MAX_NUM_FACES]; // Array of faces, each with 3 verts and a normal
 
      int num_faces = 4; // current number of faces
      int c_idx; // index of the current closest face
      int num_loose_edges; // number of current loose edges to delete
 
      glm::vec3 search_dir; // current smallest face normal vector. (normal vector that minimizes the simplex point)
      float u, v, w; // barycentric face coordinates
      float n_dist; // normal distance, distance to a face along its normal
      bool found_edge; // edge flag
      float min_dist; // minimum normal face distance of the epa
 
      //Init faces simplex with GJK values
      faces[0] = {simplex[0], simplex[1], simplex[2]};
      faces[0].normal = normalize(cross(simplex[1] - simplex[0], simplex[2] - simplex[0])); //ABC
      faces[1] = {simplex[0], simplex[2], simplex[3]};
      faces[1].normal = normalize(cross(simplex[2] - simplex[0], simplex[3] - simplex[0])); //ACD
      faces[2] = {simplex[0], simplex[3], simplex[1]};
      faces[2].normal = normalize(cross(simplex[3] - simplex[0], simplex[1] - simplex[0])); //ADB
      faces[3] = {simplex[1], simplex[3], simplex[2]};
      faces[3].normal = normalize(cross(simplex[3] - simplex[1], simplex[2] - simplex[1])); //BDC
 
 
      for (int iterations = 0; iterations < EPA_MAX_NUM_ITERATIONS; iterations++) {
        // pre load the minimum distance to the first value and index
        min_dist = dot(faces[0][0], faces[0].normal);
        c_idx = 0;
 
        // find face that's closest to origin and the distance to it
        for (int i = 1; i < num_faces; i++) {
          n_dist = dot(faces[i][0], faces[i].normal);
          if (n_dist < min_dist) {
            min_dist = n_dist;
            c_idx = i;
          }
        }
 
        search_dir = faces[c_idx].normal; // normal of the closest face
        furthestDiff(shapeA, shapeB, search_dir, current); // use the closest normal to calculate a new support point
        n_dist = dot(current, search_dir); // calculate the new normal distance.
 
        // Convergence (new point is not significantly further from origin)
        if (n_dist - min_dist < EPA_TOLERANCE) {
          // convert barycentric coordinates to world coordinates
          Barycentric({0, 0, 0}, faces[c_idx][0], faces[c_idx][1], faces[c_idx][2], u, v, w);
          glm::vec3 v1 = u * faces[c_idx][0].supportA + v * faces[c_idx][1].supportA + w * faces[c_idx][2].supportA;
          glm::vec3 v2 = u * faces[c_idx][0].supportB + v * faces[c_idx][1].supportB + w * faces[c_idx][2].supportB;
 
          return {search_dir, {v1, v2}, n_dist, true};
        }
 
        num_loose_edges = 0; // reset loose edge count
 
        // Find all triangles that are facing the current face and remove them
        for (int i = 0; i < num_faces; i++) {
          if (dot(faces[i].normal, current - faces[i][0]) > 0) {
            // triangle i faces p and falls before it on the normal, remove it
 
            // Add removed triangle's edges to loose edge list.
            // If it's already there, remove it (both edges it belonged to are gone)
            for (int j = 0; j < FACE_VERTS; j++) {
              current_edge[0] = faces[i][j]; // obtain an edge from a face
              current_edge[1] = faces[i][(j + 1) % 3]; // mod with size for looping effect
              found_edge = false;
 
              // Check if current edge is already in list
              for (int k = 0; k < num_loose_edges; k++) {
                if (l_edges[k][1] == current_edge[0] && l_edges[k][0] == current_edge[1]) {
                  //Edge is already in the list, remove it
                  //THIS ASSUMES EDGE CAN ONLY BE SHARED BY 2 TRIANGLES (which should be true)
                  //THIS ALSO ASSUMES SHARED EDGE WILL BE REVERSED IN THE TRIANGLES (which
                  //should be true provided every triangle is wound CCW)
                  l_edges[k][0] = l_edges[num_loose_edges - 1][0]; // Overwrite current edge
                  l_edges[k][1] = l_edges[num_loose_edges - 1][1]; // with last edge in list
                  num_loose_edges--;
                  found_edge = true;
                  k = num_loose_edges; // exit loop because edge can only be shared once
                }
              }
 
              // add current edge to list
              if (!found_edge) {
 
                // assert(num_loose_edges<EPA_MAX_NUM_LOOSE_EDGES);
                if (num_loose_edges >= EPA_MAX_NUM_LOOSE_EDGES) break;
 
                l_edges[num_loose_edges][0] = current_edge[0];
                l_edges[num_loose_edges][1] = current_edge[1];
                num_loose_edges++;
              }
            }
 
            // remove triangle i from list
            faces[i] = faces[num_faces - 1];
            num_faces--;
            i--;
          }
        }
 
        // Reconstruct polytope with p added
        // Make triangles out of all the loose edges of the polytope connecting all of them to the new point (current)
        //  making a cone with the point being (current) and the loose edges making up the base.
        for (int i = 0; i < num_loose_edges; i++) {
 
          // assert(num_faces<EPA_MAX_NUM_FACES);
          if (num_faces >= EPA_MAX_NUM_FACES) break;
 
          // make a new face with the added point
          faces[num_faces][0] = l_edges[i][0];
          faces[num_faces][1] = l_edges[i][1];
          faces[num_faces][2] = current;
          faces[num_faces].normal = normalize(cross(l_edges[i][0] - l_edges[i][1], l_edges[i][0] - current));
 
          // Check for inverse normal direction to maintain CCW winding,
          //  with a small bias for faces containing the origin
          if (dot(faces[num_faces][0], faces[num_faces].normal) + EPA_BIAS < 0) {
            temp = faces[num_faces][0];
            faces[num_faces][0] = faces[num_faces][1];
            faces[num_faces][1] = temp;
            faces[num_faces].normal = -faces[num_faces].normal;
          }
          num_faces++;
        }
      }
 
      // use closest point
      printf("EPA did not converge\n");
 
      search_dir = faces[c_idx].normal;
      n_dist = dot(faces[c_idx][0], search_dir);
 
      Barycentric({0, 0, 0}, faces[c_idx][0], faces[c_idx][1], faces[c_idx][2], u, v, w);
      glm::vec3 v1 = u * faces[c_idx][0].supportA + v * faces[c_idx][1].supportA + w * faces[c_idx][2].supportA;
      glm::vec3 v2 = u * faces[c_idx][0].supportB + v * faces[c_idx][1].supportB + w * faces[c_idx][2].supportB;
 
      return {search_dir, {v1, v2}, n_dist, true};
    }
 
    static bool GJK(const Shape &colliderA, const Shape &colliderB, Simplex &simplex, CollisionType type) {
      // get the the first support point in an arbitrary direction
      Support support = furthestDiff(colliderA, colliderB, glm::vec3{0.f, 0.f, 1.f}, support);
      glm::vec3 direction = -support; // set the new direction to its inverse
      simplex += support; // add the first point to the simplex
      // search for new points
      for (int iterations = 0; iterations < GJK_MAX_NUM_ITERATIONS; iterations++) {
        // find new support point
        furthestDiff(colliderA, colliderB, direction, support);
 
        // check if the support point is behind the origin, because this is a convex polygon the shapes do not collide
        if (dot(support, direction) <= 0) {
          return type == D2 && simplex.index == 3;
        }
 
        // add the point to the beginning of the list
        simplex.push(support);
 
        // Switch on the next simplex
        if (NextSimplex(simplex, direction, type)) {
          return true;
        }
      }
//      printf("GJK did not converge\n");
      return false;
    }
 
    static bool NextSimplex(Simplex &simplex, glm::vec3 &direction, CollisionType type) {
      switch (simplex.index) {
        case 2: return Line(simplex, direction);
        case 3: return Triangle(simplex, direction);
        case 4: return type == D2 || Tetrahedron(simplex, direction);
      }
 
      // never should be here
      return false;
    }
 
    static bool Line(Simplex &simplex, glm::vec3 &direction) {
      Support& a = simplex[0];
      Support& b = simplex[1];
 
      glm::vec3 ab = b - a;
      glm::vec3 ao = -a;
 
      // if the line is facing the origin,
      //  set the new direction as a perpendicular vector to this and the line to the origin
      if (dot(ab, ao) > 0) {
        direction = cross(cross(ab, ao), ab);
      } else {
        // otherwise the line is moving further away from the origin and isn't useful to us so remove it
        simplex = {a};
        direction = ao;
      }
 
      return false;
    }
 
    static bool Triangle(Simplex &simplex, glm::vec3 &direction) {
      Support& a = simplex[0];
      Support& b = simplex[1];
      Support& c = simplex[2];
 
      glm::vec3 ab = b - a;
      glm::vec3 ac = c - a;
      glm::vec3 ao = -a;
 
      glm::vec3 abc = cross(ab, ac);
 
      // compare a new ab to the value of a to the origin
      if (dot(cross(abc, ac), ao) > 0) {
        // does ac move toward the origin within the triangle of abc
        if (dot(ac, ao) > 0) {
          simplex = {a, c}; // remove point b
          // search the new direction generated from the triangle with a, c and the origin
          direction = cross(cross(ac, ao), ac);
        } else {
          // otherwise remove point c and maintain the direction
          return Line(simplex = {a, b}, direction);
        }
      } else {
        // is the triangle made from a new ac closer to the origin, i.e. is the inverse ac pointing towards the origin
        if (dot(cross(ab, abc), ao) > 0) {
          // remove the old c and maintain the direction
          return Line(simplex = {a, b}, direction);
        } else {
          // otherwise check if the triangle normal is pointing towards the origin so make it the new direction
          if (dot(abc, ao) > 0) {
            direction = abc;
          } else {
            // otherwise modify the triangle normal direction by reordering the vertices
            //  keep searching in the opposite of the triangle normal direction
            simplex = {a, c, b};
            direction = -abc;
          }
        }
      }
 
      return false;
    }
 
    static bool Tetrahedron(Simplex &simplex, glm::vec3 &direction) {
      Support& a = simplex[0];
      Support& b = simplex[1];
      Support& c = simplex[2];
      Support& d = simplex[3];
 
      glm::vec3 ab = b - a;
      glm::vec3 ac = c - a;
      glm::vec3 ad = d - a;
      glm::vec3 ao = -a;
 
      glm::vec3 abc = cross(ab, ac);
      glm::vec3 acd = cross(ac, ad);
      glm::vec3 adb = cross(ad, ab);
 
      // find the tetrahedron face pointing toward the origin and re-evaluate it
      if (dot(abc, ao) > 0) {
        return Triangle(simplex = {a, b, c}, direction);
      }
 
      if (dot(acd, ao) > 0) {
        return Triangle(simplex = {a, c, d}, direction);
      }
 
      if (dot(adb, ao) > 0) {
        return Triangle(simplex = {a, d, b}, direction);
      }
 
      return true;
    }
    //</editor-fold>
 
    //<editor-fold desc="XPBD">
    static void Update(Scene& scene) {
      if (Debug::getDebug("simulate") == 1) {
        simulateSystem(scene, (float) Time::dt);
      }
 
      std::vector<Ref<PhysicsEntity>> elements = Collect2DElements(scene);
      processCollisions(elements, (float) Time::dt, D2);
    }
 
    static void simulateSystem(Scene& scene, float dt) {
      int numPosIterations = 1;
      int numSubsteps = 10;
 
      float h = dt / (float) numSubsteps; // get minimum time step;
 
      std::vector<Ref<PhysicsEntity>> elements = CollectBodiesAndParticles(scene);
      std::vector<Ref<Joint>> joints = CollectJoints(scene);
 
      for (auto &elem : elements) {
        elem->x = elem->tf->position; // obtain position
        elem->q = elem->tf->rotation; // obtain rotation
        elem->v = elem->rb->velocity; // obtain velocity
        elem->w = elem->rb->w;
      }
 
      for (int sStep = 0; sStep < numSubsteps; sStep++) {
        for (auto &elem : elements) {
          // obtain angular velocity
          if (!elem->rb->kinematic)
            continue;
 
          elem->xPrev = elem->x; // set the previous x (for calculating dx later)
          elem->qPrev = elem->q; // save the previous rotation
 
          // get acceleration from force and then multiply by time to get delta v
          elem->v += h * elem->rb->force * elem->rb->iMass;
          elem->x += h * elem->v; // move by total v
 
          glm::mat3 t1 = elem->rb->invMoment;
          glm::mat3 t2 = elem->rb->moment;
 
          // increase the angular momentum by the torque
          elem->w += h * t1 * (elem->rb->t - glm::cross(elem->w, (t2 * elem->w)));
 
          // increase the rotation by 1/2 t (probably due to the derivative of a^2)
          elem->q = elem->q + (h * 0.5f) * glm::qua<float>{0, elem->w.x, elem->w.y, elem->w.z} * elem->q;
          elem->q = glm::normalize(elem->q);
 
          elem->clearMat();
        }
 
        for (int _ = 0; _ < numPosIterations; _++) {
          processCollisions(elements, h);
          processJoints(joints, h);
        }
 
        for (auto &elem : elements) {
          // calculate velocity from position change
          elem->v = (elem->x - elem->xPrev) / h;
          // calculate change in rotation
          elem->dQ = elem->q * glm::inverse(elem->qPrev);
          // convert from angular change to angular velocity
          elem->w = (2.0f / h) * glm::vec3(elem->dQ.x, elem->dQ.y, elem->dQ.z);
          // rotate in the correct direction
          elem->w = elem->dQ.w >= 0 ? elem->w : -elem->w;
 
          elem->v -= elem->v * glm::min(1.0f, h * elem->rb->airDrag);
          elem->w -= elem->w * glm::min(1.0f, h * elem->rb->angularDrag);
        }
      }
 
      for (auto &elem : elements) {
        // apply changes
        elem->tf->position = elem->x;
        elem->tf->rotation = elem->q;
        if (!elem->rb->kinematic)
          continue;
        elem->rb->velocity = elem->v;
        elem->rb->w = elem->w;
      }
    }
 
    static CollisionPoint processJoint(const Ref<Joint>& joint,
                                       const Ref<PhysicsEntity> &pe1,
                                       const Ref<PhysicsEntity> &pe2) {
      glm::vec3 p1 = pe1->x + pe1->q * joint->positionA;
      glm::vec3 p2;
      if (pe2 == nullptr) {
        p2 = joint->positionB;
      } else {
        p2 = pe2->x + pe2->q * joint->positionB;
      }
 
 
      glm::vec3 normal = p2 - p1;
      float sqrDist = glm::length2(normal);
      if (sqrDist > joint->maxLength * joint->maxLength) {
        float invDepth = glm::inversesqrt(sqrDist);
        return CollisionPoint{normal * invDepth, {p1, p2}, joint->maxLength - (1.0f/invDepth), true};
      } else if (sqrDist < joint->minLength * joint->minLength) {
 
        float invDepth = glm::inversesqrt(sqrDist);
        return CollisionPoint{normal * -invDepth, {p1, p2}, (1.0f/invDepth) - joint->minLength, true};
      } else {
 
        return CollisionPoint{{}, {}, 0, false};
      }
    }
 
    static void processJoints(const std::vector<Ref<Joint>> &joints, float h) {
      for (const auto& joint : joints) {
        Ref<PhysicsEntity> pe1 = joint->rigidBody->physicsEntity;
        Ref<PhysicsEntity> pe2 = joint->connectedRigidBody->physicsEntity;
 
        CollisionPoint cp = processJoint(joint, pe1, pe2);
        if (cp.HasCollision) {
          applyManifold(cp, pe1, pe2, h);
        }
      }
    }
 
    static void processCollisions(std::vector<Ref<PhysicsEntity>> &elements, float h, CollisionType type = D3) {
      for (int i = 0; i < elements.size(); ++i) {
        Ref<PhysicsEntity> pe1 = elements[i];
        if (pe1->c == nullptr) continue;
 
        for (int j = i + 1; j < elements.size(); ++j) {
          Ref<PhysicsEntity> pe2 = elements[j];
          if (pe2->c == nullptr) continue;
          CollisionPoint cp = GJK_EPA(Shape{pe1},Shape{pe2}, type);
          if (cp.HasCollision) {
            pe1->c->processCollision(pe2);
            pe2->c->processCollision(pe1);
            if (!pe1->c->trigger && !pe2->c->trigger && type != D2) {
              applyManifold(cp, pe1, pe2, h);
            }
          }
        }
      }
    }
 
 
    static void applyManifold(CollisionPoint cp, Ref<PhysicsEntity> &e1, Ref<PhysicsEntity> &e2, float h) {
      float a = 0;
      float lambda = 0;
 
      bool e1Static = (e1 == nullptr) || !e1->rb->kinematic;
      bool e2Static = (e2 == nullptr) || !e2->rb->kinematic;
 
      if (e1Static && e2Static) {
        return;
      }
 
      glm::vec3 r1 = e1Static ? cp.contacts[0] : cp.contacts[0] - e1->x;
      glm::vec3 r2 = e2Static ? cp.contacts[1] : cp.contacts[1] - e2->x;
 
      float w1 = 0;
      if (!e1Static) {
        // convert the normal vector to model space
        glm::vec3 prod1 = glm::cross(r1, cp.Normal);
        // calculate the mass impulse or mass amount of resistance in the direction (0 is non moving)
        w1 = e1->rb->iMass + glm::dot(prod1, e1->rb->invMoment * prod1);
      }
 
      float w2 = 0;
      if (!e2Static) {
        // same for e2
        glm::vec3 prod2 = glm::cross(r2, cp.Normal);
        w2 = e2->rb->iMass + glm::dot(prod2, e2->rb->invMoment * prod2);
      }
 
      // calculate the stiffness of the interaction (0 is completely stiff)
      float aTilde = a / (h * h);
 
      // compute the lagrangian multiplier
      // note that whenever there is an interaction there should be a non-zero
      // w1 + w2 + atilde
      // we need to check that if both bodies have a imass of 0 this calculation is not preformed.
      float dLambda = (-cp.PenetrationDepth - (aTilde * lambda)) / (w1 + w2 + aTilde);
      glm::vec3 p = dLambda * cp.Normal; // calculate the positional impulse
 
      // move by the positional impulse multiplied by the inverse mass
      // this will translate the impulse back to a change in position
      // calculate the change in rotation
      // convert normal to object space and invert the rotational inerta (not 100% sure about this)
      if (!e1Static) {
        e1->x += p * e1->rb->iMass;
        e1->q += 0.5f * glm::qua<float>{0.f, e1->rb->invMoment * glm::cross(r1, p)} * e1->q;
        e1->clearMat();
      }
      if (!e2Static) {
        e2->x -= p * e2->rb->iMass;
        e2->q -= 0.5f * glm::qua<float>{0.f, e2->rb->invMoment * glm::cross(r2, p)} * e2->q;
        e2->clearMat();
      }
 
 
    }
 
    static std::vector<Ref<Joint>> CollectJoints(const Scene& scene) {
      ECS ecs = scene.GetECS();
      auto out = std::vector<Ref<Joint>>();
 
      auto s_rb = CreateRef<RigidBody>();
      s_rb->setMomentCube(0, 0, 0, 0);
 
      for (auto id : ecs.GetEntitiesWith(ecs.GetArchetype<Joint, RigidBody>())) {
        Ref<Joint> joint = ecs.GetComponent<Joint>(id);
        Ref<RigidBody> rb = ecs.GetComponent<RigidBody>(id);
        joint->rigidBody = rb;
 
        if (joint->connectedRigidBody == nullptr) {
          joint->connectedRigidBody = s_rb;
        }
 
        out.emplace_back(joint);
      }
 
      return out;
    }
 
    static std::vector<Ref<PhysicsEntity>> CollectBodiesAndParticles(Scene& scene) {
      auto out = std::vector<Ref<PhysicsEntity>>();
      auto s_rb = CreateRef<RigidBody>();
      s_rb->setMomentCube(0, 0, 0, 0);
      s_rb->kinematic = false;
 
      ECS ecs = scene.GetECS();
      for (auto id : ecs.GetEntitiesWith(ecs.GetArchetype<Transform,Collider>())) {
        Ref<Transform> tf = ecs.GetComponent<Transform>(id);
        Ref<Collider> cl = ecs.GetComponent<Collider>(id);
        Ref<RigidBody> rb = ecs.GetComponent<RigidBody>(id);
 
        if (rb == nullptr) {
          rb = s_rb;
        }
 
        Ref<Entity> e = CreateRef<Entity>(id, &scene);
        Ref<PhysicsEntity> physicsEntity = CreateRef<PhysicsEntity>(e, tf, cl, rb);
        rb->physicsEntity = physicsEntity;
 
        out.emplace_back(physicsEntity);
      }
 
      return out;
    };
 
    static std::vector<Ref<PhysicsEntity>> Collect2DElements(Scene& scene) {
      auto out = std::vector<Ref<PhysicsEntity>>();
      auto s_rb = CreateRef<RigidBody>();
      s_rb->setMomentCube(0, 0, 0, 0);
      s_rb->kinematic = false;
 
      ECS ecs = scene.GetECS();
      for (auto id : ecs.GetEntitiesWith(ecs.GetArchetype<Transform, Collider2D>())) {
        Ref<Transform> tf = ecs.GetComponent<Transform>(id);
        Ref<Collider> cl = ecs.GetComponent<Collider2D>(id);
        Ref<RigidBody> rb = ecs.GetComponent<RigidBody>(id);
 
        if (rb == nullptr) {
          rb = s_rb;
        }
 
        Ref<Entity> e = CreateRef<Entity>(id, &scene);
        Ref<PhysicsEntity> physicsEntity = CreateRef<PhysicsEntity>(e, tf, cl, rb);
 
        rb->physicsEntity = physicsEntity;
        physicsEntity->x = physicsEntity->tf->position; // obtain position
        physicsEntity->q = physicsEntity->tf->rotation; // obtain rotation
        out.emplace_back(physicsEntity);
      }
 
      return out;
    };
    //</editor-fold>
 
    //<editor-fold desc="Vector-Math Functions">
    static Support furthestDiff(const Shape &shapeA, const Shape &shapeB, const glm::vec3 &norm, Support &support) {
      support.supportA = shapeA.furthestInDir(norm);
      support.supportB = shapeB.furthestInDir(-norm);
      return support = support.supportA - support.supportB;
    }
 
    static void Barycentric(const glm::vec3 &p, const glm::vec3 &a, const glm::vec3 &b, const glm::vec3 &c,
                            float &u, float &v, float &w) {
      glm::vec3 v0 = b - a, v1 = c - a, v2 = p - a;
      float d00 = dot(v0, v0);
      float d01 = dot(v0, v1);
      float d11 = dot(v1, v1);
      float d20 = dot(v2, v0);
      float d21 = dot(v2, v1);
      float invDenom = 1.0f / (d00 * d11 - d01 * d01);
      v = (d11 * d20 - d01 * d21) * invDenom;
      w = (d00 * d21 - d01 * d20) * invDenom;
      u = 1.0f - v - w;
    }
    //</editor-fold>
  };
}
 
#endif //ENGINE_PROJ_PHYSICS_HPP