////  EntityCollisionSystem.cpp//  libraries/entities/src////  Created by Brad Hefta-Gaub on 9/23/14.//  Copyright 2013-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// #include <algorithm>#include <AbstractAudioInterface.h>#include <VoxelTree.h>#include <AvatarData.h>#include <CollisionInfo.h>#include <HeadData.h>#include <HandData.h>#include <PerfStat.h>#include <SphereShape.h> #include "EntityCollisionSystem.h"#include "EntityEditPacketSender.h"#include "EntityItem.h"#include "EntityTreeElement.h"#include "EntityTree.h" const int MAX_COLLISIONS_PER_Entity = 16; EntityCollisionSystem::EntityCollisionSystem()    :   SimpleEntitySimulation(),         _packetSender(NULL),        _voxels(NULL),        _audio(NULL),        _avatars(NULL),        _collisions(MAX_COLLISIONS_PER_Entity) {} void EntityCollisionSystem::init(EntityEditPacketSender* packetSender,        EntityTree* entities, VoxelTree* voxels, AbstractAudioInterface* audio,        AvatarHashMap* avatars) {    assert(entities);    setEntityTree(entities);    _packetSender = packetSender;    _voxels = voxels;    _audio = audio;    _avatars = avatars;} EntityCollisionSystem::~EntityCollisionSystem() {} void EntityCollisionSystem::updateCollisions() {    PerformanceTimer perfTimer("collisions");    assert(_entityTree);    // update all Entities    if (_entityTree->tryLockForRead()) {        foreach (EntityItem* entity, _movingEntities) {            checkEntity(entity);        }        _entityTree->unlock();    }}  void EntityCollisionSystem::checkEntity(EntityItem* entity) {    updateCollisionWithVoxels(entity);    updateCollisionWithEntities(entity);    updateCollisionWithAvatars(entity);} void EntityCollisionSystem::emitGlobalEntityCollisionWithVoxel(EntityItem* entity,                                             VoxelDetail* voxelDetails, const CollisionInfo& collision) {    EntityItemID entityItemID = entity->getEntityItemID();    emit entityCollisionWithVoxel(entityItemID, *voxelDetails, collision);} void EntityCollisionSystem::emitGlobalEntityCollisionWithEntity(EntityItem* entityA,                                             EntityItem* entityB, const CollisionInfo& collision) {                                                EntityItemID idA = entityA->getEntityItemID();    EntityItemID idB = entityB->getEntityItemID();    emit entityCollisionWithEntity(idA, idB, collision);} void EntityCollisionSystem::updateCollisionWithVoxels(EntityItem* entity) {     if (entity->getIgnoreForCollisions() || !entity->getCollisionsWillMove()) {        return; // bail early if this entity is to be ignored or wont move    }     glm::vec3 center = entity->getPosition() * (float)(TREE_SCALE);    float radius = entity->getRadius() * (float)(TREE_SCALE);    const float ELASTICITY = 0.4f;    const float DAMPING = 0.05f;    CollisionInfo collisionInfo;    collisionInfo._damping = DAMPING;    collisionInfo._elasticity = ELASTICITY;    VoxelDetail* voxelDetails = NULL;    if (_voxels->findSpherePenetration(center, radius, collisionInfo._penetration, (void**)&voxelDetails)) {         // findSpherePenetration() only computes the penetration but we also want some other collision info        // so we compute it ourselves here.  Note that we must multiply scale by TREE_SCALE when feeding         // the results to systems outside of this octree reference frame.        collisionInfo._contactPoint = (float)TREE_SCALE * (entity->getPosition() + entity->getRadius() * glm::normalize(collisionInfo._penetration));        // let the global script run their collision scripts for Entities if they have them        emitGlobalEntityCollisionWithVoxel(entity, voxelDetails, collisionInfo);         // we must scale back down to the octree reference frame before updating the Entity properties        collisionInfo._penetration /= (float)(TREE_SCALE);        collisionInfo._contactPoint /= (float)(TREE_SCALE);         applyHardCollision(entity, collisionInfo);        delete voxelDetails; // cleanup returned details    }} void EntityCollisionSystem::updateCollisionWithEntities(EntityItem* entityA) {     if (entityA->getIgnoreForCollisions()) {        return; // bail early if this entity is to be ignored...    }     // don't collide entities with unknown IDs,    if (!entityA->isKnownID()) {        return;    }     glm::vec3 penetration;    EntityItem* entityB = NULL;        const int MAX_COLLISIONS_PER_ENTITY = 32;    CollisionList collisions(MAX_COLLISIONS_PER_ENTITY);    bool shapeCollisionsAccurate = false;        bool shapeCollisions = _entityTree->findShapeCollisions(&entityA->getCollisionShapeInMeters(),                                             collisions, Octree::NoLock, &shapeCollisionsAccurate);     if (shapeCollisions) {        for(int i = 0; i < collisions.size(); i++) {             CollisionInfo* collision = collisions[i];            penetration = collision->_penetration;            entityB = static_cast<EntityItem*>(collision->_extraData);                        // don't collide entities with unknown IDs,            if (!entityB->isKnownID()) {                continue; // skip this loop pass if the entity has an unknown ID            }                        // NOTE: 'penetration' is the depth that 'entityA' overlaps 'entityB'.  It points from A into B.            glm::vec3 penetrationInTreeUnits = penetration / (float)(TREE_SCALE);             // Even if the Entities overlap... when the Entities are already moving appart            // we don't want to count this as a collision.            glm::vec3 relativeVelocity = entityA->getVelocity() - entityB->getVelocity();                        bool fullyEnclosedCollision = glm::length(penetrationInTreeUnits) > entityA->getLargestDimension();             bool wantToMoveA = entityA->getCollisionsWillMove();            bool wantToMoveB = entityB->getCollisionsWillMove();            bool movingTowardEachOther = glm::dot(relativeVelocity, penetrationInTreeUnits) > 0.0f;             // only do collisions if the entities are moving toward each other and one or the other            // of the entities are movable from collisions            bool doCollisions = !fullyEnclosedCollision && movingTowardEachOther && (wantToMoveA || wantToMoveB);              if (doCollisions) {                 quint64 now = usecTimestampNow();                        CollisionInfo collision;                collision._penetration = penetration;                // for now the contactPoint is the average between the the two paricle centers                collision._contactPoint = (0.5f * (float)TREE_SCALE) * (entityA->getPosition() + entityB->getPosition());                emitGlobalEntityCollisionWithEntity(entityA, entityB, collision);                 glm::vec3 axis = glm::normalize(penetration);                glm::vec3 axialVelocity = glm::dot(relativeVelocity, axis) * axis;                 float massA = entityA->getMass();                float massB = entityB->getMass();                float totalMass = massA + massB;                float massRatioA = (2.0f * massB / totalMass);                float massRatioB = (2.0f * massA / totalMass);                 // in the event that one of our entities is non-moving, then fix up these ratios                if (wantToMoveA && !wantToMoveB) {                    massRatioA = 2.0f;                    massRatioB = 0.0f;                }                 if (!wantToMoveA && wantToMoveB) {                    massRatioA = 0.0f;                    massRatioB = 2.0f;                }                            // unless the entity is configured to not be moved by collision, calculate it's new position                // and velocity and apply it                if (wantToMoveA) {                    // handle Entity A                    glm::vec3 newVelocityA = entityA->getVelocity() - axialVelocity * massRatioA;                    glm::vec3 newPositionA = entityA->getPosition() - 0.5f * penetrationInTreeUnits;                     EntityItemProperties propertiesA = entityA->getProperties();                    EntityItemID idA(entityA->getID());                    propertiesA.setVelocity(newVelocityA * (float)TREE_SCALE);                    propertiesA.setPosition(newPositionA * (float)TREE_SCALE);                    propertiesA.setLastEdited(now);                     _entityTree->updateEntity(idA, propertiesA);                    _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idA, propertiesA);                }                             // unless the entity is configured to not be moved by collision, calculate it's new position                // and velocity and apply it                if (wantToMoveB) {                    glm::vec3 newVelocityB = entityB->getVelocity() + axialVelocity * massRatioB;                    glm::vec3 newPositionB = entityB->getPosition() + 0.5f * penetrationInTreeUnits;                     EntityItemProperties propertiesB = entityB->getProperties();                     EntityItemID idB(entityB->getID());                    propertiesB.setVelocity(newVelocityB * (float)TREE_SCALE);                    propertiesB.setPosition(newPositionB * (float)TREE_SCALE);                    propertiesB.setLastEdited(now);                     _entityTree->updateEntity(idB, propertiesB);                    _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idB, propertiesB);                }                  }        }    }} void EntityCollisionSystem::updateCollisionWithAvatars(EntityItem* entity) {        // Entities that are in hand, don't collide with avatars    if (!_avatars) {        return;    }     if (entity->getIgnoreForCollisions() || !entity->getCollisionsWillMove()) {        return; // bail early if this entity is to be ignored or wont move    }     glm::vec3 center = entity->getPosition() * (float)(TREE_SCALE);    float radius = entity->getRadius() * (float)(TREE_SCALE);    const float ELASTICITY = 0.9f;    const float DAMPING = 0.1f;    glm::vec3 penetration;     _collisions.clear();    foreach (const AvatarSharedPointer& avatarPointer, _avatars->getAvatarHash()) {        AvatarData* avatar = avatarPointer.data();         float totalRadius = avatar->getBoundingRadius() + radius;        glm::vec3 relativePosition = center - avatar->getPosition();        if (glm::dot(relativePosition, relativePosition) > (totalRadius * totalRadius)) {            continue;        }         if (avatar->findSphereCollisions(center, radius, _collisions)) {            int numCollisions = _collisions.size();            for (int i = 0; i < numCollisions; ++i) {                CollisionInfo* collision = _collisions.getCollision(i);                collision->_damping = DAMPING;                collision->_elasticity = ELASTICITY;                    collision->_addedVelocity /= (float)(TREE_SCALE);                glm::vec3 relativeVelocity = collision->_addedVelocity - entity->getVelocity();                    if (glm::dot(relativeVelocity, collision->_penetration) <= 0.0f) {                    // only collide when Entity and collision point are moving toward each other                    // (doing this prevents some "collision snagging" when Entity penetrates the object)                    collision->_penetration /= (float)(TREE_SCALE);                    applyHardCollision(entity, *collision);                }            }        }    }} void EntityCollisionSystem::applyHardCollision(EntityItem* entity, const CollisionInfo& collisionInfo) {     // don't collide entities with unknown IDs,    if (!entity->isKnownID()) {        return;    }     // HALTING_* params are determined using expected acceleration of gravity over some timescale.      // This is a HACK for entities that bounce in a 1.0 gravitational field and should eventually be made more universal.    const float HALTING_ENTITY_PERIOD = 0.0167f;  // ~1/60th of a second    const float HALTING_ENTITY_SPEED = 9.8 * HALTING_ENTITY_PERIOD / (float)(TREE_SCALE);     //    //  Update the entity in response to a hard collision.  Position will be reset exactly    //  to outside the colliding surface.  Velocity will be modified according to elasticity.    //    //  if elasticity = 0.0, collision is inelastic (vel normal to collision is lost)    //  if elasticity = 1.0, collision is 100% elastic.    //    glm::vec3 position = entity->getPosition();    glm::vec3 velocity = entity->getVelocity();     const float EPSILON = 0.0f;    glm::vec3 relativeVelocity = collisionInfo._addedVelocity - velocity;    float velocityDotPenetration = glm::dot(relativeVelocity, collisionInfo._penetration);    if (velocityDotPenetration < EPSILON) {        // entity is moving into collision surface        //        // TODO: do something smarter here by comparing the mass of the entity vs that of the other thing         // (other's mass could be stored in the Collision Info).  The smaller mass should surrender more         // position offset and should slave more to the other's velocity in the static-friction case.        position -= collisionInfo._penetration;         if (glm::length(relativeVelocity) < HALTING_ENTITY_SPEED) {            // static friction kicks in and entities moves with colliding object            velocity = collisionInfo._addedVelocity;        } else {            glm::vec3 direction = glm::normalize(collisionInfo._penetration);            velocity += glm::dot(relativeVelocity, direction) * (1.0f + collisionInfo._elasticity) * direction;    // dynamic reflection            velocity += glm::clamp(collisionInfo._damping, 0.0f, 1.0f) * (relativeVelocity - glm::dot(relativeVelocity, direction) * direction);   // dynamic friction        }    }     EntityItemProperties properties = entity->getProperties();    EntityItemID entityItemID(entity->getID());     properties.setPosition(position * (float)TREE_SCALE);    properties.setVelocity(velocity * (float)TREE_SCALE);    properties.setLastEdited(usecTimestampNow());     _entityTree->updateEntity(entityItemID, properties);    _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, entityItemID, properties);}