////  PhysicalEntitySimulation.cpp//  libraries/physics/src////  Created by Andrew Meadows 2015.04.27//  Copyright 2015 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//   #include "PhysicsHelpers.h"#include "PhysicsLogging.h"#include "ShapeManager.h" #include "PhysicalEntitySimulation.h" PhysicalEntitySimulation::PhysicalEntitySimulation() {} PhysicalEntitySimulation::~PhysicalEntitySimulation() {} void PhysicalEntitySimulation::init(        EntityTreePointer tree,        PhysicsEnginePointer physicsEngine,        EntityEditPacketSender* packetSender) {    assert(tree);    setEntityTree(tree);     assert(physicsEngine);    _physicsEngine = physicsEngine;     assert(packetSender);    _entityPacketSender = packetSender;} // begin EntitySimulation overridesvoid PhysicalEntitySimulation::updateEntitiesInternal(const quint64& now) {    // Do nothing here because the "internal" update the PhysicsEngine::stepSimualtion() which is done elsewhere.} void PhysicalEntitySimulation::addEntityInternal(EntityItemPointer entity) {    QMutexLocker lock(&_mutex);    assert(entity);    assert(!entity->isDead());    if (entity->shouldBePhysical()) {        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        if (!motionState) {            _entitiesToAddToPhysics.insert(entity);        }    } else if (entity->isMovingRelativeToParent()) {        _simpleKinematicEntities.insert(entity);    }} void PhysicalEntitySimulation::removeEntityInternal(EntityItemPointer entity) {    if (entity->isSimulated()) {        EntitySimulation::removeEntityInternal(entity);        QMutexLocker lock(&_mutex);        _entitiesToAddToPhysics.remove(entity);         EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        if (motionState) {            _outgoingChanges.remove(motionState);            _entitiesToRemoveFromPhysics.insert(entity);        } else {            _entitiesToDelete.insert(entity);        }    }} void PhysicalEntitySimulation::takeEntitiesToDelete(VectorOfEntities& entitiesToDelete) {    QMutexLocker lock(&_mutex);    for (auto entity : _entitiesToDelete) {        // this entity is still in its tree, so we insert into the external list        entitiesToDelete.push_back(entity);         // Someday when we invert the entities/physics lib dependencies we can let EntityItem delete its own PhysicsInfo        // rather than do it here        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        if (motionState) {            _entitiesToRemoveFromPhysics.insert(entity);        }    }    _entitiesToDelete.clear();} void PhysicalEntitySimulation::changeEntityInternal(EntityItemPointer entity) {    // queue incoming changes: from external sources (script, EntityServer, etc) to physics engine    QMutexLocker lock(&_mutex);    assert(entity);    EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());    if (motionState) {        if (!entity->shouldBePhysical()) {            // the entity should be removed from the physical simulation            _pendingChanges.remove(motionState);            _physicalObjects.remove(motionState);            _outgoingChanges.remove(motionState);            _entitiesToRemoveFromPhysics.insert(entity);            if (entity->isMovingRelativeToParent()) {                _simpleKinematicEntities.insert(entity);            }        } else {            _pendingChanges.insert(motionState);        }    } else if (entity->shouldBePhysical()) {        // The intent is for this object to be in the PhysicsEngine, but it has no MotionState yet.        // Perhaps it's shape has changed and it can now be added?        _entitiesToAddToPhysics.insert(entity);        _simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic    } else if (entity->isMovingRelativeToParent()) {        _simpleKinematicEntities.insert(entity);    } else {        _simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic    }} void PhysicalEntitySimulation::clearEntitiesInternal() {    // TODO: we should probably wait to lock the _physicsEngine so we don't mess up data structures    // while it is in the middle of a simulation step.  As it is, we're probably in shutdown mode    // anyway, so maybe the simulation was already properly shutdown?  Cross our fingers...     // copy everything into _entitiesToDelete    for (auto stateItr : _physicalObjects) {        EntityMotionState* motionState = static_cast<EntityMotionState*>(&(*stateItr));        _entitiesToDelete.insert(motionState->getEntity());    }     // then remove the objects (aka MotionStates) from physics    _physicsEngine->removeSetOfObjects(_physicalObjects);     // delete the MotionStates    // TODO: after we invert the entities/physics lib dependencies we will let EntityItem delete    // its own PhysicsInfo rather than do it here    for (auto entity : _entitiesToDelete) {        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        if (motionState) {            entity->setPhysicsInfo(nullptr);            delete motionState;        }    }     // finally clear all lists maintained by this class    _physicalObjects.clear();    _entitiesToRemoveFromPhysics.clear();    _entitiesToRelease.clear();    _entitiesToAddToPhysics.clear();    _pendingChanges.clear();    _outgoingChanges.clear();} // virtualvoid PhysicalEntitySimulation::prepareEntityForDelete(EntityItemPointer entity) {    assert(entity);    assert(entity->isDead());    entity->clearActions(getThisPointer());    removeEntityInternal(entity);}// end EntitySimulation overrides void PhysicalEntitySimulation::getObjectsToRemoveFromPhysics(VectorOfMotionStates& result) {    result.clear();    QMutexLocker lock(&_mutex);    for (auto entity: _entitiesToRemoveFromPhysics) {        // make sure it isn't on any side lists        _entitiesToAddToPhysics.remove(entity);         EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        if (motionState) {            _pendingChanges.remove(motionState);            _outgoingChanges.remove(motionState);            _physicalObjects.remove(motionState);            result.push_back(motionState);            _entitiesToRelease.insert(entity);        }         if (entity->isDead()) {            _entitiesToDelete.insert(entity);        }    }    _entitiesToRemoveFromPhysics.clear();} void PhysicalEntitySimulation::deleteObjectsRemovedFromPhysics() {    QMutexLocker lock(&_mutex);    for (auto entity: _entitiesToRelease) {        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());        assert(motionState);        entity->setPhysicsInfo(nullptr);        delete motionState;         if (entity->isDead()) {            _entitiesToDelete.insert(entity);        }    }    _entitiesToRelease.clear();} void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& result) {    result.clear();    QMutexLocker lock(&_mutex);    SetOfEntities::iterator entityItr = _entitiesToAddToPhysics.begin();    while (entityItr != _entitiesToAddToPhysics.end()) {        EntityItemPointer entity = (*entityItr);        assert(!entity->getPhysicsInfo());        if (entity->isDead()) {            prepareEntityForDelete(entity);            entityItr = _entitiesToAddToPhysics.erase(entityItr);        } else if (!entity->shouldBePhysical()) {            // this entity should no longer be on the internal _entitiesToAddToPhysics            entityItr = _entitiesToAddToPhysics.erase(entityItr);            if (entity->isMovingRelativeToParent()) {                _simpleKinematicEntities.insert(entity);            }        } else if (entity->isReadyToComputeShape()) {            ShapeInfo shapeInfo;            entity->computeShapeInfo(shapeInfo);            int numPoints = shapeInfo.getLargestSubshapePointCount();            if (shapeInfo.getType() == SHAPE_TYPE_COMPOUND) {                if (numPoints > MAX_HULL_POINTS) {                    qWarning() << "convex hull with" << numPoints                        << "points for entity" << entity->getName()                        << "at" << entity->getPosition() << " will be reduced";                }            }            btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));            if (shape) {                EntityMotionState* motionState = new EntityMotionState(shape, entity);                entity->setPhysicsInfo(static_cast<void*>(motionState));                _physicalObjects.insert(motionState);                result.push_back(motionState);                entityItr = _entitiesToAddToPhysics.erase(entityItr);            } else {                //qWarning() << "Failed to generate new shape for entity." << entity->getName();                ++entityItr;            }        } else {            ++entityItr;        }    }} void PhysicalEntitySimulation::setObjectsToChange(const VectorOfMotionStates& objectsToChange) {    QMutexLocker lock(&_mutex);    for (auto object : objectsToChange) {        _pendingChanges.insert(static_cast<EntityMotionState*>(object));    }} void PhysicalEntitySimulation::getObjectsToChange(VectorOfMotionStates& result) {    result.clear();    QMutexLocker lock(&_mutex);    for (auto stateItr : _pendingChanges) {        EntityMotionState* motionState = &(*stateItr);        result.push_back(motionState);    }    _pendingChanges.clear();} void PhysicalEntitySimulation::handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates) {    for (auto stateItr : motionStates) {        ObjectMotionState* state = &(*stateItr);        assert(state);        if (state->getType() == MOTIONSTATE_TYPE_ENTITY) {            EntityMotionState* entityState = static_cast<EntityMotionState*>(state);            entityState->handleDeactivation();            EntityItemPointer entity = entityState->getEntity();            _entitiesToSort.insert(entity);        }    }} void PhysicalEntitySimulation::handleChangedMotionStates(const VectorOfMotionStates& motionStates) {    QMutexLocker lock(&_mutex);     // walk the motionStates looking for those that correspond to entities    for (auto stateItr : motionStates) {        ObjectMotionState* state = &(*stateItr);        assert(state);        if (state->getType() == MOTIONSTATE_TYPE_ENTITY) {            EntityMotionState* entityState = static_cast<EntityMotionState*>(state);            EntityItemPointer entity = entityState->getEntity();            assert(entity.get());            if (entityState->isCandidateForOwnership()) {                _outgoingChanges.insert(entityState);            }            _entitiesToSort.insert(entity);        }    }     uint32_t numSubsteps = _physicsEngine->getNumSubsteps();    if (_lastStepSendPackets != numSubsteps) {        _lastStepSendPackets = numSubsteps;         if (Physics::getSessionUUID().isNull()) {            // usually don't get here, but if so --> nothing to do            _outgoingChanges.clear();            return;        }         // look for entities to prune or update        QSet<EntityMotionState*>::iterator stateItr = _outgoingChanges.begin();        while (stateItr != _outgoingChanges.end()) {            EntityMotionState* state = *stateItr;            if (!state->isCandidateForOwnership()) {                // prune                stateItr = _outgoingChanges.erase(stateItr);            } else if (state->shouldSendUpdate(numSubsteps)) {                // update                state->sendUpdate(_entityPacketSender, numSubsteps);                ++stateItr;            } else {                ++stateItr;            }        }    }} void PhysicalEntitySimulation::handleCollisionEvents(const CollisionEvents& collisionEvents) {    for (auto collision : collisionEvents) {        // NOTE: The collision event is always aligned such that idA is never NULL.        // however idB may be NULL.        if (!collision.idB.isNull()) {            emit entityCollisionWithEntity(collision.idA, collision.idB, collision);        }    }}  void PhysicalEntitySimulation::addDynamic(EntityDynamicPointer dynamic) {    if (_physicsEngine) {        // FIXME put fine grain locking into _physicsEngine        {            QMutexLocker lock(&_mutex);            const QUuid& dynamicID = dynamic->getID();            if (_physicsEngine->getDynamicByID(dynamicID)) {                qCDebug(physics) << "warning -- PhysicalEntitySimulation::addDynamic -- adding an "                    "dynamic that was already in _physicsEngine";            }        }        EntitySimulation::addDynamic(dynamic);    }} void PhysicalEntitySimulation::applyDynamicChanges() {    QList<EntityDynamicPointer> dynamicsFailedToAdd;    if (_physicsEngine) {        // FIXME put fine grain locking into _physicsEngine        QMutexLocker lock(&_mutex);        foreach(QUuid dynamicToRemove, _dynamicsToRemove) {            _physicsEngine->removeDynamic(dynamicToRemove);        }        foreach (EntityDynamicPointer dynamicToAdd, _dynamicsToAdd) {            if (!_dynamicsToRemove.contains(dynamicToAdd->getID())) {                if (!_physicsEngine->addDynamic(dynamicToAdd)) {                    dynamicsFailedToAdd += dynamicToAdd;                }            }        }    }    // applyDynamicChanges will clear _dynamicsToRemove and _dynamicsToAdd    EntitySimulation::applyDynamicChanges();    // put back the ones that couldn't yet be added    foreach (EntityDynamicPointer dynamicFailedToAdd, dynamicsFailedToAdd) {        addDynamic(dynamicFailedToAdd);    }}