PhysicsManager.cpp

#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable: 4127)  // conditional expression is constant
#pragma warning(disable: 5033)  // 'register' is no longer a supported storage class
#endif
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#ifdef _WIN32
#pragma warning(pop)
#endif
 
#include "PhysicsManager.h"
 
#include "RigidBodySystem.h"
 
#include "Context.h"
 
#include "Services/Time.h"
#include "ExtensionRegistry.h"
 
#include "Components/Appearance/MaterialComponent.h"
 
#include "GhostSystem.h"
#include "Types.h"
 
#include "Foundation/HashSequence.h"
#include "Foundation/Collections/Pool.h"
#include "Foundation/ComponentModel/Entity.h"
 
using namespace Cogs::Reflection;
 
namespace
{
  constexpr Cogs::StringView timeScale("physics.timeScale");
}
 
Cogs::Core::PhysicsManager::PhysicsManager(Context * context) :
  context(context),
  broadphase(std::make_unique<btDbvtBroadphase>()),
  collisionConfiguration(std::make_unique<btDefaultCollisionConfiguration>()),
  dispatcher(std::make_unique<btCollisionDispatcher>(collisionConfiguration.get())),
  solver(std::make_unique<btSequentialImpulseConstraintSolver>()),
  dynamicsWorld(std::make_unique<btDiscreteDynamicsWorld>(dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()))
{
  dynamicsWorld->setGravity(btVector3(0, 0, -9.81f));
}
 
Cogs::Core::PhysicsManager::~PhysicsManager() = default;
 
void Cogs::Core::PhysicsManager::addRigidBody(btRigidBody * rigidBody)
{
  dynamicsWorld->addRigidBody(rigidBody);
}
 
void Cogs::Core::PhysicsManager::removeRigidBody(btRigidBody * rigidBody)
{
  dynamicsWorld->removeRigidBody(rigidBody);
}
 
void Cogs::Core::PhysicsManager::addCollisionObject(btCollisionObject * btCollisionObject)
{
  if (!ghostCallbackSet) {
    dynamicsWorld->getPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
    ghostCallbackSet = true;
  }
 
  dynamicsWorld->addCollisionObject(btCollisionObject, btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
}
 
void Cogs::Core::PhysicsManager::removeCollisionObject(btCollisionObject * btCollisionObject)
{
  dynamicsWorld->removeCollisionObject(btCollisionObject);
}
 
void Cogs::Core::PhysicsManager::addConstraint(btTypedConstraint * constraint)
{
  dynamicsWorld->addConstraint(constraint);
}
 
void Cogs::Core::PhysicsManager::removeConstraint(btTypedConstraint * constraint)
{
  dynamicsWorld->removeConstraint(constraint);
}
 
void Cogs::Core::PhysicsManager::stepSimulation(float deltaTime)
{
  const float tick = 1.0f / 90.0f;
 
  float scale = context->variables->get(timeScale, 1.0f);
  dynamicsWorld->stepSimulation(deltaTime * scale, 10, tick);
}
 
void Cogs::Core::CollisionDetail::registerType()
{
  Field fields[] = {
    Field("idA", &CollisionDetail::idA),
    Field("idB", &CollisionDetail::idB),
    Field("positionA", &CollisionDetail::positionA),
    Field("positionB", &CollisionDetail::positionB),
    Field("impulse", &CollisionDetail::impulse),
  };
 
  TypeDatabase::createType<CollisionDetail>().setFields(fields);
}
 
 
void Cogs::Core::PhysicsManager::preUpdate(Context* context)
{
  if (task.isValid()) {
    context->taskManager->wait(task);
    pullGhostState();
  }
}
 
void Cogs::Core::PhysicsManager::postUpdate(Context* context)
{
  pushGhostState();
  auto elapsed = context->time->getAnimationTimeDelta();
  task = context->taskManager->enqueue(TaskManager::GlobalQueue, [elapsed, this]()
  {
    CpuInstrumentationScope(SCOPE_PHYSICS, "stepSimulation");
    stepSimulation(float(elapsed));
  });
}
 
void Cogs::Core::PhysicsManager::waitForASync(Context* context)
{
  if (task.isValid()) {
    context->taskManager->wait(task);
  }
}
 
void Cogs::Core::PhysicsManager::pullGhostState()
{
  auto * ghostSystem = ExtensionRegistry::getExtensionSystem<GhostSystem>(context);
  for (auto & ghostComp : ghostSystem->pool) {
    ghostComp.contacts.clear();
 
    if (!ghostComp.isActive()) continue;
 
    auto & ghostData = ghostSystem->getData(&ghostComp);
    if(!ghostData.collisionObject) continue;
 
    auto & pairArray = ghostData.collisionObject->getOverlappingPairCache()->getOverlappingPairArray();
 
 
    ghostComp.contacts.clear();
    btManifoldArray manifoldArray;
    for (int i = 0; i < pairArray.size(); i++) {
      manifoldArray.clear();
 
      const auto & pair = pairArray[i];
      auto * collisionPair = dynamicsWorld->getPairCache()->findPair(pair.m_pProxy0, pair.m_pProxy1);
      if (!collisionPair || !collisionPair->m_algorithm) continue;
 
      collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
      for (int j = 0; j < manifoldArray.size(); j++) {
        auto * manifold = manifoldArray[j];
 
        bool noFlip = manifold->getBody0() == ghostData.collisionObject.get();
        auto * objB = noFlip ? manifold->getBody1() : manifold->getBody0();
        auto * dataB = static_cast<SharedRigidBodyData *>(objB->getUserPointer());
        auto entityB = dataB->entity;
        if(!entityB.lock()) continue;
 
        for (int k = 0; k < manifold->getNumContacts(); k++) {
          const auto & pt = manifold->getContactPoint(k);
          if (pt.getDistance() < 0.f && ghostComp.contacts.size() < 1024) {
 
            ghostComp.contacts.push_back(GhostContact{
              entityB,
              (noFlip ? -1.f : 1.f)*(*reinterpret_cast<const glm::vec3 *>(&pt.m_normalWorldOnB)),
              *reinterpret_cast<const glm::vec3 *>(noFlip ?  &pt.m_positionWorldOnA : &pt.m_positionWorldOnB),
              pt.getDistance()
            });
          }
        }
      }
    }
 
  }
 
}
 
void Cogs::Core::PhysicsManager::pushGhostState()
{
  auto * ghostSystem = ExtensionRegistry::getExtensionSystem<GhostSystem>(context);
  auto * rigidBodySystem = ExtensionRegistry::getExtensionSystem<RigidBodySystem>(context);
 
  for (auto & ghostComp : ghostSystem->pool) {
 
    for (auto & action : ghostComp.actions) {
      if (auto e = action.target.lock(); e) {
        if (auto * c = e->getComponent<RigidBodyComponent>(); c) {
          if (auto & d = rigidBodySystem->getData(c); d.rigidBody) {
 
            switch (action.type)
            {
            case GhostAction::ApplyImpulse:
              d.rigidBody->applyImpulse(toBullet(action.value), toBullet(action.position));
              d.rigidBody->activate();
              break;
 
            default:
              break;
            }
          }
        }
      }
    }
 
    ghostComp.actions.clear();
  }
}