system.hpp

Go to the documentation of this file.

 
#ifndef TOYMAKERENGINE_PHYSICSSYSTEM_H
#define TOYMAKERENGINE_PHYSICSSYSTEM_H
 
#include <unordered_set>
 
#include <nlohmann/json.hpp>
 
#include "../core/ecs_world.hpp"
#include "../signals.hpp"
#include "../spatial_query/types.hpp"
#include "../spatial_query/sweep_prune.hpp"
 
#include "types.hpp"
 
namespace ToyMaker {
    class PhysicsSystem: public System<PhysicsSystem, std::tuple<PhysicsState, ObjectBounds>, std::tuple<>>, public SignalTracker {
    public:
        using ConstraintID = std::size_t;

        using SignalCollidedData = std::pair<CollisionPair, Collision>;

        using SignalSeparatedData = CollisionPair;

        using SignalCollided = std::shared_ptr<Signal<SignalCollidedData>>;

        using SignalSeparated = std::shared_ptr<Signal<SignalSeparatedData>>;
 

        static const std::string SignalCollidedPrefix;

        static const std::string SignalSeparatedPrefix;
 
        explicit PhysicsSystem(std::weak_ptr<ECSWorld> world):
        System<PhysicsSystem, std::tuple<PhysicsState, ObjectBounds>, std::tuple<>>{ world }
        {}

        inline static std::string getSystemTypeName() { return "PhysicsSystem"; }

        inline void setSubsteps(uint8_t newSubsteps) {
            assert(newSubsteps > 0 && "Substeps must be 1 or greater");
            mSubsteps = newSubsteps;
        }

        inline uint8_t getSubsteps() const {
            return mSubsteps;
        }

        template<typename TConstraint,
            std::enable_if_t<
                std::is_base_of<
                    ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                    TConstraint
                >::value, bool
            > = true
        >
        ConstraintID registerConstraint(
            const typename TConstraint::Config& config,
            const std::vector<std::pair<EntityID, typename TConstraint::Parameter>>& parameters,
            float compliance
        );

        template<typename TConstraint>
        TConstraint& getConstraint(ConstraintID constraint);

        template<typename TConstraint>
        const TConstraint& getConstraint(ConstraintID constraint) const;

        void unregisterConstraint(ConstraintID constraint);
 
    private:
 
        // storage for intermediate physics state
        struct PhysicsStateFull {
            ObjectBounds mBounds;
            PhysicsState mPhysics;
        };
 
        // storage for intermediate collision events
        union CollisionEvent {
            SignalCollidedData mCollided;
            SignalSeparatedData mSeparated;
        };
 
        struct CollisionReport {
            bool mCollided { false };
            CollisionEvent mEvent;
        };

        void onSimulationActivated() override;

        void onSimulationStep(uint32_t timestepMillis) override;

        void onEntityEnabled(EntityID entityID) override;

        void onEntityUpdated(EntityID entityID, ComponentType updatedComponent) override;

        void onEntityDisabled(EntityID entityID) override;

        bool isConstraintActive(ConstraintID constraint) const;

        void updateProperties(EntityID entityID);

        void refreshActiveConstraints();

        void integrateForces(float substepSeconds, std::unordered_map<EntityID, PhysicsStateFull>& previousState, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void deriveVelocities(float substepSeconds, const std::unordered_map<EntityID, PhysicsStateFull>& previousState, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void applyPositionCollisionConstraints(std::map<CollisionPair, ConstraintContactManifold>& potentialCollisions, float substepSeconds, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void applyVelocityCollisionConstraints(std::map<CollisionPair, ConstraintContactManifold>& constraints, float substepSeconds, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void applyPositionConstraints(std::map<CollisionPair, ConstraintContactManifold>& constraints, float substepSeconds, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void applyVelocityConstraints(std::map<CollisionPair, ConstraintContactManifold>& constraints, float substepSeconds, std::unordered_map<EntityID, PhysicsStateFull>& currentState);

        void updateCollisionEventQueue(
            std::map<CollisionPair, ConstraintContactManifold>& potentialCollisions,
            std::queue<CollisionReport>& queuedReports,
            std::unordered_map<EntityID, PhysicsStateFull>& previousStates,
            std::unordered_map<EntityID, PhysicsStateFull>& currentStates,
            uint8_t nthSubstep
        );

        void onCollided(const CollisionPair& pair, const Collision& collision, std::queue<CollisionReport>& queuedReports);

        void onSeparated(const CollisionPair& pair, std::queue<CollisionReport>& queuedReports);

        void registerCollisionSignal(EntityID entity);

        void unregisterCollisionSignal(EntityID entity);

        void reportCollisions(
            std::queue<CollisionReport>& reports,
            std::unordered_map<EntityID, std::pair<SignalCollided, SignalSeparated>> signallers
        ) const;

        bool wasColliding(const CollisionPair& pair) const;

        void collectPotentialCollisions(float substepSeconds, std::queue<CollisionReport>& queuedReports);

        std::set<EntityID> mEntitiesUninitialized {};

        std::unordered_map<EntityID, PhysicsStateFull> mEntityStatePrevious {};

        std::unordered_map<EntityID, PhysicsStateFull> mEntityStateCurrent {};

        std::unordered_map<EntityID, std::set<ConstraintID>> mEntityConstraintMap {};

        std::vector<std::pair<std::unique_ptr<BaseConstraint>, std::vector<EntityID>>> mConstraints {};

        std::unordered_set<ConstraintID> mConstraintsDeleted {};

        std::unordered_map<EntityID, ConstraintDampingRigidbody> mEntityDamping {};

        SweepPrune mBroadPhase {};

        std::unordered_map<EntityID, std::unordered_set<EntityID>> mEntityCollision {};

        std::unordered_set<EntityID> mPotentialColliders {};

        std::queue<CollisionReport> mCollisionReports {};

        std::unordered_map<EntityID, std::pair<SignalCollided, SignalSeparated>> mCollisionSignallers {};

        std::map<CollisionPair, ConstraintContactManifold> mCollisionConstraints {};

        uint8_t mSubsteps { 3 };

        bool mRequiresInitialization { true };

        bool mConstraintsInitialized { true };
    };
 
    inline const std::string PhysicsSystem::SignalCollidedPrefix { "collided_" };
    inline const std::string PhysicsSystem::SignalSeparatedPrefix { "separated_" };
 
    template<typename TConstraint,
        std::enable_if_t<
            std::is_base_of<
                ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                TConstraint
            >::value, bool
        >
    >
    PhysicsSystem::ConstraintID PhysicsSystem::registerConstraint(
        const typename TConstraint::Config& config,
        const std::vector<std::pair<EntityID, typename TConstraint::Parameter>>& participants,
        float compliance
    ) {
        // reclaim a deleted constraint id if possible
        ConstraintID newConstraint { mConstraints.size() };
        if(!mConstraintsDeleted.empty()) {
            newConstraint = *mConstraintsDeleted.begin();
            mConstraintsDeleted.erase(newConstraint);
        }
 
        // get separate constraint parameter and participating entity lists
        std::unordered_set<EntityID> registeredEntities {};
        std::vector<EntityID> entities {};
        std::vector<typename TConstraint::Parameter> constraintData {};
        for(const std::pair<EntityID, typename TConstraint::Parameter>& participant: participants) {
            const bool alreadyRegistered {
                registeredEntities.find(participant.first) != registeredEntities.end()
            };
            assert(!alreadyRegistered && "An entity has appeared twice in the constraint registration list, which is invalid.");
            constraintData.push_back(participant.second);
            entities.push_back(participant.first);
            registeredEntities.insert(participant.first);
        }
 
        // create constraint
        if(newConstraint == mConstraints.size()) {
            mConstraints.push_back({
                std::make_unique<TConstraint>(config, constraintData, compliance),
                entities
            });
        } else {
            mConstraints[newConstraint] = {
                std::make_unique<TConstraint>(config, constraintData, compliance),
                entities
            };
        }
 
        for(const auto& entity: entities) {
            mEntityConstraintMap[entity].insert(newConstraint);
        }
 
        return newConstraint;
    }
 
    template<typename TConstraint>
    TConstraint& PhysicsSystem::getConstraint(ConstraintID constraint) {
        return static_cast<TConstraint>(*mConstraints.at(constraint).first);
    }
 
    template<typename TConstraint>
    const TConstraint& PhysicsSystem::getConstraint(ConstraintID constraint) const {
        return static_cast<TConstraint>(*mConstraints.at(constraint).first);
    }
}
 
#endif