Dawn/src/dawnphysics/physics/PhysicsManager.cpp

// Copyright (c) 2024 Dominic Masters
// 
// This software is released under the MIT License.
// https://opensource.org/licenses/MIT

#include "PhysicsManager.hpp"
#include "game/Game.hpp"
#include "scene/Scene.hpp"
#include "component/physics/Collider.hpp"
#include "component/SceneComponentRegistry.hpp"

#include "component/physics/CubeCollider.hpp"
#include "component/physics/SphereCollider.hpp"

using namespace Dawn;
using namespace JPH;
using namespace JPH::literals;

static void TraceImpl(const char *inFMT, ...) {
  // Format the message
  va_list list;
  va_start(list, inFMT);
  char buffer[1024];
  vsnprintf(buffer, sizeof(buffer), inFMT, list);
  va_end(list);

  // Print to the TTY
  // std::cout << buffer << std::endl;
}

#ifdef JPH_ENABLE_ASSERTS
  static bool AssertFailedImpl(
    const char *inExpression,
    const char *inMessage,
    const char *inFile,
    uint inLine
  ) {
    // Print to the TTY
    std::cout << inFile << ":" << inLine << ": (" << inExpression << ") " << (inMessage != nullptr? inMessage : "") << std::endl;

    // Breakpoint
    return true;
  };
#endif

namespace Layers {
  static constexpr ObjectLayer NON_MOVING = 0;
  static constexpr ObjectLayer MOVING = 1;
  static constexpr ObjectLayer NUM_LAYERS = 2;
};

/// Class that determines if two object layers can collide
class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter {
  public:
    virtual bool ShouldCollide(
      ObjectLayer inObject1,
      ObjectLayer inObject2
    ) const override {
      switch (inObject1) {
        case Layers::NON_MOVING:
          return inObject2 == Layers::MOVING;
        case Layers::MOVING:
          return true;
        default:
          JPH_ASSERT(false);
          return false;
      }
    }
};

namespace BroadPhaseLayers {
  static constexpr BroadPhaseLayer NON_MOVING(0);
  static constexpr BroadPhaseLayer MOVING(1);
  static constexpr uint NUM_LAYERS(2);
};


class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface {
  public:
    BPLayerInterfaceImpl() {
    mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
    mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
  }

  virtual uint GetNumBroadPhaseLayers() const override {
    return BroadPhaseLayers::NUM_LAYERS;
  }

  virtual BroadPhaseLayer GetBroadPhaseLayer(
    ObjectLayer inLayer
  ) const override {
    JPH_ASSERT(inLayer < Layers::NUM_LAYERS);
    return mObjectToBroadPhase[inLayer];
  }

  #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
    virtual const char * GetBroadPhaseLayerName(
      BroadPhaseLayer inLayer
    ) const override {
      switch ((BroadPhaseLayer::Type)inLayer) {
        case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:
          return "NON_MOVING";
        case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:
          return "MOVING";
        default:
          JPH_ASSERT(false); return "INVALID";
      }
    }
  #endif

  private:
    BroadPhaseLayer mObjectToBroadPhase[Layers::NUM_LAYERS];
};

class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter {
  public:
    virtual bool ShouldCollide(
      ObjectLayer inLayer1,
      BroadPhaseLayer inLayer2
    ) const override {
      switch (inLayer1) {
        case Layers::NON_MOVING:
          return inLayer2 == BroadPhaseLayers::MOVING;
        case Layers::MOVING:
          return true;
        default:
          JPH_ASSERT(false);
          return false;
      }
    }
};

// An example contact listener
class MyContactListener : public ContactListener {
  public:
    // See: ContactListener
    virtual ValidateResult OnContactValidate(
      const Body &inBody1,
      const Body &inBody2,
      RVec3Arg inBaseOffset,
      const CollideShapeResult &inCollisionResult
    ) override {
      // std::cout << "Contact validate callback" << std::endl;
      return ValidateResult::AcceptAllContactsForThisBodyPair;
    }

    virtual void OnContactAdded(
      const Body &inBody1,
      const Body &inBody2,
      const ContactManifold &inManifold,
      ContactSettings &ioSettings
    ) override {
      // std::cout << "A contact was added" << std::endl;
    }

    virtual void OnContactPersisted(
      const Body &inBody1,
      const Body &inBody2,
      const ContactManifold &inManifold,
      ContactSettings &ioSettings
    ) override {
      // std::cout << "A contact was persisted" << std::endl;
    }

    virtual void OnContactRemoved(
      const SubShapeIDPair &inSubShapePair
    ) override {
      // std::cout << "A contact was removed" << std::endl;
    }
};

// An example activation listener
class MyBodyActivationListener : public BodyActivationListener {
  public:
    virtual void OnBodyActivated(
      const BodyID &inBodyID,
      uint64 inBodyUserData
    ) override {
      // std::cout << "A body got activated" << std::endl;
    }

    virtual void OnBodyDeactivated(
      const BodyID &inBodyID,
      uint64 inBodyUserData
    ) override {
      // std::cout << "A body went to sleep" << std::endl;
    }
};

const uint cMaxBodies = 1024;
const uint cNumBodyMutexes = 0;
const uint cMaxBodyPairs = 1024;
const uint cMaxContactConstraints = 1024;
BPLayerInterfaceImpl broadPhaseLayerInterface;
ObjectVsBroadPhaseLayerFilterImpl objectVsBroadPhaseLayerFilter;
ObjectLayerPairFilterImpl objectVsObjectLayerFilter;
MyBodyActivationListener bodyActivationListener;
MyContactListener contactListener;

std::shared_ptr<Game> PhysicsManager::getGame() {
  auto g = game.lock();
  assertNotNull(g, "Game is null");
  return g;
}

BodyInterface & PhysicsManager::getBodyInterface() {
  return physicsSystem.GetBodyInterface();
}

void PhysicsManager::init(const std::shared_ptr<Game> &game) {
  this->game = game;

  SceneComponentRegistry::reg<CubeCollider>("CubeCollider");
  SceneComponentRegistry::reg<SphereCollider>("SphereCollider");

  RegisterDefaultAllocator();

  Trace = TraceImpl;
	JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)

  Factory::sInstance = new Factory();
  RegisterTypes();

  tempAllocator = std::make_shared<TempAllocatorImpl>(10 * 1024 * 1024);
  jobSystem = std::make_shared<JobSystemThreadPool>(
    cMaxPhysicsJobs,
    cMaxPhysicsBarriers,
    thread::hardware_concurrency() - 1
  );

  physicsSystem.Init(
    cMaxBodies,
    cNumBodyMutexes,
    cMaxBodyPairs,
    cMaxContactConstraints,
    broadPhaseLayerInterface,
    objectVsBroadPhaseLayerFilter,
    objectVsObjectLayerFilter
  );

  physicsSystem.SetBodyActivationListener(&bodyActivationListener);
  physicsSystem.SetContactListener(&contactListener);
}

void PhysicsManager::update() {
  BodyInterface &bodyInterface = physicsSystem.GetBodyInterface();

  const float cDeltaTime = getGame()->timeManager.delta;
  const int cCollisionSteps = 1;

  auto scene = getGame()->getCurrentScene();
  if(scene == nullptr) return;

  // // Now we're ready to simulate the body, keep simulating until it goes to sleep
  // uint step = 0;
  // if(!bodyInterface.IsActive(sphereId)) return;
  // // ++step;

  // // Output current position and velocity of the sphere
  // RVec3 position = bodyInterface.GetCenterOfMassPosition(sphereId);
  // Vec3 velocity = bodyInterface.GetLinearVelocity(sphereId);

  // Step the world
  physicsSystem.Update(
    cDeltaTime,
    cCollisionSteps,
    tempAllocator.get(),
    jobSystem.get()
  );

  auto colliders = scene->findComponents<Collider>();
  for(auto &collider : colliders) {
    auto bodyId = collider->getBodyId();
    if(!bodyInterface.IsActive(bodyId)) continue;

    auto pos = bodyInterface.GetCenterOfMassPosition(bodyId);
    auto rot = bodyInterface.GetRotation(bodyId);
    collider->getItem()->setLocalPosition(glm::vec3(pos.GetX(), pos.GetY(), pos.GetZ()));
    collider->getItem()->setLocalRotation(glm::quat(rot.GetW(), rot.GetX(), rot.GetY(), rot.GetZ()));
  }
}

PhysicsManager::~PhysicsManager() {
  BodyInterface &bodyInterface = physicsSystem.GetBodyInterface();

  // bodyInterface.RemoveBody(sphereId);
  // bodyInterface.DestroyBody(sphereId);

  // Remove and destroy the floor
  // bodyInterface.RemoveBody(floor->GetID());
  // bodyInterface.DestroyBody(floor->GetID());

  tempAllocator = nullptr;
  jobSystem = nullptr;

  UnregisterTypes();
  delete Factory::sInstance;
  Factory::sInstance = nullptr;

  game.reset();
}