ConstraintSystem.cpp

#include "ConstraintSystem.h"
 
#include "Systems/Core/TransformSystem.h"
 
#include "ExtensionRegistry.h"
 
#include "Services/Services.h"
 
#include "PhysicsManager.h"
#include "RigidBodySystem.h"
 
#include "Foundation/ComponentModel/Entity.h"
 
namespace
{
  using namespace Cogs::Core;
 
  template<typename ComponentType, typename SystemType>
  void removeConstraint(Context * context, SystemType * system, ComponentType * component)
  {
    auto manager = context->services->getService<PhysicsManager>();
 
    auto & data = system->getData(component);
 
    if (data.constraint) {
      manager->removeConstraint(data.constraint.get());
    }
  }
 
  void updateConstraintData(Context * context, ConstraintComponent * component, ConstraintData & data, bool updateTransform = true)
  {
    auto rigidBodySystem = ExtensionRegistry::getExtensionSystem<RigidBodySystem>(context);
 
    auto rigidA = component->getComponent<RigidBodyComponent>();
    auto rigidB = component->target->getComponent<RigidBodyComponent>();
 
    if (!rigidA || !rigidB) return;
 
    auto & dataA = rigidBodySystem->getData(rigidA);
    auto & dataB = rigidBodySystem->getData(rigidB);
 
    data.rigidBody = dataA.rigidBody.get();
    data.targetRigidBody = dataB.rigidBody.get();
 
    if (dataA.rigidBody && dataB.rigidBody) {
      dataA.rigidBody->setActivationState(DISABLE_DEACTIVATION);
      dataB.rigidBody->setActivationState(DISABLE_DEACTIVATION);
 
      if (updateTransform) {
        auto transformComponentA = component->getComponent<TransformComponent>();
        auto transformComponentB = component->target->getComponent<TransformComponent>();
 
        context->transformSystem->updateLocalToWorldTransform(*transformComponentA, true);
        data.transform = context->transformSystem->getLocalToWorld(transformComponentA);
 
        context->transformSystem->updateLocalToWorldTransform(*transformComponentB, true);
        data.targetTransform = context->transformSystem->getLocalToWorld(transformComponentB);
      }
    }
  }
}
 
void Cogs::Core::FixedConstraintSystem::update(Context * context)
{
  auto manager = context->services->getService<PhysicsManager>();
 
  for (auto & component : pool) {
    auto & data = getData(&component);
 
    if (!data.constraint || component.hasChanged()) {
      updateConstraintData(context, &component, data);
 
      if (!data.rigidBody || !data.targetRigidBody) continue;
 
      auto positionA = glm::vec4(data.transform[3]);
      auto positionB = glm::vec4(data.targetTransform[3]);
 
      btTransform tA = btTransform(btQuaternion(0, 0, 0));
      btTransform tB = btTransform(btQuaternion(0, 0, 0), toBullet(positionA - positionB));
      auto constraint = data.createConstraint<btFixedConstraint>(*data.rigidBody, *data.targetRigidBody, tA, tB);
 
      manager->addConstraint(constraint);
    }
  }
}
 
void Cogs::Core::FixedConstraintSystem::destroyComponent(ComponentHandle component)
{
  removeConstraint(context, this, component.resolveComponent<FixedConstraintComponent>());
 
  ComponentSystemWithDataPool::destroyComponent(component);
}
 
void Cogs::Core::HingeConstraintSystem::update(Context * context)
{
  auto manager = context->services->getService<PhysicsManager>();
 
  for (auto & component : pool) {
    auto & data = getData(&component);
 
    if (!data.constraint || component.hasChanged()) {
      updateConstraintData(context, &component, data);
 
      if (!data.rigidBody || !data.targetRigidBody) continue;
 
      auto constraint = data.createConstraint<btHingeConstraint>(
        *data.rigidBody,
        *data.targetRigidBody,
        toBullet(component.anchor),
        toBullet(component.targetAnchor),
        toBullet(component.axis),
        toBullet(component.targetAxis),
        true);
 
      manager->addConstraint(constraint);
    }
  }
}
 
void Cogs::Core::HingeConstraintSystem::destroyComponent(ComponentHandle component)
{
  removeConstraint(context, this, component.resolveComponent<HingeConstraintComponent>());
 
  ComponentSystemWithDataPool::destroyComponent(component);
}
 
void Cogs::Core::SpringConstraintSystem::update(Context * context)
{
  auto manager = context->services->getService<PhysicsManager>();
 
  for (auto & component : pool) {
    auto & data = getData(&component);
 
    if (!data.constraint || component.hasChanged()) {
      updateConstraintData(context, &component, data);
 
      if (!data.rigidBody || !data.targetRigidBody) continue;
 
      auto positionA = glm::vec3(data.transform[3]);
      auto positionB = glm::vec3(data.targetTransform[3]);
 
      btTransform tA = btTransform(btQuaternion(0, 0, 0));
      btTransform tB = btTransform(btQuaternion(0, 0, 0), toBullet((positionA + component.anchor) - (positionB + component.targetAnchor)));
        
      auto constraint = data.createConstraint<btGeneric6DofSpring2Constraint>(*data.rigidBody, *data.targetRigidBody, tA, tB);
 
      for (int i = 0; i < 6; ++i) {
        constraint->enableSpring(i, true);
        constraint->setLimit(i, -10, 10);
        constraint->setStiffness(i, component.stiffness);
        constraint->setDamping(i, component.damping);
        constraint->setEquilibriumPoint(i, 0);
      }
 
      manager->addConstraint(constraint);
    }
  }
}
 
void Cogs::Core::SpringConstraintSystem::destroyComponent(ComponentHandle component)
{
  removeConstraint(context, this, component.resolveComponent<SpringConstraintComponent>());
 
  ComponentSystemWithDataPool::destroyComponent(component);
}
 
void Cogs::Core::GenericConstraintSystem::update(Context * context)
{
  auto manager = context->services->getService<PhysicsManager>();
 
  for (auto & component : pool) {
    auto & data = getData(&component);
 
    if (!data.constraint || component.hasChanged()) {
      updateConstraintData(context, &component, data);
 
      if (!data.rigidBody || !data.targetRigidBody) continue;
 
      auto positionA = glm::vec3(data.transform[3]);
      auto positionB = glm::vec3(data.targetTransform[3]);
 
      btTransform tA = btTransform(btQuaternion(0, 0, 0));
      btTransform tB = btTransform(btQuaternion(0, 0, 0), toBullet((positionA + component.anchor) - (positionB + component.targetAnchor)));
 
      auto constraint = data.createConstraint<btGeneric6DofSpring2Constraint>(*data.rigidBody, *data.targetRigidBody, tA, tB);
 
      manager->addConstraint(constraint);
    }
  }
}
 
void Cogs::Core::GenericConstraintSystem::destroyComponent(ComponentHandle component)
{
  removeConstraint(context, this, component.resolveComponent<GenericConstraintComponent>());
 
  ComponentSystemWithDataPool::destroyComponent(component);
}