RigidBodySystem.cpp

#include "RigidBodySystem.h"
 
#include "PhysicsManager.h"
 
#include "Systems/Core/TransformSystem.h"
 
#include "Context.h"
 
#include "Services/Services.h"
#include "Services/Time.h"
 
#include "EntityStore.h"
#include "Components/Core/MotionComponent.h"
 
#include "CollisionComponent.h"
 
#include "Platform/Instrumentation.h"
 
#include "Foundation/Logging/Logger.h"
 
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/matrix_decompose.hpp>
 
namespace
{
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("RigidBodySystem");
}
 
namespace Cogs
{
  namespace Core
  {
    void updateMassProperties(RigidBodyData & data, float mass, const glm::vec3 & scale)
    {
      const float margin = scale.x < scale.y && scale.x < scale.z ? scale.x : (scale.y < scale.z ? scale.y : scale.z);
      const float marginMultiplier = 0.1f;
 
      data.collisionShape->setLocalScaling(toBullet(scale));
      
      auto diff = scale - glm::vec3(1, 1, 1);
      
      if (glm::any(glm::greaterThan(glm::abs(diff), glm::vec3(0.01f, 0.01f, 0.01f)))) {
        data.collisionShape->setMargin(margin * marginMultiplier);
      }
 
      btVector3 inertia;
      if (mass != 0) {
        if (data.collisionShape->getShapeType() != EMPTY_SHAPE_PROXYTYPE) data.collisionShape->calculateLocalInertia(mass, inertia);
      }
 
      if (data.rigidBody) {
        data.rigidBody->setMassProps(mass, inertia);
        data.rigidBody->updateInertiaTensor();
      }
 
      data.scale = scale;
    }
  }
}
 
void Cogs::Core::RigidBodySystem::preUpdate(Context * context)
{
  base::preUpdate(context);
  auto manager = context->services->getService<PhysicsManager>();
  manager->preUpdate(context);
}
 
void Cogs::Core::RigidBodySystem::update(Context * context)
{
  if (pool.size() && glm::any(glm::notEqual(context->transformSystem->getOrigin(), glm::dvec3(0, 0, 0)))) {
    LOG_ERROR(logger, "Physics not supported using global origin.");
 
    return;
  }
 
  auto manager = context->services->getService<PhysicsManager>();
  auto elapsed = context->time->getAnimationTimeDelta();
 
  for (auto & c : pool) {
    if (!c.isActive()) continue;
 
    auto & data = getData(&c);
 
    auto transformComponent = c.getComponent<TransformComponent>();
 
    if (!data.rigidBody) {
      auto collisionComponent = c.getComponent<CollisionComponent>();
 
      if (!collisionComponent || !collisionComponent->collisionShape) {
        return;
      }
      data.collisionShape = collisionComponent->collisionShape;
 
      context->transformSystem->updateLocalToWorldTransform(*transformComponent, true);
      auto worldTransform = context->transformSystem->getLocalToWorld(transformComponent);
 
      auto scale = glm::vec3(glm::length(worldTransform[0]), glm::length(worldTransform[1]), glm::length(worldTransform[2]));
      auto rotation = glm::normalize(glm::quat_cast(glm::mat3(worldTransform * glm::scale(glm::vec3(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z)))));
      auto position = glm::vec3(worldTransform[3]);
 
      data.scale = scale;
      updateMassProperties(data, c.mass, scale);
 
      btVector3 localInertia(0, 0, 0);
 
      if (c.mass != 0) {
        if (data.collisionShape->getShapeType() != EMPTY_SHAPE_PROXYTYPE) data.collisionShape->calculateLocalInertia(c.mass, localInertia);
      }
 
      const btTransform origin(toBullet(rotation), toBullet(position));
 
      data.lastPosition = position;
 
      data.motionState = std::make_unique<btDefaultMotionState>(origin);
 
      btRigidBody::btRigidBodyConstructionInfo ci(c.mass, data.motionState.get(), data.collisionShape, localInertia);
 
      ci.m_friction = c.friction;
      ci.m_rollingFriction = c.rollingFriction;
      ci.m_spinningFriction = c.spinningFriction;
      ci.m_linearDamping = c.linearDamping;
      ci.m_angularDamping = c.angularDamping;
      ci.m_restitution = c.restitution;
 
      data.rigidBody = std::make_unique<btRigidBody>(ci);
      data.rigidBody->setCcdMotionThreshold(c.ccdMotionThreshold);
      data.rigidBody->setCcdSweptSphereRadius(c.ccdSweptSphereRadius);
      data.rigidBody->setLinearFactor(toBullet(c.linearFactor));
      data.rigidBody->setAngularFactor(toBullet(c.angularFactor));
      data.shared = std::make_unique<SharedRigidBodyData>();
      data.shared->entity = context->store->getEntity(c.getContainer()->getId());
      data.rigidBody->setUserPointer(data.shared.get());
      data.kinematic = c.kinematic;
 
      auto flags = data.rigidBody->getCollisionFlags();
      flags |= c.kinematic ? btCollisionObject::CF_KINEMATIC_OBJECT : 0;
 
      if (c.kinematic) {
        data.rigidBody->setActivationState(DISABLE_DEACTIVATION);
      }
 
      data.rigidBody->setCollisionFlags(flags);
 
      manager->addRigidBody(data.rigidBody.get());
    }
 
    if (!data.rigidBody) continue;
 
    if (c.kinematic != data.kinematic) {
      manager->removeRigidBody(data.rigidBody.get());
 
      auto flags = data.rigidBody->getCollisionFlags();
 
      if (c.kinematic) {
        flags |= btCollisionObject::CF_KINEMATIC_OBJECT;
        data.rigidBody->setActivationState(DISABLE_DEACTIVATION);
      } else {
        flags &= ~btCollisionObject::CF_KINEMATIC_OBJECT;
        data.rigidBody->activate(true);
 
        context->transformSystem->updateLocalToWorldTransform(*transformComponent, true);
        auto worldTransform = context->transformSystem->getLocalToWorld(transformComponent);
 
        auto scale = glm::vec3(glm::length(worldTransform[0]), glm::length(worldTransform[1]), glm::length(worldTransform[2]));
 
        if (glm::any(glm::greaterThan(glm::abs(scale - data.scale), glm::vec3(0.001f)))) {
          updateMassProperties(data, c.mass, scale);
        }
      }
 
      data.rigidBody->setCollisionFlags(flags);
      data.kinematic = c.kinematic;
 
      manager->addRigidBody(data.rigidBody.get());
    }
 
    if (c.kinematic) {
      auto motionState = static_cast<btDefaultMotionState *>(data.motionState.get());
 
      context->transformSystem->updateLocalToWorldTransform(*transformComponent, true);
      const auto & worldTransform = context->transformSystem->getLocalToWorld(transformComponent);
 
      const auto scale = glm::vec3(glm::length(worldTransform[0]), glm::length(worldTransform[1]), glm::length(worldTransform[2]));
      const auto rotation = glm::normalize(glm::quat_cast(glm::mat3(worldTransform * glm::scale(glm::vec3(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z)))));
      const auto position = glm::vec3(worldTransform[3]);
 
      if (glm::any(glm::greaterThan(glm::abs(scale - data.scale), glm::vec3(0.001f)))) {
        manager->removeRigidBody(data.rigidBody.get());
        updateMassProperties(data, c.mass, scale);
        manager->addRigidBody(data.rigidBody.get());
      }
 
      const btTransform origin(toBullet(rotation), toBullet(position));
 
      motionState->setWorldTransform(origin);
      
 
      auto motionComponent = c.getComponent<MotionComponent>();
      if(motionComponent != nullptr) {
        data.rigidBody->setLinearVelocity(toBullet(motionComponent->velocity));
        data.rigidBody->setAngularVelocity(toBullet(motionComponent->angularVelocity));
      }
      else {
        glm::vec3 deltaPosition = position - data.lastPosition;
        glm::vec3 velocity = deltaPosition / static_cast<float>(elapsed);
        data.rigidBody->setLinearVelocity(toBullet(velocity));
      }
 
      data.lastPosition = position;
    } else {
      btTransform transform;
      data.motionState->getWorldTransform(transform);
 
      auto btOrigin = transform.getOrigin();
      auto btRotation = transform.getRotation();
 
      transformComponent->position = *reinterpret_cast<const glm::vec3 *>(&btOrigin);
      transformComponent->rotation = *reinterpret_cast<const glm::quat *>(&btRotation);
      transformComponent->setChanged();
 
      auto motionComponent = c.getComponent<MotionComponent>();
      if (motionComponent != nullptr) {
        motionComponent->angularVelocity = *reinterpret_cast<const glm::vec3 *>(&data.rigidBody->getAngularVelocity());
        motionComponent->velocity = *reinterpret_cast<const glm::vec3 *>(&data.rigidBody->getLinearVelocity());
        motionComponent->setChanged();
      }
    }
  }
}
 
void Cogs::Core::RigidBodySystem::postUpdate(Context * context)
{
  auto * manager = context->services->getService<PhysicsManager>();
  manager->postUpdate(context);
  base::postUpdate(context);
}
 
void Cogs::Core::RigidBodySystem::destroyComponent(ComponentHandle component)
{
  auto * manager = context->services->getService<PhysicsManager>();
  manager->waitForASync(context);
 
  auto c = component.resolveComponent<RigidBodyComponent>();
  auto & data = getData(c);
 
  if (data.rigidBody) {
    manager->removeRigidBody(data.rigidBody.get());
  }
 
  base::destroyComponent(component);
}