overte-JulianGro/interface/src/raypick/CollisionPick.cpp

//
//  Created by Sabrina Shanman 7/16/2018
//  Copyright 2018 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 "CollisionPick.h"

#include <QtCore/QDebug>

#include <glm/gtx/transform.hpp>

#include "PhysicsCollisionGroups.h"
#include "ScriptEngineLogging.h"
#include "UUIDHasher.h"

PickResultPointer CollisionPickResult::compareAndProcessNewResult(const PickResultPointer& newRes) {
    const std::shared_ptr<CollisionPickResult> newCollisionResult = std::static_pointer_cast<CollisionPickResult>(newRes);

    if (entityIntersections.size()) {
        entityIntersections.insert(entityIntersections.cend(), newCollisionResult->entityIntersections.begin(), newCollisionResult->entityIntersections.end());
    } else {
        entityIntersections = newCollisionResult->entityIntersections;
    }

    if (avatarIntersections.size()) {
        avatarIntersections.insert(avatarIntersections.cend(), newCollisionResult->avatarIntersections.begin(), newCollisionResult->avatarIntersections.end());
    } else {
        avatarIntersections = newCollisionResult->avatarIntersections;
    }

    intersects = entityIntersections.size() || avatarIntersections.size();

    return std::make_shared<CollisionPickResult>(*this);
}

void buildObjectIntersectionsMap(IntersectionType intersectionType, const std::vector<ContactTestResult>& objectIntersections, std::unordered_map<QUuid, QVariantMap>& intersections, std::unordered_map<QUuid, QVariantList>& collisionPointPairs) {
    for (auto& objectIntersection : objectIntersections) {
        auto at = intersections.find(objectIntersection.foundID);
        if (at == intersections.end()) {
            QVariantMap intersectingObject;
            intersectingObject["id"] = objectIntersection.foundID;
            intersectingObject["type"] = intersectionType;
            intersections[objectIntersection.foundID] = intersectingObject;

            collisionPointPairs[objectIntersection.foundID] = QVariantList();
        }

        QVariantMap collisionPointPair;
        collisionPointPair["pointOnPick"] = vec3toVariant(objectIntersection.testCollisionPoint);
        collisionPointPair["pointOnObject"] = vec3toVariant(objectIntersection.foundCollisionPoint);
        collisionPointPair["normalOnPick"] = vec3toVariant(objectIntersection.collisionNormal);

        collisionPointPairs[objectIntersection.foundID].append(collisionPointPair);
    }
}

QVariantMap CollisionPickResult::toVariantMap() const {
    QVariantMap variantMap;

    variantMap["intersects"] = intersects;

    std::unordered_map<QUuid, QVariantMap> intersections;
    std::unordered_map<QUuid, QVariantList> collisionPointPairs;

    buildObjectIntersectionsMap(ENTITY, entityIntersections, intersections, collisionPointPairs);
    buildObjectIntersectionsMap(AVATAR, avatarIntersections, intersections, collisionPointPairs);

    QVariantList qIntersectingObjects;
    for (auto& intersectionKeyVal : intersections) {
        const QUuid& id = intersectionKeyVal.first;
        QVariantMap& intersection = intersectionKeyVal.second;

        intersection["collisionContacts"] = collisionPointPairs[id];
        qIntersectingObjects.append(intersection);
    }

    variantMap["intersectingObjects"] = qIntersectingObjects;
    variantMap["collisionRegion"] = pickVariant;

    return variantMap;
}

bool CollisionPick::isLoaded() const {
    return !_mathPick.shouldComputeShapeInfo() || (_cachedResource && _cachedResource->isLoaded());
}

bool CollisionPick::getShapeInfoReady(const CollisionRegion& pick) {
    if (_mathPick.shouldComputeShapeInfo()) {
        if (_cachedResource && _cachedResource->isLoaded()) {
            computeShapeInfo(pick, *_mathPick.shapeInfo, _cachedResource);
            _mathPick.loaded = true;
        } else {
            _mathPick.loaded = false;
        }
    } else {
        computeShapeInfoDimensionsOnly(pick, *_mathPick.shapeInfo, _cachedResource);
        _mathPick.loaded = true;
    }

    return _mathPick.loaded;
}

void CollisionPick::computeShapeInfoDimensionsOnly(const CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource) {
    ShapeType type = shapeInfo.getType();
    glm::vec3 dimensions = pick.transform.getScale();
    QString modelURL = (resource ? resource->getURL().toString() : "");
    if (type == SHAPE_TYPE_COMPOUND) {
        shapeInfo.setParams(type, dimensions, modelURL);
    } else if (type >= SHAPE_TYPE_SIMPLE_HULL && type <= SHAPE_TYPE_STATIC_MESH) {
        shapeInfo.setParams(type, 0.5f * dimensions, modelURL);
    } else {
        shapeInfo.setParams(type, 0.5f * dimensions, modelURL);
    }
}

void CollisionPick::computeShapeInfo(const CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource) {
    // This code was copied and modified from RenderableModelEntityItem::computeShapeInfo
    // TODO: Move to some shared code area (in entities-renderer? model-networking?)
    // after we verify this is working and do a diff comparison with RenderableModelEntityItem::computeShapeInfo
    // to consolidate the code.
    // We may also want to make computeShapeInfo always abstract away from the gpu model mesh, like it does here.
    const uint32_t TRIANGLE_STRIDE = 3;
    const uint32_t QUAD_STRIDE = 4;

    ShapeType type = shapeInfo.getType();
    glm::vec3 dimensions = pick.transform.getScale();
    if (type == SHAPE_TYPE_COMPOUND) {
        // should never fall in here when collision model not fully loaded
        // TODO: assert that all geometries exist and are loaded
        //assert(_model && _model->isLoaded() && _compoundShapeResource && _compoundShapeResource->isLoaded());
        const HFMModel& collisionModel = resource->getHFMModel();

        ShapeInfo::PointCollection& pointCollection = shapeInfo.getPointCollection();
        pointCollection.clear();
        uint32_t i = 0;

        // the way OBJ files get read, each section under a "g" line is its own meshPart.  We only expect
        // to find one actual "mesh" (with one or more meshParts in it), but we loop over the meshes, just in case.
        foreach (const HFMMesh& mesh, collisionModel.meshes) {
            // each meshPart is a convex hull
            foreach (const HFMMeshPart &meshPart, mesh.parts) {
                pointCollection.push_back(QVector<glm::vec3>());
                ShapeInfo::PointList& pointsInPart = pointCollection[i];

                // run through all the triangles and (uniquely) add each point to the hull
                uint32_t numIndices = (uint32_t)meshPart.triangleIndices.size();
                // TODO: assert rather than workaround after we start sanitizing HFMMesh higher up
                //assert(numIndices % TRIANGLE_STRIDE == 0);
                numIndices -= numIndices % TRIANGLE_STRIDE; // WORKAROUND lack of sanity checking in FBXSerializer

                for (uint32_t j = 0; j < numIndices; j += TRIANGLE_STRIDE) {
                    glm::vec3 p0 = mesh.vertices[meshPart.triangleIndices[j]];
                    glm::vec3 p1 = mesh.vertices[meshPart.triangleIndices[j + 1]];
                    glm::vec3 p2 = mesh.vertices[meshPart.triangleIndices[j + 2]];
                    if (!pointsInPart.contains(p0)) {
                        pointsInPart << p0;
                    }
                    if (!pointsInPart.contains(p1)) {
                        pointsInPart << p1;
                    }
                    if (!pointsInPart.contains(p2)) {
                        pointsInPart << p2;
                    }
                }

                // run through all the quads and (uniquely) add each point to the hull
                numIndices = (uint32_t)meshPart.quadIndices.size();
                // TODO: assert rather than workaround after we start sanitizing HFMMesh higher up
                //assert(numIndices % QUAD_STRIDE == 0);
                numIndices -= numIndices % QUAD_STRIDE; // WORKAROUND lack of sanity checking in FBXSerializer

                for (uint32_t j = 0; j < numIndices; j += QUAD_STRIDE) {
                    glm::vec3 p0 = mesh.vertices[meshPart.quadIndices[j]];
                    glm::vec3 p1 = mesh.vertices[meshPart.quadIndices[j + 1]];
                    glm::vec3 p2 = mesh.vertices[meshPart.quadIndices[j + 2]];
                    glm::vec3 p3 = mesh.vertices[meshPart.quadIndices[j + 3]];
                    if (!pointsInPart.contains(p0)) {
                        pointsInPart << p0;
                    }
                    if (!pointsInPart.contains(p1)) {
                        pointsInPart << p1;
                    }
                    if (!pointsInPart.contains(p2)) {
                        pointsInPart << p2;
                    }
                    if (!pointsInPart.contains(p3)) {
                        pointsInPart << p3;
                    }
                }

                if (pointsInPart.size() == 0) {
                    qCDebug(scriptengine) << "Warning -- meshPart has no faces";
                    pointCollection.pop_back();
                    continue;
                }
                ++i;
            }
        }

        // We expect that the collision model will have the same units and will be displaced
        // from its origin in the same way the visual model is.  The visual model has
        // been centered and probably scaled.  We take the scaling and offset which were applied
        // to the visual model and apply them to the collision model (without regard for the
        // collision model's extents).

        glm::vec3 scaleToFit = dimensions / resource->getHFMModel().getUnscaledMeshExtents().size();
        // multiply each point by scale
        for (int32_t i = 0; i < pointCollection.size(); i++) {
            for (int32_t j = 0; j < pointCollection[i].size(); j++) {
                // back compensate for registration so we can apply that offset to the shapeInfo later
                pointCollection[i][j] = scaleToFit * pointCollection[i][j];
            }
        }
        shapeInfo.setParams(type, dimensions, resource->getURL().toString());
    } else if (type >= SHAPE_TYPE_SIMPLE_HULL && type <= SHAPE_TYPE_STATIC_MESH) {
        const HFMModel& hfmModel = resource->getHFMModel();
        int numHFMMeshes = hfmModel.meshes.size();
        int totalNumVertices = 0;
        for (int i = 0; i < numHFMMeshes; i++) {
            const HFMMesh& mesh = hfmModel.meshes.at(i);
            totalNumVertices += mesh.vertices.size();
        }
        const int32_t MAX_VERTICES_PER_STATIC_MESH = 1e6;
        if (totalNumVertices > MAX_VERTICES_PER_STATIC_MESH) {
            qWarning() << "model" << "has too many vertices" << totalNumVertices << "and will collide as a box.";
            shapeInfo.setParams(SHAPE_TYPE_BOX, 0.5f * dimensions);
            return;
        }

        auto& meshes = resource->getHFMModel().meshes;
        int32_t numMeshes = (int32_t)(meshes.size());

        const int MAX_ALLOWED_MESH_COUNT = 1000;
        if (numMeshes > MAX_ALLOWED_MESH_COUNT) {
            // too many will cause the deadlock timer to throw...
            shapeInfo.setParams(SHAPE_TYPE_BOX, 0.5f * dimensions);
            return;
        }

        ShapeInfo::PointCollection& pointCollection = shapeInfo.getPointCollection();
        pointCollection.clear();
        if (type == SHAPE_TYPE_SIMPLE_COMPOUND) {
            pointCollection.resize(numMeshes);
        } else {
            pointCollection.resize(1);
        }

        ShapeInfo::TriangleIndices& triangleIndices = shapeInfo.getTriangleIndices();
        triangleIndices.clear();

        Extents extents;
        int32_t meshCount = 0;
        int32_t pointListIndex = 0;
        for (auto& mesh : meshes) {
            if (!mesh.vertices.size()) {
                continue;
            }
            QVector<glm::vec3> vertices = mesh.vertices;

            ShapeInfo::PointList& points = pointCollection[pointListIndex];

            // reserve room
            int32_t sizeToReserve = (int32_t)(vertices.count());
            if (type == SHAPE_TYPE_SIMPLE_COMPOUND) {
                // a list of points for each mesh
                pointListIndex++;
            } else {
                // only one list of points
                sizeToReserve += (int32_t)points.size();
            }
            points.reserve(sizeToReserve);

            // copy points
            const glm::vec3* vertexItr = vertices.cbegin();
            while (vertexItr != vertices.cend()) {
                glm::vec3 point = *vertexItr;
                points.push_back(point);
                extents.addPoint(point);
                ++vertexItr;
            }

            if (type == SHAPE_TYPE_STATIC_MESH) {
                // copy into triangleIndices
                size_t triangleIndicesCount = 0;
                for (const HFMMeshPart& meshPart : mesh.parts) {
                    triangleIndicesCount += meshPart.triangleIndices.count();
                }
                triangleIndices.reserve((int)triangleIndicesCount);

                for (const HFMMeshPart& meshPart : mesh.parts) {
                    const int* indexItr = meshPart.triangleIndices.cbegin();
                    while (indexItr != meshPart.triangleIndices.cend()) {
                        triangleIndices.push_back(*indexItr);
                        ++indexItr;
                    }
                }
            } else if (type == SHAPE_TYPE_SIMPLE_COMPOUND) {
                // for each mesh copy unique part indices, separated by special bogus (flag) index values
                for (const HFMMeshPart& meshPart : mesh.parts) {
                    // collect unique list of indices for this part
                    std::set<int32_t> uniqueIndices;
                    auto numIndices = meshPart.triangleIndices.count();
                    // TODO: assert rather than workaround after we start sanitizing HFMMesh higher up
                    //assert(numIndices% TRIANGLE_STRIDE == 0);
                    numIndices -= numIndices % TRIANGLE_STRIDE; // WORKAROUND lack of sanity checking in FBXSerializer

                    auto indexItr = meshPart.triangleIndices.cbegin();
                    while (indexItr != meshPart.triangleIndices.cend()) {
                        uniqueIndices.insert(*indexItr);
                        ++indexItr;
                    }

                    // store uniqueIndices in triangleIndices
                    triangleIndices.reserve(triangleIndices.size() + (int32_t)uniqueIndices.size());
                    for (auto index : uniqueIndices) {
                        triangleIndices.push_back(index);
                    }
                    // flag end of part
                    triangleIndices.push_back(END_OF_MESH_PART);
                }
                // flag end of mesh
                triangleIndices.push_back(END_OF_MESH);
            }
            ++meshCount;
        }

        // scale and shift
        glm::vec3 extentsSize = extents.size();
        glm::vec3 scaleToFit = dimensions / extentsSize;
        for (int32_t i = 0; i < 3; ++i) {
            if (extentsSize[i] < 1.0e-6f) {
                scaleToFit[i] = 1.0f;
            }
        }
        for (auto points : pointCollection) {
            for (int32_t i = 0; i < points.size(); ++i) {
                points[i] = (points[i] * scaleToFit);
            }
        }

        shapeInfo.setParams(type, 0.5f * dimensions, resource->getURL().toString());
    }
}

CollisionPick::CollisionPick(const PickFilter& filter, float maxDistance, bool enabled, bool scaleWithParent, CollisionRegion collisionRegion, PhysicsEnginePointer physicsEngine) :
    Pick(collisionRegion, filter, maxDistance, enabled),
    _scaleWithParent(scaleWithParent),
    _physicsEngine(physicsEngine) {
    if (collisionRegion.shouldComputeShapeInfo()) {
        _cachedResource = DependencyManager::get<ModelCache>()->getCollisionGeometryResource(collisionRegion.modelURL);
    }
    _mathPick.loaded = isLoaded();
}

CollisionRegion CollisionPick::getMathematicalPick() const {
    CollisionRegion mathPick = _mathPick;
    mathPick.loaded = isLoaded();
    if (parentTransform) {
        Transform parentTransformValue = parentTransform->getTransform();
        mathPick.transform = parentTransformValue.worldTransform(mathPick.transform);

        if (_scaleWithParent) {
            glm::vec3 scale = parentTransformValue.getScale();
            float largestDimension = glm::max(glm::max(scale.x, scale.y), scale.z);
            mathPick.threshold *= largestDimension;
        } else {
            // We need to undo parent scaling after-the-fact because the parent's scale was needed to calculate this mathPick's position
            mathPick.transform.setScale(_mathPick.transform.getScale());
        }
    }
    return mathPick;
}

void CollisionPick::filterIntersections(std::vector<ContactTestResult>& intersections) const {
    const QVector<QUuid>& ignoreItems = getIgnoreItems();
    const QVector<QUuid>& includeItems = getIncludeItems();
    bool isWhitelist = !includeItems.empty();

    if (!isWhitelist && ignoreItems.empty()) {
        return;
    }

    std::vector<ContactTestResult> filteredIntersections;

    int n = (int)intersections.size();
    for (int i = 0; i < n; i++) {
        auto& intersection = intersections[i];
        const QUuid& id = intersection.foundID;
        if (!ignoreItems.contains(id) && (!isWhitelist || includeItems.contains(id))) {
            filteredIntersections.push_back(intersection);
        }
    }

    intersections = filteredIntersections;
}

PickResultPointer CollisionPick::getEntityIntersection(const CollisionRegion& pick) {
    if (!pick.loaded) {
        // Cannot compute result
        return std::make_shared<CollisionPickResult>(pick.toVariantMap(), std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
    }
    getShapeInfoReady(pick);
    
    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *_mathPick.shapeInfo, pick.transform, pick.collisionGroup, pick.threshold);
    filterIntersections(entityIntersections);
    return std::make_shared<CollisionPickResult>(pick, entityIntersections, std::vector<ContactTestResult>());
}

PickResultPointer CollisionPick::getAvatarIntersection(const CollisionRegion& pick) {
    if (!pick.loaded) {
        // Cannot compute result
        return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
    }
    getShapeInfoReady(pick);
    
    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *_mathPick.shapeInfo, pick.transform, pick.collisionGroup, pick.threshold);
    filterIntersections(avatarIntersections);
    return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), avatarIntersections);
}

PickResultPointer CollisionPick::getHUDIntersection(const CollisionRegion& pick) {
    return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
}

Transform CollisionPick::getResultTransform() const {
    Transform transform;
    transform.setTranslation(_mathPick.transform.getTranslation());
    return transform;
}