imstkClosedSurfaceMeshToMeshCD.cpp

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#include "imstkClosedSurfaceMeshToMeshCD.h"
#include "imstkCollisionUtils.h"
#include "imstkLineMesh.h"
#include "imstkSurfaceMesh.h"
#include "imstkVecDataArray.h"

#include <unordered_set>

namespace imstk
{
struct EdgePair
{
    EdgePair(uint32_t a1, uint32_t a2, uint32_t b1, uint32_t b2)
    {
        edgeA[0] = a1;
        edgeA[1] = a2;
        edgeB[0] = b1;
        edgeB[1] = b2;

        edgeAId = getIdA();
        edgeBId = getIdB();
    }

    bool operator==(const EdgePair& other) const
    {
        return (edgeAId == other.edgeAId && edgeBId == other.edgeBId)
               || (edgeAId == other.edgeBId && edgeBId == other.edgeAId);
    }

    // These functions return a unique int for an edge, order doesn't matter
    // ie: f(vertexId1, vertexId2)=f(vertexId2, vertexId1)
    const uint32_t getIdA() const
    {
        const uint32_t max = std::max(edgeA[0], edgeA[1]);
        const uint32_t min = std::min(edgeA[0], edgeA[1]);
        return max * (max + 1) / 2 + min;
    }

    const uint32_t getIdB() const
    {
        const uint32_t max = std::max(edgeB[0], edgeB[1]);
        const uint32_t min = std::min(edgeB[0], edgeB[1]);
        return max * (max + 1) / 2 + min;
    }

    uint32_t edgeA[2];
    uint32_t edgeAId;
    uint32_t edgeB[2];
    uint32_t edgeBId;
};
} // namespace imstk

namespace std
{
template<>
struct hash<imstk::EdgePair>
{
    // EdgePair has 4 uints to hash, they bound the same range, 0 to max vertices of a mesh
    // A complete unique hash split into 4, would limit us to 256 max vertices so we will have
    // collisions but they will be unlikely given small portions of the mesh are in contact at
    // any one time
    std::size_t operator()(const imstk::EdgePair& k) const
    {
        // Shift by 8 each time, there will be overlap every 256 ints
        //return ((k.edgeA[0] ^ (k.edgeA[1] << 8)) ^ (k.edgeB[0] << 16)) ^ (k.edgeB[1] << 24);

        // The edge ids are more compact since f(1,0)=f(0,1) there are fewer permutations,
        // This should allow up to ~360 max vertices..., not that much better
        return (k.edgeAId ^ (k.edgeBId << 16));
    }
};
} // namespace std

namespace imstk
{
struct PointSetData
{
    PointSetData(std::shared_ptr<PointSet> pointSet);

    // Get geometry A data
    std::shared_ptr<PointSet> m_pointSet;
    const VecDataArray<double, 3>& vertices;
};

struct SurfMeshData
{
    SurfMeshData(std::shared_ptr<SurfaceMesh> surfMesh);

    // Get geometry B data
    std::shared_ptr<SurfaceMesh> m_surfMesh;
    const VecDataArray<int, 3>& cells;
    const VecDataArray<double, 3>& vertices;
    const std::vector<std::unordered_set<int>>& vertexFaces;
    const VecDataArray<double, 3>& faceNormals;
};

PointSetData::PointSetData(std::shared_ptr<PointSet> pointSet) :
    m_pointSet(pointSet),
    vertices(*pointSet->getVertexPositions())
{
}

SurfMeshData::SurfMeshData(std::shared_ptr<SurfaceMesh> surfMesh) :
    m_surfMesh(surfMesh),
    cells(*surfMesh->getCells()),
    vertices(*surfMesh->getVertexPositions()),
    vertexFaces(surfMesh->getVertexToCellMap()),
    faceNormals(*surfMesh->getCellNormals())
{
}

static Vec3d
vertexPseudoNormalFromTriangle(const int vertexIndex, const SurfMeshData& surfMeshData)
{
    // Compute the pseudonormal on the mesh at the point
    // Identify incident faces to the point and weight sum their normals
    double sum  = 0.0;
    Vec3d  nSum = Vec3d::Zero();
    for (int neighborFaceIndex : surfMeshData.vertexFaces[vertexIndex])
    {
        Vec3i cell = surfMeshData.cells[neighborFaceIndex];
        // Ensure vertexIndex is in 0
        if (cell[1] == vertexIndex)
        {
            std::swap(cell[0], cell[1]);
        }
        else if (cell[2] == vertexIndex)
        {
            std::swap(cell[0], cell[2]);
        }

        const Vec3d  ab    = (surfMeshData.vertices[cell[1]] - surfMeshData.vertices[vertexIndex]).normalized();
        const Vec3d  bc    = (surfMeshData.vertices[cell[2]] - surfMeshData.vertices[vertexIndex]).normalized();
        const double angle = acos(ab.dot(bc));
        const Vec3d  n     = angle * surfMeshData.faceNormals[neighborFaceIndex];

        sum  += n.norm();
        nSum += n;
    }
    return nSum / sum;
}

static Vec3d
edgePseudoNormalFromTriangle(const Vec2i& vertexIds, const SurfMeshData& surfMeshData)
{
    // Find the two cells that have both vertexIds
    double sum  = 0.0;
    Vec3d  nSum = Vec3d::Zero();
    for (int neighborFaceIndex : surfMeshData.vertexFaces[vertexIds[0]])
    {
        const Vec3i& cell     = surfMeshData.cells[neighborFaceIndex];
        bool         found[2] = { false, false };
        for (int j = 0; j < 3; j++)
        {
            if (cell[j] == vertexIds[0])
            {
                found[0] = true;
            }
            else if (cell[j] == vertexIds[1])
            {
                found[1] = true;
            }
        }
        // If it contains both vertices its a face to the edge
        if (found[0] && found[1])
        {
            const Vec3d n = PI * surfMeshData.faceNormals[neighborFaceIndex];
            sum  += n.norm();
            nSum += n;
        }
    }
    return nSum / sum;
}

static double
polySignedDist(const Vec3d& pos, const SurfMeshData& surfMeshData,
               int& caseType, Vec3i& vIds, int& closestCell)
{
    closestCell = -1;
    Vec3d  closestPt       = Vec3d::Zero();
    double minSqrDist      = IMSTK_DOUBLE_MAX;
    int    closestCellCase = -1;

    // Find the closest point out of all elements
    // \todo: We could early reject backface cull all triangles (this is effectively case 6 done early)
    for (int j = 0; j < surfMeshData.cells.size(); j++)
    {
        const Vec3i& cell = surfMeshData.cells[j];
        const Vec3d& x1   = surfMeshData.vertices[cell[0]];
        const Vec3d& x2   = surfMeshData.vertices[cell[1]];
        const Vec3d& x3   = surfMeshData.vertices[cell[2]];

        int          ptOnTriangleCaseType;
        const Vec3d  closestPtOnTri = CollisionUtils::closestPointOnTriangle(pos, x1, x2, x3, ptOnTriangleCaseType);
        const double sqrDist = (closestPtOnTri - pos).squaredNorm();
        if (sqrDist < minSqrDist)
        {
            minSqrDist      = sqrDist;
            closestPt       = closestPtOnTri;
            closestCell     = j;
            closestCellCase = ptOnTriangleCaseType;
        }
    }

    // We use the normal of the nearest element to determine sign, but we can't just use the
    // normal as there are discontinuities at the edges and vertices. We instead use the
    // "angle-weighted psuedonormal" given adjacent elements

    // Closest element is a vertex
    if (closestCellCase == 0 || closestCellCase == 1 || closestCellCase == 2)
    {
        int vertexIndex = surfMeshData.cells[closestCell][0]; // a
        if (closestCellCase == 1)
        {
            vertexIndex = surfMeshData.cells[closestCell][1]; // b
        }
        else if (closestCellCase == 2)
        {
            vertexIndex = surfMeshData.cells[closestCell][2]; // c
        }

        const Vec3d psuedoN = vertexPseudoNormalFromTriangle(vertexIndex, surfMeshData);
        caseType = 0;
        vIds[0]  = vertexIndex;
        return (pos - closestPt).dot(psuedoN);
    }
    // Closest element is an edge
    else if (closestCellCase == 3 || closestCellCase == 4 || closestCellCase == 5)
    {
        Vec2i vertexIds = { surfMeshData.cells[closestCell][0], surfMeshData.cells[closestCell][1] }; // ab
        if (closestCellCase == 4)
        {
            vertexIds = { surfMeshData.cells[closestCell][1], surfMeshData.cells[closestCell][2] }; // bc
        }
        else if (closestCellCase == 5)
        {
            vertexIds = { surfMeshData.cells[closestCell][2], surfMeshData.cells[closestCell][0] }; // ca
        }

        const Vec3d psuedoN = edgePseudoNormalFromTriangle(vertexIds, surfMeshData);
        caseType = 1;
        vIds[0]  = vertexIds[0];
        vIds[1]  = vertexIds[1];
        return (pos - closestPt).dot(psuedoN);
    }
    // Closest element is the triangle
    else if (closestCellCase == 6)
    {
        //const Vec3d psuedoN = (2.0 * PI * faceNormals[closestCell]).normalized();
        const Vec3d psuedoN = surfMeshData.faceNormals[closestCell]; // Assume normalized
        caseType = 2;
        vIds[0]  = surfMeshData.cells[closestCell][0];
        vIds[1]  = surfMeshData.cells[closestCell][1];
        vIds[2]  = surfMeshData.cells[closestCell][2];
        return (pos - closestPt).dot(psuedoN);
    }
    // Should only ever occur if there are no elements
    else
    {
        caseType = -1;
        return IMSTK_DOUBLE_MAX;
    }
}

ClosedSurfaceMeshToMeshCD::ClosedSurfaceMeshToMeshCD()
{
    setRequiredInputType<PointSet>(0);
    setRequiredInputType<SurfaceMesh>(1);
}

void
ClosedSurfaceMeshToMeshCD::computeCollisionDataAB(
    std::shared_ptr<Geometry>      geomA,
    std::shared_ptr<Geometry>      geomB,
    std::vector<CollisionElement>& elementsA,
    std::vector<CollisionElement>& elementsB)
{
    // Broad phase collision
    if (doBroadPhaseCollisionCheck(geomA, geomB))
    {
        auto pointSet = std::dynamic_pointer_cast<PointSet>(geomA);
        auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
        surfMesh->computeTrianglesNormals();
        surfMesh->computeVertexToCellMap();

        // Narrow phase
        if (m_generateVertexTriangleContacts)
        {
            if (m_vertexInside.size() < pointSet->getNumVertices())
            {
                m_vertexInside = std::vector<bool>(pointSet->getNumVertices(), false);
            }
            if (m_signedDistances.size() < pointSet->getNumVertices())
            {
                m_signedDistances = DataArray<double>(pointSet->getNumVertices());
            }

            vertexToTriangleTest(geomA, geomB, elementsA, elementsB);

            if (m_generateEdgeEdgeContacts)
            {
                if (geomA->getTypeName() == LineMesh::getStaticTypeName())
                {
                    lineMeshEdgeToTriangleTest(geomA, geomB, elementsA, elementsB);
                }
                else if (geomA->getTypeName() == SurfaceMesh::getStaticTypeName())
                {
                    surfMeshEdgeToTriangleTest(geomA, geomB, elementsA, elementsB);
                }
            }
        }
    }
}

void
ClosedSurfaceMeshToMeshCD::vertexToTriangleTest(
    std::shared_ptr<Geometry>      geomA,
    std::shared_ptr<Geometry>      geomB,
    std::vector<CollisionElement>& elementsA,
    std::vector<CollisionElement>& elementsB)
{
    PointSetData pointSetData(std::dynamic_pointer_cast<PointSet>(geomA));
    SurfMeshData surfMeshData(std::dynamic_pointer_cast<SurfaceMesh>(geomB));

    // For every vertex
    for (int i = 0; i < pointSetData.vertices.size(); i++)
    {
        const Vec3d& p = pointSetData.vertices[i];

        int          caseType    = -1;
        int          closestCell = -1;
        Vec3i        vertexIds   = Vec3i::Zero();
        const double signedDist  = polySignedDist(p, surfMeshData, caseType, vertexIds, closestCell);
        m_signedDistances[i] = signedDist;
        if (signedDist <= 0.0)
        {
            m_vertexInside[i] = true;
            // The nearest feature to this vertex is another vertex
            if (caseType == 0)
            {
                CellIndexElement elemA;
                elemA.ids[0]   = i;
                elemA.idCount  = 1;
                elemA.cellType = IMSTK_VERTEX;

                CellIndexElement elemB;
                elemB.ids[0]   = vertexIds[0];
                elemB.parentId = closestCell; // Triangle id
                elemB.idCount  = 1;
                elemB.cellType = IMSTK_VERTEX;

                elementsA.push_back(elemA);
                elementsB.push_back(elemB);
            }
            // The nearest feature to this vertex is an edge
            else if (caseType == 1)
            {
                CellIndexElement elemA;
                elemA.ids[0]   = i;
                elemA.idCount  = 1;
                elemA.cellType = IMSTK_VERTEX;

                CellIndexElement elemB;
                elemB.ids[0]   = vertexIds[0];
                elemB.ids[1]   = vertexIds[1];
                elemB.parentId = closestCell; // Triangle id
                elemB.idCount  = 2;
                elemB.cellType = IMSTK_EDGE;

                elementsA.push_back(elemA);
                elementsB.push_back(elemB);
            }
            // The nearest feature to this vertex is a triangle face
            else if (caseType == 2)
            {
                CellIndexElement elemA;
                elemA.ids[0]   = i;
                elemA.idCount  = 1;
                elemA.cellType = IMSTK_VERTEX;

                CellIndexElement elemB;
                elemB.ids[0]   = vertexIds[0];
                elemB.ids[1]   = vertexIds[1];
                elemB.ids[2]   = vertexIds[2];
                elemB.parentId = closestCell; // Triangle id
                elemB.idCount  = 3;
                elemB.cellType = IMSTK_TRIANGLE;

                elementsA.push_back(elemA);
                elementsB.push_back(elemB);
            }
        }
        else
        {
            m_vertexInside[i] = false;
        }
    }
}

void
ClosedSurfaceMeshToMeshCD::lineMeshEdgeToTriangleTest(
    std::shared_ptr<Geometry>      geomA,
    std::shared_ptr<Geometry>      geomB,
    std::vector<CollisionElement>& elementsA,
    std::vector<CollisionElement>& elementsB)
{
    SurfMeshData surfMeshBData(std::dynamic_pointer_cast<SurfaceMesh>(geomB));

    // Get geometry A data
    std::shared_ptr<LineMesh>                lineMesh = std::dynamic_pointer_cast<LineMesh>(geomA);
    std::shared_ptr<VecDataArray<double, 3>> meshAVerticesPtr = lineMesh->getVertexPositions();
    const VecDataArray<double, 3>&           meshAVertices    = *meshAVerticesPtr;
    std::shared_ptr<VecDataArray<int, 2>>    meshACellsPtr    = lineMesh->getCells();
    VecDataArray<int, 2>&                    meshACells       = *meshACellsPtr;

    const int triEdgePattern[3][2] = { { 0, 1 }, { 1, 2 }, { 2, 0 } };

    // For every edge/line segment of the line mesh
    for (int i = 0; i < meshACells.size(); i++)
    {
        const Vec2i& edgeA = meshACells[i];

        // Only check edges that don't exist totally inside
        if (!m_vertexInside[edgeA[0]] && !m_vertexInside[edgeA[1]])
        {
            double minSqrDist    = IMSTK_DOUBLE_MAX;
            int    closestTriId  = -1;
            int    closestEdgeId = -1;

            // For every triangle/cell of meshB
            for (int j = 0; j < surfMeshBData.cells.size(); j++)
            {
                const Vec3i& cellB = surfMeshBData.cells[j];

                // For every edge of that triangle
                for (int k = 0; k < 3; k++)
                {
                    const Vec2i edgeB(cellB[triEdgePattern[k][0]], cellB[triEdgePattern[k][1]]);

                    // Compute the closest point on the two edges
                    // Check the case, the edges must be within each others bounds/ranges
                    Vec3d ptA, ptB;
                    if (CollisionUtils::edgeToEdgeClosestPoints(
                        meshAVertices[edgeA[0]], meshAVertices[edgeA[1]],
                        surfMeshBData.vertices[edgeB[0]], surfMeshBData.vertices[edgeB[1]],
                        ptA, ptB) == 0)
                    {
                        // Find the closest element to this point on the edge
                        const double sqrDist = (ptB - ptA).squaredNorm();
                        // Use the closest one only
                        if (sqrDist < minSqrDist)
                        {
                            // Check if the point on the oppositie edge nearest to edgeB is inside B
                            int          caseType    = -1;
                            int          closestCell = -1;
                            Vec3i        vIds       = Vec3i::Zero();
                            const double signedDist = polySignedDist(ptA, surfMeshBData, caseType, vIds, closestCell);
                            if (signedDist <= 0.0)
                            {
                                minSqrDist    = sqrDist;
                                closestTriId  = j;
                                closestEdgeId = k;
                            }
                        }
                    }
                }
            }

            if (closestTriId != -1)
            {
                CellIndexElement elemA;
                elemA.ids[0]   = edgeA[0];
                elemA.ids[1]   = edgeA[1];
                elemA.parentId = i; // Edge id
                elemA.idCount  = 2;
                elemA.cellType = IMSTK_EDGE;

                CellIndexElement elemB;
                elemB.ids[0]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]];
                elemB.ids[1]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]];
                elemB.parentId = closestTriId; // Triangle id
                elemB.idCount  = 2;
                elemB.cellType = IMSTK_EDGE;

                elementsA.push_back(elemA);
                elementsB.push_back(elemB);
            }
        }
    }
}

void
ClosedSurfaceMeshToMeshCD::surfMeshEdgeToTriangleTest(
    std::shared_ptr<Geometry>      geomA,
    std::shared_ptr<Geometry>      geomB,
    std::vector<CollisionElement>& elementsA,
    std::vector<CollisionElement>& elementsB)
{
    SurfMeshData surfMeshBData(std::dynamic_pointer_cast<SurfaceMesh>(geomB));

    // Get geometry A data
    std::shared_ptr<SurfaceMesh>             surfMeshA = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
    std::shared_ptr<VecDataArray<double, 3>> meshAVerticesPtr = surfMeshA->getVertexPositions();
    const VecDataArray<double, 3>&           meshAVertices    = *meshAVerticesPtr;
    std::shared_ptr<VecDataArray<int, 3>>    meshACellsPtr    = surfMeshA->getCells();
    VecDataArray<int, 3>&                    meshACells       = *meshACellsPtr;

    std::unordered_set<EdgePair> hashedEdges;

    // We find the nearest points on every edge with every other edge. Sampling this point we
    // can determine the signed distance and whether that edge is "inside" another.
    //
    // We choose the nearest edge to resolve. Unfortunately we resolving to the nearest doesn't
    // give the same effect as in the vertex-triangle case.
    //
    // This works so long as the edges don't move too far past each other.
    // This is ultimately an effect of not having signs on the edges.
    //
    // Additionally we don't check edges whose vertices are already inside the closed surface (determined
    // in the vertex-triangle pass)
    const int triEdgePattern[3][2] = { { 0, 1 }, { 1, 2 }, { 2, 0 } };
    int       edgeCheckCount       = 0;
    if (m_generateEdgeEdgeContacts)
    {
        // For every edge/line segment of the line mesh
        for (int i = 0; i < meshACells.size(); i++)
        {
            const Vec3i& cellA = meshACells[i];

            // For every edge of triangle A
            for (int j = 0; j < 3; j++)
            {
                const Vec2i edgeA = Vec2i(cellA[triEdgePattern[j][0]], cellA[triEdgePattern[j][1]]);

                // Only check edges that don't exist totally inside
                if (!m_vertexInside[edgeA[0]] && !m_vertexInside[edgeA[1]])
                {
                    double minSqrDist    = IMSTK_DOUBLE_MAX;
                    int    closestTriId  = -1;
                    int    closestEdgeId = -1;

                    // If proximity is used, only check edges with vertices within proximity of the closed surface
                    if (m_proximity <= 0.0 || (m_signedDistances[edgeA[0]] < m_proximity && m_signedDistances[edgeA[1]] < m_proximity))
                    {
                        edgeCheckCount++;
                        // For every triangle/cell of meshB
                        for (int k = 0; k < surfMeshBData.cells.size(); k++)
                        {
                            const Vec3i& cellB = surfMeshBData.cells[k];

                            // For every edge of that triangle
                            for (int edgeId = 0; edgeId < 3; edgeId++)
                            {
                                const Vec2i edgeB(cellB[triEdgePattern[edgeId][0]], cellB[triEdgePattern[edgeId][1]]);

                                // Compute the closest point on the two edges
                                // Check the case, the edges must be within each others bounds/ranges
                                Vec3d ptA, ptB;
                                if (CollisionUtils::edgeToEdgeClosestPoints(
                                    meshAVertices[edgeA[0]], meshAVertices[edgeA[1]],
                                    surfMeshBData.vertices[edgeB[0]], surfMeshBData.vertices[edgeB[1]],
                                    ptA, ptB) == 0)
                                {
                                    // Find the closest element to this point on the edge
                                    const double sqrDist = (ptB - ptA).squaredNorm();
                                    // Use the closest one only
                                    if (sqrDist < minSqrDist)
                                    {
                                        // Check if the point on the oppositie edge nearest to edgeB is inside B
                                        int          caseType    = -1;
                                        int          closestCell = -1;
                                        Vec3i        vIds       = Vec3i::Zero();
                                        const double signedDist = polySignedDist(ptA, surfMeshBData, caseType, vIds, closestCell);
                                        if (signedDist <= 0.0)
                                        {
                                            minSqrDist    = sqrDist;
                                            closestTriId  = k;
                                            closestEdgeId = edgeId;
                                        }
                                    }
                                }
                            }
                        }
                    }

                    if (closestTriId != -1)
                    {
                        // Before inserting check if it already exists
                        EdgePair edgePair(
                            edgeA[0], edgeA[1],
                            surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]],
                            surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]]);
                        if (hashedEdges.count(edgePair) == 0)
                        {
                            CellIndexElement elemA;
                            elemA.ids[0]   = edgeA[0];
                            elemA.ids[1]   = edgeA[1];
                            elemA.parentId = i; // Triangle id
                            elemA.idCount  = 2;
                            elemA.cellType = IMSTK_EDGE;

                            CellIndexElement elemB;
                            elemB.ids[0]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]];
                            elemB.ids[1]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]];
                            elemB.parentId = closestTriId; // Triangle id
                            elemB.idCount  = 2;
                            elemB.cellType = IMSTK_EDGE;

                            elementsA.push_back(elemA);
                            elementsB.push_back(elemB);

                            hashedEdges.insert(edgePair);
                        }
                    }
                }
            }
        }
    }
}

bool
ClosedSurfaceMeshToMeshCD::doBroadPhaseCollisionCheck(
    std::shared_ptr<Geometry> geomA,
    std::shared_ptr<Geometry> geomB) const
{
    if (!m_doBroadPhase)
    {
        return true;
    }

    const auto mesh1 = std::dynamic_pointer_cast<PointSet>(geomA);
    const auto mesh2 = std::dynamic_pointer_cast<PointSet>(geomB);

    // Edge Case Ex: One point vs non-manifold SurfaceMesh (like a single triangle or plane)
    if (mesh1->getNumVertices() == 1 || mesh2->getNumVertices() == 1)
    {
        return true;
    }

    Vec3d min1, max1;
    mesh1->computeBoundingBox(min1, max1);

    Vec3d min2, max2;
    mesh2->computeBoundingBox(min2, max2);

    // Padding here helps with thin vs thin geometry
    min1 -= m_padding;
    max1 += m_padding;
    min2 -= m_padding;
    max2 += m_padding;

    return CollisionUtils::testAabbToAabb(
        min1[0], max1[0],
        min1[1], max1[1],
        min1[2], max1[2],
        min2[0], max2[0],
        min2[1], max2[1],
        min2[2], max2[2]);
}
} // namespace imstk