PhysicsEngine.h

//
//  PhysicsEngine.h
//  libraries/physics/src
//
//  Created by Andrew Meadows 2014.10.29
//  Copyright 2014 High Fidelity, Inc.
//
//  Distributed under the Apache License, Version 2.0.
//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
 
#ifndef hifi_PhysicsEngine_h
#define hifi_PhysicsEngine_h
 
#include <stdint.h>
#include <set>
#include <vector>
 
#include <QUuid>
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
 
#include "BulletUtil.h"
#include "ContactInfo.h"
#include "ObjectMotionState.h"
#include "ThreadSafeDynamicsWorld.h"
#include "ObjectAction.h"
#include "ObjectConstraint.h"
 
const float HALF_SIMULATION_EXTENT = 512.0f; // meters
 
class CharacterController;
class PhysicsDebugDraw;
 
// simple class for keeping track of contacts
class ContactKey {
public:
    ContactKey() = delete;
    ContactKey(void* a, void* b) : _a(a), _b(b) {}
    bool operator<(const ContactKey& other) const { return _a < other._a || (_a == other._a && _b < other._b); }
    bool operator==(const ContactKey& other) const { return _a == other._a && _b == other._b; }
    void* _a; // ObjectMotionState pointer
    void* _b; // ObjectMotionState pointer
};
 
struct ContactTestResult {
    ContactTestResult() = delete;
 
    ContactTestResult(const ContactTestResult& contactTestResult) :
        foundID(contactTestResult.foundID),
        testCollisionPoint(contactTestResult.testCollisionPoint),
        foundCollisionPoint(contactTestResult.foundCollisionPoint),
        collisionNormal(contactTestResult.collisionNormal) {
    }
 
    ContactTestResult(const QUuid& foundID, const glm::vec3& testCollisionPoint, const glm::vec3& otherCollisionPoint, const glm::vec3& collisionNormal) :
        foundID(foundID),
        testCollisionPoint(testCollisionPoint),
        foundCollisionPoint(otherCollisionPoint),
        collisionNormal(collisionNormal) {
    }
 
    QUuid foundID;
    // The deepest point of an intersection within the volume of the test shape, in world space.
    glm::vec3 testCollisionPoint;
    // The deepest point of an intersection within the volume of the found object, in world space.
    glm::vec3 foundCollisionPoint;
    // The normal vector of this intersection
    glm::vec3 collisionNormal;
};
 
using ContactMap = std::map<ContactKey, ContactInfo>;
using CollisionEvents = std::vector<Collision>;
 
class PhysicsEngine {
public:
    using ContactAddedCallback = bool (*)(btManifoldPoint& cp,
            const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
            const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1);
 
    class Transaction {
    public:
        void clear() {
            objectsToRemove.clear();
            objectsToAdd.clear();
            objectsToReinsert.clear();
            activeStaticObjects.clear();
        }
        std::vector<ObjectMotionState*> objectsToRemove;
        std::vector<ObjectMotionState*> objectsToAdd;
        std::vector<ObjectMotionState*> objectsToReinsert;
        std::vector<ObjectMotionState*> activeStaticObjects;
    };
 
    PhysicsEngine(const glm::vec3& offset);
    ~PhysicsEngine();
    void init();
 
    uint32_t getNumSubsteps() const;
    int32_t getNumCollisionObjects() const;
 
    void removeObjects(const VectorOfMotionStates& objects);
    void removeSetOfObjects(const SetOfMotionStates& objects); // only called during teardown
 
    void addObjects(const VectorOfMotionStates& objects);
    void changeObjects(const VectorOfMotionStates& objects);
    void reinsertObject(ObjectMotionState* object);
 
    void processTransaction(Transaction& transaction);
 
    void stepSimulation();
    void harvestPerformanceStats();
    void printPerformanceStatsToFile(const QString& filename);
    void updateContactMap();
    void doOwnershipInfectionForConstraints();
 
    bool hasOutgoingChanges() const { return _hasOutgoingChanges; }
 
    const VectorOfMotionStates& getChangedMotionStates();
    const VectorOfMotionStates& getDeactivatedMotionStates() const { return _dynamicsWorld->getDeactivatedMotionStates(); }
 
    const CollisionEvents& getCollisionEvents();
 
    void dumpStatsIfNecessary();
 
    void saveNextPhysicsStats(QString filename);
 
    void setOriginOffset(const glm::vec3& offset) { _originOffset = offset; }
 
    const glm::vec3& getOriginOffset() const { return _originOffset; }
 
    void setCharacterController(CharacterController* character);
 
    void dumpNextStats() { _dumpNextStats = true; }
 
    EntityDynamicPointer getDynamicByID(const QUuid& dynamicID) const;
    bool addDynamic(EntityDynamicPointer dynamic);
    void removeDynamic(const QUuid dynamicID);
    void forEachDynamic(std::function<void(EntityDynamicPointer)> actor);
 
    void setShowBulletWireframe(bool value);
    void setShowBulletAABBs(bool value);
    void setShowBulletContactPoints(bool value);
    void setShowBulletConstraints(bool value);
    void setShowBulletConstraintLimits(bool value);
 
    // Function for getting colliding objects in the world of specified type
    // See PhysicsCollisionGroups.h for mask flags.
    std::vector<ContactTestResult> contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group = USER_COLLISION_GROUP_DYNAMIC, float threshold = 0.0f) const;
 
    void setContactAddedCallback(ContactAddedCallback cb);
 
    btDiscreteDynamicsWorld* getDynamicsWorld() const { return _dynamicsWorld; }
    void removeContacts(ObjectMotionState* motionState);
 
private:
    QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);
    void addObjectToDynamicsWorld(ObjectMotionState* motionState);
 
    void bumpAndPruneContacts(ObjectMotionState* motionState);
 
    void doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB);
 
    btClock _clock;
    btDefaultCollisionConfiguration* _collisionConfig = NULL;
    btCollisionDispatcher* _collisionDispatcher = NULL;
    btBroadphaseInterface* _broadphaseFilter = NULL;
    btSequentialImpulseConstraintSolver* _constraintSolver = NULL;
    ThreadSafeDynamicsWorld* _dynamicsWorld = NULL;
    btGhostPairCallback* _ghostPairCallback = NULL;
    std::unique_ptr<PhysicsDebugDraw> _physicsDebugDraw;
 
    ContactMap _contactMap;
    CollisionEvents _collisionEvents;
    QHash<QUuid, EntityDynamicPointer> _objectDynamics;
    QHash<btRigidBody*, QSet<QUuid>> _objectDynamicsByBody;
    std::set<btRigidBody*> _activeStaticBodies;
    QString _statsFilename;
 
    glm::vec3 _originOffset;
 
    CharacterController* _myAvatarController;
 
    uint32_t _numContactFrames { 0 };
 
    bool _dumpNextStats { false };
    bool _saveNextStats { false };
    bool _hasOutgoingChanges { false };
 
};
 
typedef std::shared_ptr<PhysicsEngine> PhysicsEnginePointer;
 
#endif // hifi_PhysicsEngine_h